-
Notifications
You must be signed in to change notification settings - Fork 5
/
Copy pathchains.m
519 lines (518 loc) · 29.5 KB
/
chains.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
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
function [chain_leg_R,chain_leg_L,chain_arm_R,chain_arm_L,chain_head] = chains(in1,in2,in3,in4,in5)
%CHAINS
% [CHAIN_LEG_R,CHAIN_LEG_L,CHAIN_ARM_R,CHAIN_ARM_L,CHAIN_HEAD] = CHAINS(IN1,IN2,IN3,IN4,IN5)
%in1 are Right Leg joint angles from torso to feet
%in2 are Left Leg joint angles from torso to feet
%in1 are Right Arm joint angles from torso to hand
%in1 are Left Arm joint angles from torso to hand
%in1 are Head joint angles from torso to head
% This function was generated by the Symbolic Math Toolbox version 6.2.
% 12-Jun-2015 10:58:20
H1 = in5(1,:);
H2 = in5(2,:);
LA1 = in4(1,:);
LA2 = in4(2,:);
LA3 = in4(3,:);
LA4 = in4(4,:);
LL1 = in2(1,:);
LL2 = in2(2,:);
LL3 = in2(3,:);
LL4 = in2(4,:);
LL5 = in2(5,:);
LL6 = in2(6,:);
RA1 = in3(1,:);
RA2 = in3(2,:);
RA3 = in3(3,:);
RA4 = in3(4,:);
RL1 = in1(1,:);
RL2 = in1(2,:);
RL3 = in1(3,:);
RL4 = in1(4,:);
RL5 = in1(5,:);
RL6 = in1(6,:);
t2 = conj(RL1);
t5 = pi.*(1.0./2.0);
t3 = t2-t5;
t4 = cos(t3);
t6 = conj(RL2);
t8 = pi.*(1.0./4.0);
t7 = t6-t8;
t9 = cos(t7);
t10 = t4.*t9;
t11 = sin(t3);
t12 = sin(t7);
t20 = t11.*t12.*6.123233995736766e-17;
t13 = t10-t20;
t14 = conj(RL3);
t15 = sin(t14);
t16 = cos(t14);
t17 = t4.*t12;
t18 = t9.*t11.*6.123233995736766e-17;
t19 = t17+t18;
t21 = conj(RL4);
t22 = t13.*t16;
t23 = sin(t21);
t24 = t16.*t19.*6.123233995736766e-17;
t25 = t13.*t15;
t26 = t11.*t16;
t27 = t24+t25+t26;
t28 = cos(t21);
t29 = t15.*t19.*6.123233995736766e-17;
t30 = t11.*t15;
t31 = -t22+t29+t30;
t32 = conj(RL5);
t33 = cos(t32);
t34 = t23.*t27;
t35 = t28.*t31;
t36 = t34+t35;
t37 = sin(t32);
t38 = t23.*t31;
t43 = t27.*t28;
t39 = t38-t43;
t40 = t37.*t39;
t41 = conj(RL6);
t42 = sin(t41);
t44 = cos(t41);
t50 = t11.*6.123233995736766e-17;
t45 = t17+t18-t50;
t46 = t33.*t39;
t47 = t36.*t37;
t48 = t46+t47;
t51 = t33.*t36;
t49 = t40-t51;
t52 = t44.*t48.*6.123233995736766e-17;
t53 = t4.*t12.*6.123233995736766e-17;
t54 = t9.*t11.*3.749399456654644e-33;
t55 = t42.*t48.*6.123233995736766e-17;
t56 = t15.*t19.*6.123233995736766e-18;
t57 = t11.*t15.*(1.0./1.0e1);
t58 = t23.*t27.*1.029e-1;
t59 = t28.*t31.*1.029e-1;
t61 = t13.*t16.*(1.0./1.0e1);
t60 = t56+t57+t58+t59-t61;
t62 = sqrt(2.0);
t63 = t4.*t12.*t62.*3.061616997868383e-17;
t64 = t9.*t11.*t62.*(1.0./2.0);
t66 = t12.*t62.*(1.0./2.0);
t65 = t63+t64-t66;
t67 = t16.*t65;
t68 = t9.*t62.*(1.0./2.0);
t69 = t11.*t12.*t62.*(1.0./2.0);
t76 = t4.*t9.*t62.*3.061616997868383e-17;
t70 = t68+t69-t76;
t71 = t62.*3.061616997868383e-17;
t72 = t4.*t62.*(1.0./2.0);
t73 = t71+t72;
t74 = t15.*t73;
t77 = t15.*t70.*6.123233995736766e-17;
t75 = t67+t74-t77;
t78 = t28.*t75;
t79 = t16.*t70.*6.123233995736766e-17;
t80 = t15.*t65;
t83 = t16.*t73;
t81 = t79+t80-t83;
t84 = t23.*t81;
t82 = t78-t84;
t85 = t33.*t82;
t86 = t28.*t81;
t87 = t23.*t75;
t88 = t86+t87;
t96 = t37.*t88;
t89 = t85-t96;
t90 = t62.*1.874699728327322e-33;
t91 = t4.*t62.*3.061616997868383e-17;
t92 = t68+t69-t76+t90+t91;
t93 = t33.*t88;
t94 = t37.*t82;
t95 = t93+t94;
t97 = t62.*1.147922510829234e-49;
t98 = t4.*t62.*1.874699728327322e-33;
t99 = t9.*t62.*3.061616997868383e-17;
t100 = t11.*t12.*t62.*3.061616997868383e-17;
t101 = t44.*t89;
t102 = t15.*t70.*6.123233995736766e-18;
t103 = t23.*t81.*1.029e-1;
t105 = t16.*t65.*(1.0./1.0e1);
t106 = t28.*t75.*1.029e-1;
t107 = t15.*t73.*(1.0./1.0e1);
t104 = t102+t103-t105-t106-t107-1.0./2.0e1;
t108 = t11.*t62.*(1.0./2.0);
t109 = t68-t69+t76;
t110 = t63+t64+t66;
t111 = t71-t72;
t112 = t15.*t111;
t113 = t15.*t110;
t114 = t16.*t111;
t121 = t16.*t109.*6.123233995736766e-17;
t115 = t113+t114-t121;
t116 = t23.*t115;
t117 = t16.*t110;
t118 = t15.*t109.*6.123233995736766e-17;
t119 = -t112+t117+t118;
t122 = t28.*t119;
t120 = t116-t122;
t123 = t33.*t120;
t124 = t28.*t115;
t125 = t23.*t119;
t126 = t124+t125;
t127 = t37.*t126;
t128 = t123+t127;
t129 = t68-t69+t76+t90-t91;
t130 = t37.*t120;
t131 = t33.*t126;
t132 = -t130+t131;
t133 = t44.*t132.*6.123233995736766e-17;
t134 = t62.*(1.0./2.0);
t135 = t4.*t9.*t62.*1.874699728327322e-33;
t136 = t62.*7.02899814278107e-66;
t137 = t44.*t128;
t138 = t4.*t62.*1.147922510829234e-49;
t139 = t9.*t62.*1.874699728327322e-33;
t140 = t11.*t12.*t62.*1.874699728327322e-33;
t141 = t16.*t110.*(1.0./1.0e1);
t142 = t15.*t109.*6.123233995736766e-18;
t143 = t28.*t119.*1.029e-1;
t145 = t23.*t115.*1.029e-1;
t146 = t15.*t111.*(1.0./1.0e1);
t144 = t141+t142+t143-t145-t146-1.7e1./2.0e2;
t147 = t4.*t9.*t62.*5.187461826437308e-51;
chain_leg_R = reshape([t4,t108,-t108,0.0,-t11,t72,-t72,0.0,0.0,t134,t134,0.0,0.0,-1.0./2.0e1,-1.7e1./2.0e2,1.0,t13,t65,-t63-t64-t66,0.0,-t17-t18,-t68-t69+t76,-t68+t69-t76,0.0,-t11,t73,t111,0.0,0.0,-1.0./2.0e1,-1.7e1./2.0e2,1.0,t22-t11.*t15-t15.*t19.*6.123233995736766e-17,t75,t112-t15.*t109.*6.123233995736766e-17-t16.*t110,0.0,-t24-t25-t26,-t79-t80+t83,t115,0.0,t45,t92,t129,0.0,0.0,-1.0./2.0e1,-1.7e1./2.0e2,1.0,-t23.*t27-t28.*t31,t82,t120,0.0,t39,-t86-t87,t126,0.0,t45,t92,t129,0.0,t56+t57-t13.*t16.*(1.0./1.0e1),t102-t16.*t65.*(1.0./1.0e1)-t15.*t73.*(1.0./1.0e1)-1.0./2.0e1,t141+t142-t15.*t111.*(1.0./1.0e1)-1.7e1./2.0e2,1.0,t49,t89,t128,0.0,t48,-t93-t94,t132,0.0,t45,t92,t129,0.0,t60,t104,t144,1.0,t55-t42.*t45+t44.*t49,t101-t42.*t92-t42.*t95.*6.123233995736766e-17,t137-t42.*t129-t42.*(t130-t33.*t126).*6.123233995736766e-17,0.0,t52-t44.*t45-t42.*t49,-t42.*t89-t44.*t92-t44.*t95.*6.123233995736766e-17,t133-t42.*t128-t44.*t129,0.0,t11.*(-3.749399456654644e-33)+t46+t47+t53+t54,-t93-t94+t97+t98+t99+t100-t4.*t9.*t62.*1.874699728327322e-33,t97-t98+t99-t100-t130+t131+t135,0.0,t60,t104,t144,1.0,t11.*(-3.749399456654644e-33)+t46+t47+t53+t54+t42.*t45.*6.123233995736766e-17-t44.*t45.*7.498798913309288e-33-t42.*t48.*3.749399456654644e-33-t42.*t49.*7.498798913309288e-33+t44.*t48.*4.591690043316935e-49-t44.*t49.*6.123233995736766e-17,-t93-t94+t97+t98+t99+t100-t42.*t89.*7.498798913309288e-33-t44.*t89.*6.123233995736766e-17+t42.*t92.*6.123233995736766e-17-t44.*t92.*7.498798913309288e-33+t42.*t95.*3.749399456654644e-33-t44.*t95.*4.591690043316935e-49-t4.*t9.*t62.*1.874699728327322e-33,t97-t98+t99-t100-t130+t131+t135-t42.*t128.*7.498798913309288e-33+t42.*t129.*6.123233995736766e-17-t44.*t128.*6.123233995736766e-17-t44.*t129.*7.498798913309288e-33+t42.*(t130-t131).*3.749399456654644e-33-t44.*(t130-t131).*4.591690043316935e-49,0.0,-t52+t42.*t45.*1.224646799147353e-16+t44.*t45-t42.*t48.*7.498798913309288e-33-t44.*t49.*1.224646799147353e-16+t42.*(t40-t51),t42.*t89-t44.*t89.*1.224646799147353e-16+t42.*t92.*1.224646799147353e-16+t44.*t92+t42.*t95.*7.498798913309288e-33+t44.*t95.*6.123233995736766e-17,-t133+t42.*t128+t42.*t129.*1.224646799147353e-16-t44.*t128.*1.224646799147353e-16+t44.*t129-t42.*t132.*7.498798913309288e-33,0.0,t11.*(-2.295845021658468e-49)+t55+t4.*t12.*3.749399456654644e-33+t9.*t11.*2.295845021658468e-49+t33.*t39.*6.123233995736766e-17+t36.*t37.*6.123233995736766e-17-t42.*t45+t44.*t45.*1.224646799147353e-16-t44.*t48.*7.498798913309288e-33+t42.*(t40-t51).*1.224646799147353e-16+t44.*(t40-t51),t101+t136+t138+t139+t140-t37.*t82.*6.123233995736766e-17-t33.*t88.*6.123233995736766e-17+t42.*t89.*1.224646799147353e-16-t42.*t92+t44.*t92.*1.224646799147353e-16-t42.*t95.*6.123233995736766e-17+t44.*t95.*7.498798913309288e-33-t4.*t9.*t62.*1.147922510829234e-49,t136+t137-t138+t139-t140-t37.*t120.*6.123233995736766e-17+t33.*t126.*6.123233995736766e-17+t42.*t128.*1.224646799147353e-16-t42.*t129+t44.*t129.*1.224646799147353e-16+t42.*t132.*6.123233995736766e-17-t44.*t132.*7.498798913309288e-33+t4.*t9.*t62.*1.147922510829234e-49,0.0,t11.*1.037492365287462e-50+t56+t57+t58+t59-t61-t4.*t12.*1.694353614462234e-34-t9.*t11.*1.037492365287462e-50-t33.*t39.*2.767089442673445e-18-t36.*t37.*2.767089442673445e-18+t42.*t45.*4.519e-2-t44.*t45.*5.534178885346889e-18-t42.*t48.*2.767089442673445e-18-t42.*t49.*5.534178885346889e-18+t44.*t48.*3.388707228924467e-34-t44.*t49.*4.519e-2,t62.*(-3.176404260722766e-67)+t102+t103-t105-t106-t107+t147-t4.*t62.*5.187461826437308e-51-t9.*t62.*8.471768072311169e-35+t37.*t82.*2.767089442673445e-18+t33.*t88.*2.767089442673445e-18-t42.*t89.*5.534178885346889e-18-t44.*t89.*4.519e-2+t42.*t92.*4.519e-2-t44.*t92.*5.534178885346889e-18+t42.*t95.*2.767089442673445e-18-t44.*t95.*3.388707228924467e-34-t11.*t12.*t62.*8.471768072311169e-35-1.0./2.0e1,t62.*(-3.176404260722766e-67)+t141+t142+t143-t145-t146-t147+t4.*t62.*5.187461826437308e-51-t9.*t62.*8.471768072311169e-35+t37.*t120.*2.767089442673445e-18-t33.*t126.*2.767089442673445e-18-t42.*t128.*5.534178885346889e-18+t42.*t129.*4.519e-2-t44.*t128.*4.519e-2-t44.*t129.*5.534178885346889e-18-t42.*t132.*2.767089442673445e-18+t44.*t132.*3.388707228924467e-34+t11.*t12.*t62.*8.471768072311169e-35-1.7e1./2.0e2,1.0],[4, 4, 7]);
if nargout > 1
t148 = conj(LL1);
t149 = -t5+t148;
t150 = cos(t149);
t151 = conj(LL2);
t152 = t8+t151;
t153 = cos(t152);
t154 = t150.*t153;
t155 = sin(t149);
t156 = sin(t152);
t164 = t155.*t156.*6.123233995736766e-17;
t157 = t154-t164;
t158 = conj(LL3);
t159 = sin(t158);
t160 = cos(t158);
t161 = t150.*t156;
t162 = t153.*t155.*6.123233995736766e-17;
t163 = t161+t162;
t165 = conj(LL4);
t166 = t157.*t160;
t167 = sin(t165);
t168 = t160.*t163.*6.123233995736766e-17;
t169 = t157.*t159;
t170 = t155.*t160;
t171 = t168+t169+t170;
t172 = cos(t165);
t173 = t159.*t163.*6.123233995736766e-17;
t174 = t155.*t159;
t175 = -t166+t173+t174;
t176 = conj(LL5);
t177 = cos(t176);
t178 = t167.*t171;
t179 = t172.*t175;
t180 = t178+t179;
t181 = sin(t176);
t182 = t167.*t175;
t186 = t171.*t172;
t183 = t182-t186;
t184 = t181.*t183;
t185 = conj(LL6);
t187 = sin(t185);
t188 = cos(t185);
t192 = t177.*t180;
t189 = t184-t192;
t190 = t177.*t183;
t191 = t180.*t181;
t193 = t190+t191;
t195 = t155.*6.123233995736766e-17;
t194 = t161+t162-t195;
t196 = t177.*(t182-t186);
t197 = t150.*t156.*6.123233995736766e-17;
t198 = t153.*t155.*3.749399456654644e-33;
t199 = t159.*t163.*6.123233995736766e-18;
t200 = t155.*t159.*(1.0./1.0e1);
t201 = t167.*t171.*1.029e-1;
t202 = t172.*t175.*1.029e-1;
t204 = t157.*t160.*(1.0./1.0e1);
t203 = t199+t200+t201+t202-t204;
t205 = t62.*t153.*(1.0./2.0);
t206 = t62.*t150.*t153.*3.061616997868383e-17;
t213 = t62.*t155.*t156.*(1.0./2.0);
t207 = t205+t206-t213;
t208 = t62.*t156.*(1.0./2.0);
t209 = t62.*t150.*t156.*3.061616997868383e-17;
t210 = t62.*t153.*t155.*(1.0./2.0);
t211 = t208+t209+t210;
t214 = t62.*t150.*(1.0./2.0);
t212 = t71-t214;
t215 = t159.*t212;
t216 = t159.*t211;
t217 = t160.*t212;
t224 = t160.*t207.*6.123233995736766e-17;
t218 = t216+t217-t224;
t219 = t167.*t218;
t220 = t160.*t211;
t221 = t159.*t207.*6.123233995736766e-17;
t222 = -t215+t220+t221;
t225 = t172.*t222;
t223 = t219-t225;
t226 = t177.*t223;
t227 = t167.*t222;
t228 = t172.*t218;
t229 = t227+t228;
t230 = t181.*t229;
t231 = t226+t230;
t233 = t62.*t150.*3.061616997868383e-17;
t232 = t90+t205+t206-t213-t233;
t234 = t181.*t223;
t236 = t177.*t229;
t235 = t234-t236;
t237 = -t234+t236;
t238 = t188.*t237.*6.123233995736766e-17;
t239 = t62.*t153.*3.061616997868383e-17;
t240 = t62.*t150.*t153.*1.874699728327322e-33;
t241 = t188.*t231;
t242 = t160.*t211.*(1.0./1.0e1);
t243 = t159.*t207.*6.123233995736766e-18;
t244 = t172.*t222.*1.029e-1;
t246 = t159.*t212.*(1.0./1.0e1);
t247 = t167.*t218.*1.029e-1;
t245 = t242+t243+t244-t246-t247+1.0./2.0e1;
t248 = -t208+t209+t210;
t249 = t205-t206+t213;
t250 = t159.*t249.*6.123233995736766e-17;
t251 = t71+t214;
t252 = t160.*t248;
t253 = t159.*t251;
t254 = -t250+t252+t253;
t255 = t160.*t249.*6.123233995736766e-17;
t256 = t159.*t248;
t259 = t160.*t251;
t257 = t255+t256-t259;
t258 = t167.*t257;
t266 = t172.*t254;
t260 = t258-t266;
t261 = t177.*t260;
t262 = t167.*t254;
t263 = t172.*t257;
t264 = t262+t263;
t265 = t181.*t264;
t267 = t90+t205-t206+t213+t233;
t268 = t177.*t264;
t271 = t181.*t260;
t269 = t268-t271;
t270 = t261+t265;
t272 = t188.*t267;
t273 = t188.*t269.*6.123233995736766e-17;
t274 = -t90-t205+t206-t213-t233;
t275 = t187.*t267;
t276 = t187.*t269.*6.123233995736766e-17;
t277 = t62.*t153.*1.874699728327322e-33;
t278 = t62.*t150.*t153.*1.147922510829234e-49;
t279 = t160.*t248.*(1.0./1.0e1);
t280 = t159.*t251.*(1.0./1.0e1);
t281 = t172.*t254.*1.029e-1;
t283 = t159.*t249.*6.123233995736766e-18;
t285 = t167.*t257.*1.029e-1;
t282 = t279+t280+t281-t283-t285-1.7e1./2.0e2;
t284 = t62.*t150.*5.187461826437308e-51;
t286 = t62.*t155.*t156.*8.471768072311169e-35;
chain_leg_L = reshape([t150,t62.*t155.*(-1.0./2.0),t62.*t155.*(-1.0./2.0),0.0,-t155,-t214,-t214,0.0,0.0,t134,-t134,0.0,0.0,1.0./2.0e1,-1.7e1./2.0e2,1.0,t157,t62.*t156.*(-1.0./2.0)-t62.*t150.*t156.*3.061616997868383e-17-t62.*t153.*t155.*(1.0./2.0),t208-t209-t210,0.0,-t161-t162,-t205-t206+t213,t249,0.0,-t155,t212,-t71-t214,0.0,0.0,1.0./2.0e1,-1.7e1./2.0e2,1.0,t166-t155.*t159-t159.*t163.*6.123233995736766e-17,t215-t159.*t207.*6.123233995736766e-17-t160.*t211,t250-t160.*t248-t159.*t251,0.0,-t168-t169-t170,t218,t257,0.0,t194,t232,t274,0.0,0.0,1.0./2.0e1,-1.7e1./2.0e2,1.0,-t167.*t171-t172.*t175,t223,t260,0.0,t183,t229,t264,0.0,t194,t232,t274,0.0,t199+t200-t157.*t160.*(1.0./1.0e1),t242+t243-t159.*t212.*(1.0./1.0e1)+1.0./2.0e1,t279+t280-t159.*t249.*6.123233995736766e-18-1.7e1./2.0e2,1.0,t189,t231,t270,0.0,t191+t196,t237,t269,0.0,t194,t232,t274,0.0,t203,t245,t282,1.0,t188.*t189+t187.*t193.*6.123233995736766e-17-t187.*t194,t241-t187.*t232-t187.*t235.*6.123233995736766e-17,t275+t276+t188.*t270,0.0,t188.*(t190+t191).*6.123233995736766e-17-t187.*t189-t188.*t194,t238-t187.*t231-t188.*t232,t272+t273-t187.*t270,0.0,t155.*(-3.749399456654644e-33)+t191+t196+t197+t198,t97-t234+t236+t239+t240-t62.*t150.*1.874699728327322e-33-t62.*t155.*t156.*3.061616997868383e-17,-t97-t239+t240+t268-t271-t62.*t150.*1.874699728327322e-33-t62.*t155.*t156.*3.061616997868383e-17,0.0,t203,t245,t282,1.0,t155.*(-3.749399456654644e-33)+t191+t196+t197+t198-t187.*t189.*7.498798913309288e-33-t188.*t189.*6.123233995736766e-17-t187.*t193.*3.749399456654644e-33+t187.*t194.*6.123233995736766e-17+t188.*t193.*4.591690043316935e-49-t188.*t194.*7.498798913309288e-33,t97-t234+t236+t239+t240-t62.*t150.*1.874699728327322e-33-t187.*t231.*7.498798913309288e-33+t187.*t232.*6.123233995736766e-17-t188.*t231.*6.123233995736766e-17-t188.*t232.*7.498798913309288e-33-t188.*t235.*4.591690043316935e-49+t187.*(t234-t236).*3.749399456654644e-33-t62.*t155.*t156.*3.061616997868383e-17,-t97-t239+t240+t268-t271-t62.*t150.*1.874699728327322e-33-t187.*t267.*6.123233995736766e-17+t188.*t267.*7.498798913309288e-33-t187.*t269.*3.749399456654644e-33-t187.*t270.*7.498798913309288e-33+t188.*t269.*4.591690043316935e-49-t188.*t270.*6.123233995736766e-17-t62.*t155.*t156.*3.061616997868383e-17,0.0,t188.*t189.*(-1.224646799147353e-16)-t187.*t193.*7.498798913309288e-33+t187.*t194.*1.224646799147353e-16-t188.*t193.*6.123233995736766e-17+t188.*t194+t187.*(t184-t192),-t238+t187.*t231+t187.*t232.*1.224646799147353e-16-t188.*t231.*1.224646799147353e-16+t188.*t232-t187.*t237.*7.498798913309288e-33,-t272-t273+t187.*(t261+t265)-t187.*t267.*1.224646799147353e-16-t187.*t269.*7.498798913309288e-33-t188.*t270.*1.224646799147353e-16,0.0,t155.*(-2.295845021658468e-49)+t187.*(t190+t191).*6.123233995736766e-17+t150.*t156.*3.749399456654644e-33+t153.*t155.*2.295845021658468e-49+t180.*t181.*6.123233995736766e-17-t187.*t194-t188.*t193.*7.498798913309288e-33+t188.*t194.*1.224646799147353e-16+t177.*(t182-t186).*6.123233995736766e-17+t187.*(t184-t192).*1.224646799147353e-16+t188.*(t184-t192),t136+t241+t277+t278-t62.*t150.*1.147922510829234e-49-t181.*t223.*6.123233995736766e-17+t177.*t229.*6.123233995736766e-17+t187.*t231.*1.224646799147353e-16-t187.*t232+t188.*t232.*1.224646799147353e-16+t187.*t237.*6.123233995736766e-17-t188.*t237.*7.498798913309288e-33-t62.*t155.*t156.*1.874699728327322e-33,-t136+t275+t276-t277+t278+t187.*(t261+t265).*1.224646799147353e-16+t188.*(t261+t265)-t62.*t150.*1.147922510829234e-49+t177.*t264.*6.123233995736766e-17-t181.*t260.*6.123233995736766e-17-t188.*t267.*1.224646799147353e-16-t188.*t269.*7.498798913309288e-33-t62.*t155.*t156.*1.874699728327322e-33,0.0,t155.*1.037492365287462e-50+t199+t200+t201+t202-t204+t188.*(t190+t191).*3.388707228924467e-34-t150.*t156.*1.694353614462234e-34-t153.*t155.*1.037492365287462e-50-t177.*t183.*2.767089442673445e-18-t180.*t181.*2.767089442673445e-18-t187.*t189.*5.534178885346889e-18-t188.*t189.*4.519e-2-t187.*t193.*2.767089442673445e-18+t187.*t194.*4.519e-2-t188.*t194.*5.534178885346889e-18,t62.*(-3.176404260722766e-67)+t242+t243+t244-t246-t247+t284+t286-t62.*t153.*8.471768072311169e-35+t181.*t223.*2.767089442673445e-18-t177.*t229.*2.767089442673445e-18-t187.*t231.*5.534178885346889e-18+t187.*t232.*4.519e-2-t188.*t231.*4.519e-2-t188.*t232.*5.534178885346889e-18-t187.*t237.*2.767089442673445e-18+t188.*t237.*3.388707228924467e-34-t62.*t150.*t153.*5.187461826437308e-51+1.0./2.0e1,t62.*3.176404260722766e-67+t279+t280+t281-t283+t284-t285+t286+t62.*t153.*8.471768072311169e-35-t177.*t264.*2.767089442673445e-18-t187.*t267.*4.519e-2+t188.*t267.*5.534178885346889e-18-t187.*t269.*2.767089442673445e-18-t187.*t270.*5.534178885346889e-18+t188.*t269.*3.388707228924467e-34-t188.*t270.*4.519e-2+t181.*(t258-t266).*2.767089442673445e-18-t62.*t150.*t153.*5.187461826437308e-51-1.7e1./2.0e2,1.0],[4, 4, 7]);
end
if nargout > 2
t287 = conj(RA1);
t288 = cos(t287);
t289 = conj(RA2);
t290 = t5+t289;
t291 = cos(t290);
t292 = t288.*t291;
t293 = sin(t287);
t294 = sin(t290);
t302 = t293.*t294.*6.123233995736766e-17;
t295 = t292-t302;
t296 = conj(RA3);
t297 = sin(t296);
t298 = t288.*t294;
t299 = t291.*t293.*6.123233995736766e-17;
t300 = conj(RA4);
t301 = cos(t296);
t303 = t295.*t301;
t304 = t298+t299;
t305 = t297.*t304.*6.123233995736766e-17;
t306 = t293.*t297;
t307 = t303+t305+t306;
t308 = sin(t300);
t309 = cos(t300);
t311 = t293.*6.123233995736766e-17;
t310 = t298+t299-t311;
t312 = t301.*t304.*6.123233995736766e-17;
t313 = t293.*t301;
t315 = t295.*t297;
t314 = t312+t313-t315;
t316 = t293.*3.749399456654644e-33;
t346 = t288.*t294.*6.123233995736766e-17;
t317 = t312+t313-t315+t316-t346-t291.*t293.*3.749399456654644e-33;
t318 = t288.*t294.*(2.1e1./2.0e2);
t319 = t291.*t293.*6.429395695523604e-18;
t321 = t293.*6.429395695523604e-18;
t320 = t318+t319-t321;
t322 = t288.*t294.*3.749399456654644e-33;
t323 = t294+t299+t322;
t324 = t288.*t291.*3.749399456654644e-33;
t325 = t288.*6.123233995736766e-17;
t326 = t325-6.123233995736766e-17;
t327 = t291-t302+t324;
t328 = t301.*t323;
t337 = t288.*3.749399456654644e-33;
t329 = t291-t302+t324-t337+3.749399456654644e-33;
t330 = t297.*t323;
t331 = t301.*t326;
t332 = t301.*t327.*6.123233995736766e-17;
t333 = t330+t331+t332;
t334 = t297.*t326;
t335 = t297.*t327.*6.123233995736766e-17;
t336 = -t328+t334+t335;
t338 = t291.*6.123233995736766e-17;
t339 = t288.*t291.*2.295845021658468e-49;
t340 = t288.*(-2.295845021658468e-49)-t330-t331-t332+t338+t339-t293.*t294.*3.749399456654644e-33+2.295845021658468e-49;
t341 = t288.*3.936869429487376e-34;
t342 = t293.*t294.*6.429395695523604e-18;
t344 = t291.*(2.1e1./2.0e2);
t345 = t288.*t291.*3.936869429487376e-34;
t343 = t341+t342-t344-t345-1.13e-1;
t347 = t294.*6.123233995736766e-17;
t348 = t293.*t294;
t353 = t288.*t291.*6.123233995736766e-17;
t349 = t338+t348-t353;
t350 = t288+3.749399456654644e-33;
t351 = t291.*t293;
t352 = t346-t347+t351;
t354 = t297.*t350;
t355 = t325+t338+t348-t353+2.295845021658468e-49;
t356 = t301.*t350;
t357 = t297.*t352;
t359 = t301.*t349.*6.123233995736766e-17;
t358 = t356+t357-t359;
t360 = t301.*t352;
t361 = t297.*t349.*6.123233995736766e-17;
t362 = -t354+t360+t361;
t363 = t291.*3.749399456654644e-33;
t364 = t302-t324+t337+t356+t357-t359+t363+1.405799628556214e-65;
t365 = t288.*t291.*6.429395695523604e-18;
t367 = t288.*6.429395695523604e-18;
t368 = t291.*6.429395695523604e-18;
t369 = t293.*t294.*(2.1e1./2.0e2);
t366 = t365-t367-t368-t369+1.0e-1;
chain_arm_R = reshape([t288,t311,-t293,0.0,-t293,t325,-t288,0.0,0.0,1.0,6.123233995736766e-17,0.0,0.0,-1.13e2./1.0e3,1.0./1.0e1,1.0,t295,t323,-t346+t347-t291.*t293,0.0,-t298-t299,t327,t349,0.0,t293,-t325+6.123233995736766e-17,t350,0.0,0.0,-1.13e2./1.0e3,1.0./1.0e1,1.0,t307,t328-t297.*t326-t297.*t327.*6.123233995736766e-17,t354-t297.*t349.*6.123233995736766e-17-t301.*t352,0.0,-t312-t313+t315,t333,-t356-t357+t359,0.0,-t298-t299+t311,t329,t355,0.0,t320,t343,t366,1.0,t307.*t309-t308.*t310-t308.*t314.*6.123233995736766e-17,t308.*t329+t308.*t333.*6.123233995736766e-17-t309.*t336,t308.*t358.*(-6.123233995736766e-17)-t309.*t362+t308.*(t325+t338+t348-t288.*t291.*6.123233995736766e-17+2.295845021658468e-49),0.0,-t307.*t308-t309.*t310-t309.*t314.*6.123233995736766e-17,t309.*t329+t309.*t333.*6.123233995736766e-17+t308.*t336,t309.*t355-t309.*t358.*6.123233995736766e-17+t308.*t362,0.0,t317,t340,t364,0.0,t320,t343,t366,1.0,t307.*t308.*1.0+t307.*t309.*6.123233995736766e-17-t308.*t310.*6.123233995736766e-17+t309.*t310.*1.0-t308.*t314.*3.749399456654644e-33+t309.*t314.*6.123233995736766e-17,t308.*t329.*6.123233995736766e-17-t309.*t329.*1.0+t308.*t333.*3.749399456654644e-33-t309.*t333.*6.123233995736766e-17-t308.*t336.*1.0-t309.*t336.*6.123233995736766e-17,t308.*t355.*6.123233995736766e-17-t309.*t355.*1.0-t308.*t358.*3.749399456654644e-33+t309.*t358.*6.123233995736766e-17-t308.*t362.*1.0-t309.*t362.*6.123233995736766e-17,0.0,t307.*t308.*(-6.123233995736766e-17)+t307.*t309.*1.0-t308.*t310.*1.0-t309.*t310.*6.123233995736766e-17-t308.*t314.*6.123233995736766e-17-t309.*t314.*3.749399456654644e-33,t308.*t329.*1.0+t309.*t329.*6.123233995736766e-17+t308.*t333.*6.123233995736766e-17+t309.*t333.*3.749399456654644e-33+t308.*t336.*6.123233995736766e-17-t309.*t336.*1.0,t308.*t355.*1.0+t309.*t355.*6.123233995736766e-17-t308.*t358.*6.123233995736766e-17-t309.*t358.*3.749399456654644e-33+t308.*t362.*6.123233995736766e-17-t309.*t362.*1.0,0.0,t317,t340,t364,0.0,t318+t319-t321+t307.*t308.*1.137e-1-t307.*t309.*6.962117053152703e-18+t308.*t310.*6.962117053152703e-18+t309.*t310.*1.137e-1+t308.*t314.*4.26306718221633e-34+t309.*t314.*6.962117053152703e-18,t341+t342-t344-t345-t308.*t329.*6.962117053152703e-18-t309.*t329.*1.137e-1-t308.*t333.*4.26306718221633e-34-t309.*t333.*6.962117053152703e-18-t308.*t336.*1.137e-1+t309.*t336.*6.962117053152703e-18-1.13e-1,t365-t367-t368-t369-t308.*t355.*6.962117053152703e-18-t309.*t355.*1.137e-1+t308.*t358.*4.26306718221633e-34+t309.*t358.*6.962117053152703e-18-t308.*t362.*1.137e-1+t309.*t362.*6.962117053152703e-18+1.0e-1,1.0],[4, 4, 5]);
end
if nargout > 3
t370 = conj(LA1);
t371 = cos(t370);
t372 = conj(LA2);
t373 = -t5+t372;
t374 = cos(t373);
t375 = t371.*t374;
t376 = sin(t370);
t377 = sin(t373);
t387 = t376.*t377.*6.123233995736766e-17;
t378 = t375-t387;
t379 = conj(LA3);
t380 = sin(t379);
t381 = t371.*t377;
t382 = t374.*t376.*6.123233995736766e-17;
t383 = conj(LA4);
t384 = sin(t383);
t385 = cos(t379);
t386 = t381+t382;
t388 = t378.*t385;
t389 = cos(t383);
t391 = t376.*6.123233995736766e-17;
t390 = t381+t382-t391;
t392 = t385.*t386.*6.123233995736766e-17;
t393 = t378.*t380;
t394 = t376.*t385;
t395 = t392+t393+t394;
t396 = t380.*t386.*6.123233995736766e-17;
t397 = t376.*t380;
t398 = -t388+t396+t397;
t399 = t384.*t398;
t400 = t376.*3.749399456654644e-33;
t435 = t371.*t377.*6.123233995736766e-17;
t401 = t392+t393+t394+t400-t435-t374.*t376.*3.749399456654644e-33;
t402 = t376.*6.429395695523604e-18;
t404 = t371.*t377.*(2.1e1./2.0e2);
t405 = t374.*t376.*6.429395695523604e-18;
t403 = t402-t404-t405;
t406 = t371.*t377.*3.749399456654644e-33;
t407 = t377+t382+t406;
t408 = t371.*6.123233995736766e-17;
t409 = t408-6.123233995736766e-17;
t410 = t380.*t409;
t411 = t385.*t407;
t412 = t371.*t374.*3.749399456654644e-33;
t413 = t374-t387+t412;
t414 = t380.*t413.*6.123233995736766e-17;
t415 = t410+t411+t414;
t416 = t385.*t409;
t417 = t385.*t413.*6.123233995736766e-17;
t419 = t380.*t407;
t418 = t416+t417-t419;
t421 = t371.*3.749399456654644e-33;
t420 = t374-t387+t412-t421+3.749399456654644e-33;
t422 = t389.*t418.*6.123233995736766e-17;
t423 = t389.*t420;
t424 = t389.*t415;
t425 = t384.*t418.*6.123233995736766e-17;
t426 = t384.*t420;
t427 = t374.*6.123233995736766e-17;
t428 = t371.*t374.*2.295845021658468e-49;
t429 = t371.*(-2.295845021658468e-49)-t416-t417+t419+t427+t428-t376.*t377.*3.749399456654644e-33+2.295845021658468e-49;
t430 = t374.*(2.1e1./2.0e2);
t431 = t371.*t374.*3.936869429487376e-34;
t433 = t371.*3.936869429487376e-34;
t434 = t376.*t377.*6.429395695523604e-18;
t432 = t430+t431-t433-t434+1.13e-1;
t436 = t377.*6.123233995736766e-17;
t437 = t376.*t377;
t438 = t374.*t376;
t439 = t435-t436+t438;
t443 = t371.*t374.*6.123233995736766e-17;
t440 = t427+t437-t443;
t441 = t380.*t440.*6.123233995736766e-17;
t442 = t371+3.749399456654644e-33;
t444 = t385.*t440.*6.123233995736766e-17;
t445 = t380.*t439;
t451 = t385.*t442;
t446 = t444+t445-t451;
t447 = t408+t427+t437-t443+2.295845021658468e-49;
t448 = t385.*t439;
t449 = t380.*t442;
t450 = -t441+t448+t449;
t452 = t389.*t447;
t453 = t389.*t446.*6.123233995736766e-17;
t454 = t384.*t450;
t455 = t384.*t446.*6.123233995736766e-17;
t456 = t374.*3.749399456654644e-33;
t457 = t387-t412+t421-t444-t445+t451+t456+1.405799628556214e-65;
t458 = t371.*6.429395695523604e-18;
t459 = t374.*6.429395695523604e-18;
t460 = t376.*t377.*(2.1e1./2.0e2);
t462 = t371.*t374.*6.429395695523604e-18;
t461 = t458+t459+t460-t462+1.0e-1;
chain_arm_L = reshape([t371,t391,-t376,0.0,-t376,t408,-t371,0.0,0.0,1.0,6.123233995736766e-17,0.0,0.0,1.13e2./1.0e3,1.0./1.0e1,1.0,t378,t407,-t435+t436-t374.*t376,0.0,-t381-t382,t413,t440,0.0,t376,-t408+6.123233995736766e-17,t442,0.0,0.0,1.13e2./1.0e3,1.0./1.0e1,1.0,t388-t376.*t380-t380.*t386.*6.123233995736766e-17,t415,t441-t380.*t442-t385.*t439,0.0,-t392-t393-t394,t418,t446,0.0,-t381-t382+t391,t420,t447,0.0,t403,t432,t461,1.0,-t384.*t390-t384.*t395.*6.123233995736766e-17-t389.*t398,t424+t425+t426,t455-t389.*t450+t384.*(t408+t427+t437-t371.*t374.*6.123233995736766e-17+2.295845021658468e-49),0.0,t399-t389.*t390-t389.*t395.*6.123233995736766e-17,t422+t423-t384.*t415,t452+t453+t454,0.0,t401,t429,t457,0.0,t403,t432,t461,1.0,t399-t384.*t390.*6.123233995736766e-17-t384.*t395.*3.749399456654644e-33-t389.*t390-t389.*t395.*6.123233995736766e-17-t389.*t398.*6.123233995736766e-17,t422+t423-t384.*t415+t384.*t418.*3.749399456654644e-33+t384.*t420.*6.123233995736766e-17+t389.*t415.*6.123233995736766e-17,t452+t453+t454+t384.*t446.*3.749399456654644e-33+t384.*t447.*6.123233995736766e-17-t389.*t450.*6.123233995736766e-17,0.0,t384.*t390+t384.*t395.*6.123233995736766e-17-t389.*t390.*6.123233995736766e-17+t384.*t398.*6.123233995736766e-17-t389.*t395.*3.749399456654644e-33+t389.*t398,-t424-t425-t426-t384.*t415.*6.123233995736766e-17+t389.*t418.*3.749399456654644e-33+t389.*t420.*6.123233995736766e-17,-t455-t384.*t447+t384.*t450.*6.123233995736766e-17+t389.*t446.*3.749399456654644e-33+t389.*t447.*6.123233995736766e-17+t389.*t450,0.0,t401,t429,t457,0.0,t402-t404-t405-t384.*t390.*6.962117053152703e-18-t384.*t395.*4.26306718221633e-34-t389.*t390.*1.137e-1+t384.*t398.*1.137e-1-t389.*t395.*6.962117053152703e-18-t389.*t398.*6.962117053152703e-18,t430+t431-t433-t434-t384.*t415.*1.137e-1+t384.*t418.*4.26306718221633e-34+t384.*t420.*6.962117053152703e-18+t389.*t415.*6.962117053152703e-18+t389.*t418.*6.962117053152703e-18+t389.*t420.*1.137e-1+1.13e-1,t458+t459+t460-t462+t384.*t446.*4.26306718221633e-34+t384.*t447.*6.962117053152703e-18+t384.*t450.*1.137e-1+t389.*t446.*6.962117053152703e-18+t389.*t447.*1.137e-1-t389.*t450.*6.962117053152703e-18+1.0e-1,1.0],[4, 4, 5]);
end
if nargout > 4
t463 = conj(H1);
t464 = cos(t463);
t465 = conj(H2);
t466 = -t5+t465;
t467 = sin(t463);
t468 = cos(t466);
t469 = sin(t466);
t470 = t464.*t468;
t471 = t464.*t469.*6.123233995736766e-17;
t472 = t464.*t468.*6.123233995736766e-17;
t473 = t464.*t468.*3.749399456654644e-33;
t474 = t467.*t468;
chain_head = reshape([t464,t467,0.0,0.0,-t467,t464,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.265e-1,1.0,t470-t467.*t469.*6.123233995736766e-17,t471+t474,-t469,0.0,-t464.*t469-t467.*t468.*6.123233995736766e-17,t472-t467.*t469,-t468,0.0,-t467,t464,6.123233995736766e-17,0.0,0.0,0.0,1.265e-1,1.0,t467.*6.123233995736766e-17+t472-t464.*t469-t467.*t468.*6.123233995736766e-17-t467.*t469.*3.749399456654644e-33,t464.*(-6.123233995736766e-17)+t472+t464.*t469.*3.749399456654644e-33+t467.*t468.*6.123233995736766e-17-t467.*t469,-t468-t469.*6.123233995736766e-17-3.749399456654644e-33,0.0,-t467-t464.*t469.*6.123233995736766e-17-t467.*t468.*3.749399456654644e-33,t464+t473-t467.*t469.*6.123233995736766e-17,t468.*(-6.123233995736766e-17)+6.123233995736766e-17,0.0,t467.*(-3.749399456654644e-33)+t470+t471+t467.*t468.*3.749399456654644e-33-t467.*t469.*6.123233995736766e-17,t464.*3.749399456654644e-33+t471-t473+t474+t467.*t469.*6.123233995736766e-17,t468.*6.123233995736766e-17-t469+2.295845021658468e-49,0.0,t467.*3.300423123702117e-18+t464.*t468.*6.79e-2-t464.*t469.*5.39e-2-t467.*t468.*3.300423123702117e-18-t467.*t469.*4.157675883105264e-18,t464.*(-3.300423123702117e-18)+t464.*t468.*3.300423123702117e-18+t464.*t469.*4.157675883105264e-18+t467.*t468.*6.79e-2-t467.*t469.*5.39e-2,t468.*(-5.39e-2)-t469.*6.79e-2+1.265e-1,1.0],[4, 4, 3]);
end