1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30
| figure
hold on
for t1=-pi : pi/4 : pi
for t2=-pi : pi/4 : pi
for t3=-pi : pi/4 : pi
for t4=-pi : pi/4 : pi
T1=[cos(t1) -sin(t1) 0 0;sin(t1) cos(t1) 0 0;0 0 1 0;0 0 0 1];
T2=[cos(t2) -sin(t2) 0 90;0 0 1 0;-sin(t2) cos(t2) 0 0;0 0 0 1];
T3=[cos(t3) -sin(t3) 0 0;0 0 1 70;-sin(t3) -cos(t3) 0 0;0 0 0 1];
T4=[cos(t4) -sin(t4) 0 0;0 0 -1 320;sin(t4) -cos(t4) 0 0;0 0 0 1];
T5=[1 0 0 260;0 0 1 0;0 1 0 0;0 0 0 1];
%calcul
T=T1*T2;
T=T*T3;
T=T*T4;
T=T*T5;
px=T(1,4);
py=T(2,4);
pz=T(3,4);
psi=atan2(-T(2,3),T(3,3));
a=(T(2,3)*T(2,3))+(T(3,3)*T(3,3));
phi=atan2(T(1,3),sqrt(a));
teta=atan2(-T(1,3),T(1,1));
A=[psi px;phi py;teta pz];
plot3(px,py,pz);
drawnow
end
end
end
end |