Stewart平台运动学

最新推荐文章于 2024-04-28 22:48:16 发布

石然Mark

最新推荐文章于 2024-04-28 22:48:16 发布

阅读量8.6k

点赞数 17

分类专栏： Speedgoat&dSPACE

本文链接：https://blog.csdn.net/heroshishiran/article/details/114417461

版权

Speedgoat&dSPACE 专栏收录该内容

5 篇文章 8 订阅

订阅专栏

Stewart平台运动学

运动学建模过程：

参考硕士学位论文《Stewart平台的滑模变结构控制方法研究》

1平台定义图

在这里插入图片描述

# 逆解

并联机构逆解较简单，可以得到解析解
在这里插入图片描述

# 正解

正解用数值解
在这里插入图片描述

# Matlab代码

在这里插入图片描述

逆解
function L  = Stewart_IK_AlgebraFunc(x,y,z,a,b,g)
%%运动学参数
%零初始位置时静平台位置（相对于静平台坐标系）
B1=[12;-14;0];
B2=[12;14;0];
B3=[6.12435565;17.39230485;0];
B4=[-18.12435565;3.39230485;0];
B5=[-18.12435565;-3.39230485;0];
B6=[6.12435565;-17.39230485;0];
%零初始位置时动平台位置（相对于动平台坐标系）
P1=[12;-2;0];
P2=[12;2;0];
P3=[-4.26794919;11.39230485;0];
P4=[-7.73205081;9.39230485;0];
P5=[-7.73205081;-9.39230485;0];
P6=[-4.26794919;-11.39230485;0];


%%
L = zeros(6,1);
X=[x;y;z];
RX=[1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];
RY=[cos(b) 0 sin(b);0 1 0;-sin(b) 0 cos(b)];
RZ=[cos(g) -sin(g) 0;sin(g) cos(g) 0; 0 0 1];
R=RZ*RY*RX;
l1=norm(X+R*P1-B1);
l2=norm(X+R*P2-B2);
l3=norm(X+R*P3-B3);
l4=norm(X+R*P4-B4);
l5=norm(X+R*P5-B5);
l6=norm(X+R*P6-B6);
L=[l1;l2;l3;l4;l5;l6];

正解：牛顿迭代法
function Out  = Stewart_IK_AlgebraFunc(Length)
x0=[0;0;20;0;0;0];
Out=x0;
x1=[0;0;20;0;0;0];
x1=x0-inv(myJacobi(x0,Length))*myfun(x0,Length);
while norm(x1-x0)>1e-3
    x0=x1;
    x1=x0-inv(myJacobi(x0,Length))*myfun(x0,Length);
end
Out = x1;
end
 
function f=myfun(x,Length)
%零初始位置时静平台位置
B1=[12;-14;0];
B2=[12;14;0];
B3=[6.12435565;17.39230485;0];
B4=[-18.12435565;3.39230485;0];
B5=[-18.12435565;-3.39230485;0];
B6=[6.12435565;-17.39230485;0];
%零初始位置时动平台位置
P1=[12;-2;0];
P2=[12;2;0];
P3=[-4.26794919;11.39230485;0];
P4=[-7.73205081;9.39230485;0];
P5=[-7.73205081;-9.39230485;0];
P6=[-4.26794919;-11.39230485;0];

x1=x(1);
x2=x(2);
x3=x(3);
a=x(4);
b=x(5);
g=x(6);
% l1=19.78;
% l2=24.81;
% l3=24.54;
% l4=21.25;
% l5=21.34;
% l6=18.2;
l1=Length(1);
l2=Length(2);
l3=Length(3);
l4=Length(4);
l5=Length(5);
l6=Length(6);
X=[x1;x2;x3];
RX=[1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];
RY=[cos(b) 0 sin(b);0 1 0;-sin(b) 0 cos(b)];
RZ=[cos(g) -sin(g) 0;sin(g) cos(g) 0; 0 0 1];
R=RZ*RY*RX;
f1=(norm(X+R*P1-B1))^2-l1^2;
f2=(norm(X+R*P2-B2))^2-l2^2;
f3=(norm(X+R*P3-B3))^2-l3^2;
f4=(norm(X+R*P4-B4))^2-l4^2;
f5=(norm(X+R*P5-B5))^2-l5^2;
f6=(norm(X+R*P6-B6))^2-l6^2;
f=[f1;f2;f3;f4;f5;f6];
end
 
