forked from match-ROS/match_mobile_robotics
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Jacobi_symbolic.m
108 lines (82 loc) · 28.6 KB
/
Jacobi_symbolic.m
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
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
q1 = sym ('q1');
q2 = sym ('q2');
q3 = sym ('q3');
q4 = sym ('q4');
q5 = sym ('q5');
q6 = sym ('q6');
syms Pi
tetha = [q1,q2,q3,q4,q5,q6];
a = [0, -0.6127, -0.57155, 0,0,0];
d = [0.1807,0,0,0.17415,0.11985,0.11655];
alpha = [Pi/2,0,0,Pi/2,-Pi/2,0];
for i = 1:6
T1 = [1 0 0 0;
0 1 0 0
0 0 1 d(i)
0 0 0 1];
T2 = [1 0 0 a(i);
0 1 0 0
0 0 1 0
0 0 0 1];
R1 = [cos(tetha(i)) -sin(tetha(i)) 0 0;
sin(tetha(i)) cos(tetha(i)) 0 0;
0 0 1 0;
0 0 0 1];
R2 = [1 0 0 0;
0 cos(alpha(i)) -sin(alpha(i)) 0;
0 sin(alpha(i)) cos(alpha(i)) 0;
0 0 0 1];
T{i} = R1*T1*T2*R2;
end
T_ges = simplify(T{1} * T{2} * T{3} * T{4} * T{5} * T{6});
%%
clear all
q1 = -1;
q2 = -0.5;
q3 = 0;
q4 = 0;
q5 = 0;
q6 = 0;
Pi = pi;
DH = [ cos(q6)*(cos(q5)*(cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)) - sin(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))) + sin(Pi/2)^2*sin(q1)*sin(q5)) - sin(Pi/2)*sin(q6)*(sin(Pi/2)*cos(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)) + sin(Pi/2)*sin(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))), - sin(q6)*(cos(q5)*(cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)) - sin(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))) + sin(Pi/2)^2*sin(q1)*sin(q5)) - sin(Pi/2)*cos(q6)*(sin(Pi/2)*cos(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)) + sin(Pi/2)*sin(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))), sin(Pi/2)^3*cos(q5)*sin(q1) - sin(Pi/2)*sin(q5)*(cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)) - sin(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))), (3483*sin(Pi/2)*sin(q1))/20000 - (6127*cos(q1)*cos(q2))/10000 + (2397*sin(Pi/2)*cos(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)))/20000 + (2331*sin(Pi/2)^3*cos(q5)*sin(q1))/20000 + (2397*sin(Pi/2)*sin(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)))/20000 - (2331*sin(Pi/2)*sin(q5)*(cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)) - sin(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))))/20000 - (11431*cos(q1)*cos(q2)*cos(q3))/20000 + (11431*cos(q1)*sin(q2)*sin(q3))/20000;
- cos(q6)*(cos(q5)*(cos(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)) + sin(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))) + sin(Pi/2)^2*cos(q1)*sin(q5)) - sin(Pi/2)*sin(q6)*(sin(Pi/2)*cos(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - sin(Pi/2)*sin(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))), sin(q6)*(cos(q5)*(cos(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)) + sin(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))) + sin(Pi/2)^2*cos(q1)*sin(q5)) - sin(Pi/2)*cos(q6)*(sin(Pi/2)*cos(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - sin(Pi/2)*sin(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))), sin(Pi/2)*sin(q5)*(cos(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)) + sin(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))) - sin(Pi/2)^3*cos(q1)*cos(q5), (11431*sin(q1)*sin(q2)*sin(q3))/20000 - (3483*sin(Pi/2)*cos(q1))/20000 - (6127*cos(q2)*sin(q1))/10000 - (2331*sin(Pi/2)^3*cos(q1)*cos(q5))/20000 + (2397*sin(Pi/2)*cos(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/20000 - (2397*sin(Pi/2)*sin(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)))/20000 - (11431*cos(q2)*cos(q3)*sin(q1))/20000 + (2331*sin(Pi/2)*sin(q5)*(cos(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)) + sin(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))))/20000;
sin(Pi/2)*sin(q2 + q3 + q4)*cos(q5)*cos(q6) - sin(Pi/2)*sin(q6)*(cos(Pi + q2 + q3 + q4)/4 - cos(q2 + q3 + q4)/2 + cos(Pi)/2 + cos(q2 - Pi + q3 + q4)/4 + 1/2), - sin(Pi/2)*cos(q6)*(cos(Pi + q2 + q3 + q4)/4 - cos(q2 + q3 + q4)/2 + cos(Pi)/2 + cos(q2 - Pi + q3 + q4)/4 + 1/2) - sin(Pi/2)*sin(q2 + q3 + q4)*cos(q5)*sin(q6), sin(q2 + q3 + q4)*sin(q5)*(cos(Pi)/2 - 1/2), (2397*cos(Pi))/40000 - (6127*sin(Pi/2)*sin(q2))/10000 - (11431*sin(Pi/2)*cos(q2)*sin(q3))/20000 - (11431*sin(Pi/2)*cos(q3)*sin(q2))/20000 + (2397*cos(q2 + q3)*cos(q4)*(cos(Pi)/2 - 1/2))/20000 - (2397*sin(q2 + q3)*sin(q4)*(cos(Pi)/2 - 1/2))/20000 + (2331*sin(q2 + q3 + q4)*sin(q5)*(cos(Pi)/2 - 1/2))/20000 + 77/320;
0, 0, 0, 1]
%%
q1 = sym ('q1');
q2 = sym ('q2');
q3 = sym ('q3');
q4 = sym ('q4');
q5 = sym ('q5');
q6 = sym ('q6');
syms Pi
DH = [ cos(q6)*(cos(q5)*(cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)) - sin(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))) + sin(Pi/2)^2*sin(q1)*sin(q5)) - sin(Pi/2)*sin(q6)*(sin(Pi/2)*cos(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)) + sin(Pi/2)*sin(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))), - sin(q6)*(cos(q5)*(cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)) - sin(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))) + sin(Pi/2)^2*sin(q1)*sin(q5)) - sin(Pi/2)*cos(q6)*(sin(Pi/2)*cos(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)) + sin(Pi/2)*sin(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))), sin(Pi/2)^3*cos(q5)*sin(q1) - sin(Pi/2)*sin(q5)*(cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)) - sin(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))), (3483*sin(Pi/2)*sin(q1))/20000 - (6127*cos(q1)*cos(q2))/10000 + (2397*sin(Pi/2)*cos(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)))/20000 + (2331*sin(Pi/2)^3*cos(q5)*sin(q1))/20000 + (2397*sin(Pi/2)*sin(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)))/20000 - (2331*sin(Pi/2)*sin(q5)*(cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)) - sin(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))))/20000 - (11431*cos(q1)*cos(q2)*cos(q3))/20000 + (11431*cos(q1)*sin(q2)*sin(q3))/20000;
- cos(q6)*(cos(q5)*(cos(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)) + sin(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))) + sin(Pi/2)^2*cos(q1)*sin(q5)) - sin(Pi/2)*sin(q6)*(sin(Pi/2)*cos(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - sin(Pi/2)*sin(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))), sin(q6)*(cos(q5)*(cos(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)) + sin(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))) + sin(Pi/2)^2*cos(q1)*sin(q5)) - sin(Pi/2)*cos(q6)*(sin(Pi/2)*cos(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - sin(Pi/2)*sin(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))), sin(Pi/2)*sin(q5)*(cos(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)) + sin(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))) - sin(Pi/2)^3*cos(q1)*cos(q5), (11431*sin(q1)*sin(q2)*sin(q3))/20000 - (3483*sin(Pi/2)*cos(q1))/20000 - (6127*cos(q2)*sin(q1))/10000 - (2331*sin(Pi/2)^3*cos(q1)*cos(q5))/20000 + (2397*sin(Pi/2)*cos(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/20000 - (2397*sin(Pi/2)*sin(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)))/20000 - (11431*cos(q2)*cos(q3)*sin(q1))/20000 + (2331*sin(Pi/2)*sin(q5)*(cos(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)) + sin(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))))/20000;
sin(Pi/2)*sin(q2 + q3 + q4)*cos(q5)*cos(q6) - sin(Pi/2)*sin(q6)*(cos(Pi + q2 + q3 + q4)/4 - cos(q2 + q3 + q4)/2 + cos(Pi)/2 + cos(q2 - Pi + q3 + q4)/4 + 1/2), - sin(Pi/2)*cos(q6)*(cos(Pi + q2 + q3 + q4)/4 - cos(q2 + q3 + q4)/2 + cos(Pi)/2 + cos(q2 - Pi + q3 + q4)/4 + 1/2) - sin(Pi/2)*sin(q2 + q3 + q4)*cos(q5)*sin(q6), sin(q2 + q3 + q4)*sin(q5)*(cos(Pi)/2 - 1/2), (2397*cos(Pi))/40000 - (6127*sin(Pi/2)*sin(q2))/10000 - (11431*sin(Pi/2)*cos(q2)*sin(q3))/20000 - (11431*sin(Pi/2)*cos(q3)*sin(q2))/20000 + (2397*cos(q2 + q3)*cos(q4)*(cos(Pi)/2 - 1/2))/20000 - (2397*sin(q2 + q3)*sin(q4)*(cos(Pi)/2 - 1/2))/20000 + (2331*sin(q2 + q3 + q4)*sin(q5)*(cos(Pi)/2 - 1/2))/20000 + 77/320;
0, 0, 0, 1];
Rx = atan((sin(Pi/2)^3*cos(q1)*cos(q5) - sin(Pi/2)*sin(q5)*(cos(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)) + sin(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))))/(sin(Pi/2)^3*cos(q5)*sin(q1) - sin(Pi/2)*sin(q5)*(cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)) - sin(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)))));
Ry = atan((DH(2,3)/DH(3,3))/sin(Rx));
Rz = atan((sin(Pi/2)*cos(q6)*(cos(Pi + q2 + q3 + q4)/4 - cos(q2 + q3 + q4)/2 + cos(Pi)/2 + cos(q2 - Pi + q3 + q4)/4 + 1/2) + sin(Pi/2)*sin(q2 + q3 + q4)*cos(q5)*sin(q6))/(sin(Pi/2)*sin(q6)*(cos(Pi + q2 + q3 + q4)/4 - cos(q2 + q3 + q4)/2 + cos(Pi)/2 + cos(q2 - Pi + q3 + q4)/4 + 1/2) - sin(Pi/2)*sin(q2 + q3 + q4)*cos(q5)*cos(q6)));
J = [diff(DH(1,4),q1),diff(DH(1,4),q2),diff(DH(1,4),q3),diff(DH(1,4),q4),diff(DH(1,4),q5),diff(DH(1,4),q6);
diff(DH(2,4),q1),diff(DH(2,4),q2),diff(DH(2,4),q3),diff(DH(2,4),q4),diff(DH(2,4),q5),diff(DH(2,4),q6);
diff(DH(3,4),q1),diff(DH(3,4),q2),diff(DH(3,4),q3),diff(DH(3,4),q4),diff(DH(3,4),q5),diff(DH(3,4),q6);
diff(Rx,q1),diff(Rx,q2),diff(Rx,q3),diff(Rx,q4),diff(Rx,q5),diff(Rx,q6);
diff(Ry,q1),diff(Ry,q2),diff(Ry,q3),diff(Ry,q4),diff(Ry,q5),diff(Ry,q6);
diff(Rz,q1),diff(Rz,q2),diff(Rz,q3),diff(Rz,q4),diff(Rz,q5),diff(Rz,q6)]
[0, -(2*cos(q2 + q3 + q4)*sin(Pi/2)*(cos(Pi/2)^4*cos(q5)^2 - 2*cos(Pi/2)^2*cos(q5)^2 + 1))/(sin(q2 + q3 + q4)^2*sin(q5)*(cos(Pi) - 1)*((sin(Pi/2)^4*sin(q5)^2 + sin(q2 + q3 + q4)^2*sin(q5)^2 - sin(q5)^2 - sin(Pi/2)^4)/(sin(Pi/2)^2*sin(q2 + q3 + q4)^2*sin(q5)^2) - 1)*((sin(Pi/2)^4*cos(q5)^2 + cos(q2 + q3 + q4)^2*sin(q5)^2)/(2*cos(q2 + q3 + q4)*cos(q1)*sin(q5) - cos(q5)*sin(q1) + cos(Pi)*cos(q5)*sin(q1))^2)^(1/2)*(2*cos(q1)*cos(q2)*sin(q3)*sin(q4)*sin(q5) - 2*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q5) - cos(q5)*sin(q1)*(cos(Pi) - 1) + 2*cos(q1)*cos(q3)*sin(q2)*sin(q4)*sin(q5) + 2*cos(q1)*cos(q4)*sin(q2)*sin(q3)*sin(q5))), -(2*cos(q2 + q3 + q4)*sin(Pi/2)*(cos(Pi/2)^4*cos(q5)^2 - 2*cos(Pi/2)^2*cos(q5)^2 + 1))/(sin(q2 + q3 + q4)^2*sin(q5)*(cos(Pi) - 1)*((sin(Pi/2)^4*sin(q5)^2 + sin(q2 + q3 + q4)^2*sin(q5)^2 - sin(q5)^2 - sin(Pi/2)^4)/(sin(Pi/2)^2*sin(q2 + q3 + q4)^2*sin(q5)^2) - 1)*((sin(Pi/2)^4*cos(q5)^2 + cos(q2 + q3 + q4)^2*sin(q5)^2)/(2*cos(q2 + q3 + q4)*cos(q1)*sin(q5) - cos(q5)*sin(q1) + cos(Pi)*cos(q5)*sin(q1))^2)^(1/2)*(2*cos(q1)*cos(q2)*sin(q3)*sin(q4)*sin(q5) - 2*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q5) - cos(q5)*sin(q1)*(cos(Pi) - 1) + 2*cos(q1)*cos(q3)*sin(q2)*sin(q4)*sin(q5) + 2*cos(q1)*cos(q4)*sin(q2)*sin(q3)*sin(q5))), -(2*cos(q2 + q3 + q4)*sin(Pi/2)*(cos(Pi/2)^4*cos(q5)^2 - 2*cos(Pi/2)^2*cos(q5)^2 + 1))/(sin(q2 + q3 + q4)^2*sin(q5)*(cos(Pi) - 1)*((sin(Pi/2)^4*sin(q5)^2 + sin(q2 + q3 + q4)^2*sin(q5)^2 - sin(q5)^2 - sin(Pi/2)^4)/(sin(Pi/2)^2*sin(q2 + q3 + q4)^2*sin(q5)^2) - 1)*((sin(Pi/2)^4*cos(q5)^2 + cos(q2 + q3 + q4)^2*sin(q5)^2)/(2*cos(q2 + q3 + q4)*cos(q1)*sin(q5) - cos(q5)*sin(q1) + cos(Pi)*cos(q5)*sin(q1))^2)^(1/2)*(2*cos(q1)*cos(q2)*sin(q3)*sin(q4)*sin(q5) - 2*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q5) - cos(q5)*sin(q1)*(cos(Pi) - 1) + 2*cos(q1)*cos(q3)*sin(q2)*sin(q4)*sin(q5) + 2*cos(q1)*cos(q4)*sin(q2)*sin(q3)*sin(q5))), -(2*sin(Pi/2)^5*cos(q5))/(sin(q2 + q3 + q4)*sin(q5)^2*(cos(Pi) - 1)*((sin(Pi/2)^4*sin(q5)^2 + sin(q2 + q3 + q4)^2*sin(q5)^2 - sin(q5)^2 - sin(Pi/2)^4)/(sin(Pi/2)^2*sin(q2 + q3 + q4)^2*sin(q5)^2) - 1)*((sin(Pi/2)^4*cos(q5)^2 + cos(q2 + q3 + q4)^2*sin(q5)^2)/(2*cos(q2 + q3 + q4)*cos(q1)*sin(q5) - cos(q5)*sin(q1) + cos(Pi)*cos(q5)*sin(q1))^2)^(1/2)*(2*cos(q1)*cos(q2)*sin(q3)*sin(q4)*sin(q5) - 2*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q5) - cos(q5)*sin(q1)*(cos(Pi) - 1) + 2*cos(q1)*cos(q3)*sin(q2)*sin(q4)*sin(q5) + 2*cos(q1)*cos(q4)*sin(q2)*sin(q3)*sin(q5))), 0]
%%
q1 = 0;
q2 = 0;
q3 = 0;
q4 = 0;
q5 = 0;
q6 = 0;
Pi = pi;
J = [(6127*cos(q2)*sin(q1))/10000 + (3483*sin(Pi/2)*cos(q1))/20000 - (11431*sin(q1)*sin(q2)*sin(q3))/20000 + (2331*sin(Pi/2)^3*cos(q1)*cos(q5))/20000 + (11431*cos(q2)*cos(q3)*sin(q1))/20000 + (2331*cos(q2 + q3 + q4)*sin(Pi/2)*sin(q1)*sin(q5))/20000 - (2397*sin(Pi/2)*cos(q2 + q3)*sin(q1)*sin(q4))/20000 - (2397*sin(Pi/2)*sin(q2 + q3)*cos(q4)*sin(q1))/20000, (cos(q1)*(12254*sin(q2) + 11431*cos(q2)*sin(q3) + 11431*cos(q3)*sin(q2) - 2397*sin(Pi/2)*sin(q2 + q3)*sin(q4) + 2331*sin(Pi/2)*sin(q2 + q3 + q4)*sin(q5) + 2397*sin(Pi/2)*cos(q2 + q3)*cos(q4)))/20000, (cos(q1)*(11431*cos(q2)*sin(q3) + 11431*cos(q3)*sin(q2) - 2397*sin(Pi/2)*sin(q2 + q3)*sin(q4) + 2331*sin(Pi/2)*sin(q2 + q3 + q4)*sin(q5) + 2397*sin(Pi/2)*cos(q2 + q3)*cos(q4)))/20000, (3*sin(Pi/2)*cos(q1)*(777*sin(q2 + q3 + q4)*sin(q5) + 799*cos(q2 + q3)*cos(q4) - 799*sin(q2 + q3)*sin(q4)))/20000, - (2331*sin(Pi/2)^3*sin(q1)*sin(q5))/20000 - (2331*cos(q2 + q3 + q4)*sin(Pi/2)*cos(q1)*cos(q5))/20000, 0;
(3483*sin(Pi/2)*sin(q1))/20000 - (6127*cos(q1)*cos(q2))/10000 + (2331*sin(Pi/2)^3*cos(q5)*sin(q1))/20000 - (11431*cos(q1)*cos(q2)*cos(q3))/20000 + (11431*cos(q1)*sin(q2)*sin(q3))/20000 - (2331*cos(q2 + q3 + q4)*sin(Pi/2)*cos(q1)*sin(q5))/20000 + (2397*sin(Pi/2)*cos(q2 + q3)*cos(q1)*sin(q4))/20000 + (2397*sin(Pi/2)*sin(q2 + q3)*cos(q1)*cos(q4))/20000, (sin(q1)*(12254*sin(q2) + 11431*cos(q2)*sin(q3) + 11431*cos(q3)*sin(q2) - 2397*sin(Pi/2)*sin(q2 + q3)*sin(q4) + 2331*sin(Pi/2)*sin(q2 + q3 + q4)*sin(q5) + 2397*sin(Pi/2)*cos(q2 + q3)*cos(q4)))/20000, (sin(q1)*(11431*cos(q2)*sin(q3) + 11431*cos(q3)*sin(q2) - 2397*sin(Pi/2)*sin(q2 + q3)*sin(q4) + 2331*sin(Pi/2)*sin(q2 + q3 + q4)*sin(q5) + 2397*sin(Pi/2)*cos(q2 + q3)*cos(q4)))/20000, (3*sin(Pi/2)*sin(q1)*(777*sin(q2 + q3 + q4)*sin(q5) + 799*cos(q2 + q3)*cos(q4) - 799*sin(q2 + q3)*sin(q4)))/20000, (2331*sin(Pi/2)^3*cos(q1)*sin(q5))/20000 - (2331*cos(q2 + q3 + q4)*sin(Pi/2)*cos(q5)*sin(q1))/20000, 0;
0, (11431*sin(Pi/2)*sin(q2)*sin(q3))/20000 - (11431*sin(Pi/2)*cos(q2)*cos(q3))/20000 - (6127*sin(Pi/2)*cos(q2))/10000 - (2397*cos(q2 + q3)*sin(q4)*(cos(Pi)/2 - 1/2))/20000 - (2397*sin(q2 + q3)*cos(q4)*(cos(Pi)/2 - 1/2))/20000 + (2331*cos(q2 + q3 + q4)*sin(q5)*(cos(Pi)/2 - 1/2))/20000, (11431*sin(Pi/2)*sin(q2)*sin(q3))/20000 - (11431*sin(Pi/2)*cos(q2)*cos(q3))/20000 - (2397*cos(q2 + q3)*sin(q4)*(cos(Pi)/2 - 1/2))/20000 - (2397*sin(q2 + q3)*cos(q4)*(cos(Pi)/2 - 1/2))/20000 + (2331*cos(q2 + q3 + q4)*sin(q5)*(cos(Pi)/2 - 1/2))/20000, -(3*(cos(Pi) - 1)*(799*cos(q2 + q3)*sin(q4) - 777*cos(q2 + q3 + q4)*sin(q5) + 799*sin(q2 + q3)*cos(q4)))/40000, (2331*sin(q2 + q3 + q4)*cos(q5)*(cos(Pi)/2 - 1/2))/20000, 0;
-1, -(8*sin(q2 + q3 + q4)*sin(2*q5)*(cos(Pi) - 1))/(4*cos(2*q2 + 2*q3 + 2*q4 - 2*q5) + 4*cos(2*q2 + 2*q3 + 2*q4 + 2*q5) - 2*cos(2*Pi) + 2*cos(2*q5) - 8*cos(2*q2 + 2*q3 + 2*q4) - cos(2*Pi - 2*q5) - cos(2*Pi + 2*q5) + 8*cos(Pi) + 4*cos(Pi - 2*q5) + 4*cos(Pi + 2*q5) - 14), -(8*sin(q2 + q3 + q4)*sin(2*q5)*(cos(Pi) - 1))/(4*cos(2*q2 + 2*q3 + 2*q4 - 2*q5) + 4*cos(2*q2 + 2*q3 + 2*q4 + 2*q5) - 2*cos(2*Pi) + 2*cos(2*q5) - 8*cos(2*q2 + 2*q3 + 2*q4) - cos(2*Pi - 2*q5) - cos(2*Pi + 2*q5) + 8*cos(Pi) + 4*cos(Pi - 2*q5) + 4*cos(Pi + 2*q5) - 14), -(8*sin(q2 + q3 + q4)*sin(2*q5)*(cos(Pi) - 1))/(4*cos(2*q2 + 2*q3 + 2*q4 - 2*q5) + 4*cos(2*q2 + 2*q3 + 2*q4 + 2*q5) - 2*cos(2*Pi) + 2*cos(2*q5) - 8*cos(2*q2 + 2*q3 + 2*q4) - cos(2*Pi - 2*q5) - cos(2*Pi + 2*q5) + 8*cos(Pi) + 4*cos(Pi - 2*q5) + 4*cos(Pi + 2*q5) - 14), (4*(cos(Pi + q2 + q3 + q4)/2 - cos(q2 + q3 + q4) + cos(q2 - Pi + q3 + q4)/2))/(cos(2*q2 + 2*q3 + 2*q4 - 2*q5) + cos(2*q2 + 2*q3 + 2*q4 + 2*q5) - cos(2*Pi)/2 + cos(2*q5)/2 - 2*cos(2*q2 + 2*q3 + 2*q4) - cos(2*Pi - 2*q5)/4 - cos(2*Pi + 2*q5)/4 + 2*cos(Pi) + cos(Pi - 2*q5) + cos(Pi + 2*q5) - 7/2), 0;
0, -(2*cos(q2 + q3 + q4)*sin(Pi/2)*(cos(Pi/2)^4*cos(q5)^2 - 2*cos(Pi/2)^2*cos(q5)^2 + 1))/(sin(q2 + q3 + q4)^2*sin(q5)*(cos(Pi) - 1)*((sin(Pi/2)^4*sin(q5)^2 + sin(q2 + q3 + q4)^2*sin(q5)^2 - sin(q5)^2 - sin(Pi/2)^4)/(sin(Pi/2)^2*sin(q2 + q3 + q4)^2*sin(q5)^2) - 1)*((sin(Pi/2)^4*cos(q5)^2 + cos(q2 + q3 + q4)^2*sin(q5)^2)/(2*cos(q2 + q3 + q4)*cos(q1)*sin(q5) - cos(q5)*sin(q1) + cos(Pi)*cos(q5)*sin(q1))^2)^(1/2)*(2*cos(q1)*cos(q2)*sin(q3)*sin(q4)*sin(q5) - 2*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q5) - cos(q5)*sin(q1)*(cos(Pi) - 1) + 2*cos(q1)*cos(q3)*sin(q2)*sin(q4)*sin(q5) + 2*cos(q1)*cos(q4)*sin(q2)*sin(q3)*sin(q5))), -(2*cos(q2 + q3 + q4)*sin(Pi/2)*(cos(Pi/2)^4*cos(q5)^2 - 2*cos(Pi/2)^2*cos(q5)^2 + 1))/(sin(q2 + q3 + q4)^2*sin(q5)*(cos(Pi) - 1)*((sin(Pi/2)^4*sin(q5)^2 + sin(q2 + q3 + q4)^2*sin(q5)^2 - sin(q5)^2 - sin(Pi/2)^4)/(sin(Pi/2)^2*sin(q2 + q3 + q4)^2*sin(q5)^2) - 1)*((sin(Pi/2)^4*cos(q5)^2 + cos(q2 + q3 + q4)^2*sin(q5)^2)/(2*cos(q2 + q3 + q4)*cos(q1)*sin(q5) - cos(q5)*sin(q1) + cos(Pi)*cos(q5)*sin(q1))^2)^(1/2)*(2*cos(q1)*cos(q2)*sin(q3)*sin(q4)*sin(q5) - 2*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q5) - cos(q5)*sin(q1)*(cos(Pi) - 1) + 2*cos(q1)*cos(q3)*sin(q2)*sin(q4)*sin(q5) + 2*cos(q1)*cos(q4)*sin(q2)*sin(q3)*sin(q5))), -(2*cos(q2 + q3 + q4)*sin(Pi/2)*(cos(Pi/2)^4*cos(q5)^2 - 2*cos(Pi/2)^2*cos(q5)^2 + 1))/(sin(q2 + q3 + q4)^2*sin(q5)*(cos(Pi) - 1)*((sin(Pi/2)^4*sin(q5)^2 + sin(q2 + q3 + q4)^2*sin(q5)^2 - sin(q5)^2 - sin(Pi/2)^4)/(sin(Pi/2)^2*sin(q2 + q3 + q4)^2*sin(q5)^2) - 1)*((sin(Pi/2)^4*cos(q5)^2 + cos(q2 + q3 + q4)^2*sin(q5)^2)/(2*cos(q2 + q3 + q4)*cos(q1)*sin(q5) - cos(q5)*sin(q1) + cos(Pi)*cos(q5)*sin(q1))^2)^(1/2)*(2*cos(q1)*cos(q2)*sin(q3)*sin(q4)*sin(q5) - 2*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q5) - cos(q5)*sin(q1)*(cos(Pi) - 1) + 2*cos(q1)*cos(q3)*sin(q2)*sin(q4)*sin(q5) + 2*cos(q1)*cos(q4)*sin(q2)*sin(q3)*sin(q5))), -(2*sin(Pi/2)^5*cos(q5))/(sin(q2 + q3 + q4)*sin(q5)^2*(cos(Pi) - 1)*((sin(Pi/2)^4*sin(q5)^2 + sin(q2 + q3 + q4)^2*sin(q5)^2 - sin(q5)^2 - sin(Pi/2)^4)/(sin(Pi/2)^2*sin(q2 + q3 + q4)^2*sin(q5)^2) - 1)*((sin(Pi/2)^4*cos(q5)^2 + cos(q2 + q3 + q4)^2*sin(q5)^2)/(2*cos(q2 + q3 + q4)*cos(q1)*sin(q5) - cos(q5)*sin(q1) + cos(Pi)*cos(q5)*sin(q1))^2)^(1/2)*(2*cos(q1)*cos(q2)*sin(q3)*sin(q4)*sin(q5) - 2*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q5) - cos(q5)*sin(q1)*(cos(Pi) - 1) + 2*cos(q1)*cos(q3)*sin(q2)*sin(q4)*sin(q5) + 2*cos(q1)*cos(q4)*sin(q2)*sin(q3)*sin(q5))), 0;
0, (16*cos(q5)*(cos(q2 + q3 + q4) + cos(Pi + q2 + q3 + q4)/2 + cos(Pi) + cos(q2 - Pi + q3 + q4)/2 - 1))/(6*cos(2*Pi) - 4*cos(2*q2 + 2*q3 + 2*q4 + 2*q5) - 4*cos(2*q2 + 2*q3 + 2*q4 - 2*q5) - 8*cos(q2 + q3 + q4) + 8*cos(2*q5) - 2*cos(2*q2 + 2*q3 + 2*q4) - 4*cos(Pi + 2*q2 + 2*q3 + 2*q4) + 8*cos(Pi) + 4*cos(q2 - 2*Pi + q3 + q4) + 4*cos(2*Pi + q2 + q3 + q4) - 4*cos(2*q2 - Pi + 2*q3 + 2*q4) + cos(2*q2 - 2*Pi + 2*q3 + 2*q4) + cos(2*Pi + 2*q2 + 2*q3 + 2*q4) + 26), (16*cos(q5)*(cos(q2 + q3 + q4) + cos(Pi + q2 + q3 + q4)/2 + cos(Pi) + cos(q2 - Pi + q3 + q4)/2 - 1))/(6*cos(2*Pi) - 4*cos(2*q2 + 2*q3 + 2*q4 + 2*q5) - 4*cos(2*q2 + 2*q3 + 2*q4 - 2*q5) - 8*cos(q2 + q3 + q4) + 8*cos(2*q5) - 2*cos(2*q2 + 2*q3 + 2*q4) - 4*cos(Pi + 2*q2 + 2*q3 + 2*q4) + 8*cos(Pi) + 4*cos(q2 - 2*Pi + q3 + q4) + 4*cos(2*Pi + q2 + q3 + q4) - 4*cos(2*q2 - Pi + 2*q3 + 2*q4) + cos(2*q2 - 2*Pi + 2*q3 + 2*q4) + cos(2*Pi + 2*q2 + 2*q3 + 2*q4) + 26), (16*cos(q5)*(cos(q2 + q3 + q4) + cos(Pi + q2 + q3 + q4)/2 + cos(Pi) + cos(q2 - Pi + q3 + q4)/2 - 1))/(6*cos(2*Pi) - 4*cos(2*q2 + 2*q3 + 2*q4 + 2*q5) - 4*cos(2*q2 + 2*q3 + 2*q4 - 2*q5) - 8*cos(q2 + q3 + q4) + 8*cos(2*q5) - 2*cos(2*q2 + 2*q3 + 2*q4) - 4*cos(Pi + 2*q2 + 2*q3 + 2*q4) + 8*cos(Pi) + 4*cos(q2 - 2*Pi + q3 + q4) + 4*cos(2*Pi + q2 + q3 + q4) - 4*cos(2*q2 - Pi + 2*q3 + 2*q4) + cos(2*q2 - 2*Pi + 2*q3 + 2*q4) + cos(2*Pi + 2*q2 + 2*q3 + 2*q4) + 26), -(8*sin(q2 + q3 + q4)*sin(q5)*(cos(Pi + q2 + q3 + q4) - 2*cos(q2 + q3 + q4) + 2*cos(Pi) + cos(q2 - Pi + q3 + q4) + 2))/(6*cos(2*Pi) - 4*cos(2*q2 + 2*q3 + 2*q4 + 2*q5) - 4*cos(2*q2 + 2*q3 + 2*q4 - 2*q5) - 8*cos(q2 + q3 + q4) + 8*cos(2*q5) - 2*cos(2*q2 + 2*q3 + 2*q4) - 4*cos(Pi + 2*q2 + 2*q3 + 2*q4) + 8*cos(Pi) + 4*cos(q2 - 2*Pi + q3 + q4) + 4*cos(2*Pi + q2 + q3 + q4) - 4*cos(2*q2 - Pi + 2*q3 + 2*q4) + cos(2*q2 - 2*Pi + 2*q3 + 2*q4) + cos(2*Pi + 2*q2 + 2*q3 + 2*q4) + 26), -1]
%%
atan((DH(2,3)/DH(3,3))/Rx)