IdentificationToolbox/gWorldHfun.m at master · JosephAv/IdentificationToolbox · GitHub

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
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
function [ gWorldH ] = gWorldHfun( par, angles )
%UNTITLED6 Summary of this function goes here
%   Detailed explanation goes here

 DX = par(1);
 DY = par(2);
 DZ = par(3);

 DX1 = par(4);
 DY1 = par(5);
 DZ1 = par(6);

 DX2 = par(7);
 DY2 = par(8);
 DZ2 = par(9);

 DX3 = par(10);
 DY3 = par(11);
 DZ3 = par(12);

 L1 = par(13);
 L2 = par(14);
 Suppx = par(15);
 Suppy = par(16);

 q1 = angles(1);
 q2 = angles(2);
 q3 = angles(3);
 q4 = angles(4);
 q5 = angles(5);
 q6 = angles(6);
 q7 = angles(7);


gWorldH=[(cos(q7).*(cos(q6).*(cos(q4).*(cos(q3).*sin(q1).*sin(q2)+cos(q1).*sin( ...
  q3))+cos(q2).*sin(q1).*sin(q4))+(cos(q5).*(cos(q1).*cos(q3)+(-1).*sin( ...
  q1).*sin(q2).*sin(q3))+((-1).*cos(q2).*cos(q4).*sin(q1)+(cos(q3).*sin( ...
  q1).*sin(q2)+cos(q1).*sin(q3)).*sin(q4)).*sin(q5)).*sin(q6))+(cos(q5).*( ...
  (-1).*cos(q2).*cos(q4).*sin(q1)+(cos(q3).*sin(q1).*sin(q2)+cos(q1).*sin( ...
  q3)).*sin(q4))+(-1).*(cos(q1).*cos(q3)+(-1).*sin(q1).*sin(q2).*sin(q3)) ...
  .*sin(q5)).*sin(q7)),(cos(q7).*(cos(q5).*((-1).*cos(q2).*cos(q4).*sin(q1)+ ...
  (cos(q3).*sin(q1).*sin(q2)+cos(q1).*sin(q3)).*sin(q4))+(-1).*(cos(q1).* ...
  cos(q3)+(-1).*sin(q1).*sin(q2).*sin(q3)).*sin(q5))+(-1).*(cos(q6).*(cos( ...
  q4).*(cos(q3).*sin(q1).*sin(q2)+cos(q1).*sin(q3))+cos(q2).*sin(q1).*sin( ...
  q4))+(cos(q5).*(cos(q1).*cos(q3)+(-1).*sin(q1).*sin(q2).*sin(q3))+((-1) ...
  .*cos(q2).*cos(q4).*sin(q1)+(cos(q3).*sin(q1).*sin(q2)+cos(q1).*sin(q3)) ...
  .*sin(q4)).*sin(q5)).*sin(q6)).*sin(q7)),((-1).*cos(q6).*(cos(q5).*(cos( ...
  q1).*cos(q3)+(-1).*sin(q1).*sin(q2).*sin(q3))+((-1).*cos(q2).*cos(q4).* ...
  sin(q1)+(cos(q3).*sin(q1).*sin(q2)+cos(q1).*sin(q3)).*sin(q4)).*sin(q5)) ...
  +(cos(q4).*(cos(q3).*sin(q1).*sin(q2)+cos(q1).*sin(q3))+cos(q2).*sin(q1) ...
  .*sin(q4)).*sin(q6)),(DX.*(1+(-1).*cos(q1))+DY.*sin(q1)+(-1).*sin(q1).*( ...
  DY.*(1+(-1).*cos(q2))+DZ.*sin(q2))+sin(q1).*sin(q2).*(DZ.*(1+(-1).*cos( ...
  q3))+DX.*sin(q3))+cos(q1).*(DX.*(1+(-1).*cos(q3))+(-1).*DZ.*sin(q3))+( ...
  cos(q3).*sin(q1).*sin(q2)+cos(q1).*sin(q3)).*((DZ+L1).*(1+(-1).*cos(q4)) ...
  +(-1).*DY.*sin(q4))+(-1).*cos(q2).*sin(q1).*(DY.*(1+(-1).*cos(q4))+(DZ+ ...
  L1).*sin(q4))+((-1).*cos(q2).*cos(q4).*sin(q1)+(cos(q3).*sin(q1).*sin( ...
  q2)+cos(q1).*sin(q3)).*sin(q4)).*(DY.*(1+(-1).*cos(q5))+(-1).*DX.*sin( ...
  q5))+(cos(q1).*cos(q3)+(-1).*sin(q1).*sin(q2).*sin(q3)).*(DX.*(1+(-1).* ...
  cos(q5))+DY.*sin(q5))+(cos(q4).*(cos(q3).*sin(q1).*sin(q2)+cos(q1).*sin( ...
  q3))+cos(q2).*sin(q1).*sin(q4)).*((DZ+L1+L2).*(1+(-1).*cos(q6))+DX.*sin( ...
  q6))+(cos(q5).*(cos(q1).*cos(q3)+(-1).*sin(q1).*sin(q2).*sin(q3))+((-1) ...
  .*cos(q2).*cos(q4).*sin(q1)+(cos(q3).*sin(q1).*sin(q2)+cos(q1).*sin(q3)) ...
  .*sin(q4)).*sin(q5)).*(DX.*(1+(-1).*cos(q6))+(-1).*(DZ+L1+L2).*sin(q6))+ ...
  (DX+(-1).*DZ3).*(cos(q6).*(cos(q5).*(cos(q1).*cos(q3)+(-1).*sin(q1).* ...
  sin(q2).*sin(q3))+((-1).*cos(q2).*cos(q4).*sin(q1)+(cos(q3).*sin(q1).* ...
  sin(q2)+cos(q1).*sin(q3)).*sin(q4)).*sin(q5))+(-1).*(cos(q4).*(cos(q3).* ...
  sin(q1).*sin(q2)+cos(q1).*sin(q3))+cos(q2).*sin(q1).*sin(q4)).*sin(q6))+ ...
  (cos(q6).*(cos(q4).*(cos(q3).*sin(q1).*sin(q2)+cos(q1).*sin(q3))+cos(q2) ...
  .*sin(q1).*sin(q4))+(cos(q5).*(cos(q1).*cos(q3)+(-1).*sin(q1).*sin(q2).* ...
  sin(q3))+((-1).*cos(q2).*cos(q4).*sin(q1)+(cos(q3).*sin(q1).*sin(q2)+ ...
  cos(q1).*sin(q3)).*sin(q4)).*sin(q5)).*sin(q6)).*((DZ+L1+L2).*(1+(-1).* ...
  cos(q7))+DY.*sin(q7))+(cos(q5).*((-1).*cos(q2).*cos(q4).*sin(q1)+(cos( ...
  q3).*sin(q1).*sin(q2)+cos(q1).*sin(q3)).*sin(q4))+(-1).*(cos(q1).*cos( ...
  q3)+(-1).*sin(q1).*sin(q2).*sin(q3)).*sin(q5)).*(DY.*(1+(-1).*cos(q7))+( ...
  -1).*(DZ+L1+L2).*sin(q7))+(DX3+DZ+L1+L2).*(cos(q7).*(cos(q6).*(cos(q4).* ...
  (cos(q3).*sin(q1).*sin(q2)+cos(q1).*sin(q3))+cos(q2).*sin(q1).*sin(q4))+ ...
  (cos(q5).*(cos(q1).*cos(q3)+(-1).*sin(q1).*sin(q2).*sin(q3))+((-1).*cos( ...
  q2).*cos(q4).*sin(q1)+(cos(q3).*sin(q1).*sin(q2)+cos(q1).*sin(q3)).*sin( ...
  q4)).*sin(q5)).*sin(q6))+(cos(q5).*((-1).*cos(q2).*cos(q4).*sin(q1)+( ...
  cos(q3).*sin(q1).*sin(q2)+cos(q1).*sin(q3)).*sin(q4))+(-1).*(cos(q1).* ...
  cos(q3)+(-1).*sin(q1).*sin(q2).*sin(q3)).*sin(q5)).*sin(q7))+(DY+DY3).*( ...
  cos(q7).*(cos(q5).*((-1).*cos(q2).*cos(q4).*sin(q1)+(cos(q3).*sin(q1).* ...
  sin(q2)+cos(q1).*sin(q3)).*sin(q4))+(-1).*(cos(q1).*cos(q3)+(-1).*sin( ...
  q1).*sin(q2).*sin(q3)).*sin(q5))+(-1).*(cos(q6).*(cos(q4).*(cos(q3).* ...
  sin(q1).*sin(q2)+cos(q1).*sin(q3))+cos(q2).*sin(q1).*sin(q4))+(cos(q5).* ...
  (cos(q1).*cos(q3)+(-1).*sin(q1).*sin(q2).*sin(q3))+((-1).*cos(q2).*cos( ...
  q4).*sin(q1)+(cos(q3).*sin(q1).*sin(q2)+cos(q1).*sin(q3)).*sin(q4)).* ...
  sin(q5)).*sin(q6)).*sin(q7)));(cos(q7).*(cos(q6).*(cos(q4).*((-1).*cos(q1) ...
  .*cos(q3).*sin(q2)+sin(q1).*sin(q3))+(-1).*cos(q1).*cos(q2).*sin(q4))+( ...
  cos(q5).*(cos(q3).*sin(q1)+cos(q1).*sin(q2).*sin(q3))+(cos(q1).*cos(q2) ...
  .*cos(q4)+((-1).*cos(q1).*cos(q3).*sin(q2)+sin(q1).*sin(q3)).*sin(q4)).* ...
  sin(q5)).*sin(q6))+(cos(q5).*(cos(q1).*cos(q2).*cos(q4)+((-1).*cos(q1).* ...
  cos(q3).*sin(q2)+sin(q1).*sin(q3)).*sin(q4))+(-1).*(cos(q3).*sin(q1)+ ...
  cos(q1).*sin(q2).*sin(q3)).*sin(q5)).*sin(q7)),(cos(q7).*(cos(q5).*(cos( ...
  q1).*cos(q2).*cos(q4)+((-1).*cos(q1).*cos(q3).*sin(q2)+sin(q1).*sin(q3)) ...
  .*sin(q4))+(-1).*(cos(q3).*sin(q1)+cos(q1).*sin(q2).*sin(q3)).*sin(q5))+ ...
  (-1).*(cos(q6).*(cos(q4).*((-1).*cos(q1).*cos(q3).*sin(q2)+sin(q1).*sin( ...
  q3))+(-1).*cos(q1).*cos(q2).*sin(q4))+(cos(q5).*(cos(q3).*sin(q1)+cos( ...
  q1).*sin(q2).*sin(q3))+(cos(q1).*cos(q2).*cos(q4)+((-1).*cos(q1).*cos( ...
  q3).*sin(q2)+sin(q1).*sin(q3)).*sin(q4)).*sin(q5)).*sin(q6)).*sin(q7)),(( ...
  -1).*cos(q6).*(cos(q5).*(cos(q3).*sin(q1)+cos(q1).*sin(q2).*sin(q3))+( ...
  cos(q1).*cos(q2).*cos(q4)+((-1).*cos(q1).*cos(q3).*sin(q2)+sin(q1).*sin( ...
  q3)).*sin(q4)).*sin(q5))+(cos(q4).*((-1).*cos(q1).*cos(q3).*sin(q2)+sin( ...
  q1).*sin(q3))+(-1).*cos(q1).*cos(q2).*sin(q4)).*sin(q6)),(DY.*(1+(-1).* ...
  cos(q1))+(-1).*DX.*sin(q1)+cos(q1).*(DY.*(1+(-1).*cos(q2))+DZ.*sin(q2))+ ...
  (-1).*cos(q1).*sin(q2).*(DZ.*(1+(-1).*cos(q3))+DX.*sin(q3))+sin(q1).*( ...
  DX.*(1+(-1).*cos(q3))+(-1).*DZ.*sin(q3))+((-1).*cos(q1).*cos(q3).*sin( ...
  q2)+sin(q1).*sin(q3)).*((DZ+L1).*(1+(-1).*cos(q4))+(-1).*DY.*sin(q4))+ ...
  cos(q1).*cos(q2).*(DY.*(1+(-1).*cos(q4))+(DZ+L1).*sin(q4))+(cos(q1).* ...
  cos(q2).*cos(q4)+((-1).*cos(q1).*cos(q3).*sin(q2)+sin(q1).*sin(q3)).* ...
  sin(q4)).*(DY.*(1+(-1).*cos(q5))+(-1).*DX.*sin(q5))+(cos(q3).*sin(q1)+ ...
  cos(q1).*sin(q2).*sin(q3)).*(DX.*(1+(-1).*cos(q5))+DY.*sin(q5))+(cos(q4) ...
  .*((-1).*cos(q1).*cos(q3).*sin(q2)+sin(q1).*sin(q3))+(-1).*cos(q1).*cos( ...
  q2).*sin(q4)).*((DZ+L1+L2).*(1+(-1).*cos(q6))+DX.*sin(q6))+(cos(q5).*( ...
  cos(q3).*sin(q1)+cos(q1).*sin(q2).*sin(q3))+(cos(q1).*cos(q2).*cos(q4)+( ...
  (-1).*cos(q1).*cos(q3).*sin(q2)+sin(q1).*sin(q3)).*sin(q4)).*sin(q5)).*( ...
  DX.*(1+(-1).*cos(q6))+(-1).*(DZ+L1+L2).*sin(q6))+(DX+(-1).*DZ3).*(cos( ...
  q6).*(cos(q5).*(cos(q3).*sin(q1)+cos(q1).*sin(q2).*sin(q3))+(cos(q1).* ...
  cos(q2).*cos(q4)+((-1).*cos(q1).*cos(q3).*sin(q2)+sin(q1).*sin(q3)).* ...
  sin(q4)).*sin(q5))+(-1).*(cos(q4).*((-1).*cos(q1).*cos(q3).*sin(q2)+sin( ...
  q1).*sin(q3))+(-1).*cos(q1).*cos(q2).*sin(q4)).*sin(q6))+(cos(q6).*(cos( ...
  q4).*((-1).*cos(q1).*cos(q3).*sin(q2)+sin(q1).*sin(q3))+(-1).*cos(q1).* ...
  cos(q2).*sin(q4))+(cos(q5).*(cos(q3).*sin(q1)+cos(q1).*sin(q2).*sin(q3)) ...
  +(cos(q1).*cos(q2).*cos(q4)+((-1).*cos(q1).*cos(q3).*sin(q2)+sin(q1).* ...
  sin(q3)).*sin(q4)).*sin(q5)).*sin(q6)).*((DZ+L1+L2).*(1+(-1).*cos(q7))+ ...
  DY.*sin(q7))+(cos(q5).*(cos(q1).*cos(q2).*cos(q4)+((-1).*cos(q1).*cos( ...
  q3).*sin(q2)+sin(q1).*sin(q3)).*sin(q4))+(-1).*(cos(q3).*sin(q1)+cos(q1) ...
  .*sin(q2).*sin(q3)).*sin(q5)).*(DY.*(1+(-1).*cos(q7))+(-1).*(DZ+L1+L2).* ...
  sin(q7))+(DX3+DZ+L1+L2).*(cos(q7).*(cos(q6).*(cos(q4).*((-1).*cos(q1).* ...
  cos(q3).*sin(q2)+sin(q1).*sin(q3))+(-1).*cos(q1).*cos(q2).*sin(q4))+( ...
  cos(q5).*(cos(q3).*sin(q1)+cos(q1).*sin(q2).*sin(q3))+(cos(q1).*cos(q2) ...
  .*cos(q4)+((-1).*cos(q1).*cos(q3).*sin(q2)+sin(q1).*sin(q3)).*sin(q4)).* ...
  sin(q5)).*sin(q6))+(cos(q5).*(cos(q1).*cos(q2).*cos(q4)+((-1).*cos(q1).* ...
  cos(q3).*sin(q2)+sin(q1).*sin(q3)).*sin(q4))+(-1).*(cos(q3).*sin(q1)+ ...
  cos(q1).*sin(q2).*sin(q3)).*sin(q5)).*sin(q7))+(DY+DY3).*(cos(q7).*(cos( ...
  q5).*(cos(q1).*cos(q2).*cos(q4)+((-1).*cos(q1).*cos(q3).*sin(q2)+sin(q1) ...
  .*sin(q3)).*sin(q4))+(-1).*(cos(q3).*sin(q1)+cos(q1).*sin(q2).*sin(q3)) ...
  .*sin(q5))+(-1).*(cos(q6).*(cos(q4).*((-1).*cos(q1).*cos(q3).*sin(q2)+ ...
  sin(q1).*sin(q3))+(-1).*cos(q1).*cos(q2).*sin(q4))+(cos(q5).*(cos(q3).* ...
  sin(q1)+cos(q1).*sin(q2).*sin(q3))+(cos(q1).*cos(q2).*cos(q4)+((-1).* ...
  cos(q1).*cos(q3).*sin(q2)+sin(q1).*sin(q3)).*sin(q4)).*sin(q5)).*sin(q6) ...
  ).*sin(q7)));(cos(q7).*(cos(q6).*(cos(q2).*cos(q3).*cos(q4)+(-1).*sin(q2) ...
  .*sin(q4))+((-1).*cos(q2).*cos(q5).*sin(q3)+(cos(q4).*sin(q2)+cos(q2).* ...
  cos(q3).*sin(q4)).*sin(q5)).*sin(q6))+(cos(q5).*(cos(q4).*sin(q2)+cos( ...
  q2).*cos(q3).*sin(q4))+cos(q2).*sin(q3).*sin(q5)).*sin(q7)),(cos(q7).*( ...
  cos(q5).*(cos(q4).*sin(q2)+cos(q2).*cos(q3).*sin(q4))+cos(q2).*sin(q3).* ...
  sin(q5))+(-1).*(cos(q6).*(cos(q2).*cos(q3).*cos(q4)+(-1).*sin(q2).*sin( ...
  q4))+((-1).*cos(q2).*cos(q5).*sin(q3)+(cos(q4).*sin(q2)+cos(q2).*cos(q3) ...
  .*sin(q4)).*sin(q5)).*sin(q6)).*sin(q7)),((-1).*cos(q6).*((-1).*cos(q2).* ...
  cos(q5).*sin(q3)+(cos(q4).*sin(q2)+cos(q2).*cos(q3).*sin(q4)).*sin(q5))+ ...
  (cos(q2).*cos(q3).*cos(q4)+(-1).*sin(q2).*sin(q4)).*sin(q6)),(DZ.*(1+(-1) ...
  .*cos(q2))+(-1).*DY.*sin(q2)+cos(q2).*(DZ.*(1+(-1).*cos(q3))+DX.*sin(q3) ...
  )+cos(q2).*cos(q3).*((DZ+L1).*(1+(-1).*cos(q4))+(-1).*DY.*sin(q4))+sin( ...
  q2).*(DY.*(1+(-1).*cos(q4))+(DZ+L1).*sin(q4))+(cos(q4).*sin(q2)+cos(q2) ...
  .*cos(q3).*sin(q4)).*(DY.*(1+(-1).*cos(q5))+(-1).*DX.*sin(q5))+(-1).* ...
  cos(q2).*sin(q3).*(DX.*(1+(-1).*cos(q5))+DY.*sin(q5))+(cos(q2).*cos(q3) ...
  .*cos(q4)+(-1).*sin(q2).*sin(q4)).*((DZ+L1+L2).*(1+(-1).*cos(q6))+DX.* ...
  sin(q6))+((-1).*cos(q2).*cos(q5).*sin(q3)+(cos(q4).*sin(q2)+cos(q2).* ...
  cos(q3).*sin(q4)).*sin(q5)).*(DX.*(1+(-1).*cos(q6))+(-1).*(DZ+L1+L2).* ...
  sin(q6))+(DX+(-1).*DZ3).*(cos(q6).*((-1).*cos(q2).*cos(q5).*sin(q3)+( ...
  cos(q4).*sin(q2)+cos(q2).*cos(q3).*sin(q4)).*sin(q5))+(-1).*(cos(q2).* ...
  cos(q3).*cos(q4)+(-1).*sin(q2).*sin(q4)).*sin(q6))+(cos(q6).*(cos(q2).* ...
  cos(q3).*cos(q4)+(-1).*sin(q2).*sin(q4))+((-1).*cos(q2).*cos(q5).*sin( ...
  q3)+(cos(q4).*sin(q2)+cos(q2).*cos(q3).*sin(q4)).*sin(q5)).*sin(q6)).*(( ...
  DZ+L1+L2).*(1+(-1).*cos(q7))+DY.*sin(q7))+(cos(q5).*(cos(q4).*sin(q2)+ ...
  cos(q2).*cos(q3).*sin(q4))+cos(q2).*sin(q3).*sin(q5)).*(DY.*(1+(-1).* ...
  cos(q7))+(-1).*(DZ+L1+L2).*sin(q7))+(DX3+DZ+L1+L2).*(cos(q7).*(cos(q6).* ...
  (cos(q2).*cos(q3).*cos(q4)+(-1).*sin(q2).*sin(q4))+((-1).*cos(q2).*cos( ...
  q5).*sin(q3)+(cos(q4).*sin(q2)+cos(q2).*cos(q3).*sin(q4)).*sin(q5)).* ...
  sin(q6))+(cos(q5).*(cos(q4).*sin(q2)+cos(q2).*cos(q3).*sin(q4))+cos(q2) ...
  .*sin(q3).*sin(q5)).*sin(q7))+(DY+DY3).*(cos(q7).*(cos(q5).*(cos(q4).* ...
  sin(q2)+cos(q2).*cos(q3).*sin(q4))+cos(q2).*sin(q3).*sin(q5))+(-1).*( ...
  cos(q6).*(cos(q2).*cos(q3).*cos(q4)+(-1).*sin(q2).*sin(q4))+((-1).*cos( ...
  q2).*cos(q5).*sin(q3)+(cos(q4).*sin(q2)+cos(q2).*cos(q3).*sin(q4)).*sin( ...
  q5)).*sin(q6)).*sin(q7)));(0),(0),(0),(1)];

end