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
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;
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);
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);
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];
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];
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 *
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);
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];
1413 RLcp1_151 + RLcp1_152));
1425 RLcp1_151 + RLcp1_152));
1640 RLcp1_151 + RLcp1_152));
1697 OMcp1_153 = OMcp1_149 + ROcp1_751 * qd[53];
1698 OMcp1_253 = OMcp1_249 + ROcp1_851 * qd[53];
1699 OMcp1_353 = OMcp1_349 + ROcp1_951 * qd[53];
1703
1704 // = = Block_1_0_0_2_1_0 = =
1705
1706 // Symbolic Outputs
1707
1708 sens->P[1] = POcp1_152;
1709 sens->P[2] = POcp1_252;
1710 sens->P[3] = POcp1_352;
1711 sens->R[1][1] = ROcp1_153;
1712 sens->R[1][2] = ROcp1_253;
1713 sens->R[1][3] = ROcp1_353;
1714 sens->R[2][1] = ROcp1_453;
1715 sens->R[2][2] = ROcp1_553;
1716 sens->R[2][3] = ROcp1_653;
1717 sens->R[3][1] = ROcp1_751;
1718 sens->R[3][2] = ROcp1_851;
1719 sens->R[3][3] = ROcp1_951;
1720 sens->V[1] = VIcp1_152;
1721 sens->V[2] = VIcp1_252;
1722 sens->V[3] = VIcp1_352;
1723 sens->OM[1] = OMcp1_153;
1724 sens->OM[2] = OMcp1_253;
1725 sens->OM[3] = OMcp1_353;
1726 sens->J[1][1] = (1.0);
1727 sens->J[1][5] = JTcp1_152_5;
1728 sens->J[1][6] = JTcp1_152_6;
1729 sens->J[1][7] = JTcp1_152_7;
1730 sens->J[1][8] = ROcp1_77;
1731 sens->J[1][9] = JTcp1_152_9;
1732 sens->J[1][10] = JTcp1_152_10;
1733 sens->J[1][11] = JTcp1_152_11;
1734 sens->J[1][12] = ROcp1_710;
1735 sens->J[1][13] = JTcp1_152_13;
1736 sens->J[1][14] = JTcp1_152_14;
1737 sens->J[1][15] = JTcp1_152_15;
1738 sens->J[1][16] = ROcp1_714;
1739 sens->J[1][17] = JTcp1_152_17;
1740 sens->J[1][18] = JTcp1_152_18;
1741 sens->J[1][19] = ROcp1_714;
1742 sens->J[1][20] = JTcp1_152_20;
1743 sens->J[1][21] = JTcp1_152_21;
1744 sens->J[1][22] = JTcp1_152_22;
1745 sens->J[1][23] = ROcp1_721;
1746 sens->J[1][24] = ROcp1_721;
1747 sens->J[1][25] = JTcp1_152_25;
1748 sens->J[1][26] = ROcp1_721;
1749 sens->J[1][27] = JTcp1_152_27;
1750 sens->J[1][28] = JTcp1_152_28;
1751 sens->J[1][29] = JTcp1_152_29;
1752 sens->J[1][30] = JTcp1_152_30;
1753 sens->J[1][31] = JTcp1_152_31;
1754 sens->J[1][32] = JTcp1_152_32;
1755 sens->J[1][33] = JTcp1_152_33;
1756 sens->J[1][34] = JTcp1_152_34;
1757 sens->J[1][35] = JTcp1_152_35;
1758 sens->J[1][36] = JTcp1_152_36;
1759 sens->J[1][37] = JTcp1_152_37;
1760 sens->J[1][38] = JTcp1_152_38;
1761 sens->J[1][39] = JTcp1_152_39;
1762 sens->J[1][40] = JTcp1_152_40;
1763 sens->J[1][41] = JTcp1_152_41;
1764 sens->J[1][42] = JTcp1_152_42;
1765 sens->J[1][43] = JTcp1_152_43;
1766 sens->J[1][44] = JTcp1_152_44;
1767 sens->J[1][45] = ROcp1_744;
1768 sens->J[1][46] = JTcp1_152_46;
1769 sens->J[1][47] = ROcp1_746;
1770 sens->J[1][48] = JTcp1_152_48;
1771 sens->J[1][49] = JTcp1_152_49;
1772 sens->J[1][50] = JTcp1_152_50;
1773 sens->J[1][51] = JTcp1_152_51;
1774 sens->J[1][52] = ROcp1_751;
1775 sens->J[2][2] = (1.0);
1776 sens->J[2][4] = JTcp1_252_4;
1777 sens->J[2][5] = JTcp1_252_5;
1778 sens->J[2][6] = JTcp1_252_6;
1779 sens->J[2][7] = JTcp1_252_7;
1780 sens->J[2][8] = ROcp1_87;
1781 sens->J[2][9] = JTcp1_252_9;
1782 sens->J[2][10] = JTcp1_252_10;
1783 sens->J[2][11] = JTcp1_252_11;
1784 sens->J[2][12] = ROcp1_810;
1785 sens->J[2][13] = JTcp1_252_13;
1786 sens->J[2][14] = JTcp1_252_14;
1787 sens->J[2][15] = JTcp1_252_15;
1788 sens->J[2][16] = ROcp1_814;
1789 sens->J[2][17] = JTcp1_252_17;
1790 sens->J[2][18] = JTcp1_252_18;
1791 sens->J[2][19] = ROcp1_814;
1792 sens->J[2][20] = JTcp1_252_20;
1793 sens->J[2][21] = JTcp1_252_21;
1794 sens->J[2][22] = JTcp1_252_22;
1795 sens->J[2][23] = ROcp1_821;
1796 sens->J[2][24] = ROcp1_821;
1797 sens->J[2][25] = JTcp1_252_25;
1798 sens->J[2][26] = ROcp1_821;
1799 sens->J[2][27] = JTcp1_252_27;
1800 sens->J[2][28] = JTcp1_252_28;
1801 sens->J[2][29] = JTcp1_252_29;
1802 sens->J[2][30] = JTcp1_252_30;
1803 sens->J[2][31] = JTcp1_252_31;
1804 sens->J[2][32] = JTcp1_252_32;
1805 sens->J[2][33] = JTcp1_252_33;
1806 sens->J[2][34] = JTcp1_252_34;
1807 sens->J[2][35] = JTcp1_252_35;
1808 sens->J[2][36] = JTcp1_252_36;
1809 sens->J[2][37] = JTcp1_252_37;
1810 sens->J[2][38] = JTcp1_252_38;
1811 sens->J[2][39] = JTcp1_252_39;
1812 sens->J[2][40] = JTcp1_252_40;
1813 sens->J[2][41] = JTcp1_252_41;
1814 sens->J[2][42] = JTcp1_252_42;
1815 sens->J[2][43] = JTcp1_252_43;
1816 sens->J[2][44] = JTcp1_252_44;
1817 sens->J[2][45] = ROcp1_844;
1818 sens->J[2][46] = JTcp1_252_46;
1819 sens->J[2][47] = ROcp1_846;
1820 sens->J[2][48] = JTcp1_252_48;
1821 sens->J[2][49] = JTcp1_252_49;
1822 sens->J[2][50] = JTcp1_252_50;
1823 sens->J[2][51] = JTcp1_252_51;
1824 sens->J[2][52] = ROcp1_851;
1825 sens->J[3][3] = (1.0);
1826 sens->J[3][4] = JTcp1_352_4;
1827 sens->J[3][5] = JTcp1_352_5;
1828 sens->J[3][6] = JTcp1_352_6;
1829 sens->J[3][7] = JTcp1_352_7;
1830 sens->J[3][8] = ROcp1_97;
1831 sens->J[3][9] = JTcp1_352_9;
1832 sens->J[3][10] = JTcp1_352_10;
1833 sens->J[3][11] = JTcp1_352_11;
1834 sens->J[3][12] = ROcp1_910;
1835 sens->J[3][13] = JTcp1_352_13;
1836 sens->J[3][14] = JTcp1_352_14;
1837 sens->J[3][15] = JTcp1_352_15;
1838 sens->J[3][16] = ROcp1_914;
1839 sens->J[3][17] = JTcp1_352_17;
1840 sens->J[3][18] = JTcp1_352_18;
1841 sens->J[3][19] = ROcp1_914;
1842 sens->J[3][20] = JTcp1_352_20;
1843 sens->J[3][21] = JTcp1_352_21;
1844 sens->J[3][22] = JTcp1_352_22;
1845 sens->J[3][23] = ROcp1_921;
1846 sens->J[3][24] = ROcp1_921;
1847 sens->J[3][25] = JTcp1_352_25;
1848 sens->J[3][26] = ROcp1_921;
1849 sens->J[3][27] = JTcp1_352_27;
1850 sens->J[3][28] = JTcp1_352_28;
1851 sens->J[3][29] = JTcp1_352_29;
1852 sens->J[3][30] = JTcp1_352_30;
1853 sens->J[3][31] = JTcp1_352_31;
1854 sens->J[3][32] = JTcp1_352_32;
1855 sens->J[3][33] = JTcp1_352_33;
1856 sens->J[3][34] = JTcp1_352_34;
1857 sens->J[3][35] = JTcp1_352_35;
1858 sens->J[3][36] = JTcp1_352_36;
1859 sens->J[3][37] = JTcp1_352_37;
1860 sens->J[3][38] = JTcp1_352_38;
1861 sens->J[3][39] = JTcp1_352_39;
1862 sens->J[3][40] = JTcp1_352_40;
1863 sens->J[3][41] = JTcp1_352_41;
1864 sens->J[3][42] = JTcp1_352_42;
1865 sens->J[3][43] = JTcp1_352_43;
1866 sens->J[3][44] = JTcp1_352_44;
1867 sens->J[3][45] = ROcp1_944;
1868 sens->J[3][46] = JTcp1_352_46;
1869 sens->J[3][47] = ROcp1_946;
1870 sens->J[3][48] = JTcp1_352_48;
1871 sens->J[3][49] = JTcp1_352_49;
1872 sens->J[3][50] = JTcp1_352_50;
1873 sens->J[3][51] = JTcp1_352_51;
1874 sens->J[3][52] = ROcp1_951;
1875 sens->J[4][4] = (1.0);
1876 sens->J[4][6] = S5;
1877 sens->J[4][7] = ROcp1_16;
1878 sens->J[4][9] = ROcp1_16;
1879 sens->J[4][10] = ROcp1_49;
1880 sens->J[4][11] = ROcp1_710;
1881 sens->J[4][13] = ROcp1_111;
1882 sens->J[4][14] = ROcp1_413;
1883 sens->J[4][15] = ROcp1_714;
1884 sens->J[4][17] = ROcp1_714;
1885 sens->J[4][18] = ROcp1_714;
1886 sens->J[4][20] = ROcp1_118;
1887 sens->J[4][21] = ROcp1_420;
1888 sens->J[4][22] = ROcp1_721;
1889 sens->J[4][25] = ROcp1_721;
1890 sens->J[4][27] = ROcp1_125;
1891 sens->J[4][28] = ROcp1_727;
1892 sens->J[4][29] = ROcp1_128;
1893 sens->J[4][30] = ROcp1_729;
1894 sens->J[4][31] = ROcp1_130;
1895 sens->J[4][32] = ROcp1_130;
1896 sens->J[4][33] = ROcp1_432;
1897 sens->J[4][34] = ROcp1_733;
1898 sens->J[4][35] = ROcp1_733;
1899 sens->J[4][36] = ROcp1_135;
1900 sens->J[4][37] = ROcp1_436;
1901 sens->J[4][38] = ROcp1_737;
1902 sens->J[4][39] = ROcp1_138;
1903 sens->J[4][40] = ROcp1_739;
1904 sens->J[4][41] = ROcp1_140;
1905 sens->J[4][42] = ROcp1_140;
1906 sens->J[4][43] = ROcp1_742;
1907 sens->J[4][44] = ROcp1_143;
1908 sens->J[4][46] = ROcp1_143;
1909 sens->J[4][48] = ROcp1_143;
1910 sens->J[4][49] = ROcp1_748;
1911 sens->J[4][50] = ROcp1_149;
1912 sens->J[4][51] = ROcp1_149;
1913 sens->J[4][53] = ROcp1_751;
1914 sens->J[5][5] = C4;
1915 sens->J[5][6] = ROcp1_85;
1916 sens->J[5][7] = ROcp1_26;
1917 sens->J[5][9] = ROcp1_26;
1918 sens->J[5][10] = ROcp1_59;
1919 sens->J[5][11] = ROcp1_810;
1920 sens->J[5][13] = ROcp1_211;
1921 sens->J[5][14] = ROcp1_513;
1922 sens->J[5][15] = ROcp1_814;
1923 sens->J[5][17] = ROcp1_814;
1924 sens->J[5][18] = ROcp1_814;
1925 sens->J[5][20] = ROcp1_218;
1926 sens->J[5][21] = ROcp1_520;
1927 sens->J[5][22] = ROcp1_821;
1928 sens->J[5][25] = ROcp1_821;
1929 sens->J[5][27] = ROcp1_225;
1930 sens->J[5][28] = ROcp1_827;
1931 sens->J[5][29] = ROcp1_228;
1932 sens->J[5][30] = ROcp1_829;
1933 sens->J[5][31] = ROcp1_230;
1934 sens->J[5][32] = ROcp1_230;
1935 sens->J[5][33] = ROcp1_532;
1936 sens->J[5][34] = ROcp1_833;
1937 sens->J[5][35] = ROcp1_833;
1938 sens->J[5][36] = ROcp1_235;
1939 sens->J[5][37] = ROcp1_536;
1940 sens->J[5][38] = ROcp1_837;
1941 sens->J[5][39] = ROcp1_238;
1942 sens->J[5][40] = ROcp1_839;
1943 sens->J[5][41] = ROcp1_240;
1944 sens->J[5][42] = ROcp1_240;
1945 sens->J[5][43] = ROcp1_842;
1946 sens->J[5][44] = ROcp1_243;
1947 sens->J[5][46] = ROcp1_243;
1948 sens->J[5][48] = ROcp1_243;
1949 sens->J[5][49] = ROcp1_848;
1950 sens->J[5][50] = ROcp1_249;
1951 sens->J[5][51] = ROcp1_249;
1952 sens->J[5][53] = ROcp1_851;
1953 sens->J[6][5] = S4;
1954 sens->J[6][6] = ROcp1_95;
1955 sens->J[6][7] = ROcp1_36;
1956 sens->J[6][9] = ROcp1_36;
1957 sens->J[6][10] = ROcp1_69;
1958 sens->J[6][11] = ROcp1_910;
1959 sens->J[6][13] = ROcp1_311;
1960 sens->J[6][14] = ROcp1_613;
1961 sens->J[6][15] = ROcp1_914;
1962 sens->J[6][17] = ROcp1_914;
1963 sens->J[6][18] = ROcp1_914;
1964 sens->J[6][20] = ROcp1_318;
1965 sens->J[6][21] = ROcp1_620;
1966 sens->J[6][22] = ROcp1_921;
1967 sens->J[6][25] = ROcp1_921;
1968 sens->J[6][27] = ROcp1_325;
1969 sens->J[6][28] = ROcp1_927;
1970 sens->J[6][29] = ROcp1_328;
1971 sens->J[6][30] = ROcp1_929;
1972 sens->J[6][31] = ROcp1_330;
1973 sens->J[6][32] = ROcp1_330;
1974 sens->J[6][33] = ROcp1_632;
1975 sens->J[6][34] = ROcp1_933;
1976 sens->J[6][35] = ROcp1_933;
1977 sens->J[6][36] = ROcp1_335;
1978 sens->J[6][37] = ROcp1_636;
1979 sens->J[6][38] = ROcp1_937;
1980 sens->J[6][39] = ROcp1_338;
1981 sens->J[6][40] = ROcp1_939;
1982 sens->J[6][41] = ROcp1_340;
1983 sens->J[6][42] = ROcp1_340;
1984 sens->J[6][43] = ROcp1_942;
1985 sens->J[6][44] = ROcp1_343;
1986 sens->J[6][46] = ROcp1_343;
1987 sens->J[6][48] = ROcp1_343;
1988 sens->J[6][49] = ROcp1_948;
1989 sens->J[6][50] = ROcp1_349;
1990 sens->J[6][51] = ROcp1_349;
1991 sens->J[6][53] = ROcp1_951;
1992 sens->A[1] = ACcp1_152;
1993 sens->A[2] = ACcp1_252;
1994 sens->A[3] = ACcp1_352;
1995 sens->OMP[1] = OPcp1_153;
1996 sens->OMP[2] = OPcp1_253;
1997 sens->OMP[3] = OPcp1_353;
1998
1999 //
2000 break;
2001 case 3:
2002
2003
2004
2005 // = = Block_1_0_0_3_0_1 = =
2006
2007 // Sensor Kinematics
2008
2009
2010 ROcp2_25 = S4 * S5;
2011 ROcp2_35 = -C4 * S5;
2012 ROcp2_85 = -S4 * C5;
2013 ROcp2_95 = C4 * C5;
2014 ROcp2_16 = C5 * C6;
2015 ROcp2_26 = ROcp2_25 * C6 + C4 * S6;
2016 ROcp2_36 = ROcp2_35 * C6 + S4 * S6;
2017 ROcp2_46 = -C5 * S6;
2018 ROcp2_56 = -(ROcp2_25 * S6 - C4 * C6);
2019 ROcp2_66 = -(ROcp2_35 * S6 - S4 * C6);
2020 ROcp2_47 = ROcp2_46 * C7 + S5 * S7;
2021 ROcp2_57 = ROcp2_56 * C7 + ROcp2_85 * S7;
2022 ROcp2_67 = ROcp2_66 * C7 + ROcp2_95 * S7;
2023 ROcp2_77 = -(ROcp2_46 * S7 - S5 * C7);
2024 ROcp2_87 = -(ROcp2_56 * S7 - ROcp2_85 * C7);
2025 ROcp2_97 = -(ROcp2_66 * S7 - ROcp2_95 * C7);
2026 ROcp2_49 = ROcp2_47 * C9 + ROcp2_77 * S9;
2027 ROcp2_59 = ROcp2_57 * C9 + ROcp2_87 * S9;
2028 ROcp2_69 = ROcp2_67 * C9 + ROcp2_97 * S9;
2029 ROcp2_79 = -(ROcp2_47 * S9 - ROcp2_77 * C9);
2030 ROcp2_89 = -(ROcp2_57 * S9 - ROcp2_87 * C9);
2031 ROcp2_99 = -(ROcp2_67 * S9 - ROcp2_97 * C9);
2041 ROcp2_411 = -(ROcp2_110 * S11 - ROcp2_49 * C11);
2042 ROcp2_511 = -(ROcp2_210 * S11 - ROcp2_59 * C11);
2043 ROcp2_611 = -(ROcp2_310 * S11 - ROcp2_69 * C11);
2047 ROcp2_713 = -(ROcp2_411 * S13 - ROcp2_710 * C13);
2048 ROcp2_813 = -(ROcp2_511 * S13 - ROcp2_810 * C13);
2049 ROcp2_913 = -(ROcp2_611 * S13 - ROcp2_910 * C13);
2059 ROcp2_415 = -(ROcp2_114 * S15 - ROcp2_413 * C15);
2060 ROcp2_515 = -(ROcp2_214 * S15 - ROcp2_513 * C15);
2061 ROcp2_615 = -(ROcp2_314 * S15 - ROcp2_613 * C15);
2065 ROcp2_417 = -(ROcp2_115 * S17 - ROcp2_415 * C17);
2066 ROcp2_517 = -(ROcp2_215 * S17 - ROcp2_515 * C17);
2067 ROcp2_617 = -(ROcp2_315 * S17 - ROcp2_615 * C17);
2071 ROcp2_418 = -(ROcp2_117 * S18 - ROcp2_417 * C18);
2072 ROcp2_518 = -(ROcp2_217 * S18 - ROcp2_517 * C18);
2073 ROcp2_618 = -(ROcp2_317 * S18 - ROcp2_617 * C18);
2077 ROcp2_720 = -(ROcp2_418 * S20 - ROcp2_714 * C20);
2078 ROcp2_820 = -(ROcp2_518 * S20 - ROcp2_814 * C20);
2079 ROcp2_920 = -(ROcp2_618 * S20 - ROcp2_914 * C20);
2089 ROcp2_422 = -(ROcp2_121 * S22 - ROcp2_420 * C22);
2090 ROcp2_522 = -(ROcp2_221 * S22 - ROcp2_520 * C22);
2091 ROcp2_622 = -(ROcp2_321 * S22 - ROcp2_620 * C22);
2095 ROcp2_425 = -(ROcp2_122 * S25 - ROcp2_422 * C25);
2096 ROcp2_525 = -(ROcp2_222 * S25 - ROcp2_522 * C25);
2097 ROcp2_625 = -(ROcp2_322 * S25 - ROcp2_622 * C25);
2101 ROcp2_727 = -(ROcp2_425 * S27 - ROcp2_721 * C27);
2102 ROcp2_827 = -(ROcp2_525 * S27 - ROcp2_821 * C27);
2103 ROcp2_927 = -(ROcp2_625 * S27 - ROcp2_921 * C27);
2107 ROcp2_428 = -(ROcp2_125 * S28 - ROcp2_427 * C28);
2108 ROcp2_528 = -(ROcp2_225 * S28 - ROcp2_527 * C28);
2109 ROcp2_628 = -(ROcp2_325 * S28 - ROcp2_627 * C28);
2113 ROcp2_729 = -(ROcp2_428 * S29 - ROcp2_727 * C29);
2114 ROcp2_829 = -(ROcp2_528 * S29 - ROcp2_827 * C29);
2115 ROcp2_929 = -(ROcp2_628 * S29 - ROcp2_927 * C29);
2119 ROcp2_430 = -(ROcp2_128 * S30 - ROcp2_429 * C30);
2120 ROcp2_530 = -(ROcp2_228 * S30 - ROcp2_529 * C30);
2121 ROcp2_630 = -(ROcp2_328 * S30 - ROcp2_629 * C30);
2125 ROcp2_731 = -(ROcp2_430 * S31 - ROcp2_729 * C31);
2126 ROcp2_831 = -(ROcp2_530 * S31 - ROcp2_829 * C31);
2127 ROcp2_931 = -(ROcp2_630 * S31 - ROcp2_929 * C31);
2131 ROcp2_732 = -(ROcp2_431 * S32 - ROcp2_731 * C32);
2132 ROcp2_832 = -(ROcp2_531 * S32 - ROcp2_831 * C32);
2133 ROcp2_932 = -(ROcp2_631 * S32 - ROcp2_931 * C32);
2143 ROcp2_434 = -(ROcp2_133 * S34 - ROcp2_432 * C34);
2144 ROcp2_534 = -(ROcp2_233 * S34 - ROcp2_532 * C34);
2145 ROcp2_634 = -(ROcp2_333 * S34 - ROcp2_632 * C34);
2149 ROcp2_435 = -(ROcp2_134 * S35 - ROcp2_434 * C35);
2150 ROcp2_535 = -(ROcp2_234 * S35 - ROcp2_534 * C35);
2151 ROcp2_635 = -(ROcp2_334 * S35 - ROcp2_634 * C35);
2155 ROcp2_736 = -(ROcp2_435 * S36 - ROcp2_733 * C36);
2156 ROcp2_836 = -(ROcp2_535 * S36 - ROcp2_833 * C36);
2157 ROcp2_936 = -(ROcp2_635 * S36 - ROcp2_933 * C36);
2167 ROcp2_438 = -(ROcp2_137 * S38 - ROcp2_436 * C38);
2168 ROcp2_538 = -(ROcp2_237 * S38 - ROcp2_536 * C38);
2169 ROcp2_638 = -(ROcp2_337 * S38 - ROcp2_636 * C38);
2173 ROcp2_739 = -(ROcp2_438 * S39 - ROcp2_737 * C39);
2174 ROcp2_839 = -(ROcp2_538 * S39 - ROcp2_837 * C39);
2175 ROcp2_939 = -(ROcp2_638 * S39 - ROcp2_937 * C39);
2179 ROcp2_440 = -(ROcp2_138 * S40 - ROcp2_439 * C40);
2180 ROcp2_540 = -(ROcp2_238 * S40 - ROcp2_539 * C40);
2181 ROcp2_640 = -(ROcp2_338 * S40 - ROcp2_639 * C40);
2185 ROcp2_741 = -(ROcp2_440 * S41 - ROcp2_739 * C41);
2186 ROcp2_841 = -(ROcp2_540 * S41 - ROcp2_839 * C41);
2187 ROcp2_941 = -(ROcp2_640 * S41 - ROcp2_939 * C41);
2191 ROcp2_742 = -(ROcp2_441 * S42 - ROcp2_741 * C42);
2192 ROcp2_842 = -(ROcp2_541 * S42 - ROcp2_841 * C42);
2193 ROcp2_942 = -(ROcp2_641 * S42 - ROcp2_941 * C42);
2197 ROcp2_443 = -(ROcp2_140 * S43 - ROcp2_442 * C43);
2198 ROcp2_543 = -(ROcp2_240 * S43 - ROcp2_542 * C43);
2199 ROcp2_643 = -(ROcp2_340 * S43 - ROcp2_642 * C43);
2203 ROcp2_744 = -(ROcp2_443 * S44 - ROcp2_742 * C44);
2204 ROcp2_844 = -(ROcp2_543 * S44 - ROcp2_842 * C44);
2205 ROcp2_944 = -(ROcp2_643 * S44 - ROcp2_942 * C44);
2209 ROcp2_746 = -(ROcp2_444 * S46 - ROcp2_744 * C46);
2210 ROcp2_846 = -(ROcp2_544 * S46 - ROcp2_844 * C46);
2211 ROcp2_946 = -(ROcp2_644 * S46 - ROcp2_944 * C46);
2215 ROcp2_748 = -(ROcp2_446 * S48 - ROcp2_746 * C48);
2216 ROcp2_848 = -(ROcp2_546 * S48 - ROcp2_846 * C48);
2217 ROcp2_948 = -(ROcp2_646 * S48 - ROcp2_946 * C48);
2221 ROcp2_449 = -(ROcp2_143 * S49 - ROcp2_448 * C49);
2222 ROcp2_549 = -(ROcp2_243 * S49 - ROcp2_548 * C49);
2223 ROcp2_649 = -(ROcp2_343 * S49 - ROcp2_648 * C49);
2227 ROcp2_750 = -(ROcp2_449 * S50 - ROcp2_748 * C50);
2228 ROcp2_850 = -(ROcp2_549 * S50 - ROcp2_848 * C50);
2229 ROcp2_950 = -(ROcp2_649 * S50 - ROcp2_948 * C50);
2233 ROcp2_751 = -(ROcp2_450 * S51 - ROcp2_750 * C51);
2234 ROcp2_851 = -(ROcp2_550 * S51 - ROcp2_850 * C51);
2235 ROcp2_951 = -(ROcp2_650 * S51 - ROcp2_950 * C51);
2239 ROcp2_453 = -(ROcp2_149 * S53 - ROcp2_451 * C53);
2240 ROcp2_553 = -(ROcp2_249 * S53 - ROcp2_551 * C53);
2241 ROcp2_653 = -(ROcp2_349 * S53 - ROcp2_651 * C53);
2251 ROcp2_456 = -(ROcp2_155 * S56 - ROcp2_453 * C56);
2252 ROcp2_556 = -(ROcp2_255 * S56 - ROcp2_553 * C56);
2253 ROcp2_656 = -(ROcp2_355 * S56 - ROcp2_653 * C56);
2254 OMcp2_25 = qd[5] * C4;
2255 OMcp2_35 = qd[5] * S4;
2256 OMcp2_16 = qd[4] + qd[6] * S5;
2257 OMcp2_26 = OMcp2_25 + ROcp2_85 * qd[6];
2258 OMcp2_36 = OMcp2_35 + ROcp2_95 * qd[6];
2259 OPcp2_16 = qdd[4] + qdd[6] * S5 + qd[5] * qd[6] * C5;
2260 OPcp2_26 = ROcp2_85 * qdd[6] + qdd[5] * C4 - qd[4] * qd[5] * S4 + qd[6] * (OMcp2_35 * S5 - ROcp2_95 * qd[4]);
2261 OPcp2_36 = ROcp2_95 * qdd[6] + qdd[5] * S4 + qd[4] * qd[5] * C4 - qd[6] * (OMcp2_25 * S5 - ROcp2_85 * qd[4]);
2262 RLcp2_18 = ROcp2_77 * q[8];
2263 RLcp2_28 = ROcp2_87 * q[8];
2264 RLcp2_38 = ROcp2_97 * q[8];
2268 RLcp2_112 = ROcp2_710 * q[12];
2269 RLcp2_212 = ROcp2_810 * q[12];
2270 RLcp2_312 = ROcp2_910 * q[12];
2274 RLcp2_116 = ROcp2_714 * q[16];
2275 RLcp2_216 = ROcp2_814 * q[16];
2276 RLcp2_316 = ROcp2_914 * q[16];
2280 OMcp2_118 = OMcp2_16 + ROcp2_714 * qd[18];
2281 OMcp2_218 = OMcp2_26 + ROcp2_814 * qd[18];
2282 OMcp2_318 = OMcp2_36 + ROcp2_914 * qd[18];
2286 RLcp2_119 = ROcp2_714 * q[19];
2287 RLcp2_219 = ROcp2_814 * q[19];
2288 RLcp2_319 = ROcp2_914 * q[19];
2295 OMcp2_120 = OMcp2_118 + ROcp2_118 * qd[20];
2296 OMcp2_220 = OMcp2_218 + ROcp2_218 * qd[20];
2297 OMcp2_320 = OMcp2_318 + ROcp2_318 * qd[20];
2301 OMcp2_121 = OMcp2_120 + ROcp2_420 * qd[21];
2302 OMcp2_221 = OMcp2_220 + ROcp2_520 * qd[21];
2303 OMcp2_321 = OMcp2_320 + ROcp2_620 * qd[21];
2304 OMcp2_122 = OMcp2_121 + ROcp2_721 * qd[22];
2305 OMcp2_222 = OMcp2_221 + ROcp2_821 * qd[22];
2306 OMcp2_322 = OMcp2_321 + ROcp2_921 * qd[22];
2307 OPcp2_122 = OPcp2_118 + ROcp2_118 * qdd[20] + ROcp2_420 * qdd[21] + ROcp2_721 * qdd[22] + qd[20] * (OMcp2_218 * ROcp2_318 - OMcp2_318 *
2309 OPcp2_222 = OPcp2_218 + ROcp2_218 * qdd[20] + ROcp2_520 * qdd[21] + ROcp2_821 * qdd[22] - qd[20] * (OMcp2_118 * ROcp2_318 - OMcp2_318 *
2311 OPcp2_322 = OPcp2_318 + ROcp2_318 * qdd[20] + ROcp2_620 * qdd[21] + ROcp2_921 * qdd[22] + qd[20] * (OMcp2_118 * ROcp2_218 - OMcp2_218 *
2325 RLcp2_126 = ROcp2_721 * q[26];
2326 RLcp2_226 = ROcp2_821 * q[26];
2327 RLcp2_326 = ROcp2_921 * q[26];
2331 OMcp2_130 = OMcp2_122 + ROcp2_729 * qd[30];
2332 OMcp2_230 = OMcp2_222 + ROcp2_829 * qd[30];
2333 OMcp2_330 = OMcp2_322 + ROcp2_929 * qd[30];
2334 OMcp2_135 = OMcp2_130 + ROcp2_733 * qd[35];
2335 OMcp2_235 = OMcp2_230 + ROcp2_833 * qd[35];
2336 OMcp2_335 = OMcp2_330 + ROcp2_933 * qd[35];
2337 OMcp2_140 = OMcp2_135 + ROcp2_739 * qd[40];
2338 OMcp2_240 = OMcp2_235 + ROcp2_839 * qd[40];
2339 OMcp2_340 = OMcp2_335 + ROcp2_939 * qd[40];
2340 OPcp2_140 = OPcp2_122 + ROcp2_729 * qdd[30] + ROcp2_733 * qdd[35] + ROcp2_739 * qdd[40] + qd[30] * (OMcp2_222 * ROcp2_929 - OMcp2_322 *
2342 OPcp2_240 = OPcp2_222 + ROcp2_829 * qdd[30] + ROcp2_833 * qdd[35] + ROcp2_839 * qdd[40] - qd[30] * (OMcp2_122 * ROcp2_929 - OMcp2_322 *
2344 OPcp2_340 = OPcp2_322 + ROcp2_929 * qdd[30] + ROcp2_933 * qdd[35] + ROcp2_939 * qdd[40] + qd[30] * (OMcp2_122 * ROcp2_829 - OMcp2_222 *
2352 OMcp2_143 = OMcp2_140 + ROcp2_742 * qd[43];
2353 OMcp2_243 = OMcp2_240 + ROcp2_842 * qd[43];
2354 OMcp2_343 = OMcp2_340 + ROcp2_942 * qd[43];
2358 RLcp2_145 = ROcp2_744 * q[45];
2359 RLcp2_245 = ROcp2_844 * q[45];
2360 RLcp2_345 = ROcp2_944 * q[45];
2370 RLcp2_147 = ROcp2_746 * q[47];
2371 RLcp2_247 = ROcp2_846 * q[47];
2372 RLcp2_347 = ROcp2_946 * q[47];
2382 OMcp2_149 = OMcp2_143 + ROcp2_748 * qd[49];
2383 OMcp2_249 = OMcp2_243 + ROcp2_848 * qd[49];
2384 OMcp2_349 = OMcp2_343 + ROcp2_948 * qd[49];
2394 RLcp2_152 = ROcp2_751 * q[52];
2395 RLcp2_252 = ROcp2_851 * q[52];
2396 RLcp2_352 = ROcp2_951 * q[52];
2400 OMcp2_153 = OMcp2_149 + ROcp2_751 * qd[53];
2401 OMcp2_253 = OMcp2_249 + ROcp2_851 * qd[53];
2402 OMcp2_353 = OMcp2_349 + ROcp2_951 * qd[53];
2406 RLcp2_154 = ROcp2_153 * q[54];
2407 RLcp2_254 = ROcp2_253 * q[54];
2408 RLcp2_354 = ROcp2_353 * q[54];
2422 + RLcp2_254);
2434 ) - S5 * (RLcp2_348 + RLcp2_351);
2439 //JTcp2_154_7 = ROcp2_26 * (RLcp2_312 + RLcp2_316 + RLcp2_319 + RLcp2_320 + RLcp2_323 + RLcp2_324 + RLcp2_326 + RLcp2_342 + RLcp2_345 +
2440 // RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354 + RLcp2_38) - ROcp2_36 * (RLcp2_212 + RLcp2_28) - ROcp2_36 * (RLcp2_216 +
2441 // RLcp2_219) - ROcp2_36 * (RLcp2_220 + RLcp2_223) - ROcp2_36 * (RLcp2_224 + RLcp2_226) - ROcp2_36 * (RLcp2_242 + RLcp2_245) - ROcp2_36 * (RLcp2_246 +
2442 // RLcp2_247) - ROcp2_36 * (RLcp2_248 + RLcp2_251) - ROcp2_36 * (RLcp2_252 + RLcp2_254);
2443 //JTcp2_254_7 = -(ROcp2_16 * (RLcp2_312 + RLcp2_316 + RLcp2_319 + RLcp2_320 + RLcp2_323 + RLcp2_324 + RLcp2_326 + RLcp2_342 + RLcp2_345 +
2444 // RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354 + RLcp2_38) - ROcp2_36 * (RLcp2_112 + RLcp2_18) - ROcp2_36 * (RLcp2_116 +
2445 // RLcp2_119) - ROcp2_36 * (RLcp2_120 + RLcp2_123) - ROcp2_36 * (RLcp2_124 + RLcp2_126) - ROcp2_36 * (RLcp2_142 + RLcp2_145) - ROcp2_36 * (RLcp2_146 +
2446 // RLcp2_147) - ROcp2_36 * (RLcp2_148 + RLcp2_151) - ROcp2_36 * (RLcp2_152 + RLcp2_154));
2447 //JTcp2_354_7 = ROcp2_16 * (RLcp2_212 + RLcp2_216 + RLcp2_219 + RLcp2_220 + RLcp2_223 + RLcp2_224 + RLcp2_226 + RLcp2_242 + RLcp2_245 +
2448 // RLcp2_246 + RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252 + RLcp2_254 + RLcp2_28) - ROcp2_26 * (RLcp2_112 + RLcp2_18) - ROcp2_26 * (RLcp2_116 +
2449 // RLcp2_119) - ROcp2_26 * (RLcp2_120 + RLcp2_123) - ROcp2_26 * (RLcp2_124 + RLcp2_126) - ROcp2_26 * (RLcp2_142 + RLcp2_145) - ROcp2_26 * (RLcp2_146 +
2450 // RLcp2_147) - ROcp2_26 * (RLcp2_148 + RLcp2_151) - ROcp2_26 * (RLcp2_152 + RLcp2_154);
2451 //JTcp2_154_9 = ROcp2_26 * (RLcp2_312 + RLcp2_316 + RLcp2_319 + RLcp2_320 + RLcp2_323 + RLcp2_324 + RLcp2_326 + RLcp2_342 + RLcp2_345 +
2452 // RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352) - ROcp2_36 * (RLcp2_212 + RLcp2_216) - ROcp2_36 * (RLcp2_219 + RLcp2_220) - ROcp2_36 * (
2453 // RLcp2_223 + RLcp2_224) - ROcp2_36 * (RLcp2_226 + RLcp2_242) - ROcp2_36 * (RLcp2_245 + RLcp2_246) - ROcp2_36 * (RLcp2_247 + RLcp2_248) - ROcp2_36 * (
2454 // RLcp2_251 + RLcp2_252) - RLcp2_254 * ROcp2_36 + RLcp2_354 * ROcp2_26;
2455 //JTcp2_254_9 = RLcp2_154 * ROcp2_36 - RLcp2_354 * ROcp2_16 - ROcp2_16 * (RLcp2_312 + RLcp2_316 + RLcp2_319 + RLcp2_320 + RLcp2_323 +
2456 // RLcp2_324 + RLcp2_326 + RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352) + ROcp2_36 * (RLcp2_112 + RLcp2_116) +
2457 // ROcp2_36 * (RLcp2_119 + RLcp2_120) + ROcp2_36 * (RLcp2_123 + RLcp2_124) + ROcp2_36 * (RLcp2_126 + RLcp2_142) + ROcp2_36 * (RLcp2_145 + RLcp2_146) +
2458 // ROcp2_36 * (RLcp2_147 + RLcp2_148) + ROcp2_36 * (RLcp2_151 + RLcp2_152);
2459 //JTcp2_354_9 = ROcp2_16 * (RLcp2_212 + RLcp2_216 + RLcp2_219 + RLcp2_220 + RLcp2_223 + RLcp2_224 + RLcp2_226 + RLcp2_242 + RLcp2_245 +
2460 // RLcp2_246 + RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252) - ROcp2_26 * (RLcp2_112 + RLcp2_116) - ROcp2_26 * (RLcp2_119 + RLcp2_120) - ROcp2_26 * (
2461 // RLcp2_123 + RLcp2_124) - ROcp2_26 * (RLcp2_126 + RLcp2_142) - ROcp2_26 * (RLcp2_145 + RLcp2_146) - ROcp2_26 * (RLcp2_147 + RLcp2_148) - ROcp2_26 * (
2462 // RLcp2_151 + RLcp2_152) - RLcp2_154 * ROcp2_26 + RLcp2_254 * ROcp2_16;
2463 //JTcp2_154_10 = ROcp2_59 * (RLcp2_312 + RLcp2_316 + RLcp2_319 + RLcp2_320 + RLcp2_323 + RLcp2_324 + RLcp2_326 + RLcp2_342 + RLcp2_345 +
2464 // RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352) - ROcp2_69 * (RLcp2_212 + RLcp2_216) - ROcp2_69 * (RLcp2_219 + RLcp2_220) - ROcp2_69 * (
2465 // RLcp2_223 + RLcp2_224) - ROcp2_69 * (RLcp2_226 + RLcp2_242) - ROcp2_69 * (RLcp2_245 + RLcp2_246) - ROcp2_69 * (RLcp2_247 + RLcp2_248) - ROcp2_69 * (
2466 // RLcp2_251 + RLcp2_252) - RLcp2_254 * ROcp2_69 + RLcp2_354 * ROcp2_59;
2467 //JTcp2_254_10 = RLcp2_154 * ROcp2_69 - RLcp2_354 * ROcp2_49 - ROcp2_49 * (RLcp2_312 + RLcp2_316 + RLcp2_319 + RLcp2_320 + RLcp2_323 +
2468 // RLcp2_324 + RLcp2_326 + RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352) + ROcp2_69 * (RLcp2_112 + RLcp2_116) +
2469 // ROcp2_69 * (RLcp2_119 + RLcp2_120) + ROcp2_69 * (RLcp2_123 + RLcp2_124) + ROcp2_69 * (RLcp2_126 + RLcp2_142) + ROcp2_69 * (RLcp2_145 + RLcp2_146) +
2470 // ROcp2_69 * (RLcp2_147 + RLcp2_148) + ROcp2_69 * (RLcp2_151 + RLcp2_152);
2471 //JTcp2_354_10 = ROcp2_49 * (RLcp2_212 + RLcp2_216 + RLcp2_219 + RLcp2_220 + RLcp2_223 + RLcp2_224 + RLcp2_226 + RLcp2_242 + RLcp2_245 +
2472 // RLcp2_246 + RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252) - ROcp2_59 * (RLcp2_112 + RLcp2_116) - ROcp2_59 * (RLcp2_119 + RLcp2_120) - ROcp2_59 * (
2473 // RLcp2_123 + RLcp2_124) - ROcp2_59 * (RLcp2_126 + RLcp2_142) - ROcp2_59 * (RLcp2_145 + RLcp2_146) - ROcp2_59 * (RLcp2_147 + RLcp2_148) - ROcp2_59 * (
2474 // RLcp2_151 + RLcp2_152) - RLcp2_154 * ROcp2_59 + RLcp2_254 * ROcp2_49;
2475 //JTcp2_154_11 = ROcp2_810 * (RLcp2_312 + RLcp2_316 + RLcp2_319 + RLcp2_320 + RLcp2_323 + RLcp2_324 + RLcp2_326 + RLcp2_342 + RLcp2_345 +
2476 // RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352) - ROcp2_910 * (RLcp2_212 + RLcp2_216) - ROcp2_910 * (RLcp2_219 + RLcp2_220) - ROcp2_910
2477 // * (RLcp2_223 + RLcp2_224) - ROcp2_910 * (RLcp2_226 + RLcp2_242) - ROcp2_910 * (RLcp2_245 + RLcp2_246) - ROcp2_910 * (RLcp2_247 + RLcp2_248) -
2478 // ROcp2_910 * (RLcp2_251 + RLcp2_252) - RLcp2_254 * ROcp2_910 + RLcp2_354 * ROcp2_810;
2479 //JTcp2_254_11 = RLcp2_154 * ROcp2_910 - RLcp2_354 * ROcp2_710 - ROcp2_710 * (RLcp2_312 + RLcp2_316 + RLcp2_319 + RLcp2_320 + RLcp2_323 +
2480 // RLcp2_324 + RLcp2_326 + RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352) + ROcp2_910 * (RLcp2_112 + RLcp2_116) +
2481 // ROcp2_910 * (RLcp2_119 + RLcp2_120) + ROcp2_910 * (RLcp2_123 + RLcp2_124) + ROcp2_910 * (RLcp2_126 + RLcp2_142) + ROcp2_910 * (RLcp2_145 +
2482 // RLcp2_146) + ROcp2_910 * (RLcp2_147 + RLcp2_148) + ROcp2_910 * (RLcp2_151 + RLcp2_152);
2483 //JTcp2_354_11 = ROcp2_710 * (RLcp2_212 + RLcp2_216 + RLcp2_219 + RLcp2_220 + RLcp2_223 + RLcp2_224 + RLcp2_226 + RLcp2_242 + RLcp2_245 +
2484 // RLcp2_246 + RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252) - ROcp2_810 * (RLcp2_112 + RLcp2_116) - ROcp2_810 * (RLcp2_119 + RLcp2_120) - ROcp2_810
2485 // * (RLcp2_123 + RLcp2_124) - ROcp2_810 * (RLcp2_126 + RLcp2_142) - ROcp2_810 * (RLcp2_145 + RLcp2_146) - ROcp2_810 * (RLcp2_147 + RLcp2_148) -
2486 // ROcp2_810 * (RLcp2_151 + RLcp2_152) - RLcp2_154 * ROcp2_810 + RLcp2_254 * ROcp2_710;
2487 //JTcp2_154_13 = ROcp2_211 * (RLcp2_316 + RLcp2_319 + RLcp2_320 + RLcp2_323 + RLcp2_324 + RLcp2_326 + RLcp2_342 + RLcp2_345 + RLcp2_346 +
2488 // RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_311 * (RLcp2_216 + RLcp2_219) - ROcp2_311 * (RLcp2_220 + RLcp2_223) - ROcp2_311
2489 // * (RLcp2_224 + RLcp2_226) - ROcp2_311 * (RLcp2_242 + RLcp2_245) - ROcp2_311 * (RLcp2_246 + RLcp2_247) - ROcp2_311 * (RLcp2_248 + RLcp2_251) -
2490 // ROcp2_311 * (RLcp2_252 + RLcp2_254);
2491 //JTcp2_254_13 = -(ROcp2_111 * (RLcp2_316 + RLcp2_319 + RLcp2_320 + RLcp2_323 + RLcp2_324 + RLcp2_326 + RLcp2_342 + RLcp2_345 + RLcp2_346 +
2492 // RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_311 * (RLcp2_116 + RLcp2_119) - ROcp2_311 * (RLcp2_120 + RLcp2_123) - ROcp2_311
2493 // * (RLcp2_124 + RLcp2_126) - ROcp2_311 * (RLcp2_142 + RLcp2_145) - ROcp2_311 * (RLcp2_146 + RLcp2_147) - ROcp2_311 * (RLcp2_148 + RLcp2_151) -
2494 // ROcp2_311 * (RLcp2_152 + RLcp2_154));
2495 //JTcp2_354_13 = ROcp2_111 * (RLcp2_216 + RLcp2_219 + RLcp2_220 + RLcp2_223 + RLcp2_224 + RLcp2_226 + RLcp2_242 + RLcp2_245 + RLcp2_246 +
2496 // RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252 + RLcp2_254) - ROcp2_211 * (RLcp2_116 + RLcp2_119) - ROcp2_211 * (RLcp2_120 + RLcp2_123) - ROcp2_211
2497 // * (RLcp2_124 + RLcp2_126) - ROcp2_211 * (RLcp2_142 + RLcp2_145) - ROcp2_211 * (RLcp2_146 + RLcp2_147) - ROcp2_211 * (RLcp2_148 + RLcp2_151) -
2498 // ROcp2_211 * (RLcp2_152 + RLcp2_154);
2499 //JTcp2_154_14 = ROcp2_513 * (RLcp2_316 + RLcp2_319 + RLcp2_320 + RLcp2_323 + RLcp2_324 + RLcp2_326 + RLcp2_342 + RLcp2_345 + RLcp2_346 +
2500 // RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_613 * (RLcp2_216 + RLcp2_219) - ROcp2_613 * (RLcp2_220 + RLcp2_223) - ROcp2_613
2501 // * (RLcp2_224 + RLcp2_226) - ROcp2_613 * (RLcp2_242 + RLcp2_245) - ROcp2_613 * (RLcp2_246 + RLcp2_247) - ROcp2_613 * (RLcp2_248 + RLcp2_251) -
2502 // ROcp2_613 * (RLcp2_252 + RLcp2_254);
2503 //JTcp2_254_14 = -(ROcp2_413 * (RLcp2_316 + RLcp2_319 + RLcp2_320 + RLcp2_323 + RLcp2_324 + RLcp2_326 + RLcp2_342 + RLcp2_345 + RLcp2_346 +
2504 // RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_613 * (RLcp2_116 + RLcp2_119) - ROcp2_613 * (RLcp2_120 + RLcp2_123) - ROcp2_613
2505 // * (RLcp2_124 + RLcp2_126) - ROcp2_613 * (RLcp2_142 + RLcp2_145) - ROcp2_613 * (RLcp2_146 + RLcp2_147) - ROcp2_613 * (RLcp2_148 + RLcp2_151) -
2506 // ROcp2_613 * (RLcp2_152 + RLcp2_154));
2507 //JTcp2_354_14 = ROcp2_413 * (RLcp2_216 + RLcp2_219 + RLcp2_220 + RLcp2_223 + RLcp2_224 + RLcp2_226 + RLcp2_242 + RLcp2_245 + RLcp2_246 +
2508 // RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252 + RLcp2_254) - ROcp2_513 * (RLcp2_116 + RLcp2_119) - ROcp2_513 * (RLcp2_120 + RLcp2_123) - ROcp2_513
2509 // * (RLcp2_124 + RLcp2_126) - ROcp2_513 * (RLcp2_142 + RLcp2_145) - ROcp2_513 * (RLcp2_146 + RLcp2_147) - ROcp2_513 * (RLcp2_148 + RLcp2_151) -
2510 // ROcp2_513 * (RLcp2_152 + RLcp2_154);
2511 //JTcp2_154_15 = ROcp2_814 * (RLcp2_316 + RLcp2_319 + RLcp2_320 + RLcp2_323 + RLcp2_324 + RLcp2_326 + RLcp2_342 + RLcp2_345 + RLcp2_346 +
2512 // RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_914 * (RLcp2_216 + RLcp2_219) - ROcp2_914 * (RLcp2_220 + RLcp2_223) - ROcp2_914
2513 // * (RLcp2_224 + RLcp2_226) - ROcp2_914 * (RLcp2_242 + RLcp2_245) - ROcp2_914 * (RLcp2_246 + RLcp2_247) - ROcp2_914 * (RLcp2_248 + RLcp2_251) -
2514 // ROcp2_914 * (RLcp2_252 + RLcp2_254);
2515 //JTcp2_254_15 = -(ROcp2_714 * (RLcp2_316 + RLcp2_319 + RLcp2_320 + RLcp2_323 + RLcp2_324 + RLcp2_326 + RLcp2_342 + RLcp2_345 + RLcp2_346 +
2516 // RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_914 * (RLcp2_116 + RLcp2_119) - ROcp2_914 * (RLcp2_120 + RLcp2_123) - ROcp2_914
2517 // * (RLcp2_124 + RLcp2_126) - ROcp2_914 * (RLcp2_142 + RLcp2_145) - ROcp2_914 * (RLcp2_146 + RLcp2_147) - ROcp2_914 * (RLcp2_148 + RLcp2_151) -
2518 // ROcp2_914 * (RLcp2_152 + RLcp2_154));
2519 //JTcp2_354_15 = ROcp2_714 * (RLcp2_216 + RLcp2_219 + RLcp2_220 + RLcp2_223 + RLcp2_224 + RLcp2_226 + RLcp2_242 + RLcp2_245 + RLcp2_246 +
2520 // RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252 + RLcp2_254) - ROcp2_814 * (RLcp2_116 + RLcp2_119) - ROcp2_814 * (RLcp2_120 + RLcp2_123) - ROcp2_814
2521 // * (RLcp2_124 + RLcp2_126) - ROcp2_814 * (RLcp2_142 + RLcp2_145) - ROcp2_814 * (RLcp2_146 + RLcp2_147) - ROcp2_814 * (RLcp2_148 + RLcp2_151) -
2522 // ROcp2_814 * (RLcp2_152 + RLcp2_154);
2523 //JTcp2_154_17 = ROcp2_814 * (RLcp2_319 + RLcp2_320 + RLcp2_323 + RLcp2_324 + RLcp2_326 + RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 +
2524 // RLcp2_348 + RLcp2_351 + RLcp2_352) - ROcp2_914 * (RLcp2_219 + RLcp2_220) - ROcp2_914 * (RLcp2_223 + RLcp2_224) - ROcp2_914 * (RLcp2_226 +
2525 // RLcp2_242) - ROcp2_914 * (RLcp2_245 + RLcp2_246) - ROcp2_914 * (RLcp2_247 + RLcp2_248) - ROcp2_914 * (RLcp2_251 + RLcp2_252) - RLcp2_254 *
2526 // ROcp2_914 + RLcp2_354 * ROcp2_814;
2527 //JTcp2_254_17 = RLcp2_154 * ROcp2_914 - RLcp2_354 * ROcp2_714 - ROcp2_714 * (RLcp2_319 + RLcp2_320 + RLcp2_323 + RLcp2_324 + RLcp2_326 +
2528 // RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352) + ROcp2_914 * (RLcp2_119 + RLcp2_120) + ROcp2_914 * (RLcp2_123 +
2529 // RLcp2_124) + ROcp2_914 * (RLcp2_126 + RLcp2_142) + ROcp2_914 * (RLcp2_145 + RLcp2_146) + ROcp2_914 * (RLcp2_147 + RLcp2_148) + ROcp2_914 * (
2530 // RLcp2_151 + RLcp2_152);
2531 //JTcp2_354_17 = ROcp2_714 * (RLcp2_219 + RLcp2_220 + RLcp2_223 + RLcp2_224 + RLcp2_226 + RLcp2_242 + RLcp2_245 + RLcp2_246 + RLcp2_247 +
2532 // RLcp2_248 + RLcp2_251 + RLcp2_252) - ROcp2_814 * (RLcp2_119 + RLcp2_120) - ROcp2_814 * (RLcp2_123 + RLcp2_124) - ROcp2_814 * (RLcp2_126 +
2533 // RLcp2_142) - ROcp2_814 * (RLcp2_145 + RLcp2_146) - ROcp2_814 * (RLcp2_147 + RLcp2_148) - ROcp2_814 * (RLcp2_151 + RLcp2_152) - RLcp2_154 *
2534 // ROcp2_814 + RLcp2_254 * ROcp2_714;
2574 //JTcp2_154_25 = ROcp2_821 * (RLcp2_326 + RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352) - ROcp2_921 * (
2575 // RLcp2_226 + RLcp2_242) - ROcp2_921 * (RLcp2_245 + RLcp2_246) - ROcp2_921 * (RLcp2_247 + RLcp2_248) - ROcp2_921 * (RLcp2_251 + RLcp2_252) -
2576 // RLcp2_254 * ROcp2_921 + RLcp2_354 * ROcp2_821;
2577 //JTcp2_254_25 = RLcp2_154 * ROcp2_921 - RLcp2_354 * ROcp2_721 - ROcp2_721 * (RLcp2_326 + RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 +
2578 // RLcp2_348 + RLcp2_351 + RLcp2_352) + ROcp2_921 * (RLcp2_126 + RLcp2_142) + ROcp2_921 * (RLcp2_145 + RLcp2_146) + ROcp2_921 * (RLcp2_147 +
2579 // RLcp2_148) + ROcp2_921 * (RLcp2_151 + RLcp2_152);
2580 //JTcp2_354_25 = ROcp2_721 * (RLcp2_226 + RLcp2_242 + RLcp2_245 + RLcp2_246 + RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252) - ROcp2_821 * (
2581 // RLcp2_126 + RLcp2_142) - ROcp2_821 * (RLcp2_145 + RLcp2_146) - ROcp2_821 * (RLcp2_147 + RLcp2_148) - ROcp2_821 * (RLcp2_151 + RLcp2_152) -
2582 // RLcp2_154 * ROcp2_821 + RLcp2_254 * ROcp2_721;
2583 //JTcp2_154_27 = ROcp2_225 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_325 * (
2584 // RLcp2_242 + RLcp2_245) - ROcp2_325 * (RLcp2_246 + RLcp2_247) - ROcp2_325 * (RLcp2_248 + RLcp2_251) - ROcp2_325 * (RLcp2_252 + RLcp2_254);
2585 //JTcp2_254_27 = -(ROcp2_125 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_325
2586 // * (RLcp2_142 + RLcp2_145) - ROcp2_325 * (RLcp2_146 + RLcp2_147) - ROcp2_325 * (RLcp2_148 + RLcp2_151) - ROcp2_325 * (RLcp2_152 + RLcp2_154));
2587 //JTcp2_354_27 = ROcp2_125 * (RLcp2_242 + RLcp2_245 + RLcp2_246 + RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252 + RLcp2_254) - ROcp2_225 * (
2588 // RLcp2_142 + RLcp2_145) - ROcp2_225 * (RLcp2_146 + RLcp2_147) - ROcp2_225 * (RLcp2_148 + RLcp2_151) - ROcp2_225 * (RLcp2_152 + RLcp2_154);
2589 //JTcp2_154_28 = ROcp2_827 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_927 * (
2590 // RLcp2_242 + RLcp2_245) - ROcp2_927 * (RLcp2_246 + RLcp2_247) - ROcp2_927 * (RLcp2_248 + RLcp2_251) - ROcp2_927 * (RLcp2_252 + RLcp2_254);
2591 //JTcp2_254_28 = -(ROcp2_727 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_927
2592 // * (RLcp2_142 + RLcp2_145) - ROcp2_927 * (RLcp2_146 + RLcp2_147) - ROcp2_927 * (RLcp2_148 + RLcp2_151) - ROcp2_927 * (RLcp2_152 + RLcp2_154));
2593 //JTcp2_354_28 = ROcp2_727 * (RLcp2_242 + RLcp2_245 + RLcp2_246 + RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252 + RLcp2_254) - ROcp2_827 * (
2594 // RLcp2_142 + RLcp2_145) - ROcp2_827 * (RLcp2_146 + RLcp2_147) - ROcp2_827 * (RLcp2_148 + RLcp2_151) - ROcp2_827 * (RLcp2_152 + RLcp2_154);
2595 //JTcp2_154_29 = ROcp2_228 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_328 * (
2596 // RLcp2_242 + RLcp2_245) - ROcp2_328 * (RLcp2_246 + RLcp2_247) - ROcp2_328 * (RLcp2_248 + RLcp2_251) - ROcp2_328 * (RLcp2_252 + RLcp2_254);
2597 //JTcp2_254_29 = -(ROcp2_128 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_328
2598 // * (RLcp2_142 + RLcp2_145) - ROcp2_328 * (RLcp2_146 + RLcp2_147) - ROcp2_328 * (RLcp2_148 + RLcp2_151) - ROcp2_328 * (RLcp2_152 + RLcp2_154));
2599 //JTcp2_354_29 = ROcp2_128 * (RLcp2_242 + RLcp2_245 + RLcp2_246 + RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252 + RLcp2_254) - ROcp2_228 * (
2600 // RLcp2_142 + RLcp2_145) - ROcp2_228 * (RLcp2_146 + RLcp2_147) - ROcp2_228 * (RLcp2_148 + RLcp2_151) - ROcp2_228 * (RLcp2_152 + RLcp2_154);
2607 //JTcp2_154_31 = ROcp2_230 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_330 * (
2608 // RLcp2_242 + RLcp2_245) - ROcp2_330 * (RLcp2_246 + RLcp2_247) - ROcp2_330 * (RLcp2_248 + RLcp2_251) - ROcp2_330 * (RLcp2_252 + RLcp2_254);
2609 //JTcp2_254_31 = -(ROcp2_130 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_330
2610 // * (RLcp2_142 + RLcp2_145) - ROcp2_330 * (RLcp2_146 + RLcp2_147) - ROcp2_330 * (RLcp2_148 + RLcp2_151) - ROcp2_330 * (RLcp2_152 + RLcp2_154));
2611 //JTcp2_354_31 = ROcp2_130 * (RLcp2_242 + RLcp2_245 + RLcp2_246 + RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252 + RLcp2_254) - ROcp2_230 * (
2612 // RLcp2_142 + RLcp2_145) - ROcp2_230 * (RLcp2_146 + RLcp2_147) - ROcp2_230 * (RLcp2_148 + RLcp2_151) - ROcp2_230 * (RLcp2_152 + RLcp2_154);
2613 //JTcp2_154_32 = ROcp2_230 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_330 * (
2614 // RLcp2_242 + RLcp2_245) - ROcp2_330 * (RLcp2_246 + RLcp2_247) - ROcp2_330 * (RLcp2_248 + RLcp2_251) - ROcp2_330 * (RLcp2_252 + RLcp2_254);
2615 //JTcp2_254_32 = -(ROcp2_130 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_330
2616 // * (RLcp2_142 + RLcp2_145) - ROcp2_330 * (RLcp2_146 + RLcp2_147) - ROcp2_330 * (RLcp2_148 + RLcp2_151) - ROcp2_330 * (RLcp2_152 + RLcp2_154));
2617 //JTcp2_354_32 = ROcp2_130 * (RLcp2_242 + RLcp2_245 + RLcp2_246 + RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252 + RLcp2_254) - ROcp2_230 * (
2618 // RLcp2_142 + RLcp2_145) - ROcp2_230 * (RLcp2_146 + RLcp2_147) - ROcp2_230 * (RLcp2_148 + RLcp2_151) - ROcp2_230 * (RLcp2_152 + RLcp2_154);
2619 //JTcp2_154_33 = ROcp2_532 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_632 * (
2620 // RLcp2_242 + RLcp2_245) - ROcp2_632 * (RLcp2_246 + RLcp2_247) - ROcp2_632 * (RLcp2_248 + RLcp2_251) - ROcp2_632 * (RLcp2_252 + RLcp2_254);
2621 //JTcp2_254_33 = -(ROcp2_432 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_632
2622 // * (RLcp2_142 + RLcp2_145) - ROcp2_632 * (RLcp2_146 + RLcp2_147) - ROcp2_632 * (RLcp2_148 + RLcp2_151) - ROcp2_632 * (RLcp2_152 + RLcp2_154));
2623 //JTcp2_354_33 = ROcp2_432 * (RLcp2_242 + RLcp2_245 + RLcp2_246 + RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252 + RLcp2_254) - ROcp2_532 * (
2624 // RLcp2_142 + RLcp2_145) - ROcp2_532 * (RLcp2_146 + RLcp2_147) - ROcp2_532 * (RLcp2_148 + RLcp2_151) - ROcp2_532 * (RLcp2_152 + RLcp2_154);
2625 //JTcp2_154_34 = ROcp2_833 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_933 * (
2626 // RLcp2_242 + RLcp2_245) - ROcp2_933 * (RLcp2_246 + RLcp2_247) - ROcp2_933 * (RLcp2_248 + RLcp2_251) - ROcp2_933 * (RLcp2_252 + RLcp2_254);
2627 //JTcp2_254_34 = -(ROcp2_733 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_933
2628 // * (RLcp2_142 + RLcp2_145) - ROcp2_933 * (RLcp2_146 + RLcp2_147) - ROcp2_933 * (RLcp2_148 + RLcp2_151) - ROcp2_933 * (RLcp2_152 + RLcp2_154));
2629 //JTcp2_354_34 = ROcp2_733 * (RLcp2_242 + RLcp2_245 + RLcp2_246 + RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252 + RLcp2_254) - ROcp2_833 * (
2630 // RLcp2_142 + RLcp2_145) - ROcp2_833 * (RLcp2_146 + RLcp2_147) - ROcp2_833 * (RLcp2_148 + RLcp2_151) - ROcp2_833 * (RLcp2_152 + RLcp2_154);
2637 //JTcp2_154_36 = ROcp2_235 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_335 * (
2638 // RLcp2_242 + RLcp2_245) - ROcp2_335 * (RLcp2_246 + RLcp2_247) - ROcp2_335 * (RLcp2_248 + RLcp2_251) - ROcp2_335 * (RLcp2_252 + RLcp2_254);
2639 //JTcp2_254_36 = -(ROcp2_135 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_335
2640 // * (RLcp2_142 + RLcp2_145) - ROcp2_335 * (RLcp2_146 + RLcp2_147) - ROcp2_335 * (RLcp2_148 + RLcp2_151) - ROcp2_335 * (RLcp2_152 + RLcp2_154));
2641 //JTcp2_354_36 = ROcp2_135 * (RLcp2_242 + RLcp2_245 + RLcp2_246 + RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252 + RLcp2_254) - ROcp2_235 * (
2642 // RLcp2_142 + RLcp2_145) - ROcp2_235 * (RLcp2_146 + RLcp2_147) - ROcp2_235 * (RLcp2_148 + RLcp2_151) - ROcp2_235 * (RLcp2_152 + RLcp2_154);
2643 //JTcp2_154_37 = ROcp2_536 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_636 * (
2644 // RLcp2_242 + RLcp2_245) - ROcp2_636 * (RLcp2_246 + RLcp2_247) - ROcp2_636 * (RLcp2_248 + RLcp2_251) - ROcp2_636 * (RLcp2_252 + RLcp2_254);
2645 //JTcp2_254_37 = -(ROcp2_436 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_636
2646 // * (RLcp2_142 + RLcp2_145) - ROcp2_636 * (RLcp2_146 + RLcp2_147) - ROcp2_636 * (RLcp2_148 + RLcp2_151) - ROcp2_636 * (RLcp2_152 + RLcp2_154));
2647 //JTcp2_354_37 = ROcp2_436 * (RLcp2_242 + RLcp2_245 + RLcp2_246 + RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252 + RLcp2_254) - ROcp2_536 * (
2648 // RLcp2_142 + RLcp2_145) - ROcp2_536 * (RLcp2_146 + RLcp2_147) - ROcp2_536 * (RLcp2_148 + RLcp2_151) - ROcp2_536 * (RLcp2_152 + RLcp2_154);
2649 //JTcp2_154_38 = ROcp2_837 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_937 * (
2650 // RLcp2_242 + RLcp2_245) - ROcp2_937 * (RLcp2_246 + RLcp2_247) - ROcp2_937 * (RLcp2_248 + RLcp2_251) - ROcp2_937 * (RLcp2_252 + RLcp2_254);
2651 //JTcp2_254_38 = -(ROcp2_737 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_937
2652 // * (RLcp2_142 + RLcp2_145) - ROcp2_937 * (RLcp2_146 + RLcp2_147) - ROcp2_937 * (RLcp2_148 + RLcp2_151) - ROcp2_937 * (RLcp2_152 + RLcp2_154));
2653 //JTcp2_354_38 = ROcp2_737 * (RLcp2_242 + RLcp2_245 + RLcp2_246 + RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252 + RLcp2_254) - ROcp2_837 * (
2654 // RLcp2_142 + RLcp2_145) - ROcp2_837 * (RLcp2_146 + RLcp2_147) - ROcp2_837 * (RLcp2_148 + RLcp2_151) - ROcp2_837 * (RLcp2_152 + RLcp2_154);
2655 //JTcp2_154_39 = ROcp2_238 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_338 * (
2656 // RLcp2_242 + RLcp2_245) - ROcp2_338 * (RLcp2_246 + RLcp2_247) - ROcp2_338 * (RLcp2_248 + RLcp2_251) - ROcp2_338 * (RLcp2_252 + RLcp2_254);
2657 //JTcp2_254_39 = -(ROcp2_138 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_338
2658 // * (RLcp2_142 + RLcp2_145) - ROcp2_338 * (RLcp2_146 + RLcp2_147) - ROcp2_338 * (RLcp2_148 + RLcp2_151) - ROcp2_338 * (RLcp2_152 + RLcp2_154));
2659 //JTcp2_354_39 = ROcp2_138 * (RLcp2_242 + RLcp2_245 + RLcp2_246 + RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252 + RLcp2_254) - ROcp2_238 * (
2660 // RLcp2_142 + RLcp2_145) - ROcp2_238 * (RLcp2_146 + RLcp2_147) - ROcp2_238 * (RLcp2_148 + RLcp2_151) - ROcp2_238 * (RLcp2_152 + RLcp2_154);
2667 //JTcp2_154_41 = ROcp2_240 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_340 * (
2668 // RLcp2_242 + RLcp2_245) - ROcp2_340 * (RLcp2_246 + RLcp2_247) - ROcp2_340 * (RLcp2_248 + RLcp2_251) - ROcp2_340 * (RLcp2_252 + RLcp2_254);
2669 //JTcp2_254_41 = -(ROcp2_140 * (RLcp2_342 + RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352 + RLcp2_354) - ROcp2_340
2670 // * (RLcp2_142 + RLcp2_145) - ROcp2_340 * (RLcp2_146 + RLcp2_147) - ROcp2_340 * (RLcp2_148 + RLcp2_151) - ROcp2_340 * (RLcp2_152 + RLcp2_154));
2671 //JTcp2_354_41 = ROcp2_140 * (RLcp2_242 + RLcp2_245 + RLcp2_246 + RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252 + RLcp2_254) - ROcp2_240 * (
2672 // RLcp2_142 + RLcp2_145) - ROcp2_240 * (RLcp2_146 + RLcp2_147) - ROcp2_240 * (RLcp2_148 + RLcp2_151) - ROcp2_240 * (RLcp2_152 + RLcp2_154);
2673 //JTcp2_154_42 = ROcp2_240 * (RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352) - ROcp2_340 * (RLcp2_245 + RLcp2_246)
2674 // - ROcp2_340 * (RLcp2_247 + RLcp2_248) - ROcp2_340 * (RLcp2_251 + RLcp2_252) - RLcp2_254 * ROcp2_340 + RLcp2_354 * ROcp2_240;
2675 //JTcp2_254_42 = RLcp2_154 * ROcp2_340 - RLcp2_354 * ROcp2_140 - ROcp2_140 * (RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 +
2676 // RLcp2_352) + ROcp2_340 * (RLcp2_145 + RLcp2_146) + ROcp2_340 * (RLcp2_147 + RLcp2_148) + ROcp2_340 * (RLcp2_151 + RLcp2_152);
2677 //JTcp2_354_42 = ROcp2_140 * (RLcp2_245 + RLcp2_246 + RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252) - ROcp2_240 * (RLcp2_145 + RLcp2_146)
2678 // - ROcp2_240 * (RLcp2_147 + RLcp2_148) - ROcp2_240 * (RLcp2_151 + RLcp2_152) - RLcp2_154 * ROcp2_240 + RLcp2_254 * ROcp2_140;
2679 //JTcp2_154_43 = ROcp2_842 * (RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352) - ROcp2_942 * (RLcp2_245 + RLcp2_246)
2680 // - ROcp2_942 * (RLcp2_247 + RLcp2_248) - ROcp2_942 * (RLcp2_251 + RLcp2_252) - RLcp2_254 * ROcp2_942 + RLcp2_354 * ROcp2_842;
2681 //JTcp2_254_43 = RLcp2_154 * ROcp2_942 - RLcp2_354 * ROcp2_742 - ROcp2_742 * (RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 +
2682 // RLcp2_352) + ROcp2_942 * (RLcp2_145 + RLcp2_146) + ROcp2_942 * (RLcp2_147 + RLcp2_148) + ROcp2_942 * (RLcp2_151 + RLcp2_152);
2683 //JTcp2_354_43 = ROcp2_742 * (RLcp2_245 + RLcp2_246 + RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252) - ROcp2_842 * (RLcp2_145 + RLcp2_146)
2684 // - ROcp2_842 * (RLcp2_147 + RLcp2_148) - ROcp2_842 * (RLcp2_151 + RLcp2_152) - RLcp2_154 * ROcp2_842 + RLcp2_254 * ROcp2_742;
2685 //JTcp2_154_44 = ROcp2_243 * (RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352) - ROcp2_343 * (RLcp2_245 + RLcp2_246)
2686 // - ROcp2_343 * (RLcp2_247 + RLcp2_248) - ROcp2_343 * (RLcp2_251 + RLcp2_252) - RLcp2_254 * ROcp2_343 + RLcp2_354 * ROcp2_243;
2687 //JTcp2_254_44 = RLcp2_154 * ROcp2_343 - RLcp2_354 * ROcp2_143 - ROcp2_143 * (RLcp2_345 + RLcp2_346 + RLcp2_347 + RLcp2_348 + RLcp2_351 +
2688 // RLcp2_352) + ROcp2_343 * (RLcp2_145 + RLcp2_146) + ROcp2_343 * (RLcp2_147 + RLcp2_148) + ROcp2_343 * (RLcp2_151 + RLcp2_152);
2689 //JTcp2_354_44 = ROcp2_143 * (RLcp2_245 + RLcp2_246 + RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252) - ROcp2_243 * (RLcp2_145 + RLcp2_146)
2690 // - ROcp2_243 * (RLcp2_147 + RLcp2_148) - ROcp2_243 * (RLcp2_151 + RLcp2_152) - RLcp2_154 * ROcp2_243 + RLcp2_254 * ROcp2_143;
2691 //JTcp2_154_46 = ROcp2_243 * (RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352) - ROcp2_343 * (RLcp2_247 + RLcp2_248) - ROcp2_343 * (
2692 // RLcp2_251 + RLcp2_252) - RLcp2_254 * ROcp2_343 + RLcp2_354 * ROcp2_243;
2693 //JTcp2_254_46 = RLcp2_154 * ROcp2_343 - RLcp2_354 * ROcp2_143 - ROcp2_143 * (RLcp2_347 + RLcp2_348 + RLcp2_351 + RLcp2_352) + ROcp2_343 * (
2694 // RLcp2_147 + RLcp2_148) + ROcp2_343 * (RLcp2_151 + RLcp2_152);
2695 //JTcp2_354_46 = ROcp2_143 * (RLcp2_247 + RLcp2_248 + RLcp2_251 + RLcp2_252) - ROcp2_243 * (RLcp2_147 + RLcp2_148) - ROcp2_243 * (
2696 // RLcp2_151 + RLcp2_152) - RLcp2_154 * ROcp2_243 + RLcp2_254 * ROcp2_143;
2697 //JTcp2_154_48 = ROcp2_243 * (RLcp2_351 + RLcp2_352) - ROcp2_343 * (RLcp2_251 + RLcp2_252) - RLcp2_254 * ROcp2_343 + RLcp2_354 * ROcp2_243;
2698 //JTcp2_254_48 = RLcp2_154 * ROcp2_343 - RLcp2_354 * ROcp2_143 - ROcp2_143 * (RLcp2_351 + RLcp2_352) + ROcp2_343 * (RLcp2_151 + RLcp2_152);
2699 //JTcp2_354_48 = ROcp2_143 * (RLcp2_251 + RLcp2_252) - ROcp2_243 * (RLcp2_151 + RLcp2_152) - RLcp2_154 * ROcp2_243 + RLcp2_254 * ROcp2_143;
2703 //JTcp2_154_50 = ROcp2_249 * (RLcp2_351 + RLcp2_352) - ROcp2_349 * (RLcp2_251 + RLcp2_252) - RLcp2_254 * ROcp2_349 + RLcp2_354 * ROcp2_249;
2704 //JTcp2_254_50 = RLcp2_154 * ROcp2_349 - RLcp2_354 * ROcp2_149 - ROcp2_149 * (RLcp2_351 + RLcp2_352) + ROcp2_349 * (RLcp2_151 + RLcp2_152);
2705 //JTcp2_354_50 = ROcp2_149 * (RLcp2_251 + RLcp2_252) - ROcp2_249 * (RLcp2_151 + RLcp2_152) - RLcp2_154 * ROcp2_249 + RLcp2_254 * ROcp2_149;
2706 //JTcp2_154_51 = ROcp2_249 * (RLcp2_352 + RLcp2_354) - ROcp2_349 * (RLcp2_252 + RLcp2_254);
2707 //JTcp2_254_51 = -(ROcp2_149 * (RLcp2_352 + RLcp2_354) - ROcp2_349 * (RLcp2_152 + RLcp2_154));
2708 //JTcp2_354_51 = ROcp2_149 * (RLcp2_252 + RLcp2_254) - ROcp2_249 * (RLcp2_152 + RLcp2_154);
2757 OMcp2_155 = OMcp2_153 + ROcp2_453 * qd[55];
2758 OMcp2_255 = OMcp2_253 + ROcp2_553 * qd[55];
2759 OMcp2_355 = OMcp2_353 + ROcp2_653 * qd[55];
2760 OMcp2_156 = OMcp2_155 + ROcp2_755 * qd[56];
2761 OMcp2_256 = OMcp2_255 + ROcp2_855 * qd[56];
2762 OMcp2_356 = OMcp2_355 + ROcp2_955 * qd[56];
2763 OPcp2_156 = OPcp2_153 + ROcp2_453 * qdd[55] + ROcp2_755 * qdd[56] + qd[55] * (OMcp2_253 * ROcp2_653 - OMcp2_353 * ROcp2_553) + qd[56] * (
2765 OPcp2_256 = OPcp2_253 + ROcp2_553 * qdd[55] + ROcp2_855 * qdd[56] - qd[55] * (OMcp2_153 * ROcp2_653 - OMcp2_353 * ROcp2_453) - qd[56] * (
2767 OPcp2_356 = OPcp2_353 + ROcp2_653 * qdd[55] + ROcp2_955 * qdd[56] + qd[55] * (OMcp2_153 * ROcp2_553 - OMcp2_253 * ROcp2_453) + qd[56] * (
2769
2770 // = = Block_1_0_0_3_1_0 = =
2771
2772 // Symbolic Outputs
2773
2774 sens->P[1] = POcp2_154;
2775 sens->P[2] = POcp2_254;
2776 sens->P[3] = POcp2_354;
2777 sens->R[1][1] = ROcp2_156;
2778 sens->R[1][2] = ROcp2_256;
2779 sens->R[1][3] = ROcp2_356;
2780 sens->R[2][1] = ROcp2_456;
2781 sens->R[2][2] = ROcp2_556;
2782 sens->R[2][3] = ROcp2_656;
2783 sens->R[3][1] = ROcp2_755;
2784 sens->R[3][2] = ROcp2_855;
2785 sens->R[3][3] = ROcp2_955;
2786 sens->V[1] = VIcp2_154;
2787 sens->V[2] = VIcp2_254;
2788 sens->V[3] = VIcp2_354;
2789 sens->OM[1] = OMcp2_156;
2790 sens->OM[2] = OMcp2_256;
2791 sens->OM[3] = OMcp2_356;
2792 sens->J[1][1] = (1.0);
2793 sens->J[1][5] = JTcp2_154_5;
2794 sens->J[1][6] = JTcp2_154_6;
2795 // sens->J[1][7] = JTcp2_154_7;
2796 // sens->J[1][8] = ROcp2_77;
2797 // sens->J[1][9] = JTcp2_154_9;
2798 // sens->J[1][10] = JTcp2_154_10;
2799 // sens->J[1][11] = JTcp2_154_11;
2800 // sens->J[1][12] = ROcp2_710;
2801 // sens->J[1][13] = JTcp2_154_13;
2802 // sens->J[1][14] = JTcp2_154_14;
2803 // sens->J[1][15] = JTcp2_154_15;
2804 // sens->J[1][16] = ROcp2_714;
2805 // sens->J[1][17] = JTcp2_154_17;
2806 sens->J[1][18 - 11] = JTcp2_154_18;
2807 // sens->J[1][19] = ROcp2_714;
2808 sens->J[1][20 - 12] = JTcp2_154_20;
2809 sens->J[1][21 - 12] = JTcp2_154_21;
2810 sens->J[1][22 - 12] = JTcp2_154_22;
2811 // sens->J[1][23] = ROcp2_721;
2812 // sens->J[1][24] = ROcp2_721;
2813 // sens->J[1][25] = JTcp2_154_25;
2814 // sens->J[1][26] = ROcp2_721;
2815 // sens->J[1][27] = JTcp2_154_27;
2816 // sens->J[1][28] = JTcp2_154_28;
2817 // sens->J[1][29] = JTcp2_154_29;
2818 sens->J[1][30 - 19] = JTcp2_154_30;
2819 // sens->J[1][31] = JTcp2_154_31;
2820 // sens->J[1][32] = JTcp2_154_32;
2821 // sens->J[1][33] = JTcp2_154_33;
2822 // sens->J[1][34] = JTcp2_154_34;
2823 sens->J[1][35 - 23] = JTcp2_154_35;
2824 // sens->J[1][36] = JTcp2_154_36;
2825 // sens->J[1][37] = JTcp2_154_37;
2826 // sens->J[1][38] = JTcp2_154_38;
2827 // sens->J[1][39] = JTcp2_154_39;
2828 sens->J[1][40 - 27] = JTcp2_154_40;
2829 // sens->J[1][41] = JTcp2_154_41;
2830 // sens->J[1][42] = JTcp2_154_42;
2831 // sens->J[1][43-29] = JTcp2_154_43;
2832 // sens->J[1][44] = JTcp2_154_44;
2833 // sens->J[1][45] = ROcp2_744;
2834 // sens->J[1][46] = JTcp2_154_46;
2835 // sens->J[1][47] = ROcp2_746;
2836 // sens->J[1][48] = JTcp2_154_48;
2837 sens->J[1][49 - 35] = JTcp2_154_49;
2838 // sens->J[1][50] = JTcp2_154_50;
2839 // sens->J[1][51] = JTcp2_154_51;
2840 // sens->J[1][52] = ROcp2_751;
2841 sens->J[1][53 - 38] = JTcp2_154_53;
2842 sens->J[1][54 - 38] = ROcp2_153;
2843 sens->J[2][2] = (1.0);
2844 sens->J[2][4] = JTcp2_254_4;
2845 sens->J[2][5] = JTcp2_254_5;
2846 sens->J[2][6] = JTcp2_254_6;
2847 // sens->J[2][7] = JTcp2_254_7;
2848 // sens->J[2][8] = ROcp2_87;
2849 // sens->J[2][9] = JTcp2_254_9;
2850 // sens->J[2][10] = JTcp2_254_10;
2851 // sens->J[2][11] = JTcp2_254_11;
2852 // sens->J[2][12] = ROcp2_810;
2853 // sens->J[2][13] = JTcp2_254_13;
2854 // sens->J[2][14] = JTcp2_254_14;
2855 // sens->J[2][15] = JTcp2_254_15;
2856 // sens->J[2][16] = ROcp2_814;
2857 // sens->J[2][17] = JTcp2_254_17;
2858 sens->J[2][18 - 11] = JTcp2_254_18;
2859 // sens->J[2][19] = ROcp2_814;
2860 sens->J[2][20 - 12] = JTcp2_254_20;
2861 sens->J[2][21 - 12] = JTcp2_254_21;
2862 sens->J[2][22 - 12] = JTcp2_254_22;
2863 // sens->J[2][23] = ROcp2_821;
2864 // sens->J[2][24] = ROcp2_821;
2865 // sens->J[2][25] = JTcp2_254_25;
2866 // sens->J[2][26] = ROcp2_821;
2867 // sens->J[2][27] = JTcp2_254_27;
2868 // sens->J[2][28] = JTcp2_254_28;
2869 // sens->J[2][29] = JTcp2_254_29;
2870 sens->J[2][30 - 19] = JTcp2_254_30;
2871 // sens->J[2][31] = JTcp2_254_31;
2872 // sens->J[2][32] = JTcp2_254_32;
2873 // sens->J[2][33] = JTcp2_254_33;
2874 // sens->J[2][34] = JTcp2_254_34;
2875 sens->J[2][35 - 23] = JTcp2_254_35;
2876 // sens->J[2][36] = JTcp2_254_36;
2877 // sens->J[2][37] = JTcp2_254_37;
2878 // sens->J[2][38] = JTcp2_254_38;
2879 // sens->J[2][39] = JTcp2_254_39;
2880 sens->J[2][40 - 27] = JTcp2_254_40;
2881 // sens->J[2][41] = JTcp2_254_41;
2882 // sens->J[2][42] = JTcp2_254_42;
2883 // sens->J[2][43-29]= JTcp2_254_43;
2884 // sens->J[2][44] = JTcp2_254_44;
2885 // sens->J[2][45] = ROcp2_844;
2886 // sens->J[2][46] = JTcp2_254_46;
2887 // sens->J[2][47] = ROcp2_846;
2888 // sens->J[2][48] = JTcp2_254_48;
2889 sens->J[2][49 - 35] = JTcp2_254_49;
2890 // sens->J[2][50] = JTcp2_254_50;
2891 // sens->J[2][51] = JTcp2_254_51;
2892 // sens->J[2][52] = ROcp2_851;
2893 sens->J[2][53 - 38] = JTcp2_254_53;
2894 sens->J[2][54 - 38] = ROcp2_253;
2895 sens->J[3][3] = (1.0);
2896 sens->J[3][4] = JTcp2_354_4;
2897 sens->J[3][5] = JTcp2_354_5;
2898 sens->J[3][6] = JTcp2_354_6;
2899 // sens->J[3][7] = JTcp2_354_7;
2900 // sens->J[3][8] = ROcp2_97;
2901 // sens->J[3][9] = JTcp2_354_9;
2902 // sens->J[3][10] = JTcp2_354_10;
2903 // sens->J[3][11] = JTcp2_354_11;
2904 // sens->J[3][12] = ROcp2_910;
2905 // sens->J[3][13] = JTcp2_354_13;
2906 // sens->J[3][14] = JTcp2_354_14;
2907 // sens->J[3][15] = JTcp2_354_15;
2908 // sens->J[3][16] = ROcp2_914;
2909 // sens->J[3][17] = JTcp2_354_17;
2910 sens->J[3][18 - 11] = JTcp2_354_18;
2911 // sens->J[3][19] = ROcp2_914;
2912 sens->J[3][20 - 12] = JTcp2_354_20;
2913 sens->J[3][21 - 12] = JTcp2_354_21;
2914 sens->J[3][22 - 12] = JTcp2_354_22;
2915 // sens->J[3][23] = ROcp2_921;
2916 // sens->J[3][24] = ROcp2_921;
2917 // sens->J[3][25] = JTcp2_354_25;
2918 // sens->J[3][26] = ROcp2_921;
2919 // sens->J[3][27] = JTcp2_354_27;
2920 // sens->J[3][28] = JTcp2_354_28;
2921 // sens->J[3][29] = JTcp2_354_29;
2922 sens->J[3][30 - 19] = JTcp2_354_30;
2923 // sens->J[3][31] = JTcp2_354_31;
2924 // sens->J[3][32] = JTcp2_354_32;
2925 // sens->J[3][33] = JTcp2_354_33;
2926 // sens->J[3][34] = JTcp2_354_34;
2927 sens->J[3][35 - 23] = JTcp2_354_35;
2928 // sens->J[3][36] = JTcp2_354_36;
2929 // sens->J[3][37] = JTcp2_354_37;
2930 // sens->J[3][38] = JTcp2_354_38;
2931 // sens->J[3][39] = JTcp2_354_39;
2932 sens->J[3][40 - 27] = JTcp2_354_40;
2933 // sens->J[3][41] = JTcp2_354_41;
2934 // sens->J[3][42] = JTcp2_354_42;
2935 // sens->J[3][43-29] = JTcp2_354_43;
2936 // sens->J[3][44] = JTcp2_354_44;
2937 // sens->J[3][45] = ROcp2_944;
2938 // sens->J[3][46] = JTcp2_354_46;
2939 // sens->J[3][47] = ROcp2_946;
2940 // sens->J[3][48] = JTcp2_354_48;
2941 sens->J[3][49 - 35] = JTcp2_354_49;
2942 // sens->J[3][50] = JTcp2_354_50;
2943 // sens->J[3][51] = JTcp2_354_51;
2944 // sens->J[3][52] = ROcp2_951;
2945 sens->J[3][53 - 38] = JTcp2_354_53;
2946 sens->J[3][54 - 38] = ROcp2_353;
2947 sens->J[4][4] = (1.0);
2948 sens->J[4][6] = S5;
2949 // sens->J[4][7] = ROcp2_16;
2950 // sens->J[4][9] = ROcp2_16;
2951 // sens->J[4][10] = ROcp2_49;
2952 // sens->J[4][11] = ROcp2_710;
2953 // sens->J[4][13] = ROcp2_111;
2954 // sens->J[4][14] = ROcp2_413;
2955 // sens->J[4][15] = ROcp2_714;
2956 // sens->J[4][17] = ROcp2_714;
2957 sens->J[4][18 - 11] = ROcp2_714;
2958 sens->J[4][20 - 12] = ROcp2_118;
2959 sens->J[4][21 - 12] = ROcp2_420;
2960 sens->J[4][22 - 12] = ROcp2_721;
2961 // sens->J[4][25] = ROcp2_721;
2962 // sens->J[4][27] = ROcp2_125;
2963 // sens->J[4][28] = ROcp2_727;
2964 // sens->J[4][29] = ROcp2_128;
2965 sens->J[4][30 - 19] = ROcp2_729;
2966 // sens->J[4][31] = ROcp2_130;
2967 // sens->J[4][32] = ROcp2_130;
2968 // sens->J[4][33] = ROcp2_432;
2969 // sens->J[4][34] = ROcp2_733;
2970 sens->J[4][35 - 23] = ROcp2_733;
2971 // sens->J[4][36] = ROcp2_135;
2972 // sens->J[4][37] = ROcp2_436;
2973 // sens->J[4][38] = ROcp2_737;
2974 // sens->J[4][39] = ROcp2_138;
2975 sens->J[4][40 - 27] = ROcp2_739;
2976 // sens->J[4][41] = ROcp2_140;
2977 // sens->J[4][42] = ROcp2_140;
2978 // sens->J[4][43-29] = ROcp2_742;
2979 // sens->J[4][44] = ROcp2_143;
2980 // sens->J[4][46] = ROcp2_143;
2981 // sens->J[4][48] = ROcp2_143;
2982 sens->J[4][49 - 35] = ROcp2_748;
2983 // sens->J[4][50] = ROcp2_149;
2984 // sens->J[4][51] = ROcp2_149;
2985 sens->J[4][53 - 38] = ROcp2_751;
2986 sens->J[4][55 - 38] = ROcp2_453;
2987 sens->J[4][56 - 38] = ROcp2_755;
2988 sens->J[5][5] = C4;
2989 sens->J[5][6] = ROcp2_85;
2990 // sens->J[5][7] = ROcp2_26;
2991 // sens->J[5][9] = ROcp2_26;
2992 // sens->J[5][10] = ROcp2_59;
2993 // sens->J[5][11] = ROcp2_810;
2994 // sens->J[5][13] = ROcp2_211;
2995 // sens->J[5][14] = ROcp2_513;
2996 // sens->J[5][15] = ROcp2_814;
2997 // sens->J[5][17] = ROcp2_814;
2998 sens->J[5][18 - 11] = ROcp2_814;
2999 sens->J[5][20 - 12] = ROcp2_218;
3000 sens->J[5][21 - 12] = ROcp2_520;
3001 sens->J[5][22 - 12] = ROcp2_821;
3002 // sens->J[5][25] = ROcp2_821;
3003 // sens->J[5][27] = ROcp2_225;
3004 // sens->J[5][28] = ROcp2_827;
3005 // sens->J[5][29] = ROcp2_228;
3006 sens->J[5][30 - 19] = ROcp2_829;
3007 // sens->J[5][31] = ROcp2_230;
3008 // sens->J[5][32] = ROcp2_230;
3009 // sens->J[5][33] = ROcp2_532;
3010 // sens->J[5][34] = ROcp2_833;
3011 sens->J[5][35 - 23] = ROcp2_833;
3012 // sens->J[5][36] = ROcp2_235;
3013 // sens->J[5][37] = ROcp2_536;
3014 // sens->J[5][38] = ROcp2_837;
3015 // sens->J[5][39] = ROcp2_238;
3016 sens->J[5][40 - 27] = ROcp2_839;
3017 // sens->J[5][41] = ROcp2_240;
3018 // sens->J[5][42] = ROcp2_240;
3019 // sens->J[5][43-29] = ROcp2_842;
3020 // sens->J[5][44] = ROcp2_243;
3021 // sens->J[5][46] = ROcp2_243;
3022 // sens->J[5][48] = ROcp2_243;
3023 sens->J[5][49 - 35] = ROcp2_848;
3024 // sens->J[5][50] = ROcp2_249;
3025 // sens->J[5][51] = ROcp2_249;
3026 sens->J[5][53 - 38] = ROcp2_851;
3027 sens->J[5][55 - 38] = ROcp2_553;
3028 sens->J[5][56 - 38] = ROcp2_855;
3029 sens->J[6][5] = S4;
3030 sens->J[6][6] = ROcp2_95;
3031 // sens->J[6][7] = ROcp2_36;
3032 // sens->J[6][9] = ROcp2_36;
3033 // sens->J[6][10] = ROcp2_69;
3034 // sens->J[6][11] = ROcp2_910;
3035 // sens->J[6][13] = ROcp2_311;
3036 // sens->J[6][14] = ROcp2_613;
3037 // sens->J[6][15] = ROcp2_914;
3038 // sens->J[6][17] = ROcp2_914;
3039 sens->J[6][18 - 11] = ROcp2_914;
3040 sens->J[6][20 - 12] = ROcp2_318;
3041 sens->J[6][21 - 12] = ROcp2_620;
3042 sens->J[6][22 - 12] = ROcp2_921;
3043 // sens->J[6][25] = ROcp2_921;
3044 // sens->J[6][27] = ROcp2_325;
3045 // sens->J[6][28] = ROcp2_927;
3046 // sens->J[6][29] = ROcp2_328;
3047 sens->J[6][30 - 19] = ROcp2_929;
3048 // sens->J[6][31] = ROcp2_330;
3049 // sens->J[6][32] = ROcp2_330;
3050 // sens->J[6][33] = ROcp2_632;
3051 // sens->J[6][34] = ROcp2_933;
3052 sens->J[6][35 - 23] = ROcp2_933;
3053 // sens->J[6][36] = ROcp2_335;
3054 // sens->J[6][37] = ROcp2_636;
3055 // sens->J[6][38] = ROcp2_937;
3056 // sens->J[6][39] = ROcp2_338;
3057 sens->J[6][40 - 27] = ROcp2_939;
3058 // sens->J[6][41] = ROcp2_340;
3059 // sens->J[6][42] = ROcp2_340;
3060 // sens->J[6][43-29] = ROcp2_942;
3061 // sens->J[6][44] = ROcp2_343;
3062 // sens->J[6][46] = ROcp2_343;
3063 // sens->J[6][48] = ROcp2_343;
3064 sens->J[6][49 - 35] = ROcp2_948;
3065 // sens->J[6][50] = ROcp2_349;
3066 // sens->J[6][51] = ROcp2_349;
3067 sens->J[6][53 - 38] = ROcp2_951;
3068 sens->J[6][55 - 38] = ROcp2_653;
3069 sens->J[6][56 - 38] = ROcp2_955;
3070 sens->A[1] = ACcp2_154;
3071 sens->A[2] = ACcp2_254;
3072 sens->A[3] = ACcp2_354;
3073 sens->OMP[1] = OPcp2_156;
3074 sens->OMP[2] = OPcp2_256;
3075 sens->OMP[3] = OPcp2_356;
3076
3077 break;
3078 default:
3079 break;
3080 }
3081
3082
3083 // ====== END Task 1 ======
3084
3085
3086}
3087
3088
#define M_PI
Definition MathTools.h:17
void mbs_sensor_ArmarIII_simplified_virt(MbsSensor *sens, double *q_dof, double *qd_dof, int isens)
#define q
#define qdd
#define qd
double OM[4]
Angular velocity vector of the sensor expressed in the inertial frame: .
Definition mbs_sensor2.h:28
double V[4]
Velocity vector of the sensor expressed in the inertial frame: .
Definition mbs_sensor2.h:26
double OMP[4]
Angular acceleration vector of the sensor expressed in the inertial frame: .
Definition mbs_sensor2.h:32
double P[4]
Position vector of the sensor expressed in the inertial frame: .
Definition mbs_sensor2.h:21
double A[4]
Acceleration vector of the sensor expressed in the inertial frame: .
Definition mbs_sensor2.h:30
double * J[7]
Jacobian matrix of the sensor: .
Definition mbs_sensor2.h:37
double R[4][4]
Rotation matrix from the inertial frame to the sensor frame: .
Definition mbs_sensor2.h:24
#define DPT_3_10
#define DPT_1_19
#define DPT_1_17
#define DPT_3_9
#define DPT_3_15
#define DPT_2_17
#define DPT_3_6
#define DPT_3_17
#define DPT_2_9
#define DPT_2_6
#define DPT_3_20
#define DPT_3_18