function J=myJacobi(x,Length)
%零初始位置时静平台位置
%零初始位置时静平台位置
B1=[12;-14;0];
B2=[12;14;0];
B3=[6.12435565;17.39230485;0];
B4=[-18.12435565;3.39230485;0];
B5=[-18.12435565;-3.39230485;0];
B6=[6.12435565;-17.39230485;0];
%零初始位置时动平台位置
P1=[12;-2;0];
P2=[12;2;0];
P3=[-4.26794919;11.39230485;0];
P4=[-7.73205081;9.39230485;0];
P5=[-7.73205081;-9.39230485;0];
P6=[-4.26794919;-11.39230485;0];

x1=x(1);
x2=x(2);
x3=x(3);
a=x(4);
b=x(5);
g=x(6);
l1=Length(1);
l2=Length(2);
l3=Length(3);
l4=Length(4);
l5=Length(5);
l6=Length(6);
X=[x1;x2;x3];
RX=[1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];
RY=[cos(b) 0 sin(b);0 1 0;-sin(b) 0 cos(b)];
RZ=[cos(g) -sin(g) 0;sin(g) cos(g) 0; 0 0 1];
R=RZ*RY*RX;
f1=(norm(X+R*P1-B1))^2-l1^2;
f2=(norm(X+R*P2-B2))^2-l2^2;
f3=(norm(X+R*P3-B3))^2-l3^2;
f4=(norm(X+R*P4-B4))^2-l4^2;
f5=(norm(X+R*P5-B5))^2-l5^2;
f6=(norm(X+R*P6-B6))^2-l6^2;

