7/10/22 10:01 PM Untitled5 1 of 2

syms x1 x2 x3 x4 x5 x6 a1 a2 a3 d1 d2 d3 d4 d5 d6
x1=0;x3=0;x5=0;x6=0;
T01=[cos(x1) 0 -sin(x1) 0;sin(x1) 0 cos(x1) 0;0 -1 0 d1;0 0 0 1];
T12=[1 -0 0 a2;0 1 0 0;0 0 1 0;0 0 0 1];
%T12=[sin(x2) -cos(x2) 0 a2*sin(x2);cos(x2) sin(x2) 0 a2*cos(x2);0 0 1 0;0 0 0 1];
T23=[cos(x3) -sin(x3) 0 a3*cos(x3);sin(x3) cos(x3) 0 a3*sin(x3);0 0 1 0;0 0 0 1];
%T34=[sin(x4) 0 -cos(x4) 0;-cos(x4) 0 sin(x4) 0;0 -1 0 d4;0 0 0 1];
T34=[-1 0 0 0;0 0 -1 0;0 -1 0 d4;0 0 0 1];
T45=[cos(x5) 0 sin(x5) 0;sin(x5) 0 -cos(x5) 0;0 1 0 d5;0 0 0 1];
T56=[cos(x6) -sin(x6) 0 0;sin(x6) cos(x6) 0 0;0 0 1 d6;0 0 0 1];
T06=T01*T12*T23*T34*T45*T56;
T06

T06 =

