mbs_sensor_ArmarIV-GazeStab-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 : Sun Oct 16 17:35:45 2016
15 //
16 // ==> Project name : ArmarIV-GazeStab-virt
17 // ==> using XML input file
18 //
19 // ==> Number of joints : 16
20 //
21 // ==> Function : F 6 : Sensors Kinematical Informations (sens)
22 // ==> Flops complexity : 1020
23 //
24 // ==> Generation Time : 0.020 seconds
25 // ==> Post-Processing : 0.010 seconds
26 //
27 //-------------------------------------------------------------
28 //
29 
30 #include <math.h>
31 
32 //#include "mbs_data.h"
33 //#include "mbs_project_interface.h"
34 #include "user_hard_param_WO_torso.h"
35 #include "mbs_sensor2.h"
36 
37 void mbs_sensor_ArmarIV_GazeStab_virt(MbsSensor* sens,
38  double* q,
39  double* qd,
40  double* qdd,
41  int isens)
42 {
43 
44 #include "mbs_sensor_ArmarIV-GazeStab-virt.h"
45 
46  //double *q = s->q;
47  //double *qd = s->qd;
48  //double *qdd = s->qdd;
49 
50  //double **frc = s->frc;
51  //double **trq = s->trq;
52 
53 
54 
55  // === begin imp_aux ===
56 
57  // === end imp_aux ===
58 
59  // ===== BEGIN task 0 =====
60 
61  // Sensor Kinematics
62 
63 
64 
65  // = = Block_0_0_0_0_0_1 = =
66 
67  // Trigonometric Variables
68 
69  C4 = cos(q[4]);
70  S4 = sin(q[4]);
71  C5 = cos(q[5]);
72  S5 = sin(q[5]);
73  C6 = cos(q[6]);
74  S6 = sin(q[6]);
75  C7 = cos(q[7]);
76  S7 = sin(q[7]);
77  C8 = cos(q[8]);
78  S8 = sin(q[8]);
79  C9 = cos(q[9]);
80  S9 = sin(q[9]);
81  C10 = cos(q[10]);
82  S10 = sin(q[10]);
83  C11 = cos(q[11]);
84  S11 = sin(q[11]);
85  C12 = cos(q[12]);
86  S12 = sin(q[12]);
87  C13 = cos(q[13]);
88  S13 = sin(q[13]);
89  C15 = cos(q[15]);
90  S15 = sin(q[15]);
91  C16 = cos(q[16]);
92  S16 = sin(q[16]);
93 
94  // ====== END Task 0 ======
95 
96  // ===== BEGIN task 1 =====
97 
98  switch (isens)
99  {
100 
101  case 1:
102 
103 
104 
105  // = = Block_1_0_0_1_0_1 = =
106 
107  // Sensor Kinematics
108 
109 
110  ROcp0_25 = S4 * S5;
111  ROcp0_35 = -C4 * S5;
112  ROcp0_85 = -S4 * C5;
113  ROcp0_95 = C4 * C5;
114  ROcp0_16 = C5 * C6;
115  ROcp0_26 = ROcp0_25 * C6 + C4 * S6;
116  ROcp0_36 = ROcp0_35 * C6 + S4 * S6;
117  ROcp0_46 = -C5 * S6;
118  ROcp0_56 = -(ROcp0_25 * S6 - C4 * C6);
119  ROcp0_66 = -(ROcp0_35 * S6 - S4 * C6);
120  ROcp0_47 = ROcp0_46 * C7 + S5 * S7;
121  ROcp0_57 = ROcp0_56 * C7 + ROcp0_85 * S7;
122  ROcp0_67 = ROcp0_66 * C7 + ROcp0_95 * S7;
123  ROcp0_77 = -(ROcp0_46 * S7 - S5 * C7);
124  ROcp0_87 = -(ROcp0_56 * S7 - ROcp0_85 * C7);
125  ROcp0_97 = -(ROcp0_66 * S7 - ROcp0_95 * C7);
126  ROcp0_18 = ROcp0_16 * C8 - ROcp0_77 * S8;
127  ROcp0_28 = ROcp0_26 * C8 - ROcp0_87 * S8;
128  ROcp0_38 = ROcp0_36 * C8 - ROcp0_97 * S8;
129  ROcp0_78 = ROcp0_16 * S8 + ROcp0_77 * C8;
130  ROcp0_88 = ROcp0_26 * S8 + ROcp0_87 * C8;
131  ROcp0_98 = ROcp0_36 * S8 + ROcp0_97 * C8;
132  ROcp0_19 = ROcp0_18 * C9 + ROcp0_47 * S9;
133  ROcp0_29 = ROcp0_28 * C9 + ROcp0_57 * S9;
134  ROcp0_39 = ROcp0_38 * C9 + ROcp0_67 * S9;
135  ROcp0_49 = -(ROcp0_18 * S9 - ROcp0_47 * C9);
136  ROcp0_59 = -(ROcp0_28 * S9 - ROcp0_57 * C9);
137  ROcp0_69 = -(ROcp0_38 * S9 - ROcp0_67 * C9);
138  ROcp0_410 = ROcp0_49 * C10 + ROcp0_78 * S10;
139  ROcp0_510 = ROcp0_59 * C10 + ROcp0_88 * S10;
140  ROcp0_610 = ROcp0_69 * C10 + ROcp0_98 * S10;
141  ROcp0_710 = -(ROcp0_49 * S10 - ROcp0_78 * C10);
142  ROcp0_810 = -(ROcp0_59 * S10 - ROcp0_88 * C10);
143  ROcp0_910 = -(ROcp0_69 * S10 - ROcp0_98 * C10);
144  ROcp0_111 = ROcp0_19 * C11 - ROcp0_710 * S11;
145  ROcp0_211 = ROcp0_29 * C11 - ROcp0_810 * S11;
146  ROcp0_311 = ROcp0_39 * C11 - ROcp0_910 * S11;
147  ROcp0_711 = ROcp0_19 * S11 + ROcp0_710 * C11;
148  ROcp0_811 = ROcp0_29 * S11 + ROcp0_810 * C11;
149  ROcp0_911 = ROcp0_39 * S11 + ROcp0_910 * C11;
150  ROcp0_412 = ROcp0_410 * C12 + ROcp0_711 * S12;
151  ROcp0_512 = ROcp0_510 * C12 + ROcp0_811 * S12;
152  ROcp0_612 = ROcp0_610 * C12 + ROcp0_911 * S12;
153  ROcp0_712 = -(ROcp0_410 * S12 - ROcp0_711 * C12);
154  ROcp0_812 = -(ROcp0_510 * S12 - ROcp0_811 * C12);
155  ROcp0_912 = -(ROcp0_610 * S12 - ROcp0_911 * C12);
156  ROcp0_113 = ROcp0_111 * C13 + ROcp0_412 * S13;
157  ROcp0_213 = ROcp0_211 * C13 + ROcp0_512 * S13;
158  ROcp0_313 = ROcp0_311 * C13 + ROcp0_612 * S13;
159  ROcp0_413 = -(ROcp0_111 * S13 - ROcp0_412 * C13);
160  ROcp0_513 = -(ROcp0_211 * S13 - ROcp0_512 * C13);
161  ROcp0_613 = -(ROcp0_311 * S13 - ROcp0_612 * C13);
162  ROcp0_415 = ROcp0_413 * C15 + ROcp0_712 * S15;
163  ROcp0_515 = ROcp0_513 * C15 + ROcp0_812 * S15;
164  ROcp0_615 = ROcp0_613 * C15 + ROcp0_912 * S15;
165  ROcp0_715 = -(ROcp0_413 * S15 - ROcp0_712 * C15);
166  ROcp0_815 = -(ROcp0_513 * S15 - ROcp0_812 * C15);
167  ROcp0_915 = -(ROcp0_613 * S15 - ROcp0_912 * C15);
168  ROcp0_116 = ROcp0_113 * C16 + ROcp0_415 * S16;
169  ROcp0_216 = ROcp0_213 * C16 + ROcp0_515 * S16;
170  ROcp0_316 = ROcp0_313 * C16 + ROcp0_615 * S16;
171  ROcp0_416 = -(ROcp0_113 * S16 - ROcp0_415 * C16);
172  ROcp0_516 = -(ROcp0_213 * S16 - ROcp0_515 * C16);
173  ROcp0_616 = -(ROcp0_313 * S16 - ROcp0_615 * C16);
174  OMcp0_25 = qd[5] * C4;
175  OMcp0_35 = qd[5] * S4;
176  OMcp0_16 = qd[4] + qd[6] * S5;
177  OMcp0_26 = OMcp0_25 + ROcp0_85 * qd[6];
178  OMcp0_36 = OMcp0_35 + ROcp0_95 * qd[6];
179  OMcp0_17 = OMcp0_16 + ROcp0_16 * qd[7];
180  OMcp0_27 = OMcp0_26 + ROcp0_26 * qd[7];
181  OMcp0_37 = OMcp0_36 + ROcp0_36 * qd[7];
182  OMcp0_18 = OMcp0_17 + ROcp0_47 * qd[8];
183  OMcp0_28 = OMcp0_27 + ROcp0_57 * qd[8];
184  OMcp0_38 = OMcp0_37 + ROcp0_67 * qd[8];
185  OPcp0_18 = qdd[4] + ROcp0_16 * qdd[7] + ROcp0_47 * qdd[8] + qdd[6] * S5 + qd[5] * qd[6] * C5 + qd[7] * (OMcp0_26 * ROcp0_36 - OMcp0_36 * ROcp0_26)
186  + qd[8] * (OMcp0_27 * ROcp0_67 - OMcp0_37 * ROcp0_57);
187  OPcp0_28 = ROcp0_26 * qdd[7] + ROcp0_57 * qdd[8] + ROcp0_85 * qdd[6] + qdd[5] * C4 - qd[4] * qd[5] * S4 + qd[6] * (OMcp0_35 * S5 - ROcp0_95 * qd[4])
188  - qd[7] * (OMcp0_16 * ROcp0_36 - OMcp0_36 * ROcp0_16) - qd[8] * (OMcp0_17 * ROcp0_67 - OMcp0_37 * ROcp0_47);
189  OPcp0_38 = ROcp0_36 * qdd[7] + ROcp0_67 * qdd[8] + ROcp0_95 * qdd[6] + qdd[5] * S4 + qd[4] * qd[5] * C4 - qd[6] * (OMcp0_25 * S5 - ROcp0_85 * qd[4])
190  + qd[7] * (OMcp0_16 * ROcp0_26 - OMcp0_26 * ROcp0_16) + qd[8] * (OMcp0_17 * ROcp0_57 - OMcp0_27 * ROcp0_47);
191  RLcp0_19 = ROcp0_78 * DPT_3_1;
192  RLcp0_29 = ROcp0_88 * DPT_3_1;
193  RLcp0_39 = ROcp0_98 * DPT_3_1;
194  OMcp0_19 = OMcp0_18 + ROcp0_78 * qd[9];
195  OMcp0_29 = OMcp0_28 + ROcp0_88 * qd[9];
196  OMcp0_39 = OMcp0_38 + ROcp0_98 * qd[9];
197  ORcp0_19 = OMcp0_28 * RLcp0_39 - OMcp0_38 * RLcp0_29;
198  ORcp0_29 = -(OMcp0_18 * RLcp0_39 - OMcp0_38 * RLcp0_19);
199  ORcp0_39 = OMcp0_18 * RLcp0_29 - OMcp0_28 * RLcp0_19;
200  OPcp0_19 = OPcp0_18 + ROcp0_78 * qdd[9] + qd[9] * (OMcp0_28 * ROcp0_98 - OMcp0_38 * ROcp0_88);
201  OPcp0_29 = OPcp0_28 + ROcp0_88 * qdd[9] - qd[9] * (OMcp0_18 * ROcp0_98 - OMcp0_38 * ROcp0_78);
202  OPcp0_39 = OPcp0_38 + ROcp0_98 * qdd[9] + qd[9] * (OMcp0_18 * ROcp0_88 - OMcp0_28 * ROcp0_78);
203  RLcp0_110 = ROcp0_78 * DPT_3_2;
204  RLcp0_210 = ROcp0_88 * DPT_3_2;
205  RLcp0_310 = ROcp0_98 * DPT_3_2;
206  OMcp0_110 = OMcp0_19 + ROcp0_19 * qd[10];
207  OMcp0_210 = OMcp0_29 + ROcp0_29 * qd[10];
208  OMcp0_310 = OMcp0_39 + ROcp0_39 * qd[10];
209  ORcp0_110 = OMcp0_29 * RLcp0_310 - OMcp0_39 * RLcp0_210;
210  ORcp0_210 = -(OMcp0_19 * RLcp0_310 - OMcp0_39 * RLcp0_110);
211  ORcp0_310 = OMcp0_19 * RLcp0_210 - OMcp0_29 * RLcp0_110;
212  OPcp0_110 = OPcp0_19 + ROcp0_19 * qdd[10] + qd[10] * (OMcp0_29 * ROcp0_39 - OMcp0_39 * ROcp0_29);
213  OPcp0_210 = OPcp0_29 + ROcp0_29 * qdd[10] - qd[10] * (OMcp0_19 * ROcp0_39 - OMcp0_39 * ROcp0_19);
214  OPcp0_310 = OPcp0_39 + ROcp0_39 * qdd[10] + qd[10] * (OMcp0_19 * ROcp0_29 - OMcp0_29 * ROcp0_19);
215  RLcp0_111 = ROcp0_410 * DPT_2_3;
216  RLcp0_211 = ROcp0_510 * DPT_2_3;
217  RLcp0_311 = ROcp0_610 * DPT_2_3;
218  OMcp0_111 = OMcp0_110 + ROcp0_410 * qd[11];
219  OMcp0_211 = OMcp0_210 + ROcp0_510 * qd[11];
220  OMcp0_311 = OMcp0_310 + ROcp0_610 * qd[11];
221  ORcp0_111 = OMcp0_210 * RLcp0_311 - OMcp0_310 * RLcp0_211;
222  ORcp0_211 = -(OMcp0_110 * RLcp0_311 - OMcp0_310 * RLcp0_111);
223  ORcp0_311 = OMcp0_110 * RLcp0_211 - OMcp0_210 * RLcp0_111;
224  OPcp0_111 = OPcp0_110 + ROcp0_410 * qdd[11] + qd[11] * (OMcp0_210 * ROcp0_610 - OMcp0_310 * ROcp0_510);
225  OPcp0_211 = OPcp0_210 + ROcp0_510 * qdd[11] - qd[11] * (OMcp0_110 * ROcp0_610 - OMcp0_310 * ROcp0_410);
226  OPcp0_311 = OPcp0_310 + ROcp0_610 * qdd[11] + qd[11] * (OMcp0_110 * ROcp0_510 - OMcp0_210 * ROcp0_410);
227  RLcp0_112 = ROcp0_111 * DPT_1_4 + ROcp0_410 * DPT_2_4 + ROcp0_711 * DPT_3_4;
228  RLcp0_212 = ROcp0_211 * DPT_1_4 + ROcp0_510 * DPT_2_4 + ROcp0_811 * DPT_3_4;
229  RLcp0_312 = ROcp0_311 * DPT_1_4 + ROcp0_610 * DPT_2_4 + ROcp0_911 * DPT_3_4;
230  OMcp0_112 = OMcp0_111 + ROcp0_111 * qd[12];
231  OMcp0_212 = OMcp0_211 + ROcp0_211 * qd[12];
232  OMcp0_312 = OMcp0_311 + ROcp0_311 * qd[12];
233  ORcp0_112 = OMcp0_211 * RLcp0_312 - OMcp0_311 * RLcp0_212;
234  ORcp0_212 = -(OMcp0_111 * RLcp0_312 - OMcp0_311 * RLcp0_112);
235  ORcp0_312 = OMcp0_111 * RLcp0_212 - OMcp0_211 * RLcp0_112;
236  OMcp0_113 = OMcp0_112 + ROcp0_712 * qd[13];
237  OMcp0_213 = OMcp0_212 + ROcp0_812 * qd[13];
238  OMcp0_313 = OMcp0_312 + ROcp0_912 * qd[13];
239  OPcp0_113 = OPcp0_111 + ROcp0_111 * qdd[12] + ROcp0_712 * qdd[13] + qd[12] * (OMcp0_211 * ROcp0_311 - OMcp0_311 * ROcp0_211) + qd[13] * (
240  OMcp0_212 * ROcp0_912 - OMcp0_312 * ROcp0_812);
241  OPcp0_213 = OPcp0_211 + ROcp0_211 * qdd[12] + ROcp0_812 * qdd[13] - qd[12] * (OMcp0_111 * ROcp0_311 - OMcp0_311 * ROcp0_111) - qd[13] * (
242  OMcp0_112 * ROcp0_912 - OMcp0_312 * ROcp0_712);
243  OPcp0_313 = OPcp0_311 + ROcp0_311 * qdd[12] + ROcp0_912 * qdd[13] + qd[12] * (OMcp0_111 * ROcp0_211 - OMcp0_211 * ROcp0_111) + qd[13] * (
244  OMcp0_112 * ROcp0_812 - OMcp0_212 * ROcp0_712);
245  RLcp0_114 = ROcp0_413 * q[14];
246  RLcp0_214 = ROcp0_513 * q[14];
247  RLcp0_314 = ROcp0_613 * q[14];
248  POcp0_114 = RLcp0_110 + RLcp0_111 + RLcp0_112 + RLcp0_114 + RLcp0_19 + q[1];
249  POcp0_214 = RLcp0_210 + RLcp0_211 + RLcp0_212 + RLcp0_214 + RLcp0_29 + q[2];
250  POcp0_314 = RLcp0_310 + RLcp0_311 + RLcp0_312 + RLcp0_314 + RLcp0_39 + q[3];
251  JTcp0_214_4 = -(RLcp0_310 + RLcp0_311 + RLcp0_312 + RLcp0_314 + RLcp0_39);
252  JTcp0_314_4 = RLcp0_210 + RLcp0_211 + RLcp0_212 + RLcp0_214 + RLcp0_29;
253  JTcp0_114_5 = C4 * (RLcp0_310 + RLcp0_311 + RLcp0_312 + RLcp0_39) - S4 * (RLcp0_210 + RLcp0_29) - S4 * (RLcp0_211 + RLcp0_212) - RLcp0_214 *
254  S4 + RLcp0_314 * C4;
255  JTcp0_214_5 = S4 * (RLcp0_110 + RLcp0_111 + RLcp0_112 + RLcp0_114 + RLcp0_19);
256  JTcp0_314_5 = -C4 * (RLcp0_110 + RLcp0_111 + RLcp0_112 + RLcp0_114 + RLcp0_19);
257  JTcp0_114_6 = ROcp0_85 * (RLcp0_310 + RLcp0_311 + RLcp0_312 + RLcp0_39) - ROcp0_95 * (RLcp0_210 + RLcp0_29) - ROcp0_95 * (RLcp0_211 +
258  RLcp0_212) - RLcp0_214 * ROcp0_95 + RLcp0_314 * ROcp0_85;
259  JTcp0_214_6 = -(RLcp0_314 * S5 - ROcp0_95 * (RLcp0_110 + RLcp0_111 + RLcp0_112 + RLcp0_114 + RLcp0_19) + S5 * (RLcp0_310 + RLcp0_39) + S5 * (
260  RLcp0_311 + RLcp0_312));
261  JTcp0_314_6 = RLcp0_214 * S5 - ROcp0_85 * (RLcp0_110 + RLcp0_111 + RLcp0_112 + RLcp0_114 + RLcp0_19) + S5 * (RLcp0_210 + RLcp0_29) + S5 * (
262  RLcp0_211 + RLcp0_212);
263  JTcp0_114_7 = ROcp0_26 * (RLcp0_310 + RLcp0_311 + RLcp0_312 + RLcp0_39) - ROcp0_36 * (RLcp0_210 + RLcp0_29) - ROcp0_36 * (RLcp0_211 +
264  RLcp0_212) - RLcp0_214 * ROcp0_36 + RLcp0_314 * ROcp0_26;
265  JTcp0_214_7 = RLcp0_114 * ROcp0_36 - RLcp0_314 * ROcp0_16 - ROcp0_16 * (RLcp0_310 + RLcp0_311 + RLcp0_312 + RLcp0_39) + ROcp0_36 * (
266  RLcp0_110 + RLcp0_19) + ROcp0_36 * (RLcp0_111 + RLcp0_112);
267  JTcp0_314_7 = ROcp0_16 * (RLcp0_210 + RLcp0_211 + RLcp0_212 + RLcp0_29) - ROcp0_26 * (RLcp0_110 + RLcp0_19) - ROcp0_26 * (RLcp0_111 +
268  RLcp0_112) - RLcp0_114 * ROcp0_26 + RLcp0_214 * ROcp0_16;
269  JTcp0_114_8 = ROcp0_57 * (RLcp0_310 + RLcp0_311 + RLcp0_312 + RLcp0_39) - ROcp0_67 * (RLcp0_210 + RLcp0_29) - ROcp0_67 * (RLcp0_211 +
270  RLcp0_212) - RLcp0_214 * ROcp0_67 + RLcp0_314 * ROcp0_57;
271  JTcp0_214_8 = RLcp0_114 * ROcp0_67 - RLcp0_314 * ROcp0_47 - ROcp0_47 * (RLcp0_310 + RLcp0_311 + RLcp0_312 + RLcp0_39) + ROcp0_67 * (
272  RLcp0_110 + RLcp0_19) + ROcp0_67 * (RLcp0_111 + RLcp0_112);
273  JTcp0_314_8 = ROcp0_47 * (RLcp0_210 + RLcp0_211 + RLcp0_212 + RLcp0_29) - ROcp0_57 * (RLcp0_110 + RLcp0_19) - ROcp0_57 * (RLcp0_111 +
274  RLcp0_112) - RLcp0_114 * ROcp0_57 + RLcp0_214 * ROcp0_47;
275  JTcp0_114_9 = ROcp0_88 * (RLcp0_310 + RLcp0_311 + RLcp0_312 + RLcp0_314) - ROcp0_98 * (RLcp0_210 + RLcp0_211) - ROcp0_98 * (RLcp0_212 +
276  RLcp0_214);
277  JTcp0_214_9 = -(ROcp0_78 * (RLcp0_310 + RLcp0_311 + RLcp0_312 + RLcp0_314) - ROcp0_98 * (RLcp0_110 + RLcp0_111) - ROcp0_98 * (RLcp0_112 +
278  RLcp0_114));
279  JTcp0_314_9 = ROcp0_78 * (RLcp0_210 + RLcp0_211 + RLcp0_212 + RLcp0_214) - ROcp0_88 * (RLcp0_110 + RLcp0_111) - ROcp0_88 * (RLcp0_112 +
280  RLcp0_114);
281  JTcp0_114_10 = ROcp0_29 * (RLcp0_311 + RLcp0_312) - ROcp0_39 * (RLcp0_211 + RLcp0_212) - RLcp0_214 * ROcp0_39 + RLcp0_314 * ROcp0_29;
282  JTcp0_214_10 = RLcp0_114 * ROcp0_39 - RLcp0_314 * ROcp0_19 - ROcp0_19 * (RLcp0_311 + RLcp0_312) + ROcp0_39 * (RLcp0_111 + RLcp0_112);
283  JTcp0_314_10 = ROcp0_19 * (RLcp0_211 + RLcp0_212) - ROcp0_29 * (RLcp0_111 + RLcp0_112) - RLcp0_114 * ROcp0_29 + RLcp0_214 * ROcp0_19;
284  JTcp0_114_11 = ROcp0_510 * (RLcp0_312 + RLcp0_314) - ROcp0_610 * (RLcp0_212 + RLcp0_214);
285  JTcp0_214_11 = -(ROcp0_410 * (RLcp0_312 + RLcp0_314) - ROcp0_610 * (RLcp0_112 + RLcp0_114));
286  JTcp0_314_11 = ROcp0_410 * (RLcp0_212 + RLcp0_214) - ROcp0_510 * (RLcp0_112 + RLcp0_114);
287  JTcp0_114_12 = -(RLcp0_214 * ROcp0_311 - RLcp0_314 * ROcp0_211);
288  JTcp0_214_12 = RLcp0_114 * ROcp0_311 - RLcp0_314 * ROcp0_111;
289  JTcp0_314_12 = -(RLcp0_114 * ROcp0_211 - RLcp0_214 * ROcp0_111);
290  JTcp0_114_13 = -(RLcp0_214 * ROcp0_912 - RLcp0_314 * ROcp0_812);
291  JTcp0_214_13 = RLcp0_114 * ROcp0_912 - RLcp0_314 * ROcp0_712;
292  JTcp0_314_13 = -(RLcp0_114 * ROcp0_812 - RLcp0_214 * ROcp0_712);
293  ORcp0_114 = OMcp0_213 * RLcp0_314 - OMcp0_313 * RLcp0_214;
294  ORcp0_214 = -(OMcp0_113 * RLcp0_314 - OMcp0_313 * RLcp0_114);
295  ORcp0_314 = OMcp0_113 * RLcp0_214 - OMcp0_213 * RLcp0_114;
296  VIcp0_114 = ORcp0_110 + ORcp0_111 + ORcp0_112 + ORcp0_114 + ORcp0_19 + qd[1] + ROcp0_413 * qd[14];
297  VIcp0_214 = ORcp0_210 + ORcp0_211 + ORcp0_212 + ORcp0_214 + ORcp0_29 + qd[2] + ROcp0_513 * qd[14];
298  VIcp0_314 = ORcp0_310 + ORcp0_311 + ORcp0_312 + ORcp0_314 + ORcp0_39 + qd[3] + ROcp0_613 * qd[14];
299  ACcp0_114 = qdd[1] + OMcp0_210 * ORcp0_311 + OMcp0_211 * ORcp0_312 + OMcp0_213 * ORcp0_314 + OMcp0_28 * ORcp0_39 + OMcp0_29 * ORcp0_310 -
300  OMcp0_310 * ORcp0_211 - OMcp0_311 * ORcp0_212 - OMcp0_313 * ORcp0_214 - OMcp0_38 * ORcp0_29 - OMcp0_39 * ORcp0_210 + OPcp0_210 * RLcp0_311 +
301  OPcp0_211 * RLcp0_312 + OPcp0_213 * RLcp0_314 + OPcp0_28 * RLcp0_39 + OPcp0_29 * RLcp0_310 - OPcp0_310 * RLcp0_211 - OPcp0_311 * RLcp0_212 -
302  OPcp0_313 * RLcp0_214 - OPcp0_38 * RLcp0_29 - OPcp0_39 * RLcp0_210 + ROcp0_413 * qdd[14] + (2.0) * qd[14] * (OMcp0_213 * ROcp0_613 - OMcp0_313 * ROcp0_513);
303  ACcp0_214 = qdd[2] - OMcp0_110 * ORcp0_311 - OMcp0_111 * ORcp0_312 - OMcp0_113 * ORcp0_314 - OMcp0_18 * ORcp0_39 - OMcp0_19 * ORcp0_310 +
304  OMcp0_310 * ORcp0_111 + OMcp0_311 * ORcp0_112 + OMcp0_313 * ORcp0_114 + OMcp0_38 * ORcp0_19 + OMcp0_39 * ORcp0_110 - OPcp0_110 * RLcp0_311 -
305  OPcp0_111 * RLcp0_312 - OPcp0_113 * RLcp0_314 - OPcp0_18 * RLcp0_39 - OPcp0_19 * RLcp0_310 + OPcp0_310 * RLcp0_111 + OPcp0_311 * RLcp0_112 +
306  OPcp0_313 * RLcp0_114 + OPcp0_38 * RLcp0_19 + OPcp0_39 * RLcp0_110 + ROcp0_513 * qdd[14] - (2.0) * qd[14] * (OMcp0_113 * ROcp0_613 - OMcp0_313 * ROcp0_413);
307  ACcp0_314 = qdd[3] + OMcp0_110 * ORcp0_211 + OMcp0_111 * ORcp0_212 + OMcp0_113 * ORcp0_214 + OMcp0_18 * ORcp0_29 + OMcp0_19 * ORcp0_210 -
308  OMcp0_210 * ORcp0_111 - OMcp0_211 * ORcp0_112 - OMcp0_213 * ORcp0_114 - OMcp0_28 * ORcp0_19 - OMcp0_29 * ORcp0_110 + OPcp0_110 * RLcp0_211 +
309  OPcp0_111 * RLcp0_212 + OPcp0_113 * RLcp0_214 + OPcp0_18 * RLcp0_29 + OPcp0_19 * RLcp0_210 - OPcp0_210 * RLcp0_111 - OPcp0_211 * RLcp0_112 -
310  OPcp0_213 * RLcp0_114 - OPcp0_28 * RLcp0_19 - OPcp0_29 * RLcp0_110 + ROcp0_613 * qdd[14] + (2.0) * qd[14] * (OMcp0_113 * ROcp0_513 - OMcp0_213 * ROcp0_413);
311  OMcp0_115 = OMcp0_113 + ROcp0_113 * qd[15];
312  OMcp0_215 = OMcp0_213 + ROcp0_213 * qd[15];
313  OMcp0_315 = OMcp0_313 + ROcp0_313 * qd[15];
314  OMcp0_116 = OMcp0_115 + ROcp0_715 * qd[16];
315  OMcp0_216 = OMcp0_215 + ROcp0_815 * qd[16];
316  OMcp0_316 = OMcp0_315 + ROcp0_915 * qd[16];
317  OPcp0_116 = OPcp0_113 + ROcp0_113 * qdd[15] + ROcp0_715 * qdd[16] + qd[15] * (OMcp0_213 * ROcp0_313 - OMcp0_313 * ROcp0_213) + qd[16] * (
318  OMcp0_215 * ROcp0_915 - OMcp0_315 * ROcp0_815);
319  OPcp0_216 = OPcp0_213 + ROcp0_213 * qdd[15] + ROcp0_815 * qdd[16] - qd[15] * (OMcp0_113 * ROcp0_313 - OMcp0_313 * ROcp0_113) - qd[16] * (
320  OMcp0_115 * ROcp0_915 - OMcp0_315 * ROcp0_715);
321  OPcp0_316 = OPcp0_313 + ROcp0_313 * qdd[15] + ROcp0_915 * qdd[16] + qd[15] * (OMcp0_113 * ROcp0_213 - OMcp0_213 * ROcp0_113) + qd[16] * (
322  OMcp0_115 * ROcp0_815 - OMcp0_215 * ROcp0_715);
323 
324  // = = Block_1_0_0_1_1_0 = =
325 
326  // Symbolic Outputs
327 
328  sens->P[1] = POcp0_114;
329  sens->P[2] = POcp0_214;
330  sens->P[3] = POcp0_314;
331  sens->R[1][1] = ROcp0_116;
332  sens->R[1][2] = ROcp0_216;
333  sens->R[1][3] = ROcp0_316;
334  sens->R[2][1] = ROcp0_416;
335  sens->R[2][2] = ROcp0_516;
336  sens->R[2][3] = ROcp0_616;
337  sens->R[3][1] = ROcp0_715;
338  sens->R[3][2] = ROcp0_815;
339  sens->R[3][3] = ROcp0_915;
340  sens->V[1] = VIcp0_114;
341  sens->V[2] = VIcp0_214;
342  sens->V[3] = VIcp0_314;
343  sens->OM[1] = OMcp0_116;
344  sens->OM[2] = OMcp0_216;
345  sens->OM[3] = OMcp0_316;
346  sens->J[1][1] = (1.0);
347  sens->J[1][5] = JTcp0_114_5;
348  sens->J[1][6] = JTcp0_114_6;
349  sens->J[1][7] = JTcp0_114_7;
350  sens->J[1][8] = JTcp0_114_8;
351  sens->J[1][9] = JTcp0_114_9;
352  sens->J[1][10] = JTcp0_114_10;
353  sens->J[1][11] = JTcp0_114_11;
354  sens->J[1][12] = JTcp0_114_12;
355  sens->J[1][13] = JTcp0_114_13;
356  sens->J[1][14] = ROcp0_413;
357  sens->J[2][2] = (1.0);
358  sens->J[2][4] = JTcp0_214_4;
359  sens->J[2][5] = JTcp0_214_5;
360  sens->J[2][6] = JTcp0_214_6;
361  sens->J[2][7] = JTcp0_214_7;
362  sens->J[2][8] = JTcp0_214_8;
363  sens->J[2][9] = JTcp0_214_9;
364  sens->J[2][10] = JTcp0_214_10;
365  sens->J[2][11] = JTcp0_214_11;
366  sens->J[2][12] = JTcp0_214_12;
367  sens->J[2][13] = JTcp0_214_13;
368  sens->J[2][14] = ROcp0_513;
369  sens->J[3][3] = (1.0);
370  sens->J[3][4] = JTcp0_314_4;
371  sens->J[3][5] = JTcp0_314_5;
372  sens->J[3][6] = JTcp0_314_6;
373  sens->J[3][7] = JTcp0_314_7;
374  sens->J[3][8] = JTcp0_314_8;
375  sens->J[3][9] = JTcp0_314_9;
376  sens->J[3][10] = JTcp0_314_10;
377  sens->J[3][11] = JTcp0_314_11;
378  sens->J[3][12] = JTcp0_314_12;
379  sens->J[3][13] = JTcp0_314_13;
380  sens->J[3][14] = ROcp0_613;
381  sens->J[4][4] = (1.0);
382  sens->J[4][6] = S5;
383  sens->J[4][7] = ROcp0_16;
384  sens->J[4][8] = ROcp0_47;
385  sens->J[4][9] = ROcp0_78;
386  sens->J[4][10] = ROcp0_19;
387  sens->J[4][11] = ROcp0_410;
388  sens->J[4][12] = ROcp0_111;
389  sens->J[4][13] = ROcp0_712;
390  sens->J[4][15] = ROcp0_113;
391  sens->J[4][16] = ROcp0_715;
392  sens->J[5][5] = C4;
393  sens->J[5][6] = ROcp0_85;
394  sens->J[5][7] = ROcp0_26;
395  sens->J[5][8] = ROcp0_57;
396  sens->J[5][9] = ROcp0_88;
397  sens->J[5][10] = ROcp0_29;
398  sens->J[5][11] = ROcp0_510;
399  sens->J[5][12] = ROcp0_211;
400  sens->J[5][13] = ROcp0_812;
401  sens->J[5][15] = ROcp0_213;
402  sens->J[5][16] = ROcp0_815;
403  sens->J[6][5] = S4;
404  sens->J[6][6] = ROcp0_95;
405  sens->J[6][7] = ROcp0_36;
406  sens->J[6][8] = ROcp0_67;
407  sens->J[6][9] = ROcp0_98;
408  sens->J[6][10] = ROcp0_39;
409  sens->J[6][11] = ROcp0_610;
410  sens->J[6][12] = ROcp0_311;
411  sens->J[6][13] = ROcp0_912;
412  sens->J[6][15] = ROcp0_313;
413  sens->J[6][16] = ROcp0_915;
414  sens->A[1] = ACcp0_114;
415  sens->A[2] = ACcp0_214;
416  sens->A[3] = ACcp0_314;
417  sens->OMP[1] = OPcp0_116;
418  sens->OMP[2] = OPcp0_216;
419  sens->OMP[3] = OPcp0_316;
420 
421  break;
422  default:
423  break;
424  }
425 
426 
427  // ====== END Task 1 ======
428 
429 
430 }
431 
ROcp0_97
double ROcp0_97
Definition: mbs_sensor_ArmarIII_simplified_virt.h:102
C12
double C12
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:17
ROcp0_815
double ROcp0_815
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:81
ORcp0_212
double ORcp0_212
Definition: mbs_sensor_ArmarIII_simplified_virt.h:295
RLcp0_114
double RLcp0_114
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:154
ROcp0_113
double ROcp0_113
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:71
OPcp0_18
double OPcp0_18
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:100
C4
double C4
Definition: mbs_sensor_ArmarIII_simplified_virt.h:3
JTcp0_214_6
double JTcp0_214_6
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:166
mbs_sensor2.h
ROcp0_28
double ROcp0_28
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:42
POcp0_114
double POcp0_114
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:157
ROcp0_29
double ROcp0_29
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:48
OMcp0_28
double OMcp0_28
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:98
ROcp0_18
double ROcp0_18
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:41
ROcp0_57
double ROcp0_57
Definition: mbs_sensor_ArmarIII_simplified_virt.h:98
OMcp0_36
double OMcp0_36
Definition: mbs_sensor_ArmarIII_simplified_virt.h:281
JTcp0_214_9
double JTcp0_214_9
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:175
qd
#define qd
MbsSensor::V
double V[4]
Velocity vector of the sensor expressed in the inertial frame: .
Definition: mbs_sensor2.h:21
JTcp0_314_7
double JTcp0_314_7
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:170
MbsSensor::OMP
double OMP[4]
Angular acceleration vector of the sensor expressed in the inertial frame: .
Definition: mbs_sensor2.h:24
POcp0_214
double POcp0_214
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:158
S8
double S8
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:10
RLcp0_211
double RLcp0_211
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:128
ORcp0_112
double ORcp0_112
Definition: mbs_sensor_ArmarIII_simplified_virt.h:294
OMcp0_16
double OMcp0_16
Definition: mbs_sensor_ArmarIII_simplified_virt.h:279
ROcp0_87
double ROcp0_87
Definition: mbs_sensor_ArmarIII_simplified_virt.h:101
ACcp0_114
double ACcp0_114
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:195
ROcp0_95
double ROcp0_95
Definition: mbs_sensor_ArmarIII_simplified_virt.h:90
OMcp0_17
double OMcp0_17
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:94
ROcp0_35
double ROcp0_35
Definition: mbs_sensor_ArmarIII_simplified_virt.h:88
C16
double C16
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:23
OMcp0_213
double OMcp0_213
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:149
RLcp0_19
double RLcp0_19
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:103
ROcp0_88
double ROcp0_88
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:45
JTcp0_114_9
double JTcp0_114_9
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:174
user_hard_param_WO_torso.h
ROcp0_211
double ROcp0_211
Definition: mbs_sensor_ArmarIII_simplified_virt.h:116
ORcp0_210
double ORcp0_210
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:122
OMcp0_38
double OMcp0_38
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:99
JTcp0_314_8
double JTcp0_314_8
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:173
JTcp0_314_5
double JTcp0_314_5
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:164
ROcp0_85
double ROcp0_85
Definition: mbs_sensor_ArmarIII_simplified_virt.h:89
ORcp0_311
double ORcp0_311
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:135
S16
double S16
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:24
OMcp0_311
double OMcp0_311
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:132
DPT_2_3
#define DPT_2_3
Definition: user_hard_param_WO_torso.h:74
ROcp0_36
double ROcp0_36
Definition: mbs_sensor_ArmarIII_simplified_virt.h:93
OMcp0_39
double OMcp0_39
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:108
ORcp0_19
double ORcp0_19
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:109
OPcp0_116
double OPcp0_116
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:204
ROcp0_515
double ROcp0_515
Definition: mbs_sensor_ArmarIII_simplified_virt.h:137
C7
double C7
Definition: mbs_sensor_ArmarIII_simplified_virt.h:9
DPT_3_2
#define DPT_3_2
Definition: user_hard_param_W_torso.h:86
MbsSensor::J
double * J[7]
Jacobian matrix of the sensor: .
Definition: mbs_sensor2.h:29
RLcp0_310
double RLcp0_310
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:117
C8
double C8
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:9
RLcp0_314
double RLcp0_314
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:156
OMcp0_315
double OMcp0_315
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:200
S4
double S4
Definition: mbs_sensor_ArmarIII_simplified_virt.h:4
ROcp0_412
double ROcp0_412
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:65
S10
double S10
Definition: mbs_sensor_ArmarIII_simplified_virt.h:14
MbsSensor::P
double P[4]
Position vector of the sensor expressed in the inertial frame: .
Definition: mbs_sensor2.h:19
ROcp0_911
double ROcp0_911
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:64
OPcp0_310
double OPcp0_310
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:126
JTcp0_114_10
double JTcp0_114_10
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:177
ROcp0_612
double ROcp0_612
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:67
ROcp0_711
double ROcp0_711
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:62
ROcp0_111
double ROcp0_111
Definition: mbs_sensor_ArmarIII_simplified_virt.h:115
OPcp0_316
double OPcp0_316
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:206
ORcp0_310
double ORcp0_310
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:123
OPcp0_311
double OPcp0_311
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:138
OMcp0_316
double OMcp0_316
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:203
ORcp0_214
double ORcp0_214
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:190
ROcp0_810
double ROcp0_810
Definition: mbs_sensor_ArmarIII_simplified_virt.h:113
JTcp0_214_12
double JTcp0_214_12
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:184
OMcp0_210
double OMcp0_210
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:119
JTcp0_214_5
double JTcp0_214_5
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:163
ROcp0_812
double ROcp0_812
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:69
ORcp0_110
double ORcp0_110
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:121
S15
double S15
Definition: mbs_sensor_ArmarIII_simplified_virt.h:22
DPT_1_4
#define DPT_1_4
Definition: user_hard_param_WO_torso.h:75
JTcp0_314_13
double JTcp0_314_13
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:188
OMcp0_312
double OMcp0_312
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:144
OMcp0_110
double OMcp0_110
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:118
ROcp0_39
double ROcp0_39
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:49
OMcp0_310
double OMcp0_310
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:120
ROcp0_213
double ROcp0_213
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:72
ROcp0_38
double ROcp0_38
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:43
JTcp0_314_12
double JTcp0_314_12
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:185
ROcp0_49
double ROcp0_49
Definition: mbs_sensor_ArmarIII_simplified_virt.h:103
OPcp0_39
double OPcp0_39
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:114
JTcp0_214_13
double JTcp0_214_13
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:187
OPcp0_210
double OPcp0_210
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:125
ORcp0_29
double ORcp0_29
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:110
ROcp0_510
double ROcp0_510
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:54
ORcp0_114
double ORcp0_114
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:189
ROcp0_19
double ROcp0_19
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:47
ORcp0_111
double ORcp0_111
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:133
OPcp0_28
double OPcp0_28
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:101
JTcp0_114_7
double JTcp0_114_7
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:168
RLcp0_29
double RLcp0_29
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:104
ROcp0_915
double ROcp0_915
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:82
ROcp0_26
double ROcp0_26
Definition: mbs_sensor_ArmarIII_simplified_virt.h:92
S7
double S7
Definition: mbs_sensor_ArmarIII_simplified_virt.h:10
ROcp0_410
double ROcp0_410
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:53
C10
double C10
Definition: mbs_sensor_ArmarIII_simplified_virt.h:13
OMcp0_113
double OMcp0_113
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:148
RLcp0_110
double RLcp0_110
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:115
OPcp0_211
double OPcp0_211
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:137
JTcp0_114_12
double JTcp0_114_12
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:183
ACcp0_214
double ACcp0_214
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:196
mbs_sensor_ArmarIV_GazeStab_virt
void mbs_sensor_ArmarIV_GazeStab_virt(MbsSensor *sens, double *q, double *qd, double *qdd, int isens)
Definition: mbs_sensor_ArmarIV-GazeStab-virt_new.c:37
ROcp0_46
double ROcp0_46
Definition: mbs_sensor_ArmarIII_simplified_virt.h:94
DPT_2_4
#define DPT_2_4
Definition: user_hard_param_WO_torso.h:76
JTcp0_214_10
double JTcp0_214_10
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:178
VIcp0_114
double VIcp0_114
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:192
ROcp0_311
double ROcp0_311
Definition: mbs_sensor_ArmarIII_simplified_virt.h:117
ROcp0_56
double ROcp0_56
Definition: mbs_sensor_ArmarIII_simplified_virt.h:95
ROcp0_513
double ROcp0_513
Definition: mbs_sensor_ArmarIII_simplified_virt.h:122
OMcp0_27
double OMcp0_27
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:95
POcp0_314
double POcp0_314
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:159
S5
double S5
Definition: mbs_sensor_ArmarIII_simplified_virt.h:6
RLcp0_212
double RLcp0_212
Definition: mbs_sensor_ArmarIII_simplified_virt.h:292
C6
double C6
Definition: mbs_sensor_ArmarIII_simplified_virt.h:7
OMcp0_19
double OMcp0_19
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:106
mbs_sensor_ArmarIV-GazeStab-virt.h
ROcp0_67
double ROcp0_67
Definition: mbs_sensor_ArmarIII_simplified_virt.h:99
ROcp0_811
double ROcp0_811
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:63
VIcp0_314
double VIcp0_314
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:194
ROcp0_47
double ROcp0_47
Definition: mbs_sensor_ArmarIII_simplified_virt.h:97
q
#define q
OMcp0_313
double OMcp0_313
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:150
ROcp0_66
double ROcp0_66
Definition: mbs_sensor_ArmarIII_simplified_virt.h:96
OPcp0_19
double OPcp0_19
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:112
C15
double C15
Definition: mbs_sensor_ArmarIII_simplified_virt.h:21
MbsSensor::OM
double OM[4]
Angular velocity vector of the sensor expressed in the inertial frame: .
Definition: mbs_sensor2.h:22
ROcp0_912
double ROcp0_912
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:70
ROcp0_910
double ROcp0_910
Definition: mbs_sensor_ArmarIII_simplified_virt.h:114
ORcp0_211
double ORcp0_211
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:134
JTcp0_214_7
double JTcp0_214_7
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:169
ROcp0_78
double ROcp0_78
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:44
JTcp0_214_4
double JTcp0_214_4
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:160
S13
double S13
Definition: mbs_sensor_ArmarIII_simplified_virt.h:18
RLcp0_312
double RLcp0_312
Definition: mbs_sensor_ArmarIII_simplified_virt.h:293
ACcp0_314
double ACcp0_314
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:197
OMcp0_211
double OMcp0_211
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:131
JTcp0_114_5
double JTcp0_114_5
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:162
RLcp0_39
double RLcp0_39
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:105
S9
double S9
Definition: mbs_sensor_ArmarIII_simplified_virt.h:12
S6
double S6
Definition: mbs_sensor_ArmarIII_simplified_virt.h:8
ROcp0_69
double ROcp0_69
Definition: mbs_sensor_ArmarIII_simplified_virt.h:105
ORcp0_312
double ORcp0_312
Definition: mbs_sensor_ArmarIII_simplified_virt.h:296
JTcp0_114_6
double JTcp0_114_6
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:165
ROcp0_616
double ROcp0_616
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:88
ROcp0_710
double ROcp0_710
Definition: mbs_sensor_ArmarIII_simplified_virt.h:112
OPcp0_29
double OPcp0_29
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:113
ROcp0_712
double ROcp0_712
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:68
OMcp0_216
double OMcp0_216
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:202
RLcp0_112
double RLcp0_112
Definition: mbs_sensor_ArmarIII_simplified_virt.h:291
MbsSensor
Definition: mbs_sensor2.h:17
ROcp0_416
double ROcp0_416
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:86
C13
double C13
Definition: mbs_sensor_ArmarIII_simplified_virt.h:17
C5
double C5
Definition: mbs_sensor_ArmarIII_simplified_virt.h:5
ROcp0_77
double ROcp0_77
Definition: mbs_sensor_ArmarIII_simplified_virt.h:100
OPcp0_313
double OPcp0_313
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:153
ROcp0_116
double ROcp0_116
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:83
ORcp0_314
double ORcp0_314
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:191
OPcp0_213
double OPcp0_213
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:152
JTcp0_314_9
double JTcp0_314_9
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:176
JTcp0_314_6
double JTcp0_314_6
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:167
OPcp0_38
double OPcp0_38
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:102
ROcp0_98
double ROcp0_98
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:46
OMcp0_37
double OMcp0_37
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:96
ROcp0_715
double ROcp0_715
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:80
JTcp0_114_11
double JTcp0_114_11
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:180
JTcp0_314_10
double JTcp0_314_10
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:179
ROcp0_316
double ROcp0_316
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:85
JTcp0_214_11
double JTcp0_214_11
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:181
RLcp0_311
double RLcp0_311
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:129
MbsSensor::R
double R[4][4]
Rotation matrix from the inertial frame to the sensor frame: .
Definition: mbs_sensor2.h:20
VIcp0_214
double VIcp0_214
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:193
RLcp0_210
double RLcp0_210
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:116
MbsSensor::A
double A[4]
Acceleration vector of the sensor expressed in the inertial frame: .
Definition: mbs_sensor2.h:23
ROcp0_413
double ROcp0_413
Definition: mbs_sensor_ArmarIII_simplified_virt.h:121
ROcp0_516
double ROcp0_516
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:87
OMcp0_29
double OMcp0_29
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:107
OMcp0_26
double OMcp0_26
Definition: mbs_sensor_ArmarIII_simplified_virt.h:280
JTcp0_114_13
double JTcp0_114_13
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:186
OPcp0_216
double OPcp0_216
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:205
ROcp0_216
double ROcp0_216
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:84
OMcp0_111
double OMcp0_111
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:130
ROcp0_59
double ROcp0_59
Definition: mbs_sensor_ArmarIII_simplified_virt.h:104
OMcp0_116
double OMcp0_116
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:201
JTcp0_114_8
double JTcp0_114_8
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:171
JTcp0_214_8
double JTcp0_214_8
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:172
DPT_3_4
#define DPT_3_4
Definition: user_hard_param_W_torso.h:88
ROcp0_25
double ROcp0_25
Definition: mbs_sensor_ArmarIII_simplified_virt.h:87
ROcp0_615
double ROcp0_615
Definition: mbs_sensor_ArmarIII_simplified_virt.h:138
C11
double C11
Definition: mbs_sensor_ArmarIII_simplified_virt.h:15
C9
double C9
Definition: mbs_sensor_ArmarIII_simplified_virt.h:11
OPcp0_111
double OPcp0_111
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:136
OPcp0_110
double OPcp0_110
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:124
OMcp0_112
double OMcp0_112
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:142
qdd
#define qdd
ORcp0_39
double ORcp0_39
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:111
JTcp0_314_11
double JTcp0_314_11
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:182
OMcp0_215
double OMcp0_215
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:199
OMcp0_18
double OMcp0_18
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:97
ROcp0_610
double ROcp0_610
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:55
OMcp0_212
double OMcp0_212
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:143
OPcp0_113
double OPcp0_113
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:151
OMcp0_115
double OMcp0_115
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:198
OMcp0_25
double OMcp0_25
Definition: mbs_sensor_ArmarIII_simplified_virt.h:277
ROcp0_313
double ROcp0_313
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:73
RLcp0_111
double RLcp0_111
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:127
ROcp0_613
double ROcp0_613
Definition: mbs_sensor_ArmarIII_simplified_virt.h:123
DPT_3_1
#define DPT_3_1
Definition: user_hard_param_W_torso.h:85
OMcp0_35
double OMcp0_35
Definition: mbs_sensor_ArmarIII_simplified_virt.h:278
ROcp0_415
double ROcp0_415
Definition: mbs_sensor_ArmarIII_simplified_virt.h:136
S12
double S12
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:18
ROcp0_16
double ROcp0_16
Definition: mbs_sensor_ArmarIII_simplified_virt.h:91
RLcp0_214
double RLcp0_214
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:155
S11
double S11
Definition: mbs_sensor_ArmarIII_simplified_virt.h:16
JTcp0_314_4
double JTcp0_314_4
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:161
ROcp0_512
double ROcp0_512
Definition: mbs_sensor_ArmarIV-GazeStab-virt.h:66