-
Notifications
You must be signed in to change notification settings - Fork 0
/
ik.py
57 lines (43 loc) · 107 KB
/
ik.py
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
import nupmy as np
#Generated by sympy
def l_biba(a1, a2, a3, p1, p2, p3):
return (array([[cos(a2)*cos(a3), sin(a3)*cos(a2), -sin(a2), p1 - 215*cos(a2)*cos(a3)], [sin(a1)*sin(a2)*cos(a3) - sin(a3)*cos(a1), sin(a1)*sin(a2)*sin(a3) + cos(a1)*cos(a3), sin(a1)*cos(a2), p2 - 215*sin(a1)*sin(a2)*cos(a3) + 215*sin(a3)*cos(a1)], [sin(a1)*sin(a3) + sin(a2)*cos(a1)*cos(a3), -sin(a1)*cos(a3) + sin(a2)*sin(a3)*cos(a1), cos(a1)*cos(a2), p3 - 215*sin(a1)*sin(a3) - 215*sin(a2)*cos(a1)*cos(a3) - 670], [0, 0, 0, 1]]))
def t_inv(q1, q2, q3):
return (array([[1.0*(0.000898375535716436*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q1) + 0.0204430344127473*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q1)*cos(q3 - 1.39679272585953) + 0.174447226988777*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q1)*cos(q3 - 1.39679272585953)**2 + 0.661607260875955*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q1)*cos(q3 - 1.39679272585953)**3 + 0.940952548801358*sin(q1)**2*sin(q2)**4*cos(q1)*cos(q3 - 1.39679272585953)**4 + 0.0204430344127473*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q1)*cos(q2) + 0.345300951834688*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)*cos(q2)*cos(q3 - 1.39679272585953) + 1.92349267938962*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)*cos(q2)*cos(q3 - 1.39679272585953)**2 + 3.41491574122788*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)*cos(q2)*cos(q3 - 1.39679272585953)**3 - 0.661607260875955*sin(q1)**2*sin(q2)**3*cos(q1)*cos(q2)*cos(q3 - 1.39679272585953)**4 + 0.174447226988777*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q1)*cos(q2)**2 + 1.92349267938962*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)*cos(q2)**2*cos(q3 - 1.39679272585953) + 4.95331663806734*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 1.92349267938962*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 0.174447226988777*sin(q1)**2*sin(q2)**2*cos(q1)*cos(q2)**2*cos(q3 - 1.39679272585953)**4 + 0.661607260875955*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q1)*cos(q2)**3 + 3.41491574122788*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)*cos(q2)**3*cos(q3 - 1.39679272585953) - 1.92349267938962*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.345300951834688*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 0.0204430344127473*sin(q1)**2*sin(q2)*cos(q1)*cos(q2)**3*cos(q3 - 1.39679272585953)**4 + 0.940952548801358*sin(q1)**2*sin(q3 - 1.39679272585953)**4*cos(q1)*cos(q2)**4 - 0.661607260875955*sin(q1)**2*sin(q3 - 1.39679272585953)**3*cos(q1)*cos(q2)**4*cos(q3 - 1.39679272585953) + 0.174447226988777*sin(q1)**2*sin(q3 - 1.39679272585953)**2*cos(q1)*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 0.0204430344127473*sin(q1)**2*sin(q3 - 1.39679272585953)*cos(q1)*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 0.000898375535716436*sin(q1)**2*cos(q1)*cos(q2)**4*cos(q3 - 1.39679272585953)**4 + 0.000898375535716436*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q1)**3 + 0.0204430344127473*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q1)**3*cos(q3 - 1.39679272585953) + 0.174447226988777*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q1)**3*cos(q3 - 1.39679272585953)**2 + 0.661607260875955*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q1)**3*cos(q3 - 1.39679272585953)**3 + 0.940952548801358*sin(q2)**4*cos(q1)**3*cos(q3 - 1.39679272585953)**4 + 0.0204430344127473*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q1)**3*cos(q2) + 0.345300951834688*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)**3*cos(q2)*cos(q3 - 1.39679272585953) + 1.92349267938962*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)**3*cos(q2)*cos(q3 - 1.39679272585953)**2 + 3.41491574122788*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)**3*cos(q2)*cos(q3 - 1.39679272585953)**3 - 0.661607260875955*sin(q2)**3*cos(q1)**3*cos(q2)*cos(q3 - 1.39679272585953)**4 + 0.174447226988777*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**3*cos(q2)**2 + 1.92349267938962*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**3*cos(q2)**2*cos(q3 - 1.39679272585953) + 4.95331663806734*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**3*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 1.92349267938962*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)**3*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 0.174447226988777*sin(q2)**2*cos(q1)**3*cos(q2)**2*cos(q3 - 1.39679272585953)**4 + 0.661607260875955*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q1)**3*cos(q2)**3 + 3.41491574122788*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)**3*cos(q2)**3*cos(q3 - 1.39679272585953) - 1.92349267938962*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)**3*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.345300951834688*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)**3*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 0.0204430344127473*sin(q2)*cos(q1)**3*cos(q2)**3*cos(q3 - 1.39679272585953)**4 + 0.940952548801358*sin(q3 - 1.39679272585953)**4*cos(q1)**3*cos(q2)**4 - 0.661607260875955*sin(q3 - 1.39679272585953)**3*cos(q1)**3*cos(q2)**4*cos(q3 - 1.39679272585953) + 0.174447226988777*sin(q3 - 1.39679272585953)**2*cos(q1)**3*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 0.0204430344127473*sin(q3 - 1.39679272585953)*cos(q1)**3*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 0.000898375535716436*cos(q1)**3*cos(q2)**4*cos(q3 - 1.39679272585953)**4)/(-0.00518911669778412*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**5 - 0.0885609249755156*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**4*cos(q3 - 1.39679272585953) - 0.509002378780718*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**3*cos(q3 - 1.39679272585953)**2 - 1.04394014788834*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**2*cos(q3 - 1.39679272585953)**3 - 0.503813262082934*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)*cos(q3 - 1.39679272585953)**4 - 0.955379222912822*sin(q1)**4*sin(q2)**5*cos(q3 - 1.39679272585953)**5 - 0.0885609249755156*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**5*cos(q2) - 0.992059174072515*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q2)*cos(q3 - 1.39679272585953) - 2.77757674376295*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q2)*cos(q3 - 1.39679272585953)**2 - 0.488245911989581*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 - 2.68901581878744*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q2)*cos(q3 - 1.39679272585953)**4 + 0.503813262082934*sin(q1)**4*sin(q2)**4*cos(q2)*cos(q3 - 1.39679272585953)**5 - 0.509002378780718*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**5*cos(q2)**2 - 2.77757674376295*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q2)**2*cos(q3 - 1.39679272585953) - 0.0207564667911362*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 3.82151689165129*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 0.488245911989581*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q2)**2*cos(q3 - 1.39679272585953)**4 - 1.04394014788834*sin(q1)**4*sin(q2)**3*cos(q2)**2*cos(q3 - 1.39679272585953)**5 - 1.04394014788834*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**5*cos(q2)**3 - 0.488245911989581*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q2)**3*cos(q3 - 1.39679272585953) - 3.82151689165129*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.0207564667911364*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 2.77757674376295*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q2)**3*cos(q3 - 1.39679272585953)**4 + 0.509002378780718*sin(q1)**4*sin(q2)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**5 - 0.503813262082934*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**5*cos(q2)**4 - 2.68901581878744*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q2)**4*cos(q3 - 1.39679272585953) + 0.488245911989581*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 2.77757674376295*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 0.992059174072515*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)*cos(q2)**4*cos(q3 - 1.39679272585953)**4 - 0.0885609249755156*sin(q1)**4*sin(q2)*cos(q2)**4*cos(q3 - 1.39679272585953)**5 - 0.955379222912822*sin(q1)**4*sin(q3 - 1.39679272585953)**5*cos(q2)**5 + 0.503813262082934*sin(q1)**4*sin(q3 - 1.39679272585953)**4*cos(q2)**5*cos(q3 - 1.39679272585953) - 1.04394014788834*sin(q1)**4*sin(q3 - 1.39679272585953)**3*cos(q2)**5*cos(q3 - 1.39679272585953)**2 + 0.509002378780718*sin(q1)**4*sin(q3 - 1.39679272585953)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**3 - 0.0885609249755156*sin(q1)**4*sin(q3 - 1.39679272585953)*cos(q2)**5*cos(q3 - 1.39679272585953)**4 + 0.00518911669778412*sin(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**5 - 0.0103782333955682*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**5*cos(q1)**2 - 0.177121849951031*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q3 - 1.39679272585953) - 1.01800475756144*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q3 - 1.39679272585953)**2 - 2.08788029577668*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q3 - 1.39679272585953)**3 - 1.00762652416587*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q3 - 1.39679272585953)**4 - 1.91075844582564*sin(q1)**2*sin(q2)**5*cos(q1)**2*cos(q3 - 1.39679272585953)**5 - 0.177121849951031*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2) - 1.98411834814503*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953) - 5.5551534875259*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**2 - 0.976491823979162*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 - 5.37803163757487*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**4 + 1.00762652416587*sin(q1)**2*sin(q2)**4*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**5 - 1.01800475756144*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2)**2 - 5.5551534875259*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953) - 0.0415129335822729*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 7.64303378330258*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 0.976491823979162*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**4 - 2.08788029577668*sin(q1)**2*sin(q2)**3*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**5 - 2.08788029577668*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2)**3 - 0.976491823979162*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953) - 7.64303378330258*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.0415129335822726*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 5.5551534875259*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**4 + 1.01800475756144*sin(q1)**2*sin(q2)**2*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**5 - 1.00762652416587*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2)**4 - 5.37803163757487*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953) + 0.976491823979162*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 5.5551534875259*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 1.98411834814503*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**4 - 0.177121849951031*sin(q1)**2*sin(q2)*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**5 - 1.91075844582564*sin(q1)**2*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2)**5 + 1.00762652416587*sin(q1)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953) - 2.08788029577668*sin(q1)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**2 + 1.01800475756144*sin(q1)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**3 - 0.177121849951031*sin(q1)**2*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**4 + 0.0103782333955682*sin(q1)**2*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**5 - 0.00518911669778412*sin(q2)**5*sin(q3 - 1.39679272585953)**5*cos(q1)**4 - 0.0885609249755156*sin(q2)**5*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q3 - 1.39679272585953) - 0.509002378780718*sin(q2)**5*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q3 - 1.39679272585953)**2 - 1.04394014788834*sin(q2)**5*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q3 - 1.39679272585953)**3 - 0.503813262082934*sin(q2)**5*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q3 - 1.39679272585953)**4 - 0.955379222912822*sin(q2)**5*cos(q1)**4*cos(q3 - 1.39679272585953)**5 - 0.0885609249755156*sin(q2)**4*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2) - 0.992059174072515*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953) - 2.77757674376295*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**2 - 0.488245911989581*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**3 - 2.68901581878744*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**4 + 0.503813262082934*sin(q2)**4*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**5 - 0.509002378780718*sin(q2)**3*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2)**2 - 2.77757674376295*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953) - 0.0207564667911362*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 3.82151689165129*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 0.488245911989581*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**4 - 1.04394014788834*sin(q2)**3*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**5 - 1.04394014788834*sin(q2)**2*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2)**3 - 0.488245911989581*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953) - 3.82151689165129*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.0207564667911364*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 2.77757674376295*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**4 + 0.509002378780718*sin(q2)**2*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**5 - 0.503813262082934*sin(q2)*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2)**4 - 2.68901581878744*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953) + 0.488245911989581*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 2.77757674376295*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 0.992059174072515*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**4 - 0.0885609249755156*sin(q2)*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**5 - 0.955379222912822*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2)**5 + 0.503813262082934*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953) - 1.04394014788834*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**2 + 0.509002378780718*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**3 - 0.0885609249755156*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**4 + 0.00518911669778412*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**5), -1.0*(0.000898375535716436*sin(q1)**3*sin(q2)**4*sin(q3 - 1.39679272585953)**4 + 0.0204430344127473*sin(q1)**3*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q3 - 1.39679272585953) + 0.174447226988777*sin(q1)**3*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q3 - 1.39679272585953)**2 + 0.661607260875955*sin(q1)**3*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q3 - 1.39679272585953)**3 + 0.940952548801358*sin(q1)**3*sin(q2)**4*cos(q3 - 1.39679272585953)**4 + 0.0204430344127473*sin(q1)**3*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q2) + 0.345300951834688*sin(q1)**3*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q2)*cos(q3 - 1.39679272585953) + 1.92349267938962*sin(q1)**3*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q2)*cos(q3 - 1.39679272585953)**2 + 3.41491574122788*sin(q1)**3*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q2)*cos(q3 - 1.39679272585953)**3 - 0.661607260875955*sin(q1)**3*sin(q2)**3*cos(q2)*cos(q3 - 1.39679272585953)**4 + 0.174447226988777*sin(q1)**3*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q2)**2 + 1.92349267938962*sin(q1)**3*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q2)**2*cos(q3 - 1.39679272585953) + 4.95331663806734*sin(q1)**3*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 1.92349267938962*sin(q1)**3*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 0.174447226988777*sin(q1)**3*sin(q2)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**4 + 0.661607260875955*sin(q1)**3*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q2)**3 + 3.41491574122788*sin(q1)**3*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q2)**3*cos(q3 - 1.39679272585953) - 1.92349267938962*sin(q1)**3*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.345300951834688*sin(q1)**3*sin(q2)*sin(q3 - 1.39679272585953)*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 0.0204430344127473*sin(q1)**3*sin(q2)*cos(q2)**3*cos(q3 - 1.39679272585953)**4 + 0.940952548801358*sin(q1)**3*sin(q3 - 1.39679272585953)**4*cos(q2)**4 - 0.661607260875955*sin(q1)**3*sin(q3 - 1.39679272585953)**3*cos(q2)**4*cos(q3 - 1.39679272585953) + 0.174447226988777*sin(q1)**3*sin(q3 - 1.39679272585953)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 0.0204430344127473*sin(q1)**3*sin(q3 - 1.39679272585953)*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 0.000898375535716436*sin(q1)**3*cos(q2)**4*cos(q3 - 1.39679272585953)**4 + 0.000898375535716436*sin(q1)*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q1)**2 + 0.0204430344127473*sin(q1)*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q3 - 1.39679272585953) + 0.174447226988777*sin(q1)*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q3 - 1.39679272585953)**2 + 0.661607260875955*sin(q1)*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q3 - 1.39679272585953)**3 + 0.940952548801358*sin(q1)*sin(q2)**4*cos(q1)**2*cos(q3 - 1.39679272585953)**4 + 0.0204430344127473*sin(q1)*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2) + 0.345300951834688*sin(q1)*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953) + 1.92349267938962*sin(q1)*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**2 + 3.41491574122788*sin(q1)*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 - 0.661607260875955*sin(q1)*sin(q2)**3*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**4 + 0.174447226988777*sin(q1)*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**2 + 1.92349267938962*sin(q1)*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953) + 4.95331663806734*sin(q1)*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 1.92349267938962*sin(q1)*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 0.174447226988777*sin(q1)*sin(q2)**2*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**4 + 0.661607260875955*sin(q1)*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**3 + 3.41491574122788*sin(q1)*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953) - 1.92349267938962*sin(q1)*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.345300951834688*sin(q1)*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 0.0204430344127473*sin(q1)*sin(q2)*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**4 + 0.940952548801358*sin(q1)*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**4 - 0.661607260875955*sin(q1)*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953) + 0.174447226988777*sin(q1)*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 0.0204430344127473*sin(q1)*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 0.000898375535716436*sin(q1)*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**4)/(-0.00518911669778412*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**5 - 0.0885609249755156*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**4*cos(q3 - 1.39679272585953) - 0.509002378780718*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**3*cos(q3 - 1.39679272585953)**2 - 1.04394014788834*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**2*cos(q3 - 1.39679272585953)**3 - 0.503813262082934*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)*cos(q3 - 1.39679272585953)**4 - 0.955379222912822*sin(q1)**4*sin(q2)**5*cos(q3 - 1.39679272585953)**5 - 0.0885609249755156*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**5*cos(q2) - 0.992059174072515*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q2)*cos(q3 - 1.39679272585953) - 2.77757674376295*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q2)*cos(q3 - 1.39679272585953)**2 - 0.488245911989581*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 - 2.68901581878744*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q2)*cos(q3 - 1.39679272585953)**4 + 0.503813262082934*sin(q1)**4*sin(q2)**4*cos(q2)*cos(q3 - 1.39679272585953)**5 - 0.509002378780718*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**5*cos(q2)**2 - 2.77757674376295*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q2)**2*cos(q3 - 1.39679272585953) - 0.0207564667911362*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 3.82151689165129*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 0.488245911989581*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q2)**2*cos(q3 - 1.39679272585953)**4 - 1.04394014788834*sin(q1)**4*sin(q2)**3*cos(q2)**2*cos(q3 - 1.39679272585953)**5 - 1.04394014788834*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**5*cos(q2)**3 - 0.488245911989581*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q2)**3*cos(q3 - 1.39679272585953) - 3.82151689165129*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.0207564667911364*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 2.77757674376295*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q2)**3*cos(q3 - 1.39679272585953)**4 + 0.509002378780718*sin(q1)**4*sin(q2)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**5 - 0.503813262082934*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**5*cos(q2)**4 - 2.68901581878744*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q2)**4*cos(q3 - 1.39679272585953) + 0.488245911989581*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 2.77757674376295*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 0.992059174072515*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)*cos(q2)**4*cos(q3 - 1.39679272585953)**4 - 0.0885609249755156*sin(q1)**4*sin(q2)*cos(q2)**4*cos(q3 - 1.39679272585953)**5 - 0.955379222912822*sin(q1)**4*sin(q3 - 1.39679272585953)**5*cos(q2)**5 + 0.503813262082934*sin(q1)**4*sin(q3 - 1.39679272585953)**4*cos(q2)**5*cos(q3 - 1.39679272585953) - 1.04394014788834*sin(q1)**4*sin(q3 - 1.39679272585953)**3*cos(q2)**5*cos(q3 - 1.39679272585953)**2 + 0.509002378780718*sin(q1)**4*sin(q3 - 1.39679272585953)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**3 - 0.0885609249755156*sin(q1)**4*sin(q3 - 1.39679272585953)*cos(q2)**5*cos(q3 - 1.39679272585953)**4 + 0.00518911669778412*sin(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**5 - 0.0103782333955682*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**5*cos(q1)**2 - 0.177121849951031*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q3 - 1.39679272585953) - 1.01800475756144*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q3 - 1.39679272585953)**2 - 2.08788029577668*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q3 - 1.39679272585953)**3 - 1.00762652416587*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q3 - 1.39679272585953)**4 - 1.91075844582564*sin(q1)**2*sin(q2)**5*cos(q1)**2*cos(q3 - 1.39679272585953)**5 - 0.177121849951031*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2) - 1.98411834814503*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953) - 5.5551534875259*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**2 - 0.976491823979162*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 - 5.37803163757487*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**4 + 1.00762652416587*sin(q1)**2*sin(q2)**4*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**5 - 1.01800475756144*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2)**2 - 5.5551534875259*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953) - 0.0415129335822729*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 7.64303378330258*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 0.976491823979162*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**4 - 2.08788029577668*sin(q1)**2*sin(q2)**3*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**5 - 2.08788029577668*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2)**3 - 0.976491823979162*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953) - 7.64303378330258*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.0415129335822726*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 5.5551534875259*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**4 + 1.01800475756144*sin(q1)**2*sin(q2)**2*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**5 - 1.00762652416587*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2)**4 - 5.37803163757487*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953) + 0.976491823979162*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 5.5551534875259*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 1.98411834814503*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**4 - 0.177121849951031*sin(q1)**2*sin(q2)*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**5 - 1.91075844582564*sin(q1)**2*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2)**5 + 1.00762652416587*sin(q1)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953) - 2.08788029577668*sin(q1)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**2 + 1.01800475756144*sin(q1)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**3 - 0.177121849951031*sin(q1)**2*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**4 + 0.0103782333955682*sin(q1)**2*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**5 - 0.00518911669778412*sin(q2)**5*sin(q3 - 1.39679272585953)**5*cos(q1)**4 - 0.0885609249755156*sin(q2)**5*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q3 - 1.39679272585953) - 0.509002378780718*sin(q2)**5*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q3 - 1.39679272585953)**2 - 1.04394014788834*sin(q2)**5*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q3 - 1.39679272585953)**3 - 0.503813262082934*sin(q2)**5*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q3 - 1.39679272585953)**4 - 0.955379222912822*sin(q2)**5*cos(q1)**4*cos(q3 - 1.39679272585953)**5 - 0.0885609249755156*sin(q2)**4*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2) - 0.992059174072515*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953) - 2.77757674376295*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**2 - 0.488245911989581*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**3 - 2.68901581878744*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**4 + 0.503813262082934*sin(q2)**4*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**5 - 0.509002378780718*sin(q2)**3*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2)**2 - 2.77757674376295*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953) - 0.0207564667911362*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 3.82151689165129*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 0.488245911989581*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**4 - 1.04394014788834*sin(q2)**3*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**5 - 1.04394014788834*sin(q2)**2*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2)**3 - 0.488245911989581*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953) - 3.82151689165129*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.0207564667911364*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 2.77757674376295*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**4 + 0.509002378780718*sin(q2)**2*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**5 - 0.503813262082934*sin(q2)*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2)**4 - 2.68901581878744*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953) + 0.488245911989581*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 2.77757674376295*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 0.992059174072515*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**4 - 0.0885609249755156*sin(q2)*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**5 - 0.955379222912822*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2)**5 + 0.503813262082934*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953) - 1.04394014788834*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**2 + 0.509002378780718*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**3 - 0.0885609249755156*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**4 + 0.00518911669778412*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**5), 1.0*(0.00511075860318683*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**4 + 0.0863252379586721*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q3 - 1.39679272585953) + 0.480873169847405*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q3 - 1.39679272585953)**2 + 0.853728935306969*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q3 - 1.39679272585953)**3 - 0.165401815218989*sin(q1)**4*sin(q2)**4*cos(q3 - 1.39679272585953)**4 + 0.0863252379586721*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q2) + 0.941303305282064*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q2)*cos(q3 - 1.39679272585953) + 2.30221109204489*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q2)*cos(q3 - 1.39679272585953)**2 - 1.62335360057077*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q2)*cos(q3 - 1.39679272585953)**3 - 0.853728935306969*sin(q1)**4*sin(q2)**3*cos(q2)*cos(q3 - 1.39679272585953)**4 + 0.480873169847405*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q2)**2 + 2.30221109204489*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q2)**2*cos(q3 - 1.39679272585953) - 2.88523901908443*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 2.30221109204489*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 0.480873169847405*sin(q1)**4*sin(q2)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**4 + 0.853728935306969*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q2)**3 - 1.62335360057077*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q2)**3*cos(q3 - 1.39679272585953) - 2.30221109204489*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.941303305282064*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 0.0863252379586721*sin(q1)**4*sin(q2)*cos(q2)**3*cos(q3 - 1.39679272585953)**4 - 0.165401815218989*sin(q1)**4*sin(q3 - 1.39679272585953)**4*cos(q2)**4 - 0.853728935306969*sin(q1)**4*sin(q3 - 1.39679272585953)**3*cos(q2)**4*cos(q3 - 1.39679272585953) + 0.480873169847405*sin(q1)**4*sin(q3 - 1.39679272585953)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 0.0863252379586721*sin(q1)**4*sin(q3 - 1.39679272585953)*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 0.00511075860318683*sin(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**4 + 0.0102215172063737*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q1)**2 + 0.172650475917344*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q3 - 1.39679272585953) + 0.961746339694811*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q3 - 1.39679272585953)**2 + 1.70745787061394*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q3 - 1.39679272585953)**3 - 0.330803630437977*sin(q1)**2*sin(q2)**4*cos(q1)**2*cos(q3 - 1.39679272585953)**4 + 0.172650475917344*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2) + 1.88260661056413*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953) + 4.60442218408978*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**2 - 3.24670720114153*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 - 1.70745787061394*sin(q1)**2*sin(q2)**3*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**4 + 0.961746339694811*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**2 + 4.60442218408978*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953) - 5.77047803816887*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 4.60442218408978*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 0.961746339694811*sin(q1)**2*sin(q2)**2*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**4 + 1.70745787061394*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**3 - 3.24670720114153*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953) - 4.60442218408978*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 1.88260661056413*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 0.172650475917344*sin(q1)**2*sin(q2)*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**4 - 0.330803630437977*sin(q1)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**4 - 1.70745787061394*sin(q1)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953) + 0.961746339694811*sin(q1)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 0.172650475917344*sin(q1)**2*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 0.0102215172063737*sin(q1)**2*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**4 + 0.00511075860318683*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q1)**4 + 0.0863252379586721*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q3 - 1.39679272585953) + 0.480873169847405*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q3 - 1.39679272585953)**2 + 0.853728935306969*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q3 - 1.39679272585953)**3 - 0.165401815218989*sin(q2)**4*cos(q1)**4*cos(q3 - 1.39679272585953)**4 + 0.0863252379586721*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2) + 0.941303305282064*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953) + 2.30221109204489*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**2 - 1.62335360057077*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**3 - 0.853728935306969*sin(q2)**3*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**4 + 0.480873169847405*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**2 + 2.30221109204489*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953) - 2.88523901908443*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 2.30221109204489*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 0.480873169847405*sin(q2)**2*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**4 + 0.853728935306969*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**3 - 1.62335360057077*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953) - 2.30221109204489*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.941303305282064*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 0.0863252379586721*sin(q2)*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**4 - 0.165401815218989*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**4 - 0.853728935306969*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953) + 0.480873169847405*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 0.0863252379586721*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 0.00511075860318683*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**4)/(-0.00518911669778412*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**5 - 0.0885609249755156*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**4*cos(q3 - 1.39679272585953) - 0.509002378780718*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**3*cos(q3 - 1.39679272585953)**2 - 1.04394014788834*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**2*cos(q3 - 1.39679272585953)**3 - 0.503813262082934*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)*cos(q3 - 1.39679272585953)**4 - 0.955379222912822*sin(q1)**4*sin(q2)**5*cos(q3 - 1.39679272585953)**5 - 0.0885609249755156*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**5*cos(q2) - 0.992059174072515*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q2)*cos(q3 - 1.39679272585953) - 2.77757674376295*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q2)*cos(q3 - 1.39679272585953)**2 - 0.488245911989581*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 - 2.68901581878744*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q2)*cos(q3 - 1.39679272585953)**4 + 0.503813262082934*sin(q1)**4*sin(q2)**4*cos(q2)*cos(q3 - 1.39679272585953)**5 - 0.509002378780718*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**5*cos(q2)**2 - 2.77757674376295*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q2)**2*cos(q3 - 1.39679272585953) - 0.0207564667911362*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 3.82151689165129*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 0.488245911989581*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q2)**2*cos(q3 - 1.39679272585953)**4 - 1.04394014788834*sin(q1)**4*sin(q2)**3*cos(q2)**2*cos(q3 - 1.39679272585953)**5 - 1.04394014788834*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**5*cos(q2)**3 - 0.488245911989581*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q2)**3*cos(q3 - 1.39679272585953) - 3.82151689165129*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.0207564667911364*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 2.77757674376295*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q2)**3*cos(q3 - 1.39679272585953)**4 + 0.509002378780718*sin(q1)**4*sin(q2)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**5 - 0.503813262082934*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**5*cos(q2)**4 - 2.68901581878744*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q2)**4*cos(q3 - 1.39679272585953) + 0.488245911989581*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 2.77757674376295*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 0.992059174072515*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)*cos(q2)**4*cos(q3 - 1.39679272585953)**4 - 0.0885609249755156*sin(q1)**4*sin(q2)*cos(q2)**4*cos(q3 - 1.39679272585953)**5 - 0.955379222912822*sin(q1)**4*sin(q3 - 1.39679272585953)**5*cos(q2)**5 + 0.503813262082934*sin(q1)**4*sin(q3 - 1.39679272585953)**4*cos(q2)**5*cos(q3 - 1.39679272585953) - 1.04394014788834*sin(q1)**4*sin(q3 - 1.39679272585953)**3*cos(q2)**5*cos(q3 - 1.39679272585953)**2 + 0.509002378780718*sin(q1)**4*sin(q3 - 1.39679272585953)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**3 - 0.0885609249755156*sin(q1)**4*sin(q3 - 1.39679272585953)*cos(q2)**5*cos(q3 - 1.39679272585953)**4 + 0.00518911669778412*sin(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**5 - 0.0103782333955682*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**5*cos(q1)**2 - 0.177121849951031*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q3 - 1.39679272585953) - 1.01800475756144*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q3 - 1.39679272585953)**2 - 2.08788029577668*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q3 - 1.39679272585953)**3 - 1.00762652416587*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q3 - 1.39679272585953)**4 - 1.91075844582564*sin(q1)**2*sin(q2)**5*cos(q1)**2*cos(q3 - 1.39679272585953)**5 - 0.177121849951031*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2) - 1.98411834814503*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953) - 5.5551534875259*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**2 - 0.976491823979162*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 - 5.37803163757487*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**4 + 1.00762652416587*sin(q1)**2*sin(q2)**4*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**5 - 1.01800475756144*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2)**2 - 5.5551534875259*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953) - 0.0415129335822729*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 7.64303378330258*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 0.976491823979162*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**4 - 2.08788029577668*sin(q1)**2*sin(q2)**3*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**5 - 2.08788029577668*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2)**3 - 0.976491823979162*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953) - 7.64303378330258*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.0415129335822726*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 5.5551534875259*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**4 + 1.01800475756144*sin(q1)**2*sin(q2)**2*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**5 - 1.00762652416587*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2)**4 - 5.37803163757487*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953) + 0.976491823979162*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 5.5551534875259*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 1.98411834814503*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**4 - 0.177121849951031*sin(q1)**2*sin(q2)*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**5 - 1.91075844582564*sin(q1)**2*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2)**5 + 1.00762652416587*sin(q1)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953) - 2.08788029577668*sin(q1)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**2 + 1.01800475756144*sin(q1)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**3 - 0.177121849951031*sin(q1)**2*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**4 + 0.0103782333955682*sin(q1)**2*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**5 - 0.00518911669778412*sin(q2)**5*sin(q3 - 1.39679272585953)**5*cos(q1)**4 - 0.0885609249755156*sin(q2)**5*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q3 - 1.39679272585953) - 0.509002378780718*sin(q2)**5*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q3 - 1.39679272585953)**2 - 1.04394014788834*sin(q2)**5*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q3 - 1.39679272585953)**3 - 0.503813262082934*sin(q2)**5*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q3 - 1.39679272585953)**4 - 0.955379222912822*sin(q2)**5*cos(q1)**4*cos(q3 - 1.39679272585953)**5 - 0.0885609249755156*sin(q2)**4*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2) - 0.992059174072515*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953) - 2.77757674376295*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**2 - 0.488245911989581*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**3 - 2.68901581878744*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**4 + 0.503813262082934*sin(q2)**4*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**5 - 0.509002378780718*sin(q2)**3*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2)**2 - 2.77757674376295*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953) - 0.0207564667911362*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 3.82151689165129*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 0.488245911989581*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**4 - 1.04394014788834*sin(q2)**3*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**5 - 1.04394014788834*sin(q2)**2*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2)**3 - 0.488245911989581*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953) - 3.82151689165129*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.0207564667911364*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 2.77757674376295*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**4 + 0.509002378780718*sin(q2)**2*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**5 - 0.503813262082934*sin(q2)*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2)**4 - 2.68901581878744*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953) + 0.488245911989581*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 2.77757674376295*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 0.992059174072515*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**4 - 0.0885609249755156*sin(q2)*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**5 - 0.955379222912822*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2)**5 + 0.503813262082934*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953) - 1.04394014788834*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**2 + 0.509002378780718*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**3 - 0.0885609249755156*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**4 + 0.00518911669778412*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**5), -1.0*(-6.64206937316367*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**5 - 113.35798396866*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**4*cos(q3 - 1.39679272585953) - 0.965753700895156*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**4 - 651.523044839319*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**3*cos(q3 - 1.39679272585953)**2 - 21.9762619937032*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**3*cos(q3 - 1.39679272585953) - 1336.24338929707*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**2*cos(q3 - 1.39679272585953)**3 - 187.530769012935*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**2*cos(q3 - 1.39679272585953)**2 - 644.880975466155*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)*cos(q3 - 1.39679272585953)**4 - 711.227805441651*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)*cos(q3 - 1.39679272585953)**3 - 1222.88540532841*sin(q1)**4*sin(q2)**5*cos(q3 - 1.39679272585953)**5 - 1011.52398996146*sin(q1)**4*sin(q2)**5*cos(q3 - 1.39679272585953)**4 - 113.35798396866*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**5*cos(q2) - 1269.83574281282*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q2)*cos(q3 - 1.39679272585953) - 16.4821964952774*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q2) + 0.280293167143528*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**4 - 3555.29823201658*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q2)*cos(q3 - 1.39679272585953)**2 - 278.398892416717*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q2)*cos(q3 - 1.39679272585953) + 6.37822673677715*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q3 - 1.39679272585953) - 624.954767346664*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 - 1550.81597275788*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q2)*cos(q3 - 1.39679272585953)**2 + 54.4275348204984*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q3 - 1.39679272585953)**2 - 3441.94024804792*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q2)*cos(q3 - 1.39679272585953)**4 - 2753.27581636498*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q2)*cos(q3 - 1.39679272585953)**3 + 206.421465393298*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q3 - 1.39679272585953)**3 + 644.880975466155*sin(q1)**4*sin(q2)**4*cos(q2)*cos(q3 - 1.39679272585953)**5 + 533.420854081238*sin(q1)**4*sin(q2)**4*cos(q2)*cos(q3 - 1.39679272585953)**4 + 293.577195226024*sin(q1)**4*sin(q2)**4*cos(q3 - 1.39679272585953)**4 - 651.523044839319*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**5*cos(q2)**2 - 3555.29823201658*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q2)**2*cos(q3 - 1.39679272585953) - 94.7311382073627*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q2)**2 + 6.37822673677715*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q2) - 26.5682774926546*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 1055.85357716563*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q2)**2*cos(q3 - 1.39679272585953) + 107.733896972423*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q2)*cos(q3 - 1.39679272585953) - 4891.54162131365*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**3 - 2849.93846197413*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**2 + 600.129715969562*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q2)*cos(q3 - 1.39679272585953)**2 + 624.954767346664*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q2)**2*cos(q3 - 1.39679272585953)**4 + 322.649509730271*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 1065.4537112631*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q2)*cos(q3 - 1.39679272585953)**3 - 1336.24338929707*sin(q1)**4*sin(q2)**3*cos(q2)**2*cos(q3 - 1.39679272585953)**5 - 1105.28937446793*sin(q1)**4*sin(q2)**3*cos(q2)**2*cos(q3 - 1.39679272585953)**4 - 206.421465393298*sin(q1)**4*sin(q2)**3*cos(q2)*cos(q3 - 1.39679272585953)**4 - 1336.24338929707*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**5*cos(q2)**3 - 624.954767346664*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q2)**3*cos(q3 - 1.39679272585953) - 194.28914785569*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q2)**3 + 54.4275348204984*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q2)**2 - 4891.54162131365*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q2)**3*cos(q3 - 1.39679272585953)**2 - 1196.15749787171*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q2)**3*cos(q3 - 1.39679272585953) + 600.129715969562*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q2)**2*cos(q3 - 1.39679272585953) + 26.5682774926545*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 1033.87731517192*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 1545.43479107701*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 3555.29823201658*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q2)**3*cos(q3 - 1.39679272585953)**4 - 2846.07544717055*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 600.129715969562*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 651.523044839319*sin(q1)**4*sin(q2)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**5 + 538.914919579664*sin(q1)**4*sin(q2)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**4 + 54.4275348204984*sin(q1)**4*sin(q2)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**4 - 644.880975466155*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**5*cos(q2)**4 - 3441.94024804792*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q2)**4*cos(q3 - 1.39679272585953) - 93.7653845064676*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q2)**4 + 206.421465393298*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q2)**3 + 624.954767346664*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 1033.87731517192*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q2)**4*cos(q3 - 1.39679272585953) + 1065.4537112631*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q2)**3*cos(q3 - 1.39679272585953) - 3555.29823201658*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**3 - 2662.40769296119*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 600.129715969562*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 1269.83574281282*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)*cos(q2)**4*cos(q3 - 1.39679272585953)**4 + 1033.87731517192*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 107.733896972423*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 113.35798396866*sin(q1)**4*sin(q2)*cos(q2)**4*cos(q3 - 1.39679272585953)**5 - 93.7653845064677*sin(q1)**4*sin(q2)*cos(q2)**4*cos(q3 - 1.39679272585953)**4 - 6.37822673677715*sin(q1)**4*sin(q2)*cos(q2)**3*cos(q3 - 1.39679272585953)**4 - 1222.88540532841*sin(q1)**4*sin(q3 - 1.39679272585953)**5*cos(q2)**5 + 644.880975466155*sin(q1)**4*sin(q3 - 1.39679272585953)**4*cos(q2)**5*cos(q3 - 1.39679272585953) - 177.806951360413*sin(q1)**4*sin(q3 - 1.39679272585953)**4*cos(q2)**5 + 293.577195226024*sin(q1)**4*sin(q3 - 1.39679272585953)**4*cos(q2)**4 - 1336.24338929707*sin(q1)**4*sin(q3 - 1.39679272585953)**3*cos(q2)**5*cos(q3 - 1.39679272585953)**2 - 917.758605454992*sin(q1)**4*sin(q3 - 1.39679272585953)**3*cos(q2)**5*cos(q3 - 1.39679272585953) - 206.421465393298*sin(q1)**4*sin(q3 - 1.39679272585953)**3*cos(q2)**4*cos(q3 - 1.39679272585953) + 651.523044839319*sin(q1)**4*sin(q3 - 1.39679272585953)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**3 + 516.938657585961*sin(q1)**4*sin(q3 - 1.39679272585953)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**2 + 54.4275348204984*sin(q1)**4*sin(q3 - 1.39679272585953)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 113.35798396866*sin(q1)**4*sin(q3 - 1.39679272585953)*cos(q2)**5*cos(q3 - 1.39679272585953)**4 - 92.7996308055725*sin(q1)**4*sin(q3 - 1.39679272585953)*cos(q2)**5*cos(q3 - 1.39679272585953)**3 - 6.37822673677715*sin(q1)**4*sin(q3 - 1.39679272585953)*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 6.64206937316367*sin(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**5 + 5.49406549842584*sin(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**4 + 0.280293167143528*sin(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**4 - 13.2841387463273*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**5*cos(q1)**2 - 226.71596793732*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q3 - 1.39679272585953) - 1.93150740179031*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**4*cos(q1)**2 - 1303.04608967864*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q3 - 1.39679272585953)**2 - 43.9525239874065*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q3 - 1.39679272585953) - 2672.48677859415*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q3 - 1.39679272585953)**3 - 375.061538025871*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q3 - 1.39679272585953)**2 - 1289.76195093231*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q3 - 1.39679272585953)**4 - 1422.4556108833*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q3 - 1.39679272585953)**3 - 2445.77081065683*sin(q1)**2*sin(q2)**5*cos(q1)**2*cos(q3 - 1.39679272585953)**5 - 2023.04797992292*sin(q1)**2*sin(q2)**5*cos(q1)**2*cos(q3 - 1.39679272585953)**4 - 226.71596793732*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2) - 2539.67148562564*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953) - 32.9643929905549*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2) + 0.560586334287056*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q1)**2 - 7110.59646403316*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**2 - 556.797784833435*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953) + 12.7564534735543*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q3 - 1.39679272585953) - 1249.90953469333*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 - 3101.63194551577*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**2 + 108.855069640997*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q3 - 1.39679272585953)**2 - 6883.88049609584*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**4 - 5506.55163272995*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 + 412.842930786596*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q3 - 1.39679272585953)**3 + 1289.76195093231*sin(q1)**2*sin(q2)**4*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**5 + 1066.84170816248*sin(q1)**2*sin(q2)**4*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**4 + 587.154390452047*sin(q1)**2*sin(q2)**4*cos(q1)**2*cos(q3 - 1.39679272585953)**4 - 1303.04608967864*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2)**2 - 7110.59646403316*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953) - 189.462276414725*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**2 + 12.7564534735543*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2) - 53.1365549853091*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 2111.70715433125*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953) + 215.467793944845*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953) - 9783.0832426273*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**3 - 5699.87692394826*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**2 + 1200.25943193912*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**2 + 1249.90953469333*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**4 + 645.299019460541*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 2130.9074225262*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 - 2672.48677859415*sin(q1)**2*sin(q2)**3*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**5 - 2210.57874893586*sin(q1)**2*sin(q2)**3*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**4 - 412.842930786596*sin(q1)**2*sin(q2)**3*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**4 - 2672.48677859415*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2)**3 - 1249.90953469333*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953) - 388.578295711381*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**3 + 108.855069640997*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**2 - 9783.0832426273*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**2 - 2392.31499574342*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953) + 1200.25943193912*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953) + 53.1365549853089*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 2067.75463034384*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 3090.86958215402*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 7110.59646403316*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**4 - 5692.1508943411*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 1200.25943193912*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 1303.04608967864*sin(q1)**2*sin(q2)**2*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**5 + 1077.82983915933*sin(q1)**2*sin(q2)**2*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**4 + 108.855069640997*sin(q1)**2*sin(q2)**2*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**4 - 1289.76195093231*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2)**4 - 6883.88049609584*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953) - 187.530769012935*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**4 + 412.842930786596*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**3 + 1249.90953469333*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 2067.75463034384*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953) + 2130.9074225262*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953) - 7110.59646403316*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**3 - 5324.81538592239*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 1200.25943193912*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 2539.67148562564*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**4 + 2067.75463034384*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 215.467793944846*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 226.71596793732*sin(q1)**2*sin(q2)*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**5 - 187.530769012935*sin(q1)**2*sin(q2)*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**4 - 12.7564534735543*sin(q1)**2*sin(q2)*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**4 - 2445.77081065683*sin(q1)**2*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2)**5 + 1289.76195093231*sin(q1)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953) - 355.613902720826*sin(q1)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**5 + 587.154390452047*sin(q1)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**4 - 2672.48677859415*sin(q1)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**2 - 1835.51721090998*sin(q1)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953) - 412.842930786596*sin(q1)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953) + 1303.04608967864*sin(q1)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**3 + 1033.87731517192*sin(q1)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**2 + 108.855069640997*sin(q1)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 226.71596793732*sin(q1)**2*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**4 - 185.599261611145*sin(q1)**2*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**3 - 12.7564534735543*sin(q1)**2*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 13.2841387463273*sin(q1)**2*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**5 + 10.9881309968517*sin(q1)**2*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**4 + 0.560586334287056*sin(q1)**2*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**4 - 6.64206937316367*sin(q2)**5*sin(q3 - 1.39679272585953)**5*cos(q1)**4 - 113.35798396866*sin(q2)**5*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q3 - 1.39679272585953) - 0.965753700895156*sin(q2)**5*sin(q3 - 1.39679272585953)**4*cos(q1)**4 - 651.523044839319*sin(q2)**5*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q3 - 1.39679272585953)**2 - 21.9762619937032*sin(q2)**5*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q3 - 1.39679272585953) - 1336.24338929707*sin(q2)**5*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q3 - 1.39679272585953)**3 - 187.530769012935*sin(q2)**5*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q3 - 1.39679272585953)**2 - 644.880975466155*sin(q2)**5*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q3 - 1.39679272585953)**4 - 711.227805441651*sin(q2)**5*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q3 - 1.39679272585953)**3 - 1222.88540532841*sin(q2)**5*cos(q1)**4*cos(q3 - 1.39679272585953)**5 - 1011.52398996146*sin(q2)**5*cos(q1)**4*cos(q3 - 1.39679272585953)**4 - 113.35798396866*sin(q2)**4*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2) - 1269.83574281282*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953) - 16.4821964952774*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2) + 0.280293167143528*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q1)**4 - 3555.29823201658*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**2 - 278.398892416717*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953) + 6.37822673677715*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q3 - 1.39679272585953) - 624.954767346664*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**3 - 1550.81597275788*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**2 + 54.4275348204984*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q3 - 1.39679272585953)**2 - 3441.94024804792*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**4 - 2753.27581636498*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**3 + 206.421465393298*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q3 - 1.39679272585953)**3 + 644.880975466155*sin(q2)**4*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**5 + 533.420854081238*sin(q2)**4*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**4 + 293.577195226024*sin(q2)**4*cos(q1)**4*cos(q3 - 1.39679272585953)**4 - 651.523044839319*sin(q2)**3*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2)**2 - 3555.29823201658*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953) - 94.7311382073627*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**2 + 6.37822673677715*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2) - 26.5682774926546*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 1055.85357716563*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953) + 107.733896972423*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953) - 4891.54162131365*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**3 - 2849.93846197413*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**2 + 600.129715969562*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**2 + 624.954767346664*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**4 + 322.649509730271*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 1065.4537112631*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**3 - 1336.24338929707*sin(q2)**3*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**5 - 1105.28937446793*sin(q2)**3*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**4 - 206.421465393298*sin(q2)**3*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**4 - 1336.24338929707*sin(q2)**2*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2)**3 - 624.954767346664*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953) - 194.28914785569*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**3 + 54.4275348204984*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**2 - 4891.54162131365*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**2 - 1196.15749787171*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953) + 600.129715969562*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953) + 26.5682774926545*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 1033.87731517192*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 1545.43479107701*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 3555.29823201658*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**4 - 2846.07544717055*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 600.129715969562*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 651.523044839319*sin(q2)**2*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**5 + 538.914919579664*sin(q2)**2*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**4 + 54.4275348204984*sin(q2)**2*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**4 - 644.880975466155*sin(q2)*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2)**4 - 3441.94024804792*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953) - 93.7653845064676*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**4 + 206.421465393298*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**3 + 624.954767346664*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 1033.87731517192*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953) + 1065.4537112631*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953) - 3555.29823201658*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**3 - 2662.40769296119*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 600.129715969562*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 1269.83574281282*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**4 + 1033.87731517192*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 107.733896972423*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 113.35798396866*sin(q2)*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**5 - 93.7653845064677*sin(q2)*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**4 - 6.37822673677715*sin(q2)*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**4 - 1222.88540532841*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2)**5 + 644.880975466155*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953) - 177.806951360413*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**5 + 293.577195226024*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**4 - 1336.24338929707*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**2 - 917.758605454992*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953) - 206.421465393298*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953) + 651.523044839319*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**3 + 516.938657585961*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**2 + 54.4275348204984*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 113.35798396866*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**4 - 92.7996308055725*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**3 - 6.37822673677715*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 6.64206937316367*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**5 + 5.49406549842584*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**4 + 0.280293167143528*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**4)/(-0.00518911669778412*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**5 - 0.0885609249755156*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**4*cos(q3 - 1.39679272585953) - 0.509002378780718*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**3*cos(q3 - 1.39679272585953)**2 - 1.04394014788834*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)**2*cos(q3 - 1.39679272585953)**3 - 0.503813262082934*sin(q1)**4*sin(q2)**5*sin(q3 - 1.39679272585953)*cos(q3 - 1.39679272585953)**4 - 0.955379222912822*sin(q1)**4*sin(q2)**5*cos(q3 - 1.39679272585953)**5 - 0.0885609249755156*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**5*cos(q2) - 0.992059174072515*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q2)*cos(q3 - 1.39679272585953) - 2.77757674376295*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q2)*cos(q3 - 1.39679272585953)**2 - 0.488245911989581*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 - 2.68901581878744*sin(q1)**4*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q2)*cos(q3 - 1.39679272585953)**4 + 0.503813262082934*sin(q1)**4*sin(q2)**4*cos(q2)*cos(q3 - 1.39679272585953)**5 - 0.509002378780718*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**5*cos(q2)**2 - 2.77757674376295*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q2)**2*cos(q3 - 1.39679272585953) - 0.0207564667911362*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 3.82151689165129*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 0.488245911989581*sin(q1)**4*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q2)**2*cos(q3 - 1.39679272585953)**4 - 1.04394014788834*sin(q1)**4*sin(q2)**3*cos(q2)**2*cos(q3 - 1.39679272585953)**5 - 1.04394014788834*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**5*cos(q2)**3 - 0.488245911989581*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q2)**3*cos(q3 - 1.39679272585953) - 3.82151689165129*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.0207564667911364*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 2.77757674376295*sin(q1)**4*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q2)**3*cos(q3 - 1.39679272585953)**4 + 0.509002378780718*sin(q1)**4*sin(q2)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**5 - 0.503813262082934*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**5*cos(q2)**4 - 2.68901581878744*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q2)**4*cos(q3 - 1.39679272585953) + 0.488245911989581*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 2.77757674376295*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 0.992059174072515*sin(q1)**4*sin(q2)*sin(q3 - 1.39679272585953)*cos(q2)**4*cos(q3 - 1.39679272585953)**4 - 0.0885609249755156*sin(q1)**4*sin(q2)*cos(q2)**4*cos(q3 - 1.39679272585953)**5 - 0.955379222912822*sin(q1)**4*sin(q3 - 1.39679272585953)**5*cos(q2)**5 + 0.503813262082934*sin(q1)**4*sin(q3 - 1.39679272585953)**4*cos(q2)**5*cos(q3 - 1.39679272585953) - 1.04394014788834*sin(q1)**4*sin(q3 - 1.39679272585953)**3*cos(q2)**5*cos(q3 - 1.39679272585953)**2 + 0.509002378780718*sin(q1)**4*sin(q3 - 1.39679272585953)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**3 - 0.0885609249755156*sin(q1)**4*sin(q3 - 1.39679272585953)*cos(q2)**5*cos(q3 - 1.39679272585953)**4 + 0.00518911669778412*sin(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**5 - 0.0103782333955682*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**5*cos(q1)**2 - 0.177121849951031*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q3 - 1.39679272585953) - 1.01800475756144*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q3 - 1.39679272585953)**2 - 2.08788029577668*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q3 - 1.39679272585953)**3 - 1.00762652416587*sin(q1)**2*sin(q2)**5*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q3 - 1.39679272585953)**4 - 1.91075844582564*sin(q1)**2*sin(q2)**5*cos(q1)**2*cos(q3 - 1.39679272585953)**5 - 0.177121849951031*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2) - 1.98411834814503*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953) - 5.5551534875259*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**2 - 0.976491823979162*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 - 5.37803163757487*sin(q1)**2*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**4 + 1.00762652416587*sin(q1)**2*sin(q2)**4*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**5 - 1.01800475756144*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2)**2 - 5.5551534875259*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953) - 0.0415129335822729*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 7.64303378330258*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 0.976491823979162*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**4 - 2.08788029577668*sin(q1)**2*sin(q2)**3*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**5 - 2.08788029577668*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2)**3 - 0.976491823979162*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953) - 7.64303378330258*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.0415129335822726*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 5.5551534875259*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**4 + 1.01800475756144*sin(q1)**2*sin(q2)**2*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**5 - 1.00762652416587*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2)**4 - 5.37803163757487*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953) + 0.976491823979162*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 5.5551534875259*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 1.98411834814503*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**4 - 0.177121849951031*sin(q1)**2*sin(q2)*cos(q1)**2*cos(q2)**4*cos(q3 - 1.39679272585953)**5 - 1.91075844582564*sin(q1)**2*sin(q3 - 1.39679272585953)**5*cos(q1)**2*cos(q2)**5 + 1.00762652416587*sin(q1)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953) - 2.08788029577668*sin(q1)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**2 + 1.01800475756144*sin(q1)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**3 - 0.177121849951031*sin(q1)**2*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**4 + 0.0103782333955682*sin(q1)**2*cos(q1)**2*cos(q2)**5*cos(q3 - 1.39679272585953)**5 - 0.00518911669778412*sin(q2)**5*sin(q3 - 1.39679272585953)**5*cos(q1)**4 - 0.0885609249755156*sin(q2)**5*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q3 - 1.39679272585953) - 0.509002378780718*sin(q2)**5*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q3 - 1.39679272585953)**2 - 1.04394014788834*sin(q2)**5*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q3 - 1.39679272585953)**3 - 0.503813262082934*sin(q2)**5*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q3 - 1.39679272585953)**4 - 0.955379222912822*sin(q2)**5*cos(q1)**4*cos(q3 - 1.39679272585953)**5 - 0.0885609249755156*sin(q2)**4*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2) - 0.992059174072515*sin(q2)**4*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953) - 2.77757674376295*sin(q2)**4*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**2 - 0.488245911989581*sin(q2)**4*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**3 - 2.68901581878744*sin(q2)**4*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**4 + 0.503813262082934*sin(q2)**4*cos(q1)**4*cos(q2)*cos(q3 - 1.39679272585953)**5 - 0.509002378780718*sin(q2)**3*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2)**2 - 2.77757674376295*sin(q2)**3*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953) - 0.0207564667911362*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 3.82151689165129*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**3 + 0.488245911989581*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**4 - 1.04394014788834*sin(q2)**3*cos(q1)**4*cos(q2)**2*cos(q3 - 1.39679272585953)**5 - 1.04394014788834*sin(q2)**2*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2)**3 - 0.488245911989581*sin(q2)**2*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953) - 3.82151689165129*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.0207564667911364*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 2.77757674376295*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**4 + 0.509002378780718*sin(q2)**2*cos(q1)**4*cos(q2)**3*cos(q3 - 1.39679272585953)**5 - 0.503813262082934*sin(q2)*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2)**4 - 2.68901581878744*sin(q2)*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953) + 0.488245911989581*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**2 - 2.77757674376295*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**3 + 0.992059174072515*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**4 - 0.0885609249755156*sin(q2)*cos(q1)**4*cos(q2)**4*cos(q3 - 1.39679272585953)**5 - 0.955379222912822*sin(q3 - 1.39679272585953)**5*cos(q1)**4*cos(q2)**5 + 0.503813262082934*sin(q3 - 1.39679272585953)**4*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953) - 1.04394014788834*sin(q3 - 1.39679272585953)**3*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**2 + 0.509002378780718*sin(q3 - 1.39679272585953)**2*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**3 - 0.0885609249755156*sin(q3 - 1.39679272585953)*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**4 + 0.00518911669778412*cos(q1)**4*cos(q2)**5*cos(q3 - 1.39679272585953)**5)], [0.173126870725429*sin(q1)/(0.173126870725429*sin(q1)**2 + 0.173126870725429*cos(q1)**2), 0.173126870725429*cos(q1)/(0.173126870725429*sin(q1)**2 + 0.173126870725429*cos(q1)**2), 0, 0], [-1.0*(0.170512573822175*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1) + 0.940054173265642*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)*cos(q3 - 1.39679272585953) - 0.170512573822175*sin(q2)**2*cos(q1)*cos(q3 - 1.39679272585953)**2 + 0.940054173265642*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)*cos(q2) - 0.682050295288702*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)*cos(q2)*cos(q3 - 1.39679272585953) - 0.940054173265642*sin(q2)*cos(q1)*cos(q2)*cos(q3 - 1.39679272585953)**2 - 0.170512573822175*sin(q3 - 1.39679272585953)**2*cos(q1)*cos(q2)**2 - 0.940054173265642*sin(q3 - 1.39679272585953)*cos(q1)*cos(q2)**2*cos(q3 - 1.39679272585953) + 0.170512573822175*cos(q1)*cos(q2)**2*cos(q3 - 1.39679272585953)**2)/(-0.173126870725429*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**3 - 0.984899531237994*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q3 - 1.39679272585953) - 0.173126870725429*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q3 - 1.39679272585953)**2 - 0.984899531237994*sin(q1)**2*sin(q2)**3*cos(q3 - 1.39679272585953)**3 - 0.984899531237994*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q2) + 0.173126870725429*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q2)*cos(q3 - 1.39679272585953) - 0.984899531237994*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q2)*cos(q3 - 1.39679272585953)**2 + 0.173126870725429*sin(q1)**2*sin(q2)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 - 0.173126870725429*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q2)**2 - 0.984899531237994*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q2)**2*cos(q3 - 1.39679272585953) - 0.173126870725429*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 0.984899531237994*sin(q1)**2*sin(q2)*cos(q2)**2*cos(q3 - 1.39679272585953)**3 - 0.984899531237994*sin(q1)**2*sin(q3 - 1.39679272585953)**3*cos(q2)**3 + 0.173126870725429*sin(q1)**2*sin(q3 - 1.39679272585953)**2*cos(q2)**3*cos(q3 - 1.39679272585953) - 0.984899531237994*sin(q1)**2*sin(q3 - 1.39679272585953)*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.173126870725429*sin(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 0.173126870725429*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)**2 - 0.984899531237994*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q3 - 1.39679272585953) - 0.173126870725429*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q3 - 1.39679272585953)**2 - 0.984899531237994*sin(q2)**3*cos(q1)**2*cos(q3 - 1.39679272585953)**3 - 0.984899531237994*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2) + 0.173126870725429*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953) - 0.984899531237994*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**2 + 0.173126870725429*sin(q2)**2*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 - 0.173126870725429*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**2 - 0.984899531237994*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953) - 0.173126870725429*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 0.984899531237994*sin(q2)*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**3 - 0.984899531237994*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**3 + 0.173126870725429*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953) - 0.984899531237994*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.173126870725429*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**3), 1.0*(0.170512573822175*sin(q1)*sin(q2)**2*sin(q3 - 1.39679272585953)**2 + 0.940054173265642*sin(q1)*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q3 - 1.39679272585953) - 0.170512573822175*sin(q1)*sin(q2)**2*cos(q3 - 1.39679272585953)**2 + 0.940054173265642*sin(q1)*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q2) - 0.682050295288702*sin(q1)*sin(q2)*sin(q3 - 1.39679272585953)*cos(q2)*cos(q3 - 1.39679272585953) - 0.940054173265642*sin(q1)*sin(q2)*cos(q2)*cos(q3 - 1.39679272585953)**2 - 0.170512573822175*sin(q1)*sin(q3 - 1.39679272585953)**2*cos(q2)**2 - 0.940054173265642*sin(q1)*sin(q3 - 1.39679272585953)*cos(q2)**2*cos(q3 - 1.39679272585953) + 0.170512573822175*sin(q1)*cos(q2)**2*cos(q3 - 1.39679272585953)**2)/(-0.173126870725429*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**3 - 0.984899531237994*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q3 - 1.39679272585953) - 0.173126870725429*sin(q1)**2*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q3 - 1.39679272585953)**2 - 0.984899531237994*sin(q1)**2*sin(q2)**3*cos(q3 - 1.39679272585953)**3 - 0.984899531237994*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q2) + 0.173126870725429*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q2)*cos(q3 - 1.39679272585953) - 0.984899531237994*sin(q1)**2*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q2)*cos(q3 - 1.39679272585953)**2 + 0.173126870725429*sin(q1)**2*sin(q2)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 - 0.173126870725429*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q2)**2 - 0.984899531237994*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q2)**2*cos(q3 - 1.39679272585953) - 0.173126870725429*sin(q1)**2*sin(q2)*sin(q3 - 1.39679272585953)*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 0.984899531237994*sin(q1)**2*sin(q2)*cos(q2)**2*cos(q3 - 1.39679272585953)**3 - 0.984899531237994*sin(q1)**2*sin(q3 - 1.39679272585953)**3*cos(q2)**3 + 0.173126870725429*sin(q1)**2*sin(q3 - 1.39679272585953)**2*cos(q2)**3*cos(q3 - 1.39679272585953) - 0.984899531237994*sin(q1)**2*sin(q3 - 1.39679272585953)*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.173126870725429*sin(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**3 - 0.173126870725429*sin(q2)**3*sin(q3 - 1.39679272585953)**3*cos(q1)**2 - 0.984899531237994*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q3 - 1.39679272585953) - 0.173126870725429*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q3 - 1.39679272585953)**2 - 0.984899531237994*sin(q2)**3*cos(q1)**2*cos(q3 - 1.39679272585953)**3 - 0.984899531237994*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2) + 0.173126870725429*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953) - 0.984899531237994*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**2 + 0.173126870725429*sin(q2)**2*cos(q1)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 - 0.173126870725429*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**2 - 0.984899531237994*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953) - 0.173126870725429*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 0.984899531237994*sin(q2)*cos(q1)**2*cos(q2)**2*cos(q3 - 1.39679272585953)**3 - 0.984899531237994*sin(q3 - 1.39679272585953)**3*cos(q1)**2*cos(q2)**3 + 0.173126870725429*sin(q3 - 1.39679272585953)**2*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953) - 0.984899531237994*sin(q3 - 1.39679272585953)*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.173126870725429*cos(q1)**2*cos(q2)**3*cos(q3 - 1.39679272585953)**3), 1.0*(0.0299729133671793*sin(q2)**2*sin(q3 - 1.39679272585953)**2 + 0.341025147644351*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q3 - 1.39679272585953) + 0.970027086632821*sin(q2)**2*cos(q3 - 1.39679272585953)**2 + 0.341025147644351*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q2) + 1.88010834653128*sin(q2)*sin(q3 - 1.39679272585953)*cos(q2)*cos(q3 - 1.39679272585953) - 0.341025147644351*sin(q2)*cos(q2)*cos(q3 - 1.39679272585953)**2 + 0.970027086632821*sin(q3 - 1.39679272585953)**2*cos(q2)**2 - 0.341025147644351*sin(q3 - 1.39679272585953)*cos(q2)**2*cos(q3 - 1.39679272585953) + 0.0299729133671793*cos(q2)**2*cos(q3 - 1.39679272585953)**2)/(-0.173126870725429*sin(q2)**3*sin(q3 - 1.39679272585953)**3 - 0.984899531237994*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q3 - 1.39679272585953) - 0.173126870725429*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q3 - 1.39679272585953)**2 - 0.984899531237994*sin(q2)**3*cos(q3 - 1.39679272585953)**3 - 0.984899531237994*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q2) + 0.173126870725429*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q2)*cos(q3 - 1.39679272585953) - 0.984899531237994*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q2)*cos(q3 - 1.39679272585953)**2 + 0.173126870725429*sin(q2)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 - 0.173126870725429*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q2)**2 - 0.984899531237994*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q2)**2*cos(q3 - 1.39679272585953) - 0.173126870725429*sin(q2)*sin(q3 - 1.39679272585953)*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 0.984899531237994*sin(q2)*cos(q2)**2*cos(q3 - 1.39679272585953)**3 - 0.984899531237994*sin(q3 - 1.39679272585953)**3*cos(q2)**3 + 0.173126870725429*sin(q3 - 1.39679272585953)**2*cos(q2)**3*cos(q3 - 1.39679272585953) - 0.984899531237994*sin(q3 - 1.39679272585953)*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.173126870725429*cos(q2)**3*cos(q3 - 1.39679272585953)**3), -1.0*(-38.9535459132214*sin(q2)**3*sin(q3 - 1.39679272585953)**3 - 221.602394528549*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q3 - 1.39679272585953) + 183.301016858839*sin(q2)**3*sin(q3 - 1.39679272585953)**2 - 38.9535459132214*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q3 - 1.39679272585953)**2 + 1010.55823626056*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q3 - 1.39679272585953) - 221.602394528549*sin(q2)**3*cos(q3 - 1.39679272585953)**3 - 183.301016858839*sin(q2)**3*cos(q3 - 1.39679272585953)**2 - 221.602394528549*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q2) + 38.9535459132214*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q2)*cos(q3 - 1.39679272585953) + 1042.77911813028*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q2) - 53.1999230325188*sin(q2)**2*sin(q3 - 1.39679272585953)**2 - 221.602394528549*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q2)*cos(q3 - 1.39679272585953)**2 - 366.602033717677*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q2)*cos(q3 - 1.39679272585953) - 293.29690205888*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q3 - 1.39679272585953) + 38.9535459132214*sin(q2)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 + 32.2208818697177*sin(q2)**2*cos(q2)*cos(q3 - 1.39679272585953)**2 + 53.1999230325188*sin(q2)**2*cos(q3 - 1.39679272585953)**2 - 38.9535459132214*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q2)**2 - 221.602394528549*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q2)**2*cos(q3 - 1.39679272585953) + 183.301016858839*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q2)**2 - 293.29690205888*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q2) - 38.9535459132214*sin(q2)*sin(q3 - 1.39679272585953)*cos(q2)**2*cos(q3 - 1.39679272585953)**2 + 1010.55823626056*sin(q2)*sin(q3 - 1.39679272585953)*cos(q2)**2*cos(q3 - 1.39679272585953) + 212.799692130075*sin(q2)*sin(q3 - 1.39679272585953)*cos(q2)*cos(q3 - 1.39679272585953) - 221.602394528549*sin(q2)*cos(q2)**2*cos(q3 - 1.39679272585953)**3 - 183.301016858839*sin(q2)*cos(q2)**2*cos(q3 - 1.39679272585953)**2 + 293.29690205888*sin(q2)*cos(q2)*cos(q3 - 1.39679272585953)**2 - 221.602394528549*sin(q3 - 1.39679272585953)**3*cos(q2)**3 + 38.9535459132214*sin(q3 - 1.39679272585953)**2*cos(q2)**3*cos(q3 - 1.39679272585953) + 1042.77911813028*sin(q3 - 1.39679272585953)**2*cos(q2)**3 + 53.1999230325188*sin(q3 - 1.39679272585953)**2*cos(q2)**2 - 221.602394528549*sin(q3 - 1.39679272585953)*cos(q2)**3*cos(q3 - 1.39679272585953)**2 - 366.602033717677*sin(q3 - 1.39679272585953)*cos(q2)**3*cos(q3 - 1.39679272585953) + 293.29690205888*sin(q3 - 1.39679272585953)*cos(q2)**2*cos(q3 - 1.39679272585953) + 38.9535459132214*cos(q2)**3*cos(q3 - 1.39679272585953)**3 + 32.2208818697177*cos(q2)**3*cos(q3 - 1.39679272585953)**2 - 53.1999230325188*cos(q2)**2*cos(q3 - 1.39679272585953)**2)/(-0.173126870725429*sin(q2)**3*sin(q3 - 1.39679272585953)**3 - 0.984899531237994*sin(q2)**3*sin(q3 - 1.39679272585953)**2*cos(q3 - 1.39679272585953) - 0.173126870725429*sin(q2)**3*sin(q3 - 1.39679272585953)*cos(q3 - 1.39679272585953)**2 - 0.984899531237994*sin(q2)**3*cos(q3 - 1.39679272585953)**3 - 0.984899531237994*sin(q2)**2*sin(q3 - 1.39679272585953)**3*cos(q2) + 0.173126870725429*sin(q2)**2*sin(q3 - 1.39679272585953)**2*cos(q2)*cos(q3 - 1.39679272585953) - 0.984899531237994*sin(q2)**2*sin(q3 - 1.39679272585953)*cos(q2)*cos(q3 - 1.39679272585953)**2 + 0.173126870725429*sin(q2)**2*cos(q2)*cos(q3 - 1.39679272585953)**3 - 0.173126870725429*sin(q2)*sin(q3 - 1.39679272585953)**3*cos(q2)**2 - 0.984899531237994*sin(q2)*sin(q3 - 1.39679272585953)**2*cos(q2)**2*cos(q3 - 1.39679272585953) - 0.173126870725429*sin(q2)*sin(q3 - 1.39679272585953)*cos(q2)**2*cos(q3 - 1.39679272585953)**2 - 0.984899531237994*sin(q2)*cos(q2)**2*cos(q3 - 1.39679272585953)**3 - 0.984899531237994*sin(q3 - 1.39679272585953)**3*cos(q2)**3 + 0.173126870725429*sin(q3 - 1.39679272585953)**2*cos(q2)**3*cos(q3 - 1.39679272585953) - 0.984899531237994*sin(q3 - 1.39679272585953)*cos(q2)**3*cos(q3 - 1.39679272585953)**2 + 0.173126870725429*cos(q2)**3*cos(q3 - 1.39679272585953)**3)], [0, 0, 0, 1]]))
def T06_l(q1, q2, q3, q4, q5, q6):
return (array([[(-0.173126870725429*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1) - 0.984899531237994*sin(q2)*cos(q1)*cos(q3 - 1.39679272585953) - 0.984899531237994*sin(q3 - 1.39679272585953)*cos(q1)*cos(q2) + 0.173126870725429*cos(q1)*cos(q2)*cos(q3 - 1.39679272585953))*cos(q5) + (0.984899531237994*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1) - 0.173126870725429*sin(q2)*cos(q1)*cos(q3 - 1.39679272585953) - 0.173126870725429*sin(q3 - 1.39679272585953)*cos(q1)*cos(q2) - 0.984899531237994*cos(q1)*cos(q2)*cos(q3 - 1.39679272585953))*sin(q5)*cos(q4) + sin(q1)*sin(q4)*sin(q5), (-sin(q4)*cos(q6) - sin(q6)*cos(q4)*cos(q5))*(0.984899531237994*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1) - 0.173126870725429*sin(q2)*cos(q1)*cos(q3 - 1.39679272585953) - 0.173126870725429*sin(q3 - 1.39679272585953)*cos(q1)*cos(q2) - 0.984899531237994*cos(q1)*cos(q2)*cos(q3 - 1.39679272585953)) + (-sin(q4)*sin(q6)*cos(q5) + cos(q4)*cos(q6))*sin(q1) + (-0.173126870725429*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1) - 0.984899531237994*sin(q2)*cos(q1)*cos(q3 - 1.39679272585953) - 0.984899531237994*sin(q3 - 1.39679272585953)*cos(q1)*cos(q2) + 0.173126870725429*cos(q1)*cos(q2)*cos(q3 - 1.39679272585953))*sin(q5)*sin(q6), (-sin(q4)*sin(q6) + cos(q4)*cos(q5)*cos(q6))*(0.984899531237994*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1) - 0.173126870725429*sin(q2)*cos(q1)*cos(q3 - 1.39679272585953) - 0.173126870725429*sin(q3 - 1.39679272585953)*cos(q1)*cos(q2) - 0.984899531237994*cos(q1)*cos(q2)*cos(q3 - 1.39679272585953)) + (sin(q4)*cos(q5)*cos(q6) + sin(q6)*cos(q4))*sin(q1) - (-0.173126870725429*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1) - 0.984899531237994*sin(q2)*cos(q1)*cos(q3 - 1.39679272585953) - 0.984899531237994*sin(q3 - 1.39679272585953)*cos(q1)*cos(q2) + 0.173126870725429*cos(q1)*cos(q2)*cos(q3 - 1.39679272585953))*sin(q5)*cos(q6), 215*(-0.173126870725429*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1) - 0.984899531237994*sin(q2)*cos(q1)*cos(q3 - 1.39679272585953) - 0.984899531237994*sin(q3 - 1.39679272585953)*cos(q1)*cos(q2) + 0.173126870725429*cos(q1)*cos(q2)*cos(q3 - 1.39679272585953))*cos(q5) + 215*(0.984899531237994*sin(q2)*sin(q3 - 1.39679272585953)*cos(q1) - 0.173126870725429*sin(q2)*cos(q1)*cos(q3 - 1.39679272585953) - 0.173126870725429*sin(q3 - 1.39679272585953)*cos(q1)*cos(q2) - 0.984899531237994*cos(q1)*cos(q2)*cos(q3 - 1.39679272585953))*sin(q5)*cos(q4) + 215*sin(q1)*sin(q4)*sin(q5) - 1299.62494589785*sin(q2)*cos(q1)*cos(q3 - 1.39679272585953) - 1075*sin(q2)*cos(q1) - 1299.62494589785*sin(q3 - 1.39679272585953)*cos(q1)*cos(q2) + 312*cos(q1)], [(-0.984899531237994*sin(q1)*sin(q2)*sin(q3 - 1.39679272585953) + 0.173126870725429*sin(q1)*sin(q2)*cos(q3 - 1.39679272585953) + 0.173126870725429*sin(q1)*sin(q3 - 1.39679272585953)*cos(q2) + 0.984899531237994*sin(q1)*cos(q2)*cos(q3 - 1.39679272585953))*sin(q5)*cos(q4) + (0.173126870725429*sin(q1)*sin(q2)*sin(q3 - 1.39679272585953) + 0.984899531237994*sin(q1)*sin(q2)*cos(q3 - 1.39679272585953) + 0.984899531237994*sin(q1)*sin(q3 - 1.39679272585953)*cos(q2) - 0.173126870725429*sin(q1)*cos(q2)*cos(q3 - 1.39679272585953))*cos(q5) + sin(q4)*sin(q5)*cos(q1), (-sin(q4)*cos(q6) - sin(q6)*cos(q4)*cos(q5))*(-0.984899531237994*sin(q1)*sin(q2)*sin(q3 - 1.39679272585953) + 0.173126870725429*sin(q1)*sin(q2)*cos(q3 - 1.39679272585953) + 0.173126870725429*sin(q1)*sin(q3 - 1.39679272585953)*cos(q2) + 0.984899531237994*sin(q1)*cos(q2)*cos(q3 - 1.39679272585953)) + (-sin(q4)*sin(q6)*cos(q5) + cos(q4)*cos(q6))*cos(q1) + (0.173126870725429*sin(q1)*sin(q2)*sin(q3 - 1.39679272585953) + 0.984899531237994*sin(q1)*sin(q2)*cos(q3 - 1.39679272585953) + 0.984899531237994*sin(q1)*sin(q3 - 1.39679272585953)*cos(q2) - 0.173126870725429*sin(q1)*cos(q2)*cos(q3 - 1.39679272585953))*sin(q5)*sin(q6), (-sin(q4)*sin(q6) + cos(q4)*cos(q5)*cos(q6))*(-0.984899531237994*sin(q1)*sin(q2)*sin(q3 - 1.39679272585953) + 0.173126870725429*sin(q1)*sin(q2)*cos(q3 - 1.39679272585953) + 0.173126870725429*sin(q1)*sin(q3 - 1.39679272585953)*cos(q2) + 0.984899531237994*sin(q1)*cos(q2)*cos(q3 - 1.39679272585953)) + (sin(q4)*cos(q5)*cos(q6) + sin(q6)*cos(q4))*cos(q1) - (0.173126870725429*sin(q1)*sin(q2)*sin(q3 - 1.39679272585953) + 0.984899531237994*sin(q1)*sin(q2)*cos(q3 - 1.39679272585953) + 0.984899531237994*sin(q1)*sin(q3 - 1.39679272585953)*cos(q2) - 0.173126870725429*sin(q1)*cos(q2)*cos(q3 - 1.39679272585953))*sin(q5)*cos(q6), 215*(-0.984899531237994*sin(q1)*sin(q2)*sin(q3 - 1.39679272585953) + 0.173126870725429*sin(q1)*sin(q2)*cos(q3 - 1.39679272585953) + 0.173126870725429*sin(q1)*sin(q3 - 1.39679272585953)*cos(q2) + 0.984899531237994*sin(q1)*cos(q2)*cos(q3 - 1.39679272585953))*sin(q5)*cos(q4) + 215*(0.173126870725429*sin(q1)*sin(q2)*sin(q3 - 1.39679272585953) + 0.984899531237994*sin(q1)*sin(q2)*cos(q3 - 1.39679272585953) + 0.984899531237994*sin(q1)*sin(q3 - 1.39679272585953)*cos(q2) - 0.173126870725429*sin(q1)*cos(q2)*cos(q3 - 1.39679272585953))*cos(q5) + 1299.62494589785*sin(q1)*sin(q2)*cos(q3 - 1.39679272585953) + 1075*sin(q1)*sin(q2) + 1299.62494589785*sin(q1)*sin(q3 - 1.39679272585953)*cos(q2) - 312*sin(q1) + 215*sin(q4)*sin(q5)*cos(q1)], [(-0.984899531237994*sin(q2)*sin(q3 - 1.39679272585953) + 0.173126870725429*sin(q2)*cos(q3 - 1.39679272585953) + 0.173126870725429*sin(q3 - 1.39679272585953)*cos(q2) + 0.984899531237994*cos(q2)*cos(q3 - 1.39679272585953))*cos(q5) + (-0.173126870725429*sin(q2)*sin(q3 - 1.39679272585953) - 0.984899531237994*sin(q2)*cos(q3 - 1.39679272585953) - 0.984899531237994*sin(q3 - 1.39679272585953)*cos(q2) + 0.173126870725429*cos(q2)*cos(q3 - 1.39679272585953))*sin(q5)*cos(q4), (-sin(q4)*cos(q6) - sin(q6)*cos(q4)*cos(q5))*(-0.173126870725429*sin(q2)*sin(q3 - 1.39679272585953) - 0.984899531237994*sin(q2)*cos(q3 - 1.39679272585953) - 0.984899531237994*sin(q3 - 1.39679272585953)*cos(q2) + 0.173126870725429*cos(q2)*cos(q3 - 1.39679272585953)) + (-0.984899531237994*sin(q2)*sin(q3 - 1.39679272585953) + 0.173126870725429*sin(q2)*cos(q3 - 1.39679272585953) + 0.173126870725429*sin(q3 - 1.39679272585953)*cos(q2) + 0.984899531237994*cos(q2)*cos(q3 - 1.39679272585953))*sin(q5)*sin(q6), (-sin(q4)*sin(q6) + cos(q4)*cos(q5)*cos(q6))*(-0.173126870725429*sin(q2)*sin(q3 - 1.39679272585953) - 0.984899531237994*sin(q2)*cos(q3 - 1.39679272585953) - 0.984899531237994*sin(q3 - 1.39679272585953)*cos(q2) + 0.173126870725429*cos(q2)*cos(q3 - 1.39679272585953)) - (-0.984899531237994*sin(q2)*sin(q3 - 1.39679272585953) + 0.173126870725429*sin(q2)*cos(q3 - 1.39679272585953) + 0.173126870725429*sin(q3 - 1.39679272585953)*cos(q2) + 0.984899531237994*cos(q2)*cos(q3 - 1.39679272585953))*sin(q5)*cos(q6), 215*(-0.984899531237994*sin(q2)*sin(q3 - 1.39679272585953) + 0.173126870725429*sin(q2)*cos(q3 - 1.39679272585953) + 0.173126870725429*sin(q3 - 1.39679272585953)*cos(q2) + 0.984899531237994*cos(q2)*cos(q3 - 1.39679272585953))*cos(q5) + 215*(-0.173126870725429*sin(q2)*sin(q3 - 1.39679272585953) - 0.984899531237994*sin(q2)*cos(q3 - 1.39679272585953) - 0.984899531237994*sin(q3 - 1.39679272585953)*cos(q2) + 0.173126870725429*cos(q2)*cos(q3 - 1.39679272585953))*sin(q5)*cos(q4) - 1299.62494589785*sin(q2)*sin(q3 - 1.39679272585953) + 1299.62494589785*cos(q2)*cos(q3 - 1.39679272585953) + 1075*cos(q2) + 670], [0, 0, 0, 1]]))
def IK(*args):
r_biba = l_biba(*args)
x_, y_, z_ = r_biba[:-1, -1]
x_temp = x_
y_temp = y_
x_planar = (x_temp**2 + y_temp**2)**0.5 - 312
y_planar = z_
L1 = 1075
L2 = (1280**2 + 225**2)**0.5
q3 = np.arccos((x_planar**2 + y_planar**2 - L1**2 - L2**2)/(2*L1*L2))
if q3 > 0:
m = -1
else:
m = 1
q2 = -m*np.arctan((L2*np.cos(q3))/(L1+L2*np.sin(q3))) + np.arctan(y_planar/x_planar)
if z_ == 0:
q1 = 0 # any
else:
q1 = np.arctan2(y_*np.sign(1075*np.sin(q2)+225*np.sin(q2+q3)-1280*np.cos(q2+q3)-312), x_*np.sign(1075*np.sin(q2) - 225*np.sin(q2 + q3) + 1280*np.cos(q2 + q3) + 312))
# q3 += np.arctan2(1280, 225)
res_mat = np.matrix(t_inv(q1, q2, q3))*np.matrix(r_biba)
if res_mat[0, 0] == 1:
q5 = 0
q4 = np.arctan(res_mat[1, 2], res_mat[1, 1])/2
q6 = q4
else:
q4 = np.arctan2(res_mat[1, 0], res_mat[2, 0])
q6 = np.arctan2(res_mat[0, 1], -res_mat[0, 2])
if res_mat[2, 0] != 0:
q5 = np.arctan2(-res_mat[2, 0]/np.cos(q6), res_mat[0, 0])
else:
q5 = np.arctan2(res_mat[1, 0]/np.sin(q6), res_mat[0, 0])
return q1, q2, q3, q4, q5, q6
sys_args = [float(arg) for arg in sys.args]
print(f'q1, q2, q3, q4, q5, q6: {FK(sys_args)}')