J=[                      2.0*abs(x1 + 12.0*cos(b)*cos(g) + 2.0*cos(a)*sin(g) - 2.0*cos(g)*sin(a)*sin(b) - 12.0)*sign(x1 + 12.0*cos(b)*cos(g) + 2.0*cos(a)*sin(g) - 2.0*cos(g)*sin(a)*sin(b) - 12.0),                      2.0*abs(x2 - 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) - 2.0*sin(a)*sin(b)*sin(g) + 14.0)*sign(x2 - 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) - 2.0*sin(a)*sin(b)*sin(g) + 14.0), -2.0*abs(12.0*sin(b) - 1.0*x3 + 2.0*cos(b)*sin(a))*sign(12.0*sin(b) - 1.0*x3 + 2.0*cos(b)*sin(a)),                                                 2.0*abs(x2 - 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) - 2.0*sin(a)*sin(b)*sin(g) + 14.0)*sign(x2 - 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) - 2.0*sin(a)*sin(b)*sin(g) + 14.0)*(2.0*cos(g)*sin(a) - 2.0*cos(a)*sin(b)*sin(g)) - 2.0*abs(x1 + 12.0*cos(b)*cos(g) + 2.0*cos(a)*sin(g) - 2.0*cos(g)*sin(a)*sin(b) - 12.0)*sign(x1 + 12.0*cos(b)*cos(g) + 2.0*cos(a)*sin(g) - 2.0*cos(g)*sin(a)*sin(b) - 12.0)*(2.0*sin(a)*sin(g) + 2.0*cos(a)*cos(g)*sin(b)) + 4.0*abs(12.0*sin(b) - 1.0*x3 + 2.0*cos(b)*sin(a))*sign(12.0*sin(b) - 1.0*x3 + 2.0*cos(b)*sin(a))*cos(a)*cos(b),                                                2.0*abs(12.0*sin(b) - 1.0*x3 + 2.0*cos(b)*sin(a))*sign(12.0*sin(b) - 1.0*x3 + 2.0*cos(b)*sin(a))*(12.0*cos(b) - 2.0*sin(a)*sin(b)) - 2.0*abs(x1 + 12.0*cos(b)*cos(g) + 2.0*cos(a)*sin(g) - 2.0*cos(g)*sin(a)*sin(b) - 12.0)*sign(x1 + 12.0*cos(b)*cos(g) + 2.0*cos(a)*sin(g) - 2.0*cos(g)*sin(a)*sin(b) - 12.0)*(12.0*cos(g)*sin(b) + 2.0*cos(b)*cos(g)*sin(a)) - 2.0*abs(x2 - 2cos(g) + 12.0*cos(b)*sin(g) - 2.0*sin(a)*sin(b)*sin(g) + 14.0)*sign(x2 - 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) - 2.0*sin(a)*sin(b)*sin(g) + 14.0)*(12.0*sin(b)*sin(g) + 2.0*cos(b)*sin(a)*sin(g)),                                                   2.0*abs(x2 - 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) - 2.0*sin(a)*sin(b)*sin(g) + 14.0)*sign(x2 - 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) - 2.0*sin(a)*sin(b)*sin(g) + 14.0)*(12.0*cos(b)*cos(g) + 2.0*cos(a)*sin(g) - 2.0*cos(g)*sin(a)*sin(b)) + 2.0*abs(x1 + 12.0*cos(b)*cos(g) + 2.0*cos(a)*sin(g) - 2.0*cos(g)*sin(a)*sin(b) - 12.0)*sign(x1 + 12.0*cos(b)*cos(g) + 2.0*cos(a)*sin(g) - 2.0*cos(g)*sin(a)*sin(b) - 12.0)*(2.0*cos(a)*cos(g) - 12.0*cos(b)*sin(g) + 2.0*sin(a)*sin(b)*sin(g));
                      2.0*abs(x1 + 12.0*cos(b)*cos(g) - 2.0*cos(a)*sin(g) + 2.0*cos(g)*sin(a)*sin(b) - 12.0)*sign(x1 + 12.0*cos(b)*cos(g) - 2.0*cos(a)*sin(g) + 2.0*cos(g)*sin(a)*sin(b) - 12.0),                      2.0*abs(x2 + 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) + 2.0*sin(a)*sin(b)*sin(g) - 14.0)*sign(x2 + 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) + 2.0*sin(a)*sin(b)*sin(g) - 14.0),          2.0*abs(x3 - 12.0*sin(b) + 2.0*cos(b)*sin(a))*sign(x3 - 12.0*sin(b) + 2.0*cos(b)*sin(a)),                                                         2.0*abs(x1 + 12.0*cos(b)*cos(g) - 2.0*cos(a)*sin(g) + 2.0*cos(g)*sin(a)*sin(b) - 12.0)*sign(x1 + 12.0*cos(b)*cos(g) - 2.0*cos(a)*sin(g) + 2.0*cos(g)*sin(a)*sin(b) - 12.0)*(2.0*sin(a)*sin(g) + 2.0*cos(a)*cos(g)*sin(b)) - 2.0*abs(x2 + 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) + 2.0*sin(a)*sin(b)*sin(g) - 14.0)*sign(x2 + 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) + 2.0*sin(a)*sin(b)*sin(g) - 14.0)*(2.0*cos(g)*sin(a) - 2.0*cos(a)*sin(b)*sin(g)) + 4.0*abs(x3 - 12.0*sin(b) + 2.0*cos(b)*sin(a))*sign(x3 - 12.0*sin(b) + 2.0*cos(b)*sin(a))*cos(a)*cos(b),                                                      - 2.0*abs(x2 + 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) + 2.0*sin(a)*sin(b)*sin(g) - 14.0)*sign(x2 + 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) + 2.0*sin(a)*sin(b)*sin(g) - 14.0)*(12.0*sin(b)*sin(g) - 2.0*cos(b)*sin(a)*sin(g)) - 2.0*abs(x1 + 12.0*cos(b)*cos(g) - 2.0*cos(a)*sin(g) + 2.0*cos(g)*sin(a)*sin(b) - 12.0)*sign(x1 + 12.0*cos(b)*cos(g) - 2.0*cos(a)*sin(g) + 2.0*cos(g)*sin(a)*sin(b) - 12.0)*(12.0*cos(g)*sin(b) - 2.0*cos(b)*cos(g)*sin(a)) - 2.0*abs(x3 - 12.0*sin(b) + 2.0*cos(b)*sin(a))*sign(x3 - 12.0*sin(b) + 2.0*cos(b)*sin(a))*(12.0*cos(b) + 2.0*sin(a)*sin(b)),                                                   2.0*abs(x2 + 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) + 2.0*sin(a)*sin(b)*sin(g) - 14.0)*sign(x2 + 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) + 2.0*sin(a)*sin(b)*sin(g) - 14.0)*(12.0*cos(b)*cos(g) - 2.0*cos(a)*sin(g) + 2.0*cos(g)*sin(a)*sin(b)) - 2.0*abs(x1 + 12.0*cos(b)*cos(g) - 2.0*cos(a)*sin(g) + 2.0*cos(g)*sin(a)*sin(b) - 12.0)*sign(x1 + 12.0*cos(b)*cos(g) - 2.0*cos(a)*sin(g) + 2.0*cos(g)*sin(a)*sin(b) - 12.0)*(2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) + 2.0*sin(a)*sin(b)*sin(g));
 -2.0*sign(4.268*cos(b)*cos(g) - 1.0*x1 + 11.39*cos(a)*sin(g) - 11.39*cos(g)*sin(a)*sin(b) + 6.124)*abs(4.268*cos(b)*cos(g) - 1.0*x1 + 11.39*cos(a)*sin(g) - 11.39*cos(g)*sin(a)*sin(b) + 6.124),          2.0*abs(x2 + 11.39*cos(a)*cos(g) - 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*sign(x2 + 11.39*cos(a)*cos(g) - 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39),    2.0*abs(x3 + 4.268*sin(b) + 11.39*cos(b)*sin(a))*sign(x3 + 4.268*sin(b) + 11.39*cos(b)*sin(a)),         22.78*abs(x3 + 4.268*sin(b) + 11.39*cos(b)*sin(a))*sign(x3 + 4.268*sin(b) + 11.39*cos(b)*sin(a))*cos(a)*cos(b) - 2.0*abs(x2 + 11.39*cos(a)*cos(g) - 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*sign(x2 + 11.39*cos(a)*cos(g) - 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*(11.39*cos(g)*sin(a) - 11.39*cos(a)*sin(b)*sin(g)) - 2.0*sign(4.268*cos(b)*cos(g) - 1.0*x1 + 11.39*cos(a)*sin(g) - 11.39*cos(g)*sin(a)*sin(b) + 6.124)*abs(4.268*cos(b)*cos(g) - 1.0*x1 + 11.39*cos(a)*sin(g) - 11.39*cos(g)*sin(a)*sin(b) + 6.124)*(11.39*sin(a)*sin(g) + 11.39*cos(a)*cos(g)*sin(b)),         2.0*abs(x3 + 4.268*sin(b) + 11.39*cos(b)*sin(a))*sign(x3 + 4.268*sin(b) + 11.39*cos(b)*sin(a))*(4.268*cos(b) - 11.39*sin(a)*sin(b)) - 2.0*sign(4.268*cos(b)*cos(g) - 1.0*x1 + 11.39*cos(a)*sin(g) - 11.39*cos(g)*sin(a)*sin(b) + 6.124)*abs(4.268*cos(b)*cos(g) - 1.0*x1 + 11.39*cos(a)*sin(g) - 11.39*cos(g)*sin(a)*sin(b) + 6.124)*(4.268*cos(g)*sin(b) + 11.39*cos(b)*cos(g)*sin(a)) + 2.0*abs(x2 + 11.39*cos(a)*cos(g) - 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*sign(x2 + 11.39*cos(a)*cos(g) - 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*(4.268*sin(b)*sin(g) + 11.39*cos(b)*sin(a)*sin(g)),         2.0*sign(4.268*cos(b)*cos(g) - 1.0*x1 + 11.39*cos(a)*sin(g) - 11.39*cos(g)*sin(a)*sin(b) + 6.124)*abs(4.268*cos(b)*cos(g) - 1.0*x1 + 11.39*cos(a)*sin(g) - 11.39*cos(g)*sin(a)*sin(b) + 6.124)*(11.39*cos(a)*cos(g) - 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g)) - 2.0*abs(x2 + 11.39*cos(a)*cos(g) - 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*sign(x2 + 11.39*cos(a)*cos(g) - 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*(4.268*cos(b)*cos(g) + 11.39*cos(a)*sin(g) - 11.39*cos(g)*sin(a)*sin(b));
          2.0*abs(x1 - 7.732*cos(b)*cos(g) - 9.392*cos(a)*sin(g) + 9.392*cos(g)*sin(a)*sin(b) + 18.12)*sign(x1 - 7.732*cos(b)*cos(g) - 9.392*cos(a)*sin(g) + 9.392*cos(g)*sin(a)*sin(b) + 18.12),          2.0*abs(x2 + 9.392*cos(a)*cos(g) - 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*sign(x2 + 9.392*cos(a)*cos(g) - 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392),    2.0*abs(x3 + 7.732*sin(b) + 9.392*cos(b)*sin(a))*sign(x3 + 7.732*sin(b) + 9.392*cos(b)*sin(a)),                 2.0*abs(x1 - 7.732*cos(b)*cos(g) - 9.392*cos(a)*sin(g) + 9.392*cos(g)*sin(a)*sin(b) + 18.12)*sign(x1 - 7.732*cos(b)*cos(g) - 9.392*cos(a)*sin(g) + 9.392*cos(g)*sin(a)*sin(b) + 18.12)*(9.392*sin(a)*sin(g) + 9.392*cos(a)*cos(g)*sin(b)) - 2.0*abs(x2 + 9.392*cos(a)*cos(g) - 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*sign(x2 + 9.392*cos(a)*cos(g) - 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*(9.392*cos(g)*sin(a) - 9.392*cos(a)*sin(b)*sin(g)) + 18.78*abs(x3 + 7.732*sin(b) + 9.392*cos(b)*sin(a))*sign(x3 + 7.732*sin(b) + 9.392*cos(b)*sin(a))*cos(a)*cos(b),                 2.0*abs(x1 - 7.732*cos(b)*cos(g) -in(g) + 9.392*cos(g)*sin(a)*sin(b) + 18.12)*sign(x1 - 7.732*cos(b)*cos(g) - 9.392*cos(a)*sin(g) + 9.392*cos(g)*sin(a)*sin(b) + 18.12)*(7.732*cos(g)*sin(b) + 9.392*cos(b)*cos(g)*sin(a)) + 2.0*abs(x3 + 7.732*sin(b) + 9.392*cos(b)*sin(a))*sign(x3 + 7.732*sin(b) + 9.392*cos(b)*sin(a))*(7.732*cos(b) - 9.392*sin(a)*sin(b)) + 2.0*abs(x2 + 9.392*cos(a)*cos(g) - 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*sign(x2 + 9.392*cos(a)*cos(g) - 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*(7.732*sin(b)*sin(g) + 9.392*cos(b)*sin(a)*sin(g)),               - 2.0*abs(x1 - 7.732*cos(b)*cos(g) - 9.392*cos(a)*sin(g) + 9.392*cos(g)*sin(a)*sin(b) + 18.12)*sign(x1 - 7.732*cos(b)*cos(g) - 9.392*cos(a)*sin(g) + 9.392*cos(g)*sin(a)*sin(b) + 18.12)*(9.392*cos(a)*cos(g) - 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g)) - 2.0*abs(x2 + 9.392*cos(a)*cos(g) - 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*sign(x2 + 9.392*cos(a)*cos(g) - 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*(7.732*cos(b)*cos(g) + 9.392*cos(a)*sin(g) - 9.392*cos(g)*sin(a)*sin(b));
          2.0*abs(x1 - 7.732*cos(b)*cos(g) + 9.392*cos(a)*sin(g) - 9.392*cos(g)*sin(a)*sin(b) + 18.12)*sign(x1 - 7.732*cos(b)*cos(g) + 9.392*cos(a)*sin(g) - 9.392*cos(g)*sin(a)*sin(b) + 18.12), -2.0*abs(9.392*cos(a)*cos(g) - 1.0*x2 + 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*sign(9.392*cos(a)*cos(g) - 1.0*x2 + 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392),    2.0*abs(x3 + 7.732*sin(b) - 9.392*cos(b)*sin(a))*sign(x3 + 7.732*sin(b) - 9.392*cos(b)*sin(a)),       - 2.0*abs(9.392*cos(a)*cos(g) - 1.0*x2 + 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*sign(9.392*cos(a)*cos(g) - 1.0*x2 + 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*(9.392*cos(g)*sin(a) - 9.392*cos(a)*sin(b)*sin(g)) - 2.0*abs(x1 - 7.732*cos(b)*cos(g) + 9.392*cos(a)*sin(g) - 9.392*cos(g)*sin(a)*sin(b) + 18.12)*sign(x1 - 7.732*cos(b)*cos(g) + 9.392*cos(a)*sin(g) - 9.392*cos(g)*sin(a)*sin(b) + 18.12)*(9.392*sin(a)*sin(g) + 9.392*cos(a)*cos(g)*sin(b)) - 18.78*abs(x3 + 7.732*sin(b) - 9.392*cos(b)*sin(a))*sign(x3 + 7.732*sin(b) - 9.392*cos(b)*sin(a))*cos(a)*cos(b),         2.0*abs(x1 - 7.732*cos(b)*cos(g) + 9.392*cos(a)*sin(g) - 9.392*cos(g)*sin(a)*sin(b) + 18.12)*sign(x1 - 7.732*cos(b)*cos(g) + 9.392*cos(a)*sin(g) - 9.392*cos(g)*sin(a)*sin(b) + 18.12)*(7.732*cos(g)*sin(b) - 9.392*cos(b)*cos(g)*sin(a)) + 2.0*abs(x3 + 7.732*sin(b) - 9.392*cos(b)*sin(a))*sign(x3 + 7.732*sin(b) - 9.392*cos(b)*sin(a))*(7.732*cos(b) + 9.392*sin(a)*sin(b)) - 2.0*abs(9.392*cos(a)*cos(g) - 1.0*x2 + 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*sign(9.392*cos(a)*cos(g) - 1.0*x2 + 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*(7.732*sin(b)*sin(g) - 9.392*cos(b)*sin(a)*sin(g)),         2.0*abs(x1 - 7.732*cos(b)*cos(g) + 9.392*cos(a)*sin(g) - 9.392*cos(g)*sin(a)*sin(b) + 18.12)*sign(x1 - 7.732*cos(b)*cos(g) + 9.392*cos(a)*sin(g) - 9.392*cos(g)*sin(a)*sin(b) + 18.12)*(9.392*cos(a)*cos(g) + 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g)) + 2.0*abs(9.392*cos(a)*cos(g) - 1.0*x2 + 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*sign(9.392*cos(a)*cos(g) - 1.0*x2 + 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*(7.732*cos(b)*cos(g) - 9.392*cos(a)*sin(g) + 9.392*cos(g)*sin(a)*sin(b));
 -2.0*sign(4.268*cos(b)*cos(g) - 1.0*x1 - 11.39*cos(a)*sin(g) + 11.39*cos(g)*sin(a)*sin(b) + 6.124)*abs(4.268*cos(b)*cos(g) - 1.0*x1 - 11.39*cos(a)*sin(g) + 11.39*cos(g)*sin(a)*sin(b) + 6.124), -2.0*abs(11.39*cos(a)*cos(g) - 1.0*x2 + 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*sign(11.39*cos(a)*cos(g) - 1.0*x2 + 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39),    2.0*abs(x3 + 4.268*sin(b) - 11.39*cos(b)*sin(a))*sign(x3 + 4.268*sin(b) - 11.39*cos(b)*sin(a)), 2.0*sign(4.268*cos(b)*cos(g) - 1.0*x1 - 11.39*cos(a)*sin(g) + 11.39*cos(g)*sin(a)*sin(b) + 6.124)*abs(4.268*cos(b)*cos(g) - 1.0*x1 - 11.39*cos(a)*sin(g) + 11.39*cos(g)*sin(a)*sin(b) + 6.124)*(11.39*sin(a)*sin(g) + 11.39*cos(a)*cos(g)*sin(b)) - 2.0*abs(11.39*cos(a)*cos(g) - 1.0*x2 + 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*sign(11.39*cos(a)*cos(g) - 1.0*x2 + 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*(11.39*cos(g)*sin(a) - 11.39*cos(a)*sin(b)*sin(g)) - 22.78*abs(x3 + 4.268*sin(b) - 11.39*cos(b)*sin(a))*sign(x3 + 4.268*sin(b) - 11.39*cos(b)*sin(a))*cos(a)*cos(b), 2.0*abs(x3 + 4.268*sin(b) - 11.39*cos(b)*sin(a))*sign(x3 + 4.268*sin(b) - 11.39*cos(b)*sin(a))*(4.268*cos(b) + 11.39*sin(a)*sin(b)) - 2.0*abs(11.39*cos(a)*cos(g) - 1.0*x2 + 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*sign(11.39*cos(a)*cos(g) - 1.0*x2 + 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*(4.268*sin(b)*sin(g) - 11.39*cos(b)*sin(a)*sin(g)) - 2.0*sign(4.268*cos(b)*cos(g) - 1.0*x1 - 11.39*cos(a)*sin(g) + 11.39*cos(g)*sin(a)*sin(b) + 6.124)*abs(4.268*cos(b)*cos(g) - 1.0*x1 - 11.39*cos(a)*sin(g) + 11.39*cos(g)*sin(a)*sin(b) + 6.124)*(4.268*cos(g)*sin(b) - 11.39*cos(b)*cos(g)*sin(a)), 2.0*abs(11.39*cos(a)*cos(g) - 1.0*x2 + 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*sign(11.39*cos(a)*cos(g) - 1.0*x2 + 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*(4.268*cos(b)*cos(g) - 11.39*cos(a)*sin(g) + 11.39*cos(g)*sin(a)*sin(b)) - 2.0*sign(4.268*cos(b)*cos(g) - 1.0*x1 - 11.39*cos(a)*sin(g) + 11.39*cos(g)*sin(a)*sin(b) + 6.124)*abs(4.268*cos(b)*cos(g) - 1.0*x1 - 11.39*cos(a)*sin(g) + 11.39*cos(g)*sin(a)*sin(b) + 6.124)*(11.39*cos(a)*cos(g) + 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g));];
end