-
Notifications
You must be signed in to change notification settings - Fork 0
/
VJM.m
440 lines (343 loc) · 35.3 KB
/
VJM.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
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
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
function [Kc] = VJM(q, link_lengths)
%% Extracting the link lengths
d0 = link_lengths(1);
d1 = link_lengths(2);
d2 = link_lengths(3);
d3 = link_lengths(4);
d4 = link_lengths(5);
d6 = link_lengths(6);
q0 = q(1);
q1 = q(2);
q2 = q(3);
q3 = q(4);
q4 = q(5);
q5 = q(6);
q6 = q(7);
%% Getting the forward kinematics
T = FK(q, link_lengths);
T(1:3, 4) = 0;
%% JacobiansTheta
% active joints 1,8,15,22,35,36,37
theta = zeros(1,43);
Td = Ty(q0)*Tyd(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J1 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Txd(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J2 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Tyd(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J3 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tzd(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J4 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rxd(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J5 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ryd(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J6 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rzd(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J7 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rzd(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J8 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Txd(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J9 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Tyd(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J10 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tzd(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J11 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rxd(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J12 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ryd(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J13 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rzd(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J14 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ryd(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J15 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Txd(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J16 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Tyd(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J17 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tzd(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J18 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rxd(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J19 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ryd(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J20 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rzd(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J21 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ryd(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J22 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Txd(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J23 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Tyd(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J24 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tzd(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J25 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rxd(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J26 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ryd(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J27 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rzd(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J28 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Txd(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J29 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Tyd(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J30 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tzd(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J31 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rxd(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J32 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ryd(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J33 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rzd(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J34 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rxd(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J35 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ryd(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J36 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rxd(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J37 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Txd(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J38 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Tyd(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J39 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tzd(theta(40))*Rx(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J40 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rxd(theta(41))*Ry(theta(42))*Rz(theta(43)) / T;
J41 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ryd(theta(42))*Rz(theta(43)) / T;
J42 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Td = Ty(q0)*Ty(theta(1))*Tz(d0)*Tx(theta(2))*Ty(theta(3))*Tz(theta(4))*Rx(theta(5))*Ry(theta(6))*Rz(theta(7))...
*Rz(q1)*Rz(theta(8))*Tx(d1)*Tx(theta(9))*Ty(theta(10))*Tz(theta(11))*Rx(theta(12))*Ry(theta(13))*Rz(theta(14))...
*Ry(q2)*Ry(theta(15))*Tx(d2)*Tx(theta(16))*Ty(theta(17))*Tz(theta(18))*Rx(theta(19))*Ry(theta(20))*Rz(theta(21))...
*Ry(q3)*Ry(theta(22))*Tx(d3)*Tx(theta(23))*Ty(theta(24))*Tz(theta(25))*Rx(theta(26))*Ry(theta(27))*Rz(theta(28))...
*Tz(d4)*Tx(theta(29))*Ty(theta(30))*Tz(theta(31))*Rx(theta(32))*Ry(theta(33))*Rz(theta(34))*Rx(q4)*Rx(theta(35))...
*Ry(q5)*Ry(theta(36))*Rx(q6)*Rx(theta(37))*Tx(d6)*Tx(theta(38))*Ty(theta(39))*Tz(theta(40))*Rx(theta(41))*Ry(theta(42))*Rzd(theta(43)) / T;
J43 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];
Jtheta = [J1, J2, J3, J4, J5, J6, J7, J8, J9, J10, J11, J12, J13, J14, J15, J16, J17, J18, J19, J20, J21, J22, J23, J24,...
J25, J26, J27, J28, J29, J30, J31, J32, J33, J34, J35, J36, J37, J38, J39, J40, J41, J42, J43];
%% Stiffness Matrix for links
d = 0.1; % link diameter
K_active = 1000000; % actuator stiffness
E = 7.0000e+10; % Young's modulus
G = 2.5500e+10; % shear modulus
K_l1 = K_link(E, G, link_lengths(1), d);
K_l2 = K_link(E, G, link_lengths(2), d);
K_l3 = K_link(E, G, link_lengths(3), d);
K_l4 = K_link(E, G, link_lengths(4), d);
K_l5 = K_link(E, G, link_lengths(5), d);
K_l6 = K_link(E, G, link_lengths(6), d);
K_Joints = diag([K_active K_active K_active K_active K_active K_active K_active]);
K_Joints = [K_Joints zeros(7,36)];
K_Links = blkdiag(K_l1,K_l2,K_l3,K_l4,K_l5,K_l6);
K_Links = [zeros(36,7) K_Links];
Ktheta = [K_Joints;K_Links];
Kc0 = inv([Jtheta*inv(Ktheta)*Jtheta']);
Kc = Kc0;
end