-
Notifications
You must be signed in to change notification settings - Fork 0
/
hfun.m
111 lines (104 loc) · 45 KB
/
hfun.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
function [h]= hfun(NumericParameters, qnum)
DX = NumericParameters(1);
DY = NumericParameters(2);
DZ = NumericParameters(3);
DX1 = NumericParameters(4);
DY1 = NumericParameters(5);
DZ1 = NumericParameters(6);
DX2 = NumericParameters(7);
DY2 = NumericParameters(8);
DZ2 = NumericParameters(9);
DX3 = NumericParameters(10);
DY3 = NumericParameters(11);
DZ3 = NumericParameters(12);
L1 = NumericParameters(13);
L2 = NumericParameters(14);
Suppx = NumericParameters(15);
Suppy = NumericParameters(16);
ParX = NumericParameters(17);
ParZ = NumericParameters(18);
q1 = qnum(1);
q2 = qnum(2);
q3 = qnum(3);
q4 = qnum(4);
q5 = qnum(5);
q6 = qnum(6);
q7 = qnum(7);
%remember SymbolicParameters = [DX DY DZ DX1 DY1 DZ1 DX2 DY2 DZ2 DX3 DY3 DZ3 L1 L2 Suppx Suppy ParX ParZ];
%hn = hfun(NumericParameters, qnum);
% Depending variables :
% DX
% DX1
% DX2
% DX3
% DY
% DY1
% DY2
% DY3
% DZ
% DZ1
% DZ2
% DZ3
% L1
% L2
% ParX
% ParZ
% Suppx
% Suppy
% q1
% q2
% q3
% q4
% q5
% q6
% q7
h = [
(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))*(DX + DX1 + ParX) - sin(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + (cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(DZ + DZ1 - ParZ) + DY*sin(q1) - DX*(cos(q1) - 1) - cos(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - cos(q2)*sin(q1)*(DY + DY1) + sin(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))*(DX + DX1 + ParX) - sin(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) + (sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2))*(DZ + DZ1 - ParZ) - DX*sin(q1) - DY*(cos(q1) - 1) + cos(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) - cos(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) + cos(q1)*cos(q2)*(DY + DY1);
sin(q2)*(DY + DY1) - DY*sin(q2) - DZ*(cos(q2) - 1) + cos(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) - cos(q2)*sin(q3)*(DX + DX1 + ParX) + cos(q2)*cos(q3)*(DZ + DZ1 - ParZ);
(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))*(DX + DX1 - ParX) - sin(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + (cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(DZ + DZ1 - ParZ) + DY*sin(q1) - DX*(cos(q1) - 1) - cos(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - cos(q2)*sin(q1)*(DY + DY1) + sin(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))*(DX + DX1 - ParX) - sin(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) + (sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2))*(DZ + DZ1 - ParZ) - DX*sin(q1) - DY*(cos(q1) - 1) + cos(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) - cos(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) + cos(q1)*cos(q2)*(DY + DY1);
sin(q2)*(DY + DY1) - DY*sin(q2) - DZ*(cos(q2) - 1) + cos(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) + cos(q2)*cos(q3)*(DZ + DZ1 - ParZ) - cos(q2)*sin(q3)*(DX + DX1 - ParX);
(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))*(DX + DX1 + ParX) - sin(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + DY*sin(q1) + (cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(DZ + DZ1) - DX*(cos(q1) - 1) - cos(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - cos(q2)*sin(q1)*(DY + DY1) + sin(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))*(DX + DX1 + ParX) - sin(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - DX*sin(q1) + (sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2))*(DZ + DZ1) - DY*(cos(q1) - 1) + cos(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) - cos(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) + cos(q1)*cos(q2)*(DY + DY1);
sin(q2)*(DY + DY1) - DY*sin(q2) - DZ*(cos(q2) - 1) + cos(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) - cos(q2)*sin(q3)*(DX + DX1 + ParX) + cos(q2)*cos(q3)*(DZ + DZ1);
(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))*(DX + DX1 - ParX) - sin(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + DY*sin(q1) + (cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(DZ + DZ1) - DX*(cos(q1) - 1) - cos(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - cos(q2)*sin(q1)*(DY + DY1) + sin(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))*(DX + DX1 - ParX) - sin(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - DX*sin(q1) + (sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2))*(DZ + DZ1) - DY*(cos(q1) - 1) + cos(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) - cos(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) + cos(q1)*cos(q2)*(DY + DY1);
sin(q2)*(DY + DY1) - DY*sin(q2) - DZ*(cos(q2) - 1) + cos(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) - cos(q2)*sin(q3)*(DX + DX1 - ParX) + cos(q2)*cos(q3)*(DZ + DZ1);
(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))*(DX + DX1 + ParX) - sin(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + (cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(DZ + DZ1 + ParZ) + DY*sin(q1) - DX*(cos(q1) - 1) - cos(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - cos(q2)*sin(q1)*(DY + DY1) + sin(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))*(DX + DX1 + ParX) - sin(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) + (sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2))*(DZ + DZ1 + ParZ) - DX*sin(q1) - DY*(cos(q1) - 1) + cos(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) - cos(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) + cos(q1)*cos(q2)*(DY + DY1);
sin(q2)*(DY + DY1) - DY*sin(q2) - DZ*(cos(q2) - 1) + cos(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) + cos(q2)*cos(q3)*(DZ + DZ1 + ParZ) - cos(q2)*sin(q3)*(DX + DX1 + ParX);
(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(DZ + DZ1 + ParZ) - sin(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + (cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))*(DX + DX1 - ParX) + DY*sin(q1) - DX*(cos(q1) - 1) - cos(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - cos(q2)*sin(q1)*(DY + DY1) + sin(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2))*(DZ + DZ1 + ParZ) - sin(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) + (cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))*(DX + DX1 - ParX) - DX*sin(q1) - DY*(cos(q1) - 1) + cos(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) - cos(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) + cos(q1)*cos(q2)*(DY + DY1);
sin(q2)*(DY + DY1) - DY*sin(q2) - DZ*(cos(q2) - 1) + cos(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) + cos(q2)*cos(q3)*(DZ + DZ1 + ParZ) - cos(q2)*sin(q3)*(DX + DX1 - ParX);
(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4))*(DZ + DZ2 + L1 - ParZ) - (sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1))*(DX*sin(q5) + DY*(cos(q5) - 1)) - ((cos(q4) - 1)*(DZ + L1) + DY*sin(q4))*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - sin(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + DY*sin(q1) + (cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))*(DY*sin(q5) - DX*(cos(q5) - 1)) + (cos(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) - sin(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))*(DY + DY2) - DX*(cos(q1) - 1) + (sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))*(DX + DX2 + ParX) - cos(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - cos(q2)*sin(q1)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) + sin(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4))*(DZ + DZ2 + L1 - ParZ) - (DX*sin(q5) + DY*(cos(q5) - 1))*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - ((cos(q4) - 1)*(DZ + L1) + DY*sin(q4))*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + (cos(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))*(DY + DY2) - DX*sin(q1) + (cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))*(DY*sin(q5) - DX*(cos(q5) - 1)) - DY*(cos(q1) - 1) + (sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))*(DX + DX2 + ParX) + cos(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + cos(q1)*cos(q2)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) - cos(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(DY + DY2)*(cos(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) + cos(q2)*sin(q3)*sin(q5)) - (cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4))*(DX*sin(q5) + DY*(cos(q5) - 1)) - DY*sin(q2) + (sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3))*(DX + DX2 + ParX) - (sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4))*(DZ + DZ2 + L1 - ParZ) + sin(q2)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) - DZ*(cos(q2) - 1) + cos(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) - cos(q2)*cos(q3)*((cos(q4) - 1)*(DZ + L1) + DY*sin(q4)) - cos(q2)*sin(q3)*(DY*sin(q5) - DX*(cos(q5) - 1));
(sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))*(DX + DX2 - ParX) - (sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1))*(DX*sin(q5) + DY*(cos(q5) - 1)) - ((cos(q4) - 1)*(DZ + L1) + DY*sin(q4))*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - sin(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + (cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4))*(DZ + DZ2 + L1 - ParZ) + DY*sin(q1) + (cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))*(DY*sin(q5) - DX*(cos(q5) - 1)) + (cos(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) - sin(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))*(DY + DY2) - DX*(cos(q1) - 1) - cos(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - cos(q2)*sin(q1)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) + sin(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))*(DX + DX2 - ParX) - (DX*sin(q5) + DY*(cos(q5) - 1))*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) + (cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4))*(DZ + DZ2 + L1 - ParZ) - ((cos(q4) - 1)*(DZ + L1) + DY*sin(q4))*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + (cos(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))*(DY + DY2) - DX*sin(q1) + (cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))*(DY*sin(q5) - DX*(cos(q5) - 1)) - DY*(cos(q1) - 1) + cos(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + cos(q1)*cos(q2)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) - cos(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(DY + DY2)*(cos(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) + cos(q2)*sin(q3)*sin(q5)) - (cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4))*(DX*sin(q5) + DY*(cos(q5) - 1)) - DY*sin(q2) - (sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4))*(DZ + DZ2 + L1 - ParZ) + sin(q2)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) - DZ*(cos(q2) - 1) + (sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3))*(DX + DX2 - ParX) + cos(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) - cos(q2)*cos(q3)*((cos(q4) - 1)*(DZ + L1) + DY*sin(q4)) - cos(q2)*sin(q3)*(DY*sin(q5) - DX*(cos(q5) - 1));
DY*sin(q1) - (sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1))*(DX*sin(q5) + DY*(cos(q5) - 1)) - ((cos(q4) - 1)*(DZ + L1) + DY*sin(q4))*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - sin(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + (cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))*(DY*sin(q5) - DX*(cos(q5) - 1)) + (cos(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) - sin(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))*(DY + DY2) + (cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4))*(DZ + DZ2 + L1) - DX*(cos(q1) - 1) + (sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))*(DX + DX2 + ParX) - cos(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - cos(q2)*sin(q1)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) + sin(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(cos(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))*(DY + DY2) - (DX*sin(q5) + DY*(cos(q5) - 1))*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - ((cos(q4) - 1)*(DZ + L1) + DY*sin(q4))*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - sin(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) + (cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4))*(DZ + DZ2 + L1) - DX*sin(q1) + (cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))*(DY*sin(q5) - DX*(cos(q5) - 1)) - DY*(cos(q1) - 1) + (sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))*(DX + DX2 + ParX) + cos(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + cos(q1)*cos(q2)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) - cos(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(DY + DY2)*(cos(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) + cos(q2)*sin(q3)*sin(q5)) - (cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4))*(DX*sin(q5) + DY*(cos(q5) - 1)) - DY*sin(q2) + (sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3))*(DX + DX2 + ParX) + sin(q2)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) - DZ*(cos(q2) - 1) + cos(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) - (sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4))*(DZ + DZ2 + L1) - cos(q2)*cos(q3)*((cos(q4) - 1)*(DZ + L1) + DY*sin(q4)) - cos(q2)*sin(q3)*(DY*sin(q5) - DX*(cos(q5) - 1));
(sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))*(DX + DX2 - ParX) - (sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1))*(DX*sin(q5) + DY*(cos(q5) - 1)) - ((cos(q4) - 1)*(DZ + L1) + DY*sin(q4))*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - sin(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + DY*sin(q1) + (cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))*(DY*sin(q5) - DX*(cos(q5) - 1)) + (cos(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) - sin(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))*(DY + DY2) + (cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4))*(DZ + DZ2 + L1) - DX*(cos(q1) - 1) - cos(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - cos(q2)*sin(q1)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) + sin(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))*(DX + DX2 - ParX) - (DX*sin(q5) + DY*(cos(q5) - 1))*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - ((cos(q4) - 1)*(DZ + L1) + DY*sin(q4))*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + (cos(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))*(DY + DY2) + (cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4))*(DZ + DZ2 + L1) - DX*sin(q1) + (cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))*(DY*sin(q5) - DX*(cos(q5) - 1)) - DY*(cos(q1) - 1) + cos(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + cos(q1)*cos(q2)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) - cos(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(DY + DY2)*(cos(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) + cos(q2)*sin(q3)*sin(q5)) - (cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4))*(DX*sin(q5) + DY*(cos(q5) - 1)) - DY*sin(q2) + sin(q2)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) - DZ*(cos(q2) - 1) + (sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3))*(DX + DX2 - ParX) + cos(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) - (sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4))*(DZ + DZ2 + L1) - cos(q2)*cos(q3)*((cos(q4) - 1)*(DZ + L1) + DY*sin(q4)) - cos(q2)*sin(q3)*(DY*sin(q5) - DX*(cos(q5) - 1));
DY*sin(q1) - (sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1))*(DX*sin(q5) + DY*(cos(q5) - 1)) - ((cos(q4) - 1)*(DZ + L1) + DY*sin(q4))*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - sin(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + (cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))*(DY*sin(q5) - DX*(cos(q5) - 1)) + (cos(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) - sin(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))*(DY + DY2) - DX*(cos(q1) - 1) + (sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))*(DX + DX2 + ParX) + (cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4))*(DZ + DZ2 + L1 + ParZ) - cos(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - cos(q2)*sin(q1)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) + sin(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(cos(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))*(DY + DY2) - (DX*sin(q5) + DY*(cos(q5) - 1))*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - ((cos(q4) - 1)*(DZ + L1) + DY*sin(q4))*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - sin(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - DX*sin(q1) + (cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))*(DY*sin(q5) - DX*(cos(q5) - 1)) - DY*(cos(q1) - 1) + (sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))*(DX + DX2 + ParX) + (cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4))*(DZ + DZ2 + L1 + ParZ) + cos(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + cos(q1)*cos(q2)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) - cos(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(DY + DY2)*(cos(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) + cos(q2)*sin(q3)*sin(q5)) - (sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4))*(DZ + DZ2 + L1 + ParZ) - (cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4))*(DX*sin(q5) + DY*(cos(q5) - 1)) - DY*sin(q2) + (sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3))*(DX + DX2 + ParX) + sin(q2)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) - DZ*(cos(q2) - 1) + cos(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) - cos(q2)*cos(q3)*((cos(q4) - 1)*(DZ + L1) + DY*sin(q4)) - cos(q2)*sin(q3)*(DY*sin(q5) - DX*(cos(q5) - 1));
(sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))*(DX + DX2 - ParX) - (sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1))*(DX*sin(q5) + DY*(cos(q5) - 1)) - ((cos(q4) - 1)*(DZ + L1) + DY*sin(q4))*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - sin(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + DY*sin(q1) + (cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))*(DY*sin(q5) - DX*(cos(q5) - 1)) + (cos(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) - sin(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))*(DY + DY2) - DX*(cos(q1) - 1) + (cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4))*(DZ + DZ2 + L1 + ParZ) - cos(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - cos(q2)*sin(q1)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) + sin(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))*(DX + DX2 - ParX) - (DX*sin(q5) + DY*(cos(q5) - 1))*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - ((cos(q4) - 1)*(DZ + L1) + DY*sin(q4))*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + (cos(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))*(DY + DY2) - DX*sin(q1) + (cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))*(DY*sin(q5) - DX*(cos(q5) - 1)) - DY*(cos(q1) - 1) + (cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4))*(DZ + DZ2 + L1 + ParZ) + cos(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + cos(q1)*cos(q2)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) - cos(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(DY + DY2)*(cos(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) + cos(q2)*sin(q3)*sin(q5)) - (sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4))*(DZ + DZ2 + L1 + ParZ) - (cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4))*(DX*sin(q5) + DY*(cos(q5) - 1)) - DY*sin(q2) + sin(q2)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) - DZ*(cos(q2) - 1) + (sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3))*(DX + DX2 - ParX) + cos(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) - cos(q2)*cos(q3)*((cos(q4) - 1)*(DZ + L1) + DY*sin(q4)) - cos(q2)*sin(q3)*(DY*sin(q5) - DX*(cos(q5) - 1));
(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4))*(DX*sin(q6) - (cos(q6) - 1)*(DZ + L1 + L2)) - sin(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) - (sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1))*(DX*sin(q5) + DY*(cos(q5) - 1)) - (sin(q6)*(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4)) - cos(q6)*(sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))))*(DX - DZ3 + Suppx) - ((cos(q4) - 1)*(DZ + L1) + DY*sin(q4))*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + (cos(q6)*(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))))*(DY*sin(q7) - (cos(q7) - 1)*(DZ + L1 + L2)) - (sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))*(sin(q6)*(DZ + L1 + L2) + DX*(cos(q6) - 1)) - (cos(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) - sin(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))*(sin(q7)*(DZ + L1 + L2) + DY*(cos(q7) - 1)) + DY*sin(q1) + (cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))*(DY*sin(q5) - DX*(cos(q5) - 1)) - (sin(q7)*(cos(q6)*(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))) - cos(q7)*(cos(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) - sin(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))))*(DY + DY3) - DX*(cos(q1) - 1) + (cos(q7)*(cos(q6)*(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))) + sin(q7)*(cos(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) - sin(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))))*(DX3 + DZ + L1 + L2 - Suppy) - cos(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - cos(q2)*sin(q1)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) + sin(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(DX*sin(q6) - (cos(q6) - 1)*(DZ + L1 + L2))*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4)) - (sin(q6)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4)) - cos(q6)*(sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))))*(DX - DZ3 + Suppx) - sin(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - (DX*sin(q5) + DY*(cos(q5) - 1))*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + (cos(q6)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))))*(DY*sin(q7) - (cos(q7) - 1)*(DZ + L1 + L2)) - ((cos(q4) - 1)*(DZ + L1) + DY*sin(q4))*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - (sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))*(sin(q6)*(DZ + L1 + L2) + DX*(cos(q6) - 1)) - (cos(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))*(sin(q7)*(DZ + L1 + L2) + DY*(cos(q7) - 1)) - (sin(q7)*(cos(q6)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))) - cos(q7)*(cos(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))))*(DY + DY3) - DX*sin(q1) + (cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))*(DY*sin(q5) - DX*(cos(q5) - 1)) + (cos(q7)*(cos(q6)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))) + sin(q7)*(cos(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))))*(DX3 + DZ + L1 + L2 - Suppy) - DY*(cos(q1) - 1) + cos(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + cos(q1)*cos(q2)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) - cos(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(cos(q6)*(sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3)) + sin(q6)*(sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4)))*(DX - DZ3 + Suppx) - (sin(q7)*(DZ + L1 + L2) + DY*(cos(q7) - 1))*(cos(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) + cos(q2)*sin(q3)*sin(q5)) - (sin(q6)*(DZ + L1 + L2) + DX*(cos(q6) - 1))*(sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3)) + (DY + DY3)*(cos(q7)*(cos(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) + cos(q2)*sin(q3)*sin(q5)) - sin(q7)*(sin(q6)*(sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3)) - cos(q6)*(sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4)))) + (sin(q6)*(sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3)) - cos(q6)*(sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4)))*(DY*sin(q7) - (cos(q7) - 1)*(DZ + L1 + L2)) - (sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4))*(DX*sin(q6) - (cos(q6) - 1)*(DZ + L1 + L2)) - (cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4))*(DX*sin(q5) + DY*(cos(q5) - 1)) - DY*sin(q2) + (sin(q7)*(cos(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) + cos(q2)*sin(q3)*sin(q5)) + cos(q7)*(sin(q6)*(sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3)) - cos(q6)*(sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4))))*(DX3 + DZ + L1 + L2 - Suppy) + sin(q2)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) - DZ*(cos(q2) - 1) + cos(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) - cos(q2)*cos(q3)*((cos(q4) - 1)*(DZ + L1) + DY*sin(q4)) - cos(q2)*sin(q3)*(DY*sin(q5) - DX*(cos(q5) - 1));
(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4))*(DX*sin(q6) - (cos(q6) - 1)*(DZ + L1 + L2)) - sin(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) - (sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1))*(DX*sin(q5) + DY*(cos(q5) - 1)) + (sin(q6)*(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4)) - cos(q6)*(sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))))*(DZ3 - DX + Suppx) - ((cos(q4) - 1)*(DZ + L1) + DY*sin(q4))*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + (cos(q6)*(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))))*(DY*sin(q7) - (cos(q7) - 1)*(DZ + L1 + L2)) - (sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))*(sin(q6)*(DZ + L1 + L2) + DX*(cos(q6) - 1)) - (cos(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) - sin(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))*(sin(q7)*(DZ + L1 + L2) + DY*(cos(q7) - 1)) + DY*sin(q1) + (cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))*(DY*sin(q5) - DX*(cos(q5) - 1)) - (sin(q7)*(cos(q6)*(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))) - cos(q7)*(cos(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) - sin(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))))*(DY + DY3) - DX*(cos(q1) - 1) + (cos(q7)*(cos(q6)*(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))) + sin(q7)*(cos(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) - sin(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))))*(DX3 + DZ + L1 + L2 - Suppy) - cos(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - cos(q2)*sin(q1)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) + sin(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(sin(q6)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4)) - cos(q6)*(sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))))*(DZ3 - DX + Suppx) - sin(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) + (DX*sin(q6) - (cos(q6) - 1)*(DZ + L1 + L2))*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4)) - (DX*sin(q5) + DY*(cos(q5) - 1))*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + (cos(q6)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))))*(DY*sin(q7) - (cos(q7) - 1)*(DZ + L1 + L2)) - ((cos(q4) - 1)*(DZ + L1) + DY*sin(q4))*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - (sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))*(sin(q6)*(DZ + L1 + L2) + DX*(cos(q6) - 1)) - (cos(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))*(sin(q7)*(DZ + L1 + L2) + DY*(cos(q7) - 1)) - (sin(q7)*(cos(q6)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))) - cos(q7)*(cos(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))))*(DY + DY3) - DX*sin(q1) + (cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))*(DY*sin(q5) - DX*(cos(q5) - 1)) + (cos(q7)*(cos(q6)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))) + sin(q7)*(cos(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))))*(DX3 + DZ + L1 + L2 - Suppy) - DY*(cos(q1) - 1) + cos(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + cos(q1)*cos(q2)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) - cos(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(DY + DY3)*(cos(q7)*(cos(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) + cos(q2)*sin(q3)*sin(q5)) - sin(q7)*(sin(q6)*(sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3)) - cos(q6)*(sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4)))) - (sin(q7)*(DZ + L1 + L2) + DY*(cos(q7) - 1))*(cos(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) + cos(q2)*sin(q3)*sin(q5)) - (cos(q6)*(sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3)) + sin(q6)*(sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4)))*(DZ3 - DX + Suppx) - (sin(q6)*(DZ + L1 + L2) + DX*(cos(q6) - 1))*(sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3)) + (sin(q6)*(sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3)) - cos(q6)*(sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4)))*(DY*sin(q7) - (cos(q7) - 1)*(DZ + L1 + L2)) - (sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4))*(DX*sin(q6) - (cos(q6) - 1)*(DZ + L1 + L2)) - (cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4))*(DX*sin(q5) + DY*(cos(q5) - 1)) - DY*sin(q2) + (sin(q7)*(cos(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) + cos(q2)*sin(q3)*sin(q5)) + cos(q7)*(sin(q6)*(sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3)) - cos(q6)*(sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4))))*(DX3 + DZ + L1 + L2 - Suppy) + sin(q2)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) - DZ*(cos(q2) - 1) + cos(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) - cos(q2)*cos(q3)*((cos(q4) - 1)*(DZ + L1) + DY*sin(q4)) - cos(q2)*sin(q3)*(DY*sin(q5) - DX*(cos(q5) - 1));
(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4))*(DX*sin(q6) - (cos(q6) - 1)*(DZ + L1 + L2)) - sin(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) - (sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1))*(DX*sin(q5) + DY*(cos(q5) - 1)) - (sin(q6)*(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4)) - cos(q6)*(sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))))*(DX - DZ3 + Suppx) - ((cos(q4) - 1)*(DZ + L1) + DY*sin(q4))*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + (cos(q6)*(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))))*(DY*sin(q7) - (cos(q7) - 1)*(DZ + L1 + L2)) - (sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))*(sin(q6)*(DZ + L1 + L2) + DX*(cos(q6) - 1)) - (cos(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) - sin(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))*(sin(q7)*(DZ + L1 + L2) + DY*(cos(q7) - 1)) + (cos(q7)*(cos(q6)*(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))) + sin(q7)*(cos(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) - sin(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))))*(DX3 + DZ + L1 + L2 + Suppy) + DY*sin(q1) + (cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))*(DY*sin(q5) - DX*(cos(q5) - 1)) - (sin(q7)*(cos(q6)*(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))) - cos(q7)*(cos(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) - sin(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))))*(DY + DY3) - DX*(cos(q1) - 1) - cos(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - cos(q2)*sin(q1)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) + sin(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(DX*sin(q6) - (cos(q6) - 1)*(DZ + L1 + L2))*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4)) - (sin(q6)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4)) - cos(q6)*(sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))))*(DX - DZ3 + Suppx) - sin(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - (DX*sin(q5) + DY*(cos(q5) - 1))*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + (cos(q6)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))))*(DY*sin(q7) - (cos(q7) - 1)*(DZ + L1 + L2)) - ((cos(q4) - 1)*(DZ + L1) + DY*sin(q4))*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + (cos(q7)*(cos(q6)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))) + sin(q7)*(cos(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))))*(DX3 + DZ + L1 + L2 + Suppy) - (sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))*(sin(q6)*(DZ + L1 + L2) + DX*(cos(q6) - 1)) - (cos(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))*(sin(q7)*(DZ + L1 + L2) + DY*(cos(q7) - 1)) - (sin(q7)*(cos(q6)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))) - cos(q7)*(cos(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))))*(DY + DY3) - DX*sin(q1) + (cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))*(DY*sin(q5) - DX*(cos(q5) - 1)) - DY*(cos(q1) - 1) + cos(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + cos(q1)*cos(q2)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) - cos(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(cos(q6)*(sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3)) + sin(q6)*(sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4)))*(DX - DZ3 + Suppx) - (sin(q7)*(DZ + L1 + L2) + DY*(cos(q7) - 1))*(cos(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) + cos(q2)*sin(q3)*sin(q5)) - (sin(q6)*(DZ + L1 + L2) + DX*(cos(q6) - 1))*(sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3)) + (sin(q7)*(cos(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) + cos(q2)*sin(q3)*sin(q5)) + cos(q7)*(sin(q6)*(sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3)) - cos(q6)*(sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4))))*(DX3 + DZ + L1 + L2 + Suppy) + (DY + DY3)*(cos(q7)*(cos(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) + cos(q2)*sin(q3)*sin(q5)) - sin(q7)*(sin(q6)*(sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3)) - cos(q6)*(sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4)))) + (sin(q6)*(sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3)) - cos(q6)*(sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4)))*(DY*sin(q7) - (cos(q7) - 1)*(DZ + L1 + L2)) - (sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4))*(DX*sin(q6) - (cos(q6) - 1)*(DZ + L1 + L2)) - (cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4))*(DX*sin(q5) + DY*(cos(q5) - 1)) - DY*sin(q2) + sin(q2)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) - DZ*(cos(q2) - 1) + cos(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) - cos(q2)*cos(q3)*((cos(q4) - 1)*(DZ + L1) + DY*sin(q4)) - cos(q2)*sin(q3)*(DY*sin(q5) - DX*(cos(q5) - 1));
(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4))*(DX*sin(q6) - (cos(q6) - 1)*(DZ + L1 + L2)) - sin(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) - (sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1))*(DX*sin(q5) + DY*(cos(q5) - 1)) + (sin(q6)*(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4)) - cos(q6)*(sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))))*(DZ3 - DX + Suppx) - ((cos(q4) - 1)*(DZ + L1) + DY*sin(q4))*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + (cos(q6)*(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))))*(DY*sin(q7) - (cos(q7) - 1)*(DZ + L1 + L2)) - (sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))*(sin(q6)*(DZ + L1 + L2) + DX*(cos(q6) - 1)) - (cos(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) - sin(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))*(sin(q7)*(DZ + L1 + L2) + DY*(cos(q7) - 1)) + (cos(q7)*(cos(q6)*(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))) + sin(q7)*(cos(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) - sin(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))))*(DX3 + DZ + L1 + L2 + Suppy) + DY*sin(q1) + (cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))*(DY*sin(q5) - DX*(cos(q5) - 1)) - (sin(q7)*(cos(q6)*(cos(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) + cos(q2)*sin(q1)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) + cos(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)))) - cos(q7)*(cos(q5)*(sin(q4)*(cos(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - cos(q2)*cos(q4)*sin(q1)) - sin(q5)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3))))*(DY + DY3) - DX*(cos(q1) - 1) - cos(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) - cos(q2)*sin(q1)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) + sin(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(sin(q6)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4)) - cos(q6)*(sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))))*(DZ3 - DX + Suppx) - sin(q1)*(DZ*sin(q3) + DX*(cos(q3) - 1)) + (DX*sin(q6) - (cos(q6) - 1)*(DZ + L1 + L2))*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4)) - (DX*sin(q5) + DY*(cos(q5) - 1))*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + (cos(q6)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))))*(DY*sin(q7) - (cos(q7) - 1)*(DZ + L1 + L2)) - ((cos(q4) - 1)*(DZ + L1) + DY*sin(q4))*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + (cos(q7)*(cos(q6)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))) + sin(q7)*(cos(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))))*(DX3 + DZ + L1 + L2 + Suppy) - (sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))*(sin(q6)*(DZ + L1 + L2) + DX*(cos(q6) - 1)) - (cos(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))*(sin(q7)*(DZ + L1 + L2) + DY*(cos(q7) - 1)) - (sin(q7)*(cos(q6)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) - cos(q1)*cos(q2)*sin(q4)) + sin(q6)*(sin(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) + cos(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)))) - cos(q7)*(cos(q5)*(sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q3)*sin(q2)) + cos(q1)*cos(q2)*cos(q4)) - sin(q5)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))))*(DY + DY3) - DX*sin(q1) + (cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3))*(DY*sin(q5) - DX*(cos(q5) - 1)) - DY*(cos(q1) - 1) + cos(q1)*(DZ*sin(q2) - DY*(cos(q2) - 1)) + cos(q1)*cos(q2)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) - cos(q1)*sin(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1));
(sin(q7)*(cos(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) + cos(q2)*sin(q3)*sin(q5)) + cos(q7)*(sin(q6)*(sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3)) - cos(q6)*(sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4))))*(DX3 + DZ + L1 + L2 + Suppy) - (sin(q7)*(DZ + L1 + L2) + DY*(cos(q7) - 1))*(cos(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) + cos(q2)*sin(q3)*sin(q5)) - (cos(q6)*(sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3)) + sin(q6)*(sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4)))*(DZ3 - DX + Suppx) - (sin(q6)*(DZ + L1 + L2) + DX*(cos(q6) - 1))*(sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3)) + (DY + DY3)*(cos(q7)*(cos(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) + cos(q2)*sin(q3)*sin(q5)) - sin(q7)*(sin(q6)*(sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3)) - cos(q6)*(sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4)))) + (sin(q6)*(sin(q5)*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4)) - cos(q2)*cos(q5)*sin(q3)) - cos(q6)*(sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4)))*(DY*sin(q7) - (cos(q7) - 1)*(DZ + L1 + L2)) - (sin(q2)*sin(q4) - cos(q2)*cos(q3)*cos(q4))*(DX*sin(q6) - (cos(q6) - 1)*(DZ + L1 + L2)) - (cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4))*(DX*sin(q5) + DY*(cos(q5) - 1)) - DY*sin(q2) + sin(q2)*(sin(q4)*(DZ + L1) - DY*(cos(q4) - 1)) - DZ*(cos(q2) - 1) + cos(q2)*(DX*sin(q3) - DZ*(cos(q3) - 1)) - cos(q2)*cos(q3)*((cos(q4) - 1)*(DZ + L1) + DY*sin(q4)) - cos(q2)*sin(q3)*(DY*sin(q5) - DX*(cos(q5) - 1));
];