Stewart平台运动学逆解

format long
X=[-2.9935;-4.3657;22.5473];
a=0.1840;
b=-0.0778;
g=0.0226;
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;
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];
r1=[12;-14;5];
r2=[12;14;5];
r3=[6.12435565;17.39230485;5];
r4=[-18.12435565;3.39230485;5];
r5=[-18.12435565;-3.39230485;5];
r6=[6.12435565;-17.39230485;5];
l1=norm(X+R*p1-r1);
l2=norm(X+R*p2-r2);
l3=norm(X+R*p3-r3);
l4=norm(X+R*p4-r4);
l5=norm(X+R*p5-r5);
l6=norm(X+R*p6-r6);
L=[l1;l2;l3;l4;l5;l6]