mbs_sensor_ArmarIII_simplified_virt_new.c
Go to the documentation of this file.
1 //
2 //-------------------------------------------------------------
3 //
4 // ROBOTRAN - Version 6.6 (build : february 22, 2008)
5 //
6 // Copyright
7 // Universite catholique de Louvain
8 // Departement de Mecanique
9 // Unite de Production Mecanique et Machines
10 // 2, Place du Levant
11 // 1348 Louvain-la-Neuve
12 // http://www.robotran.be//
13 //
14 // ==> Generation Date : Wed Dec 7 15:34:00 2016
15 //
16 // ==> Project name : ArmarIII_simplified_virt
17 // ==> using XML input file
18 //
19 // ==> Number of joints : 56
20 //
21 // ==> Function : F 6 : Sensors Kinematical Informations (sens)
22 // ==> Flops complexity : 11538
23 //
24 // ==> Generation Time : 0.210 seconds
25 // ==> Post-Processing : 0.360 seconds
26 //
27 //-------------------------------------------------------------
28 //
29 
30 #include <math.h>
31 
32 #include "user_hard_param_armar3.h"
33 #include "mbs_sensor2.h"
34 
35 #ifndef M_PI
36 # define M_PI 3.14159265358979323846
37 #endif
38 
40  double* q_dof,
41  double* qd_dof,
42  int isens)
43 {
44 
46 
47 
48  // inital q full of zeros
49  double q[57] = {0.};
50  double qd[57] = {0.};
51  double qdd[57] = {0.};
52 
53  // add input config
54  q[1] = q_dof[1];
55  q[2] = q_dof[2];
56  q[3] = q_dof[3];
57  q[4] = q_dof[4];
58  q[5] = q_dof[5];
59  q[6] = q_dof[6];
60 
61  q[18] = q_dof[7];
62  q[20] = q_dof[8];
63  q[21] = q_dof[9];
64  q[22] = q_dof[10];
65 
66  q[30] = q_dof[11];
67  q[35] = q_dof[12];
68  q[40] = q_dof[13];
69  //q[43] = q_dof[14];
70  q[49] = q_dof[14];
71  q[53] = q_dof[15];
72 
73  q[54] = q_dof[16];
74  q[55] = q_dof[17];
75  q[56] = q_dof[18];
76 
77  qd[1] = qd_dof[1];
78  qd[2] = qd_dof[2];
79  qd[3] = qd_dof[3];
80  qd[4] = qd_dof[4];
81  qd[5] = qd_dof[5];
82  qd[6] = qd_dof[6];
83 
84  qd[18] = qd_dof[7];
85  qd[20] = qd_dof[8];
86  qd[21] = qd_dof[9];
87  qd[22] = qd_dof[10];
88 
89  qd[30] = qd_dof[11];
90  qd[35] = qd_dof[12];
91  qd[40] = qd_dof[13];
92  //qd[43] = qd_dof[14];
93  qd[49] = qd_dof[14];
94  qd[53] = qd_dof[15];
95 
96  qd[54] = qd_dof[16];
97  qd[55] = qd_dof[17];
98  qd[56] = qd_dof[18];
99 
100  // add blocked joints
101  q[7] = -M_PI / 2.;
102  q[9] = 2.35619449019;
103  q[10] = -M_PI / 2.;
104  q[11] = -2.35619449019;
105  q[13] = -2.35619449019;
106  q[14] = -M_PI / 2.;
107 
108  q[15] = 2.35619449019;
109  q[17] = -M_PI / 2.;
110 
111  q[25] = M_PI / 2.;
112  q[27] = M_PI / 2.;
113  q[28] = M_PI / 2.;
114  q[29] = M_PI;
115  q[31] = -M_PI;
116  q[32] = M_PI / 2;
117  q[33] = -1.96153085129E-8;
118  q[34] = M_PI / 2.;
119  q[36] = M_PI / 2.;
120  q[37] = -1.96153085129E-8;
121  q[38] = M_PI / 2.;
122  q[39] = M_PI;
123  q[41] = -M_PI;
124  q[42] = M_PI / 2.;
125  q[44] = -M_PI / 2.;
126  q[46] = -M_PI / 2.;
127  q[48] = M_PI;
128  q[50] = -M_PI;
129  q[51] = -M_PI / 2.;
130 
131  // === begin imp_aux ===
132 
133  // === end imp_aux ===
134 
135  // ===== BEGIN task 0 =====
136 
137  // Sensor Kinematics
138 
139 
140 
141  // = = Block_0_0_0_0_0_1 = =
142 
143  // Augmented Joint Position Vectors
144 
145  Dz233 = q[23] + DPT_3_9;
146  Dz243 = q[24] + DPT_3_10;
147 
148  // Trigonometric Variables
149 
150  C4 = cos(q[4]);
151  S4 = sin(q[4]);
152  C5 = cos(q[5]);
153  S5 = sin(q[5]);
154  C6 = cos(q[6]);
155  S6 = sin(q[6]);
156  C7 = cos(q[7]);
157  S7 = sin(q[7]);
158  C9 = cos(q[9]);
159  S9 = sin(q[9]);
160  C10 = cos(q[10]);
161  S10 = sin(q[10]);
162  C11 = cos(q[11]);
163  S11 = sin(q[11]);
164  C13 = cos(q[13]);
165  S13 = sin(q[13]);
166  C14 = cos(q[14]);
167  S14 = sin(q[14]);
168  C15 = cos(q[15]);
169  S15 = sin(q[15]);
170  C17 = cos(q[17]);
171  S17 = sin(q[17]);
172  C18 = cos(q[18]);
173  S18 = sin(q[18]);
174  C20 = cos(q[20]);
175  S20 = sin(q[20]);
176  C21 = cos(q[21]);
177  S21 = sin(q[21]);
178  C22 = cos(q[22]);
179  S22 = sin(q[22]);
180  C25 = cos(q[25]);
181  S25 = sin(q[25]);
182  C27 = cos(q[27]);
183  S27 = sin(q[27]);
184  C28 = cos(q[28]);
185  S28 = sin(q[28]);
186  C29 = cos(q[29]);
187  S29 = sin(q[29]);
188  C30 = cos(q[30]);
189  S30 = sin(q[30]);
190  C31 = cos(q[31]);
191  S31 = sin(q[31]);
192  C32 = cos(q[32]);
193  S32 = sin(q[32]);
194  C33 = cos(q[33]);
195  S33 = sin(q[33]);
196  C34 = cos(q[34]);
197  S34 = sin(q[34]);
198  C35 = cos(q[35]);
199  S35 = sin(q[35]);
200  C36 = cos(q[36]);
201  S36 = sin(q[36]);
202  C37 = cos(q[37]);
203  S37 = sin(q[37]);
204  C38 = cos(q[38]);
205  S38 = sin(q[38]);
206  C39 = cos(q[39]);
207  S39 = sin(q[39]);
208  C40 = cos(q[40]);
209  S40 = sin(q[40]);
210  C41 = cos(q[41]);
211  S41 = sin(q[41]);
212  C42 = cos(q[42]);
213  S42 = sin(q[42]);
214  C43 = cos(q[43]);
215  S43 = sin(q[43]);
216  C44 = cos(q[44]);
217  S44 = sin(q[44]);
218  C46 = cos(q[46]);
219  S46 = sin(q[46]);
220  C48 = cos(q[48]);
221  S48 = sin(q[48]);
222  C49 = cos(q[49]);
223  S49 = sin(q[49]);
224  C50 = cos(q[50]);
225  S50 = sin(q[50]);
226  C51 = cos(q[51]);
227  S51 = sin(q[51]);
228  C53 = cos(q[53]);
229  S53 = sin(q[53]);
230  C55 = cos(q[55]);
231  S55 = sin(q[55]);
232  C56 = cos(q[56]);
233  S56 = sin(q[56]);
234 
235  // ====== END Task 0 ======
236 
237  // ===== BEGIN task 1 =====
238 
239  switch (isens)
240  {
241 
242  //
243  break;
244  case 1:
245 
246 
247 
248  // = = Block_1_0_0_1_0_1 = =
249 
250  // Sensor Kinematics
251 
252 
253  ROcp0_25 = S4 * S5;
254  ROcp0_35 = -C4 * S5;
255  ROcp0_85 = -S4 * C5;
256  ROcp0_95 = C4 * C5;
257  ROcp0_16 = C5 * C6;
258  ROcp0_26 = ROcp0_25 * C6 + C4 * S6;
259  ROcp0_36 = ROcp0_35 * C6 + S4 * S6;
260  ROcp0_46 = -C5 * S6;
261  ROcp0_56 = -(ROcp0_25 * S6 - C4 * C6);
262  ROcp0_66 = -(ROcp0_35 * S6 - S4 * C6);
263  ROcp0_47 = ROcp0_46 * C7 + S5 * S7;
264  ROcp0_57 = ROcp0_56 * C7 + ROcp0_85 * S7;
265  ROcp0_67 = ROcp0_66 * C7 + ROcp0_95 * S7;
266  ROcp0_77 = -(ROcp0_46 * S7 - S5 * C7);
267  ROcp0_87 = -(ROcp0_56 * S7 - ROcp0_85 * C7);
268  ROcp0_97 = -(ROcp0_66 * S7 - ROcp0_95 * C7);
269  ROcp0_49 = ROcp0_47 * C9 + ROcp0_77 * S9;
270  ROcp0_59 = ROcp0_57 * C9 + ROcp0_87 * S9;
271  ROcp0_69 = ROcp0_67 * C9 + ROcp0_97 * S9;
272  ROcp0_79 = -(ROcp0_47 * S9 - ROcp0_77 * C9);
273  ROcp0_89 = -(ROcp0_57 * S9 - ROcp0_87 * C9);
274  ROcp0_99 = -(ROcp0_67 * S9 - ROcp0_97 * C9);
284  ROcp0_411 = -(ROcp0_110 * S11 - ROcp0_49 * C11);
285  ROcp0_511 = -(ROcp0_210 * S11 - ROcp0_59 * C11);
286  ROcp0_611 = -(ROcp0_310 * S11 - ROcp0_69 * C11);
290  ROcp0_713 = -(ROcp0_411 * S13 - ROcp0_710 * C13);
291  ROcp0_813 = -(ROcp0_511 * S13 - ROcp0_810 * C13);
292  ROcp0_913 = -(ROcp0_611 * S13 - ROcp0_910 * C13);
302  ROcp0_415 = -(ROcp0_114 * S15 - ROcp0_413 * C15);
303  ROcp0_515 = -(ROcp0_214 * S15 - ROcp0_513 * C15);
304  ROcp0_615 = -(ROcp0_314 * S15 - ROcp0_613 * C15);
308  ROcp0_417 = -(ROcp0_115 * S17 - ROcp0_415 * C17);
309  ROcp0_517 = -(ROcp0_215 * S17 - ROcp0_515 * C17);
310  ROcp0_617 = -(ROcp0_315 * S17 - ROcp0_615 * C17);
314  ROcp0_418 = -(ROcp0_117 * S18 - ROcp0_417 * C18);
315  ROcp0_518 = -(ROcp0_217 * S18 - ROcp0_517 * C18);
316  ROcp0_618 = -(ROcp0_317 * S18 - ROcp0_617 * C18);
320  ROcp0_720 = -(ROcp0_418 * S20 - ROcp0_714 * C20);
321  ROcp0_820 = -(ROcp0_518 * S20 - ROcp0_814 * C20);
322  ROcp0_920 = -(ROcp0_618 * S20 - ROcp0_914 * C20);
332  ROcp0_422 = -(ROcp0_121 * S22 - ROcp0_420 * C22);
333  ROcp0_522 = -(ROcp0_221 * S22 - ROcp0_520 * C22);
334  ROcp0_622 = -(ROcp0_321 * S22 - ROcp0_620 * C22);
338  ROcp0_425 = -(ROcp0_122 * S25 - ROcp0_422 * C25);
339  ROcp0_525 = -(ROcp0_222 * S25 - ROcp0_522 * C25);
340  ROcp0_625 = -(ROcp0_322 * S25 - ROcp0_622 * C25);
344  ROcp0_727 = -(ROcp0_425 * S27 - ROcp0_721 * C27);
345  ROcp0_827 = -(ROcp0_525 * S27 - ROcp0_821 * C27);
346  ROcp0_927 = -(ROcp0_625 * S27 - ROcp0_921 * C27);
350  ROcp0_428 = -(ROcp0_125 * S28 - ROcp0_427 * C28);
351  ROcp0_528 = -(ROcp0_225 * S28 - ROcp0_527 * C28);
352  ROcp0_628 = -(ROcp0_325 * S28 - ROcp0_627 * C28);
356  ROcp0_729 = -(ROcp0_428 * S29 - ROcp0_727 * C29);
357  ROcp0_829 = -(ROcp0_528 * S29 - ROcp0_827 * C29);
358  ROcp0_929 = -(ROcp0_628 * S29 - ROcp0_927 * C29);
362  ROcp0_430 = -(ROcp0_128 * S30 - ROcp0_429 * C30);
363  ROcp0_530 = -(ROcp0_228 * S30 - ROcp0_529 * C30);
364  ROcp0_630 = -(ROcp0_328 * S30 - ROcp0_629 * C30);
368  ROcp0_731 = -(ROcp0_430 * S31 - ROcp0_729 * C31);
369  ROcp0_831 = -(ROcp0_530 * S31 - ROcp0_829 * C31);
370  ROcp0_931 = -(ROcp0_630 * S31 - ROcp0_929 * C31);
374  ROcp0_732 = -(ROcp0_431 * S32 - ROcp0_731 * C32);
375  ROcp0_832 = -(ROcp0_531 * S32 - ROcp0_831 * C32);
376  ROcp0_932 = -(ROcp0_631 * S32 - ROcp0_931 * C32);
386  ROcp0_434 = -(ROcp0_133 * S34 - ROcp0_432 * C34);
387  ROcp0_534 = -(ROcp0_233 * S34 - ROcp0_532 * C34);
388  ROcp0_634 = -(ROcp0_333 * S34 - ROcp0_632 * C34);
392  ROcp0_435 = -(ROcp0_134 * S35 - ROcp0_434 * C35);
393  ROcp0_535 = -(ROcp0_234 * S35 - ROcp0_534 * C35);
394  ROcp0_635 = -(ROcp0_334 * S35 - ROcp0_634 * C35);
398  ROcp0_736 = -(ROcp0_435 * S36 - ROcp0_733 * C36);
399  ROcp0_836 = -(ROcp0_535 * S36 - ROcp0_833 * C36);
400  ROcp0_936 = -(ROcp0_635 * S36 - ROcp0_933 * C36);
410  ROcp0_438 = -(ROcp0_137 * S38 - ROcp0_436 * C38);
411  ROcp0_538 = -(ROcp0_237 * S38 - ROcp0_536 * C38);
412  ROcp0_638 = -(ROcp0_337 * S38 - ROcp0_636 * C38);
416  ROcp0_739 = -(ROcp0_438 * S39 - ROcp0_737 * C39);
417  ROcp0_839 = -(ROcp0_538 * S39 - ROcp0_837 * C39);
418  ROcp0_939 = -(ROcp0_638 * S39 - ROcp0_937 * C39);
422  ROcp0_440 = -(ROcp0_138 * S40 - ROcp0_439 * C40);
423  ROcp0_540 = -(ROcp0_238 * S40 - ROcp0_539 * C40);
424  ROcp0_640 = -(ROcp0_338 * S40 - ROcp0_639 * C40);
428  ROcp0_741 = -(ROcp0_440 * S41 - ROcp0_739 * C41);
429  ROcp0_841 = -(ROcp0_540 * S41 - ROcp0_839 * C41);
430  ROcp0_941 = -(ROcp0_640 * S41 - ROcp0_939 * C41);
434  ROcp0_742 = -(ROcp0_441 * S42 - ROcp0_741 * C42);
435  ROcp0_842 = -(ROcp0_541 * S42 - ROcp0_841 * C42);
436  ROcp0_942 = -(ROcp0_641 * S42 - ROcp0_941 * C42);
440  ROcp0_443 = -(ROcp0_140 * S43 - ROcp0_442 * C43);
441  ROcp0_543 = -(ROcp0_240 * S43 - ROcp0_542 * C43);
442  ROcp0_643 = -(ROcp0_340 * S43 - ROcp0_642 * C43);
443  OMcp0_25 = qd[5] * C4;
444  OMcp0_35 = qd[5] * S4;
445  OMcp0_16 = qd[4] + qd[6] * S5;
446  OMcp0_26 = OMcp0_25 + ROcp0_85 * qd[6];
447  OMcp0_36 = OMcp0_35 + ROcp0_95 * qd[6];
448  OPcp0_16 = qdd[4] + qdd[6] * S5 + qd[5] * qd[6] * C5;
449  OPcp0_26 = ROcp0_85 * qdd[6] + qdd[5] * C4 - qd[4] * qd[5] * S4 + qd[6] * (OMcp0_35 * S5 - ROcp0_95 * qd[4]);
450  OPcp0_36 = ROcp0_95 * qdd[6] + qdd[5] * S4 + qd[4] * qd[5] * C4 - qd[6] * (OMcp0_25 * S5 - ROcp0_85 * qd[4]);
451  RLcp0_18 = ROcp0_77 * q[8];
452  RLcp0_28 = ROcp0_87 * q[8];
453  RLcp0_38 = ROcp0_97 * q[8];
457  RLcp0_112 = ROcp0_710 * q[12];
458  RLcp0_212 = ROcp0_810 * q[12];
459  RLcp0_312 = ROcp0_910 * q[12];
463  RLcp0_116 = ROcp0_714 * q[16];
464  RLcp0_216 = ROcp0_814 * q[16];
465  RLcp0_316 = ROcp0_914 * q[16];
469  OMcp0_118 = OMcp0_16 + ROcp0_714 * qd[18];
470  OMcp0_218 = OMcp0_26 + ROcp0_814 * qd[18];
471  OMcp0_318 = OMcp0_36 + ROcp0_914 * qd[18];
475  RLcp0_119 = ROcp0_714 * q[19];
476  RLcp0_219 = ROcp0_814 * q[19];
477  RLcp0_319 = ROcp0_914 * q[19];
484  OMcp0_120 = OMcp0_118 + ROcp0_118 * qd[20];
485  OMcp0_220 = OMcp0_218 + ROcp0_218 * qd[20];
486  OMcp0_320 = OMcp0_318 + ROcp0_318 * qd[20];
490  OMcp0_121 = OMcp0_120 + ROcp0_420 * qd[21];
491  OMcp0_221 = OMcp0_220 + ROcp0_520 * qd[21];
492  OMcp0_321 = OMcp0_320 + ROcp0_620 * qd[21];
493  OMcp0_122 = OMcp0_121 + ROcp0_721 * qd[22];
494  OMcp0_222 = OMcp0_221 + ROcp0_821 * qd[22];
495  OMcp0_322 = OMcp0_321 + ROcp0_921 * qd[22];
496  OPcp0_122 = OPcp0_118 + ROcp0_118 * qdd[20] + ROcp0_420 * qdd[21] + ROcp0_721 * qdd[22] + qd[20] * (OMcp0_218 * ROcp0_318 - OMcp0_318 *
498  OPcp0_222 = OPcp0_218 + ROcp0_218 * qdd[20] + ROcp0_520 * qdd[21] + ROcp0_821 * qdd[22] - qd[20] * (OMcp0_118 * ROcp0_318 - OMcp0_318 *
500  OPcp0_322 = OPcp0_318 + ROcp0_318 * qdd[20] + ROcp0_620 * qdd[21] + ROcp0_921 * qdd[22] + qd[20] * (OMcp0_118 * ROcp0_218 - OMcp0_218 *
514  RLcp0_126 = ROcp0_721 * q[26];
515  RLcp0_226 = ROcp0_821 * q[26];
516  RLcp0_326 = ROcp0_921 * q[26];
520  OMcp0_130 = OMcp0_122 + ROcp0_729 * qd[30];
521  OMcp0_230 = OMcp0_222 + ROcp0_829 * qd[30];
522  OMcp0_330 = OMcp0_322 + ROcp0_929 * qd[30];
523  OMcp0_135 = OMcp0_130 + ROcp0_733 * qd[35];
524  OMcp0_235 = OMcp0_230 + ROcp0_833 * qd[35];
525  OMcp0_335 = OMcp0_330 + ROcp0_933 * qd[35];
526  OMcp0_140 = OMcp0_135 + ROcp0_739 * qd[40];
527  OMcp0_240 = OMcp0_235 + ROcp0_839 * qd[40];
528  OMcp0_340 = OMcp0_335 + ROcp0_939 * qd[40];
529  OPcp0_140 = OPcp0_122 + ROcp0_729 * qdd[30] + ROcp0_733 * qdd[35] + ROcp0_739 * qdd[40] + qd[30] * (OMcp0_222 * ROcp0_929 - OMcp0_322 *
531  OPcp0_240 = OPcp0_222 + ROcp0_829 * qdd[30] + ROcp0_833 * qdd[35] + ROcp0_839 * qdd[40] - qd[30] * (OMcp0_122 * ROcp0_929 - OMcp0_322 *
533  OPcp0_340 = OPcp0_322 + ROcp0_929 * qdd[30] + ROcp0_933 * qdd[35] + ROcp0_939 * qdd[40] + qd[30] * (OMcp0_122 * ROcp0_829 - OMcp0_222 *
541  OMcp0_143 = OMcp0_140 + ROcp0_742 * qd[43];
542  OMcp0_243 = OMcp0_240 + ROcp0_842 * qd[43];
543  OMcp0_343 = OMcp0_340 + ROcp0_942 * qd[43];
564  RLcp0_324 + RLcp0_326);
567  ROcp0_85 + RLcp0_257 * S5;
733 
734  // = = Block_1_0_0_1_1_0 = =
735 
736  // Symbolic Outputs
737 
738  sens->P[1] = POcp0_157;
739  sens->P[2] = POcp0_257;
740  sens->P[3] = POcp0_357;
741  sens->R[1][1] = ROcp0_143;
742  sens->R[1][2] = ROcp0_243;
743  sens->R[1][3] = ROcp0_343;
744  sens->R[2][1] = ROcp0_443;
745  sens->R[2][2] = ROcp0_543;
746  sens->R[2][3] = ROcp0_643;
747  sens->R[3][1] = ROcp0_742;
748  sens->R[3][2] = ROcp0_842;
749  sens->R[3][3] = ROcp0_942;
750  sens->V[1] = VIcp0_157;
751  sens->V[2] = VIcp0_257;
752  sens->V[3] = VIcp0_357;
753  sens->OM[1] = OMcp0_143;
754  sens->OM[2] = OMcp0_243;
755  sens->OM[3] = OMcp0_343;
756  sens->J[1][1] = (1.0);
757  sens->J[1][5] = JTcp0_157_5;
758  sens->J[1][6] = JTcp0_157_6;
759  sens->J[1][7] = JTcp0_157_7;
760  sens->J[1][8] = ROcp0_77;
761  sens->J[1][9] = JTcp0_157_9;
762  sens->J[1][10] = JTcp0_157_10;
763  sens->J[1][11] = JTcp0_157_11;
764  sens->J[1][12] = ROcp0_710;
765  sens->J[1][13] = JTcp0_157_13;
766  sens->J[1][14] = JTcp0_157_14;
767  sens->J[1][15] = JTcp0_157_15;
768  sens->J[1][16] = ROcp0_714;
769  sens->J[1][17] = JTcp0_157_17;
770  sens->J[1][18] = JTcp0_157_18;
771  sens->J[1][19] = ROcp0_714;
772  sens->J[1][20] = JTcp0_157_20;
773  sens->J[1][21] = JTcp0_157_21;
774  sens->J[1][22] = JTcp0_157_22;
775  sens->J[1][23] = ROcp0_721;
776  sens->J[1][24] = ROcp0_721;
777  sens->J[1][25] = JTcp0_157_25;
778  sens->J[1][26] = ROcp0_721;
779  sens->J[1][27] = JTcp0_157_27;
780  sens->J[1][28] = JTcp0_157_28;
781  sens->J[1][29] = JTcp0_157_29;
782  sens->J[1][30] = JTcp0_157_30;
783  sens->J[1][31] = JTcp0_157_31;
784  sens->J[1][32] = JTcp0_157_32;
785  sens->J[1][33] = JTcp0_157_33;
786  sens->J[1][34] = JTcp0_157_34;
787  sens->J[1][35] = JTcp0_157_35;
788  sens->J[1][36] = JTcp0_157_36;
789  sens->J[1][37] = JTcp0_157_37;
790  sens->J[1][38] = JTcp0_157_38;
791  sens->J[1][39] = JTcp0_157_39;
792  sens->J[1][40] = JTcp0_157_40;
793  sens->J[1][41] = JTcp0_157_41;
794  sens->J[1][42] = JTcp0_157_42;
795  sens->J[1][43] = JTcp0_157_43;
796  sens->J[2][2] = (1.0);
797  sens->J[2][4] = JTcp0_257_4;
798  sens->J[2][5] = JTcp0_257_5;
799  sens->J[2][6] = JTcp0_257_6;
800  sens->J[2][7] = JTcp0_257_7;
801  sens->J[2][8] = ROcp0_87;
802  sens->J[2][9] = JTcp0_257_9;
803  sens->J[2][10] = JTcp0_257_10;
804  sens->J[2][11] = JTcp0_257_11;
805  sens->J[2][12] = ROcp0_810;
806  sens->J[2][13] = JTcp0_257_13;
807  sens->J[2][14] = JTcp0_257_14;
808  sens->J[2][15] = JTcp0_257_15;
809  sens->J[2][16] = ROcp0_814;
810  sens->J[2][17] = JTcp0_257_17;
811  sens->J[2][18] = JTcp0_257_18;
812  sens->J[2][19] = ROcp0_814;
813  sens->J[2][20] = JTcp0_257_20;
814  sens->J[2][21] = JTcp0_257_21;
815  sens->J[2][22] = JTcp0_257_22;
816  sens->J[2][23] = ROcp0_821;
817  sens->J[2][24] = ROcp0_821;
818  sens->J[2][25] = JTcp0_257_25;
819  sens->J[2][26] = ROcp0_821;
820  sens->J[2][27] = JTcp0_257_27;
821  sens->J[2][28] = JTcp0_257_28;
822  sens->J[2][29] = JTcp0_257_29;
823  sens->J[2][30] = JTcp0_257_30;
824  sens->J[2][31] = JTcp0_257_31;
825  sens->J[2][32] = JTcp0_257_32;
826  sens->J[2][33] = JTcp0_257_33;
827  sens->J[2][34] = JTcp0_257_34;
828  sens->J[2][35] = JTcp0_257_35;
829  sens->J[2][36] = JTcp0_257_36;
830  sens->J[2][37] = JTcp0_257_37;
831  sens->J[2][38] = JTcp0_257_38;
832  sens->J[2][39] = JTcp0_257_39;
833  sens->J[2][40] = JTcp0_257_40;
834  sens->J[2][41] = JTcp0_257_41;
835  sens->J[2][42] = JTcp0_257_42;
836  sens->J[2][43] = JTcp0_257_43;
837  sens->J[3][3] = (1.0);
838  sens->J[3][4] = JTcp0_357_4;
839  sens->J[3][5] = JTcp0_357_5;
840  sens->J[3][6] = JTcp0_357_6;
841  sens->J[3][7] = JTcp0_357_7;
842  sens->J[3][8] = ROcp0_97;
843  sens->J[3][9] = JTcp0_357_9;
844  sens->J[3][10] = JTcp0_357_10;
845  sens->J[3][11] = JTcp0_357_11;
846  sens->J[3][12] = ROcp0_910;
847  sens->J[3][13] = JTcp0_357_13;
848  sens->J[3][14] = JTcp0_357_14;
849  sens->J[3][15] = JTcp0_357_15;
850  sens->J[3][16] = ROcp0_914;
851  sens->J[3][17] = JTcp0_357_17;
852  sens->J[3][18] = JTcp0_357_18;
853  sens->J[3][19] = ROcp0_914;
854  sens->J[3][20] = JTcp0_357_20;
855  sens->J[3][21] = JTcp0_357_21;
856  sens->J[3][22] = JTcp0_357_22;
857  sens->J[3][23] = ROcp0_921;
858  sens->J[3][24] = ROcp0_921;
859  sens->J[3][25] = JTcp0_357_25;
860  sens->J[3][26] = ROcp0_921;
861  sens->J[3][27] = JTcp0_357_27;
862  sens->J[3][28] = JTcp0_357_28;
863  sens->J[3][29] = JTcp0_357_29;
864  sens->J[3][30] = JTcp0_357_30;
865  sens->J[3][31] = JTcp0_357_31;
866  sens->J[3][32] = JTcp0_357_32;
867  sens->J[3][33] = JTcp0_357_33;
868  sens->J[3][34] = JTcp0_357_34;
869  sens->J[3][35] = JTcp0_357_35;
870  sens->J[3][36] = JTcp0_357_36;
871  sens->J[3][37] = JTcp0_357_37;
872  sens->J[3][38] = JTcp0_357_38;
873  sens->J[3][39] = JTcp0_357_39;
874  sens->J[3][40] = JTcp0_357_40;
875  sens->J[3][41] = JTcp0_357_41;
876  sens->J[3][42] = JTcp0_357_42;
877  sens->J[3][43] = JTcp0_357_43;
878  sens->J[4][4] = (1.0);
879  sens->J[4][6] = S5;
880  sens->J[4][7] = ROcp0_16;
881  sens->J[4][9] = ROcp0_16;
882  sens->J[4][10] = ROcp0_49;
883  sens->J[4][11] = ROcp0_710;
884  sens->J[4][13] = ROcp0_111;
885  sens->J[4][14] = ROcp0_413;
886  sens->J[4][15] = ROcp0_714;
887  sens->J[4][17] = ROcp0_714;
888  sens->J[4][18] = ROcp0_714;
889  sens->J[4][20] = ROcp0_118;
890  sens->J[4][21] = ROcp0_420;
891  sens->J[4][22] = ROcp0_721;
892  sens->J[4][25] = ROcp0_721;
893  sens->J[4][27] = ROcp0_125;
894  sens->J[4][28] = ROcp0_727;
895  sens->J[4][29] = ROcp0_128;
896  sens->J[4][30] = ROcp0_729;
897  sens->J[4][31] = ROcp0_130;
898  sens->J[4][32] = ROcp0_130;
899  sens->J[4][33] = ROcp0_432;
900  sens->J[4][34] = ROcp0_733;
901  sens->J[4][35] = ROcp0_733;
902  sens->J[4][36] = ROcp0_135;
903  sens->J[4][37] = ROcp0_436;
904  sens->J[4][38] = ROcp0_737;
905  sens->J[4][39] = ROcp0_138;
906  sens->J[4][40] = ROcp0_739;
907  sens->J[4][41] = ROcp0_140;
908  sens->J[4][42] = ROcp0_140;
909  sens->J[4][43] = ROcp0_742;
910  sens->J[5][5] = C4;
911  sens->J[5][6] = ROcp0_85;
912  sens->J[5][7] = ROcp0_26;
913  sens->J[5][9] = ROcp0_26;
914  sens->J[5][10] = ROcp0_59;
915  sens->J[5][11] = ROcp0_810;
916  sens->J[5][13] = ROcp0_211;
917  sens->J[5][14] = ROcp0_513;
918  sens->J[5][15] = ROcp0_814;
919  sens->J[5][17] = ROcp0_814;
920  sens->J[5][18] = ROcp0_814;
921  sens->J[5][20] = ROcp0_218;
922  sens->J[5][21] = ROcp0_520;
923  sens->J[5][22] = ROcp0_821;
924  sens->J[5][25] = ROcp0_821;
925  sens->J[5][27] = ROcp0_225;
926  sens->J[5][28] = ROcp0_827;
927  sens->J[5][29] = ROcp0_228;
928  sens->J[5][30] = ROcp0_829;
929  sens->J[5][31] = ROcp0_230;
930  sens->J[5][32] = ROcp0_230;
931  sens->J[5][33] = ROcp0_532;
932  sens->J[5][34] = ROcp0_833;
933  sens->J[5][35] = ROcp0_833;
934  sens->J[5][36] = ROcp0_235;
935  sens->J[5][37] = ROcp0_536;
936  sens->J[5][38] = ROcp0_837;
937  sens->J[5][39] = ROcp0_238;
938  sens->J[5][40] = ROcp0_839;
939  sens->J[5][41] = ROcp0_240;
940  sens->J[5][42] = ROcp0_240;
941  sens->J[5][43] = ROcp0_842;
942  sens->J[6][5] = S4;
943  sens->J[6][6] = ROcp0_95;
944  sens->J[6][7] = ROcp0_36;
945  sens->J[6][9] = ROcp0_36;
946  sens->J[6][10] = ROcp0_69;
947  sens->J[6][11] = ROcp0_910;
948  sens->J[6][13] = ROcp0_311;
949  sens->J[6][14] = ROcp0_613;
950  sens->J[6][15] = ROcp0_914;
951  sens->J[6][17] = ROcp0_914;
952  sens->J[6][18] = ROcp0_914;
953  sens->J[6][20] = ROcp0_318;
954  sens->J[6][21] = ROcp0_620;
955  sens->J[6][22] = ROcp0_921;
956  sens->J[6][25] = ROcp0_921;
957  sens->J[6][27] = ROcp0_325;
958  sens->J[6][28] = ROcp0_927;
959  sens->J[6][29] = ROcp0_328;
960  sens->J[6][30] = ROcp0_929;
961  sens->J[6][31] = ROcp0_330;
962  sens->J[6][32] = ROcp0_330;
963  sens->J[6][33] = ROcp0_632;
964  sens->J[6][34] = ROcp0_933;
965  sens->J[6][35] = ROcp0_933;
966  sens->J[6][36] = ROcp0_335;
967  sens->J[6][37] = ROcp0_636;
968  sens->J[6][38] = ROcp0_937;
969  sens->J[6][39] = ROcp0_338;
970  sens->J[6][40] = ROcp0_939;
971  sens->J[6][41] = ROcp0_340;
972  sens->J[6][42] = ROcp0_340;
973  sens->J[6][43] = ROcp0_942;
974  sens->A[1] = ACcp0_157;
975  sens->A[2] = ACcp0_257;
976  sens->A[3] = ACcp0_357;
977  sens->OMP[1] = OPcp0_143;
978  sens->OMP[2] = OPcp0_243;
979  sens->OMP[3] = OPcp0_343;
980 
981  //
982  break;
983  case 2:
984 
985 
986 
987  // = = Block_1_0_0_2_0_1 = =
988 
989  // Sensor Kinematics
990 
991 
992  ROcp1_25 = S4 * S5;
993  ROcp1_35 = -C4 * S5;
994  ROcp1_85 = -S4 * C5;
995  ROcp1_95 = C4 * C5;
996  ROcp1_16 = C5 * C6;
997  ROcp1_26 = ROcp1_25 * C6 + C4 * S6;
998  ROcp1_36 = ROcp1_35 * C6 + S4 * S6;
999  ROcp1_46 = -C5 * S6;
1000  ROcp1_56 = -(ROcp1_25 * S6 - C4 * C6);
1001  ROcp1_66 = -(ROcp1_35 * S6 - S4 * C6);
1002  ROcp1_47 = ROcp1_46 * C7 + S5 * S7;
1003  ROcp1_57 = ROcp1_56 * C7 + ROcp1_85 * S7;
1004  ROcp1_67 = ROcp1_66 * C7 + ROcp1_95 * S7;
1005  ROcp1_77 = -(ROcp1_46 * S7 - S5 * C7);
1006  ROcp1_87 = -(ROcp1_56 * S7 - ROcp1_85 * C7);
1007  ROcp1_97 = -(ROcp1_66 * S7 - ROcp1_95 * C7);
1008  ROcp1_49 = ROcp1_47 * C9 + ROcp1_77 * S9;
1009  ROcp1_59 = ROcp1_57 * C9 + ROcp1_87 * S9;
1010  ROcp1_69 = ROcp1_67 * C9 + ROcp1_97 * S9;
1011  ROcp1_79 = -(ROcp1_47 * S9 - ROcp1_77 * C9);
1012  ROcp1_89 = -(ROcp1_57 * S9 - ROcp1_87 * C9);
1013  ROcp1_99 = -(ROcp1_67 * S9 - ROcp1_97 * C9);
1014  ROcp1_110 = ROcp1_16 * C10 - ROcp1_79 * S10;
1015  ROcp1_210 = ROcp1_26 * C10 - ROcp1_89 * S10;
1016  ROcp1_310 = ROcp1_36 * C10 - ROcp1_99 * S10;
1017  ROcp1_710 = ROcp1_16 * S10 + ROcp1_79 * C10;
1018  ROcp1_810 = ROcp1_26 * S10 + ROcp1_89 * C10;
1019  ROcp1_910 = ROcp1_36 * S10 + ROcp1_99 * C10;
1023  ROcp1_411 = -(ROcp1_110 * S11 - ROcp1_49 * C11);
1024  ROcp1_511 = -(ROcp1_210 * S11 - ROcp1_59 * C11);
1025  ROcp1_611 = -(ROcp1_310 * S11 - ROcp1_69 * C11);
1029  ROcp1_713 = -(ROcp1_411 * S13 - ROcp1_710 * C13);
1030  ROcp1_813 = -(ROcp1_511 * S13 - ROcp1_810 * C13);
1031  ROcp1_913 = -(ROcp1_611 * S13 - ROcp1_910 * C13);
1041  ROcp1_415 = -(ROcp1_114 * S15 - ROcp1_413 * C15);
1042  ROcp1_515 = -(ROcp1_214 * S15 - ROcp1_513 * C15);
1043  ROcp1_615 = -(ROcp1_314 * S15 - ROcp1_613 * C15);
1047  ROcp1_417 = -(ROcp1_115 * S17 - ROcp1_415 * C17);
1048  ROcp1_517 = -(ROcp1_215 * S17 - ROcp1_515 * C17);
1049  ROcp1_617 = -(ROcp1_315 * S17 - ROcp1_615 * C17);
1053  ROcp1_418 = -(ROcp1_117 * S18 - ROcp1_417 * C18);
1054  ROcp1_518 = -(ROcp1_217 * S18 - ROcp1_517 * C18);
1055  ROcp1_618 = -(ROcp1_317 * S18 - ROcp1_617 * C18);
1059  ROcp1_720 = -(ROcp1_418 * S20 - ROcp1_714 * C20);
1060  ROcp1_820 = -(ROcp1_518 * S20 - ROcp1_814 * C20);
1061  ROcp1_920 = -(ROcp1_618 * S20 - ROcp1_914 * C20);
1071  ROcp1_422 = -(ROcp1_121 * S22 - ROcp1_420 * C22);
1072  ROcp1_522 = -(ROcp1_221 * S22 - ROcp1_520 * C22);
1073  ROcp1_622 = -(ROcp1_321 * S22 - ROcp1_620 * C22);
1077  ROcp1_425 = -(ROcp1_122 * S25 - ROcp1_422 * C25);
1078  ROcp1_525 = -(ROcp1_222 * S25 - ROcp1_522 * C25);
1079  ROcp1_625 = -(ROcp1_322 * S25 - ROcp1_622 * C25);
1083  ROcp1_727 = -(ROcp1_425 * S27 - ROcp1_721 * C27);
1084  ROcp1_827 = -(ROcp1_525 * S27 - ROcp1_821 * C27);
1085  ROcp1_927 = -(ROcp1_625 * S27 - ROcp1_921 * C27);
1089  ROcp1_428 = -(ROcp1_125 * S28 - ROcp1_427 * C28);
1090  ROcp1_528 = -(ROcp1_225 * S28 - ROcp1_527 * C28);
1091  ROcp1_628 = -(ROcp1_325 * S28 - ROcp1_627 * C28);
1095  ROcp1_729 = -(ROcp1_428 * S29 - ROcp1_727 * C29);
1096  ROcp1_829 = -(ROcp1_528 * S29 - ROcp1_827 * C29);
1097  ROcp1_929 = -(ROcp1_628 * S29 - ROcp1_927 * C29);
1101  ROcp1_430 = -(ROcp1_128 * S30 - ROcp1_429 * C30);
1102  ROcp1_530 = -(ROcp1_228 * S30 - ROcp1_529 * C30);
1103  ROcp1_630 = -(ROcp1_328 * S30 - ROcp1_629 * C30);
1107  ROcp1_731 = -(ROcp1_430 * S31 - ROcp1_729 * C31);
1108  ROcp1_831 = -(ROcp1_530 * S31 - ROcp1_829 * C31);
1109  ROcp1_931 = -(ROcp1_630 * S31 - ROcp1_929 * C31);
1113  ROcp1_732 = -(ROcp1_431 * S32 - ROcp1_731 * C32);
1114  ROcp1_832 = -(ROcp1_531 * S32 - ROcp1_831 * C32);
1115  ROcp1_932 = -(ROcp1_631 * S32 - ROcp1_931 * C32);
1125  ROcp1_434 = -(ROcp1_133 * S34 - ROcp1_432 * C34);
1126  ROcp1_534 = -(ROcp1_233 * S34 - ROcp1_532 * C34);
1127  ROcp1_634 = -(ROcp1_333 * S34 - ROcp1_632 * C34);
1131  ROcp1_435 = -(ROcp1_134 * S35 - ROcp1_434 * C35);
1132  ROcp1_535 = -(ROcp1_234 * S35 - ROcp1_534 * C35);
1133  ROcp1_635 = -(ROcp1_334 * S35 - ROcp1_634 * C35);
1137  ROcp1_736 = -(ROcp1_435 * S36 - ROcp1_733 * C36);
1138  ROcp1_836 = -(ROcp1_535 * S36 - ROcp1_833 * C36);
1139  ROcp1_936 = -(ROcp1_635 * S36 - ROcp1_933 * C36);
1149  ROcp1_438 = -(ROcp1_137 * S38 - ROcp1_436 * C38);
1150  ROcp1_538 = -(ROcp1_237 * S38 - ROcp1_536 * C38);
1151  ROcp1_638 = -(ROcp1_337 * S38 - ROcp1_636 * C38);
1155  ROcp1_739 = -(ROcp1_438 * S39 - ROcp1_737 * C39);
1156  ROcp1_839 = -(ROcp1_538 * S39 - ROcp1_837 * C39);
1157  ROcp1_939 = -(ROcp1_638 * S39 - ROcp1_937 * C39);
1161  ROcp1_440 = -(ROcp1_138 * S40 - ROcp1_439 * C40);
1162  ROcp1_540 = -(ROcp1_238 * S40 - ROcp1_539 * C40);
1163  ROcp1_640 = -(ROcp1_338 * S40 - ROcp1_639 * C40);
1167  ROcp1_741 = -(ROcp1_440 * S41 - ROcp1_739 * C41);
1168  ROcp1_841 = -(ROcp1_540 * S41 - ROcp1_839 * C41);
1169  ROcp1_941 = -(ROcp1_640 * S41 - ROcp1_939 * C41);
1173  ROcp1_742 = -(ROcp1_441 * S42 - ROcp1_741 * C42);
1174  ROcp1_842 = -(ROcp1_541 * S42 - ROcp1_841 * C42);
1175  ROcp1_942 = -(ROcp1_641 * S42 - ROcp1_941 * C42);
1179  ROcp1_443 = -(ROcp1_140 * S43 - ROcp1_442 * C43);
1180  ROcp1_543 = -(ROcp1_240 * S43 - ROcp1_542 * C43);
1181  ROcp1_643 = -(ROcp1_340 * S43 - ROcp1_642 * C43);
1185  ROcp1_744 = -(ROcp1_443 * S44 - ROcp1_742 * C44);
1186  ROcp1_844 = -(ROcp1_543 * S44 - ROcp1_842 * C44);
1187  ROcp1_944 = -(ROcp1_643 * S44 - ROcp1_942 * C44);
1191  ROcp1_746 = -(ROcp1_444 * S46 - ROcp1_744 * C46);
1192  ROcp1_846 = -(ROcp1_544 * S46 - ROcp1_844 * C46);
1193  ROcp1_946 = -(ROcp1_644 * S46 - ROcp1_944 * C46);
1197  ROcp1_748 = -(ROcp1_446 * S48 - ROcp1_746 * C48);
1198  ROcp1_848 = -(ROcp1_546 * S48 - ROcp1_846 * C48);
1199  ROcp1_948 = -(ROcp1_646 * S48 - ROcp1_946 * C48);
1203  ROcp1_449 = -(ROcp1_143 * S49 - ROcp1_448 * C49);
1204  ROcp1_549 = -(ROcp1_243 * S49 - ROcp1_548 * C49);
1205  ROcp1_649 = -(ROcp1_343 * S49 - ROcp1_648 * C49);
1209  ROcp1_750 = -(ROcp1_449 * S50 - ROcp1_748 * C50);
1210  ROcp1_850 = -(ROcp1_549 * S50 - ROcp1_848 * C50);
1211  ROcp1_950 = -(ROcp1_649 * S50 - ROcp1_948 * C50);
1215  ROcp1_751 = -(ROcp1_450 * S51 - ROcp1_750 * C51);
1216  ROcp1_851 = -(ROcp1_550 * S51 - ROcp1_850 * C51);
1217  ROcp1_951 = -(ROcp1_650 * S51 - ROcp1_950 * C51);
1221  ROcp1_453 = -(ROcp1_149 * S53 - ROcp1_451 * C53);
1222  ROcp1_553 = -(ROcp1_249 * S53 - ROcp1_551 * C53);
1223  ROcp1_653 = -(ROcp1_349 * S53 - ROcp1_651 * C53);
1224  OMcp1_25 = qd[5] * C4;
1225  OMcp1_35 = qd[5] * S4;
1226  OMcp1_16 = qd[4] + qd[6] * S5;
1227  OMcp1_26 = OMcp1_25 + ROcp1_85 * qd[6];
1228  OMcp1_36 = OMcp1_35 + ROcp1_95 * qd[6];
1229  OPcp1_16 = qdd[4] + qdd[6] * S5 + qd[5] * qd[6] * C5;
1230  OPcp1_26 = ROcp1_85 * qdd[6] + qdd[5] * C4 - qd[4] * qd[5] * S4 + qd[6] * (OMcp1_35 * S5 - ROcp1_95 * qd[4]);
1231  OPcp1_36 = ROcp1_95 * qdd[6] + qdd[5] * S4 + qd[4] * qd[5] * C4 - qd[6] * (OMcp1_25 * S5 - ROcp1_85 * qd[4]);
1232  RLcp1_18 = ROcp1_77 * q[8];
1233  RLcp1_28 = ROcp1_87 * q[8];
1234  RLcp1_38 = ROcp1_97 * q[8];
1238  RLcp1_112 = ROcp1_710 * q[12];
1239  RLcp1_212 = ROcp1_810 * q[12];
1240  RLcp1_312 = ROcp1_910 * q[12];
1244  RLcp1_116 = ROcp1_714 * q[16];
1245  RLcp1_216 = ROcp1_814 * q[16];
1246  RLcp1_316 = ROcp1_914 * q[16];
1250  OMcp1_118 = OMcp1_16 + ROcp1_714 * qd[18];
1251  OMcp1_218 = OMcp1_26 + ROcp1_814 * qd[18];
1252  OMcp1_318 = OMcp1_36 + ROcp1_914 * qd[18];
1256  RLcp1_119 = ROcp1_714 * q[19];
1257  RLcp1_219 = ROcp1_814 * q[19];
1258  RLcp1_319 = ROcp1_914 * q[19];
1265  OMcp1_120 = OMcp1_118 + ROcp1_118 * qd[20];
1266  OMcp1_220 = OMcp1_218 + ROcp1_218 * qd[20];
1267  OMcp1_320 = OMcp1_318 + ROcp1_318 * qd[20];
1271  OMcp1_121 = OMcp1_120 + ROcp1_420 * qd[21];
1272  OMcp1_221 = OMcp1_220 + ROcp1_520 * qd[21];
1273  OMcp1_321 = OMcp1_320 + ROcp1_620 * qd[21];
1274  OMcp1_122 = OMcp1_121 + ROcp1_721 * qd[22];
1275  OMcp1_222 = OMcp1_221 + ROcp1_821 * qd[22];
1276  OMcp1_322 = OMcp1_321 + ROcp1_921 * qd[22];
1277  OPcp1_122 = OPcp1_118 + ROcp1_118 * qdd[20] + ROcp1_420 * qdd[21] + ROcp1_721 * qdd[22] + qd[20] * (OMcp1_218 * ROcp1_318 - OMcp1_318 *
1279  OPcp1_222 = OPcp1_218 + ROcp1_218 * qdd[20] + ROcp1_520 * qdd[21] + ROcp1_821 * qdd[22] - qd[20] * (OMcp1_118 * ROcp1_318 - OMcp1_318 *
1281  OPcp1_322 = OPcp1_318 + ROcp1_318 * qdd[20] + ROcp1_620 * qdd[21] + ROcp1_921 * qdd[22] + qd[20] * (OMcp1_118 * ROcp1_218 - OMcp1_218 *
1295  RLcp1_126 = ROcp1_721 * q[26];
1296  RLcp1_226 = ROcp1_821 * q[26];
1297  RLcp1_326 = ROcp1_921 * q[26];
1301  OMcp1_130 = OMcp1_122 + ROcp1_729 * qd[30];
1302  OMcp1_230 = OMcp1_222 + ROcp1_829 * qd[30];
1303  OMcp1_330 = OMcp1_322 + ROcp1_929 * qd[30];
1304  OMcp1_135 = OMcp1_130 + ROcp1_733 * qd[35];
1305  OMcp1_235 = OMcp1_230 + ROcp1_833 * qd[35];
1306  OMcp1_335 = OMcp1_330 + ROcp1_933 * qd[35];
1307  OMcp1_140 = OMcp1_135 + ROcp1_739 * qd[40];
1308  OMcp1_240 = OMcp1_235 + ROcp1_839 * qd[40];
1309  OMcp1_340 = OMcp1_335 + ROcp1_939 * qd[40];
1310  OPcp1_140 = OPcp1_122 + ROcp1_729 * qdd[30] + ROcp1_733 * qdd[35] + ROcp1_739 * qdd[40] + qd[30] * (OMcp1_222 * ROcp1_929 - OMcp1_322 *
1312  OPcp1_240 = OPcp1_222 + ROcp1_829 * qdd[30] + ROcp1_833 * qdd[35] + ROcp1_839 * qdd[40] - qd[30] * (OMcp1_122 * ROcp1_929 - OMcp1_322 *
1314  OPcp1_340 = OPcp1_322 + ROcp1_929 * qdd[30] + ROcp1_933 * qdd[35] + ROcp1_939 * qdd[40] + qd[30] * (OMcp1_122 * ROcp1_829 - OMcp1_222 *
1322  OMcp1_143 = OMcp1_140 + ROcp1_742 * qd[43];
1323  OMcp1_243 = OMcp1_240 + ROcp1_842 * qd[43];
1324  OMcp1_343 = OMcp1_340 + ROcp1_942 * qd[43];
1328  RLcp1_145 = ROcp1_744 * q[45];
1329  RLcp1_245 = ROcp1_844 * q[45];
1330  RLcp1_345 = ROcp1_944 * q[45];
1340  RLcp1_147 = ROcp1_746 * q[47];
1341  RLcp1_247 = ROcp1_846 * q[47];
1342  RLcp1_347 = ROcp1_946 * q[47];
1352  OMcp1_149 = OMcp1_143 + ROcp1_748 * qd[49];
1353  OMcp1_249 = OMcp1_243 + ROcp1_848 * qd[49];
1354  OMcp1_349 = OMcp1_343 + ROcp1_948 * qd[49];
1364  RLcp1_152 = ROcp1_751 * q[52];
1365  RLcp1_252 = ROcp1_851 * q[52];
1366  RLcp1_352 = ROcp1_951 * q[52];
1409  RLcp1_251 + RLcp1_252);
1413  RLcp1_151 + RLcp1_152));
1417  RLcp1_151 + RLcp1_152);
1421  RLcp1_251 + RLcp1_252);
1425  RLcp1_151 + RLcp1_152));
1429  RLcp1_151 + RLcp1_152);
1437  ROcp1_910 * (RLcp1_151 + RLcp1_152));
1449  RLcp1_148 + RLcp1_151);
1461  RLcp1_148 + RLcp1_151);
1473  RLcp1_148 + RLcp1_151);
1539  JTcp1_352_28 = ROcp1_727 * (