[ cos(x6)*(sin(x1)*sin(x5) + cos(x5)*(cos(x4)*(cos(x1)*sin(x2)*sin(x3) + cos(x1)*cos

(x2)*cos(x3)) - sin(x4)*(cos(x1)*cos(x2)*sin(x3) - cos(x1)*cos(x3)*sin(x2)))) + sin
(x6)*(cos(x4)*(cos(x1)*cos(x2)*sin(x3) - cos(x1)*cos(x3)*sin(x2)) - sin(x4)*(cos(x1)
*sin(x2)*sin(x3) + cos(x1)*cos(x2)*cos(x3))), cos(x6)*(cos(x4)*(cos(x1)*cos(x2)*sin
(x3) - cos(x1)*cos(x3)*sin(x2)) - sin(x4)*(cos(x1)*sin(x2)*sin(x3) + cos(x1)*cos(x2)
*cos(x3))) - sin(x6)*(sin(x1)*sin(x5) + cos(x5)*(cos(x4)*(cos(x1)*sin(x2)*sin(x3) +
cos(x1)*cos(x2)*cos(x3)) - sin(x4)*(cos(x1)*cos(x2)*sin(x3) - cos(x1)*cos(x3)*sin
(x2)))), sin(x5)*(cos(x4)*(cos(x1)*sin(x2)*sin(x3) + cos(x1)*cos(x2)*cos(x3)) - sin
(x4)*(cos(x1)*cos(x2)*sin(x3) - cos(x1)*cos(x3)*sin(x2))) - cos(x5)*sin(x1), d5*(cos
(x4)*(cos(x1)*cos(x2)*sin(x3) - cos(x1)*cos(x3)*sin(x2)) - sin(x4)*(cos(x1)*sin(x2)
*sin(x3) + cos(x1)*cos(x2)*cos(x3))) - d6*(cos(x5)*sin(x1) - sin(x5)*(cos(x4)*(cos
(x1)*sin(x2)*sin(x3) + cos(x1)*cos(x2)*cos(x3)) - sin(x4)*(cos(x1)*cos(x2)*sin(x3) -
cos(x1)*cos(x3)*sin(x2)))) - d4*sin(x1) + a2*cos(x1)*sin(x2) - a3*cos(x1)*cos(x2)*sin
(x3) + a3*cos(x1)*cos(x3)*sin(x2)]
[ sin(x6)*(cos(x4)*(cos(x2)*sin(x1)*sin(x3) - cos(x3)*sin(x1)*sin(x2)) - sin(x4)*(cos
(x2)*cos(x3)*sin(x1) + sin(x1)*sin(x2)*sin(x3))) + cos(x6)*(cos(x5)*(cos(x4)*(cos(x2)
*cos(x3)*sin(x1) + sin(x1)*sin(x2)*sin(x3)) - sin(x4)*(cos(x2)*sin(x1)*sin(x3) - cos
(x3)*sin(x1)*sin(x2))) - cos(x1)*sin(x5)), cos(x6)*(cos(x4)*(cos(x2)*sin(x1)*sin(x3)
- cos(x3)*sin(x1)*sin(x2)) - sin(x4)*(cos(x2)*cos(x3)*sin(x1) + sin(x1)*sin(x2)*sin
(x3))) - sin(x6)*(cos(x5)*(cos(x4)*(cos(x2)*cos(x3)*sin(x1) + sin(x1)*sin(x2)*sin
(x3)) - sin(x4)*(cos(x2)*sin(x1)*sin(x3) - cos(x3)*sin(x1)*sin(x2))) - cos(x1)*sin
(x5)), cos(x1)*cos(x5) + sin(x5)*(cos(x4)*(cos(x2)*cos(x3)*sin(x1) + sin(x1)*sin(x2)
*sin(x3)) - sin(x4)*(cos(x2)*sin(x1)*sin(x3) - cos(x3)*sin(x1)*sin(x2))), d6*(cos(x1)
*cos(x5) + sin(x5)*(cos(x4)*(cos(x2)*cos(x3)*sin(x1) + sin(x1)*sin(x2)*sin(x3)) - sin
(x4)*(cos(x2)*sin(x1)*sin(x3) - cos(x3)*sin(x1)*sin(x2)))) + d4*cos(x1) + d5*(cos(x4)
*(cos(x2)*sin(x1)*sin(x3) - cos(x3)*sin(x1)*sin(x2)) - sin(x4)*(cos(x2)*cos(x3)*sin
(x1) + sin(x1)*sin(x2)*sin(x3))) + a2*sin(x1)*sin(x2) - a3*cos(x2)*sin(x1)*sin(x3) +
a3*cos(x3)*sin(x1)*sin(x2)]
[
sin(x6)*(cos(x4)*(cos(x2)*cos(x3) + sin(x2)*sin(x3)) + sin(x4)*(cos(x2)*sin(x3) - cos
(x3)*sin(x2))) - cos(x5)*cos(x6)*(cos(x4)*(cos(x2)*sin(x3) - cos(x3)*sin(x2)) + sin
(x4)*(cos(x2)*cos(x3) + sin(x2)*sin(x3))),
cos(x6)*(cos(x4)*(cos(x2)*cos(x3) + sin(x2)*sin(x3)) + sin(x4)*(cos(x2)*sin(x3) - cos
7/10/22 10:01 PM Untitled5 2 of 2

(x3)*sin(x2))) + cos(x5)*sin(x6)*(cos(x4)*(cos(x2)*sin(x3) - cos(x3)*sin(x2)) + sin

(x4)*(cos(x2)*cos(x3) + sin(x2)*sin(x3))),
-sin(x5)*(cos(x4)*(cos(x2)*sin(x3) - cos(x3)*sin(x2)) + sin(x4)*(cos(x2)*cos(x3) +
sin(x2)*sin(x3))),
d1 + d5*(cos(x4)*(cos(x2)*cos(x3) + sin(x2)*sin(x3)) + sin(x4)*(cos(x2)*sin(x3) - cos
(x3)*sin(x2))) - a2*cos(x2) - a3*cos(x2)*cos(x3) - a3*sin(x2)*sin(x3) - d6*sin(x5)*
(cos(x4)*(cos(x2)*sin(x3) - cos(x3)*sin(x2)) + sin(x4)*(cos(x2)*cos(x3) + sin(x2)*sin
(x3)))]
[
0,
0,
0,
1]

T06 =

[ -1, 0, 0, a2 + a3]
[ 0, 0, 1, d4 + d6]
[ 0, 1, 0, d1 + d5]
[ 0, 0, 0, 1]