mbs_sensor_ArmarIV-W-Torso-virt.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 : Fri Oct 28 11:08:15 2016
15//
16// ==> Project name : ArmarIV-W-Torso-virt
17// ==> using XML input file
18//
19// ==> Number of joints : 18
20//
21// ==> Function : F 6 : Sensors Kinematical Informations (sens)
22// ==> Flops complexity : 1319
23//
24// ==> Generation Time : 0.020 seconds
25// ==> Post-Processing : 0.030 seconds
26//
27//-------------------------------------------------------------
28//
29
30#include <math.h>
31
32#include "mbs_data.h"
33#include "mbs_project_interface.h"
34#include "mbs_sensor.h"
35
36void mbs_sensor_ArmarIV - W - Torso - virt(MbsSensor* sens,
37 MbsData* s,
38 int isens)
39{
40
41#include "mbs_sensor_ArmarIV-W-Torso-virt.h"
42#define q s->q
43#define qd s->qd
44#define qdd s->qdd
45
46
47
48 // === begin imp_aux ===
49
50 // === end imp_aux ===
51
52 // ===== BEGIN task 0 =====
53
54 // Sensor Kinematics
55
56
57
58 // = = Block_0_0_0_0_0_1 = =
59
60 // Trigonometric Variables
61
62 C4 = cos(q[4]);
63 S4 = sin(q[4]);
64 C5 = cos(q[5]);
65 S5 = sin(q[5]);
66 C6 = cos(q[6]);
67 S6 = sin(q[6]);
68 C7 = cos(q[7]);
69 S7 = sin(q[7]);
70 C8 = cos(q[8]);
71 S8 = sin(q[8]);
72 C9 = cos(q[9]);
73 S9 = sin(q[9]);
74 C10 = cos(q[10]);
75 S10 = sin(q[10]);
76 C11 = cos(q[11]);
77 S11 = sin(q[11]);
78 C12 = cos(q[12]);
79 S12 = sin(q[12]);
80 C13 = cos(q[13]);
81 S13 = sin(q[13]);
82 C14 = cos(q[14]);
83 S14 = sin(q[14]);
84 C15 = cos(q[15]);
85 S15 = sin(q[15]);
86 C17 = cos(q[17]);
87 S17 = sin(q[17]);
88 C18 = cos(q[18]);
89 S18 = sin(q[18]);
90
91 // = = Block_0_0_0_1_0_1 = =
92
93 // Trigonometric Variables
94
95 C6p7 = C6 * C7 - S6 * S7;
96 S6p7 = C6 * S7 + S6 * C7;
97
98 // ====== END Task 0 ======
99
100 // ===== BEGIN task 1 =====
101
102 switch (isens)
103 {
104
105 //
106 break;
107 case 1:
108
109
110
111 // = = Block_1_0_0_1_0_1 = =
112
113 // Sensor Kinematics
114
115
116 ROcp0_25 = S4 * S5;
117 ROcp0_35 = -C4 * S5;
118 ROcp0_85 = -S4 * C5;
119 ROcp0_95 = C4 * C5;
120 ROcp0_26 = ROcp0_25 * C6 + C4 * S6;
121 ROcp0_36 = ROcp0_35 * C6 + S4 * S6;
122 ROcp0_56 = -(ROcp0_25 * S6 - C4 * C6);
123 ROcp0_66 = -(ROcp0_35 * S6 - S4 * C6);
124 ROcp0_17 = C6p7 * C5;
125 ROcp0_27 = ROcp0_26 * C7 + ROcp0_56 * S7;
126 ROcp0_37 = ROcp0_36 * C7 + ROcp0_66 * S7;
127 ROcp0_47 = -S6p7 * C5;
128 ROcp0_57 = -(ROcp0_26 * S7 - ROcp0_56 * C7);
129 ROcp0_67 = -(ROcp0_36 * S7 - ROcp0_66 * C7);
130 ROcp0_48 = ROcp0_47 * C8 + S5 * S8;
131 ROcp0_58 = ROcp0_57 * C8 + ROcp0_85 * S8;
132 ROcp0_68 = ROcp0_67 * C8 + ROcp0_95 * S8;
133 ROcp0_78 = -(ROcp0_47 * S8 - S5 * C8);
134 ROcp0_88 = -(ROcp0_57 * S8 - ROcp0_85 * C8);
135 ROcp0_98 = -(ROcp0_67 * S8 - ROcp0_95 * C8);
136 ROcp0_49 = ROcp0_48 * C9 + ROcp0_78 * S9;
137 ROcp0_59 = ROcp0_58 * C9 + ROcp0_88 * S9;
138 ROcp0_69 = ROcp0_68 * C9 + ROcp0_98 * S9;
139 ROcp0_79 = -(ROcp0_48 * S9 - ROcp0_78 * C9);
140 ROcp0_89 = -(ROcp0_58 * S9 - ROcp0_88 * C9);
141 ROcp0_99 = -(ROcp0_68 * S9 - ROcp0_98 * C9);
142 ROcp0_110 = ROcp0_17 * C10 - ROcp0_79 * S10;
143 ROcp0_210 = ROcp0_27 * C10 - ROcp0_89 * S10;
144 ROcp0_310 = ROcp0_37 * C10 - ROcp0_99 * S10;
145 ROcp0_710 = ROcp0_17 * S10 + ROcp0_79 * C10;
146 ROcp0_810 = ROcp0_27 * S10 + ROcp0_89 * C10;
147 ROcp0_910 = ROcp0_37 * S10 + ROcp0_99 * C10;
148 ROcp0_111 = ROcp0_110 * C11 + ROcp0_49 * S11;
149 ROcp0_211 = ROcp0_210 * C11 + ROcp0_59 * S11;
150 ROcp0_311 = ROcp0_310 * C11 + ROcp0_69 * S11;
151 ROcp0_411 = -(ROcp0_110 * S11 - ROcp0_49 * C11);
152 ROcp0_511 = -(ROcp0_210 * S11 - ROcp0_59 * C11);
153 ROcp0_611 = -(ROcp0_310 * S11 - ROcp0_69 * C11);
154 ROcp0_412 = ROcp0_411 * C12 + ROcp0_710 * S12;
155 ROcp0_512 = ROcp0_511 * C12 + ROcp0_810 * S12;
156 ROcp0_612 = ROcp0_611 * C12 + ROcp0_910 * S12;
157 ROcp0_712 = -(ROcp0_411 * S12 - ROcp0_710 * C12);
158 ROcp0_812 = -(ROcp0_511 * S12 - ROcp0_810 * C12);
159 ROcp0_912 = -(ROcp0_611 * S12 - ROcp0_910 * C12);
160 ROcp0_113 = ROcp0_111 * C13 - ROcp0_712 * S13;
161 ROcp0_213 = ROcp0_211 * C13 - ROcp0_812 * S13;
162 ROcp0_313 = ROcp0_311 * C13 - ROcp0_912 * S13;
163 ROcp0_713 = ROcp0_111 * S13 + ROcp0_712 * C13;
164 ROcp0_813 = ROcp0_211 * S13 + ROcp0_812 * C13;
165 ROcp0_913 = ROcp0_311 * S13 + ROcp0_912 * C13;
166 ROcp0_414 = ROcp0_412 * C14 + ROcp0_713 * S14;
167 ROcp0_514 = ROcp0_512 * C14 + ROcp0_813 * S14;
168 ROcp0_614 = ROcp0_612 * C14 + ROcp0_913 * S14;
169 ROcp0_714 = -(ROcp0_412 * S14 - ROcp0_713 * C14);
170 ROcp0_814 = -(ROcp0_512 * S14 - ROcp0_813 * C14);
171 ROcp0_914 = -(ROcp0_612 * S14 - ROcp0_913 * C14);
172 ROcp0_115 = ROcp0_113 * C15 + ROcp0_414 * S15;
173 ROcp0_215 = ROcp0_213 * C15 + ROcp0_514 * S15;
174 ROcp0_315 = ROcp0_313 * C15 + ROcp0_614 * S15;
175 ROcp0_415 = -(ROcp0_113 * S15 - ROcp0_414 * C15);
176 ROcp0_515 = -(ROcp0_213 * S15 - ROcp0_514 * C15);
177 ROcp0_615 = -(ROcp0_313 * S15 - ROcp0_614 * C15);
178 ROcp0_417 = ROcp0_415 * C17 + ROcp0_714 * S17;
179 ROcp0_517 = ROcp0_515 * C17 + ROcp0_814 * S17;
180 ROcp0_617 = ROcp0_615 * C17 + ROcp0_914 * S17;
181 ROcp0_717 = -(ROcp0_415 * S17 - ROcp0_714 * C17);
182 ROcp0_817 = -(ROcp0_515 * S17 - ROcp0_814 * C17);
183 ROcp0_917 = -(ROcp0_615 * S17 - ROcp0_914 * C17);
184 ROcp0_118 = ROcp0_115 * C18 + ROcp0_417 * S18;
185 ROcp0_218 = ROcp0_215 * C18 + ROcp0_517 * S18;
186 ROcp0_318 = ROcp0_315 * C18 + ROcp0_617 * S18;
187 ROcp0_418 = -(ROcp0_115 * S18 - ROcp0_417 * C18);
188 ROcp0_518 = -(ROcp0_215 * S18 - ROcp0_517 * C18);
189 ROcp0_618 = -(ROcp0_315 * S18 - ROcp0_617 * C18);
190 OMcp0_25 = qd[5] * C4;
191 OMcp0_35 = qd[5] * S4;
192 OMcp0_16 = qd[4] + qd[6] * S5;
193 OMcp0_26 = OMcp0_25 + ROcp0_85 * qd[6];
194 OMcp0_36 = OMcp0_35 + ROcp0_95 * qd[6];
195 OMcp0_17 = OMcp0_16 + qd[7] * S5;
196 OMcp0_27 = OMcp0_26 + ROcp0_85 * qd[7];
197 OMcp0_37 = OMcp0_36 + ROcp0_95 * qd[7];
198 OPcp0_17 = qdd[4] + qdd[6] * S5 + qdd[7] * S5 + qd[5] * qd[6] * C5 + qd[7] * (OMcp0_26 * ROcp0_95 - OMcp0_36 * ROcp0_85);
199 OPcp0_27 = ROcp0_85 * (qdd[6] + qdd[7]) + qdd[5] * C4 - qd[4] * qd[5] * S4 + qd[6] * (OMcp0_35 * S5 - ROcp0_95 * qd[4]) - qd[7] * (OMcp0_16 *
200 ROcp0_95 - OMcp0_36 * S5);
201 OPcp0_37 = ROcp0_95 * (qdd[6] + qdd[7]) + qdd[5] * S4 + qd[4] * qd[5] * C4 - qd[6] * (OMcp0_25 * S5 - ROcp0_85 * qd[4]) + qd[7] * (OMcp0_16 *
202 ROcp0_85 - OMcp0_26 * S5);
203 RLcp0_18 = s->dpt[3][1] * S5;
204 RLcp0_28 = ROcp0_85 * s->dpt[3][1];
205 RLcp0_38 = ROcp0_95 * s->dpt[3][1];
206 OMcp0_18 = OMcp0_17 + ROcp0_17 * qd[8];
207 OMcp0_28 = OMcp0_27 + ROcp0_27 * qd[8];
208 OMcp0_38 = OMcp0_37 + ROcp0_37 * qd[8];
209 ORcp0_18 = OMcp0_27 * RLcp0_38 - OMcp0_37 * RLcp0_28;
210 ORcp0_28 = -(OMcp0_17 * RLcp0_38 - OMcp0_37 * RLcp0_18);
211 ORcp0_38 = OMcp0_17 * RLcp0_28 - OMcp0_27 * RLcp0_18;
212 OPcp0_18 = OPcp0_17 + ROcp0_17 * qdd[8] + qd[8] * (OMcp0_27 * ROcp0_37 - OMcp0_37 * ROcp0_27);
213 OPcp0_28 = OPcp0_27 + ROcp0_27 * qdd[8] - qd[8] * (OMcp0_17 * ROcp0_37 - OMcp0_37 * ROcp0_17);
214 OPcp0_38 = OPcp0_37 + ROcp0_37 * qdd[8] + qd[8] * (OMcp0_17 * ROcp0_27 - OMcp0_27 * ROcp0_17);
215 RLcp0_19 = ROcp0_78 * s->dpt[3][2];
216 RLcp0_29 = ROcp0_88 * s->dpt[3][2];
217 RLcp0_39 = ROcp0_98 * s->dpt[3][2];
218 OMcp0_19 = OMcp0_18 + ROcp0_17 * qd[9];
219 OMcp0_29 = OMcp0_28 + ROcp0_27 * qd[9];
220 OMcp0_39 = OMcp0_38 + ROcp0_37 * qd[9];
221 ORcp0_19 = OMcp0_28 * RLcp0_39 - OMcp0_38 * RLcp0_29;
222 ORcp0_29 = -(OMcp0_18 * RLcp0_39 - OMcp0_38 * RLcp0_19);
223 ORcp0_39 = OMcp0_18 * RLcp0_29 - OMcp0_28 * RLcp0_19;
224 OMcp0_110 = OMcp0_19 + ROcp0_49 * qd[10];
225 OMcp0_210 = OMcp0_29 + ROcp0_59 * qd[10];
226 OMcp0_310 = OMcp0_39 + ROcp0_69 * qd[10];
227 OPcp0_110 = OPcp0_18 + ROcp0_17 * qdd[9] + ROcp0_49 * qdd[10] + qd[10] * (OMcp0_29 * ROcp0_69 - OMcp0_39 * ROcp0_59) + qd[9] * (OMcp0_28 *
228 ROcp0_37 - OMcp0_38 * ROcp0_27);
229 OPcp0_210 = OPcp0_28 + ROcp0_27 * qdd[9] + ROcp0_59 * qdd[10] - qd[10] * (OMcp0_19 * ROcp0_69 - OMcp0_39 * ROcp0_49) - qd[9] * (OMcp0_18 *
230 ROcp0_37 - OMcp0_38 * ROcp0_17);
231 OPcp0_310 = OPcp0_38 + ROcp0_37 * qdd[9] + ROcp0_69 * qdd[10] + qd[10] * (OMcp0_19 * ROcp0_59 - OMcp0_29 * ROcp0_49) + qd[9] * (OMcp0_18 *
232 ROcp0_27 - OMcp0_28 * ROcp0_17);
233 RLcp0_111 = ROcp0_710 * s->dpt[3][3];
234 RLcp0_211 = ROcp0_810 * s->dpt[3][3];
235 RLcp0_311 = ROcp0_910 * s->dpt[3][3];
236 OMcp0_111 = OMcp0_110 + ROcp0_710 * qd[11];
237 OMcp0_211 = OMcp0_210 + ROcp0_810 * qd[11];
238 OMcp0_311 = OMcp0_310 + ROcp0_910 * qd[11];
239 ORcp0_111 = OMcp0_210 * RLcp0_311 - OMcp0_310 * RLcp0_211;
240 ORcp0_211 = -(OMcp0_110 * RLcp0_311 - OMcp0_310 * RLcp0_111);
241 ORcp0_311 = OMcp0_110 * RLcp0_211 - OMcp0_210 * RLcp0_111;
242 OPcp0_111 = OPcp0_110 + ROcp0_710 * qdd[11] + qd[11] * (OMcp0_210 * ROcp0_910 - OMcp0_310 * ROcp0_810);
243 OPcp0_211 = OPcp0_210 + ROcp0_810 * qdd[11] - qd[11] * (OMcp0_110 * ROcp0_910 - OMcp0_310 * ROcp0_710);
244 OPcp0_311 = OPcp0_310 + ROcp0_910 * qdd[11] + qd[11] * (OMcp0_110 * ROcp0_810 - OMcp0_210 * ROcp0_710);
245 RLcp0_112 = ROcp0_710 * s->dpt[3][4];
246 RLcp0_212 = ROcp0_810 * s->dpt[3][4];
247 RLcp0_312 = ROcp0_910 * s->dpt[3][4];
248 OMcp0_112 = OMcp0_111 + ROcp0_111 * qd[12];
249 OMcp0_212 = OMcp0_211 + ROcp0_211 * qd[12];
250 OMcp0_312 = OMcp0_311 + ROcp0_311 * qd[12];
251 ORcp0_112 = OMcp0_211 * RLcp0_312 - OMcp0_311 * RLcp0_212;
252 ORcp0_212 = -(OMcp0_111 * RLcp0_312 - OMcp0_311 * RLcp0_112);
253 ORcp0_312 = OMcp0_111 * RLcp0_212 - OMcp0_211 * RLcp0_112;
254 OPcp0_112 = OPcp0_111 + ROcp0_111 * qdd[12] + qd[12] * (OMcp0_211 * ROcp0_311 - OMcp0_311 * ROcp0_211);
255 OPcp0_212 = OPcp0_211 + ROcp0_211 * qdd[12] - qd[12] * (OMcp0_111 * ROcp0_311 - OMcp0_311 * ROcp0_111);
256 OPcp0_312 = OPcp0_311 + ROcp0_311 * qdd[12] + qd[12] * (OMcp0_111 * ROcp0_211 - OMcp0_211 * ROcp0_111);
257 RLcp0_113 = ROcp0_412 * s->dpt[2][5];
258 RLcp0_213 = ROcp0_512 * s->dpt[2][5];
259 RLcp0_313 = ROcp0_612 * s->dpt[2][5];
260 OMcp0_113 = OMcp0_112 + ROcp0_412 * qd[13];
261 OMcp0_213 = OMcp0_212 + ROcp0_512 * qd[13];
262 OMcp0_313 = OMcp0_312 + ROcp0_612 * qd[13];
263 ORcp0_113 = OMcp0_212 * RLcp0_313 - OMcp0_312 * RLcp0_213;
264 ORcp0_213 = -(OMcp0_112 * RLcp0_313 - OMcp0_312 * RLcp0_113);
265 ORcp0_313 = OMcp0_112 * RLcp0_213 - OMcp0_212 * RLcp0_113;
266 OPcp0_113 = OPcp0_112 + ROcp0_412 * qdd[13] + qd[13] * (OMcp0_212 * ROcp0_612 - OMcp0_312 * ROcp0_512);
267 OPcp0_213 = OPcp0_212 + ROcp0_512 * qdd[13] - qd[13] * (OMcp0_112 * ROcp0_612 - OMcp0_312 * ROcp0_412);
268 OPcp0_313 = OPcp0_312 + ROcp0_612 * qdd[13] + qd[13] * (OMcp0_112 * ROcp0_512 - OMcp0_212 * ROcp0_412);
269 RLcp0_114 = ROcp0_113 * s->dpt[1][6] + ROcp0_412 * s->dpt[2][6] + ROcp0_713 * s->dpt[3][6];
270 RLcp0_214 = ROcp0_213 * s->dpt[1][6] + ROcp0_512 * s->dpt[2][6] + ROcp0_813 * s->dpt[3][6];
271 RLcp0_314 = ROcp0_313 * s->dpt[1][6] + ROcp0_612 * s->dpt[2][6] + ROcp0_913 * s->dpt[3][6];
272 OMcp0_114 = OMcp0_113 + ROcp0_113 * qd[14];
273 OMcp0_214 = OMcp0_213 + ROcp0_213 * qd[14];
274 OMcp0_314 = OMcp0_313 + ROcp0_313 * qd[14];
275 ORcp0_114 = OMcp0_213 * RLcp0_314 - OMcp0_313 * RLcp0_214;
276 ORcp0_214 = -(OMcp0_113 * RLcp0_314 - OMcp0_313 * RLcp0_114);
277 ORcp0_314 = OMcp0_113 * RLcp0_214 - OMcp0_213 * RLcp0_114;
278 OMcp0_115 = OMcp0_114 + ROcp0_714 * qd[15];
279 OMcp0_215 = OMcp0_214 + ROcp0_814 * qd[15];
280 OMcp0_315 = OMcp0_314 + ROcp0_914 * qd[15];
281 OPcp0_115 = OPcp0_113 + ROcp0_113 * qdd[14] + ROcp0_714 * qdd[15] + qd[14] * (OMcp0_213 * ROcp0_313 - OMcp0_313 * ROcp0_213) + qd[15] * (
282 OMcp0_214 * ROcp0_914 - OMcp0_314 * ROcp0_814);
283 OPcp0_215 = OPcp0_213 + ROcp0_213 * qdd[14] + ROcp0_814 * qdd[15] - qd[14] * (OMcp0_113 * ROcp0_313 - OMcp0_313 * ROcp0_113) - qd[15] * (
284 OMcp0_114 * ROcp0_914 - OMcp0_314 * ROcp0_714);
285 OPcp0_315 = OPcp0_313 + ROcp0_313 * qdd[14] + ROcp0_914 * qdd[15] + qd[14] * (OMcp0_113 * ROcp0_213 - OMcp0_213 * ROcp0_113) + qd[15] * (
286 OMcp0_114 * ROcp0_814 - OMcp0_214 * ROcp0_714);
287 RLcp0_116 = ROcp0_415 * q[16];
288 RLcp0_216 = ROcp0_515 * q[16];
289 RLcp0_316 = ROcp0_615 * q[16];
290 POcp0_116 = RLcp0_111 + RLcp0_112 + RLcp0_113 + RLcp0_114 + RLcp0_116 + RLcp0_18 + RLcp0_19 + q[1];
291 POcp0_216 = RLcp0_211 + RLcp0_212 + RLcp0_213 + RLcp0_214 + RLcp0_216 + RLcp0_28 + RLcp0_29 + q[2];
292 POcp0_316 = RLcp0_311 + RLcp0_312 + RLcp0_313 + RLcp0_314 + RLcp0_316 + RLcp0_38 + RLcp0_39 + q[3];
293 JTcp0_216_4 = -(RLcp0_311 + RLcp0_312 + RLcp0_313 + RLcp0_314 + RLcp0_316 + RLcp0_38 + RLcp0_39);
294 JTcp0_316_4 = RLcp0_211 + RLcp0_212 + RLcp0_213 + RLcp0_214 + RLcp0_216 + RLcp0_28 + RLcp0_29;
295 JTcp0_116_5 = s->dpt[3][1] * C5 + C4 * (RLcp0_311 + RLcp0_39) + C4 * (RLcp0_312 + RLcp0_313) + C4 * (RLcp0_314 + RLcp0_316) - S4 * (RLcp0_211 +
296 RLcp0_29) - S4 * (RLcp0_212 + RLcp0_213) - S4 * (RLcp0_214 + RLcp0_216);
297 JTcp0_216_5 = S4 * (RLcp0_111 + RLcp0_112 + RLcp0_113 + RLcp0_114 + RLcp0_116 + RLcp0_18 + RLcp0_19);
298 JTcp0_316_5 = -C4 * (RLcp0_111 + RLcp0_112 + RLcp0_113 + RLcp0_114 + RLcp0_116 + RLcp0_18 + RLcp0_19);
299 JTcp0_116_6 = ROcp0_85 * (RLcp0_311 + RLcp0_312 + RLcp0_313 + RLcp0_314 + RLcp0_316 + RLcp0_39) - ROcp0_95 * (RLcp0_211 + RLcp0_29) -
300 ROcp0_95 * (RLcp0_212 + RLcp0_213) - ROcp0_95 * (RLcp0_214 + RLcp0_216);
301 JTcp0_216_6 = RLcp0_116 * ROcp0_95 - RLcp0_314 * S5 - RLcp0_316 * S5 + ROcp0_95 * (RLcp0_111 + RLcp0_112 + RLcp0_113 + RLcp0_114 + RLcp0_19)
302 - S5 * (RLcp0_311 + RLcp0_39) - S5 * (RLcp0_312 + RLcp0_313);
303 JTcp0_316_6 = RLcp0_214 * S5 - ROcp0_85 * (RLcp0_111 + RLcp0_112 + RLcp0_113 + RLcp0_114 + RLcp0_19) + S5 * (RLcp0_211 + RLcp0_29) + S5 * (
304 RLcp0_212 + RLcp0_213) - RLcp0_116 * ROcp0_85 + RLcp0_216 * S5;
305 JTcp0_116_7 = ROcp0_85 * (RLcp0_311 + RLcp0_312 + RLcp0_313 + RLcp0_314 + RLcp0_316 + RLcp0_39) - ROcp0_95 * (RLcp0_211 + RLcp0_29) -
306 ROcp0_95 * (RLcp0_212 + RLcp0_213) - ROcp0_95 * (RLcp0_214 + RLcp0_216);
307 JTcp0_216_7 = RLcp0_116 * ROcp0_95 - RLcp0_314 * S5 - RLcp0_316 * S5 + ROcp0_95 * (RLcp0_111 + RLcp0_112 + RLcp0_113 + RLcp0_114 + RLcp0_19)
308 - S5 * (RLcp0_311 + RLcp0_39) - S5 * (RLcp0_312 + RLcp0_313);
309 JTcp0_316_7 = RLcp0_214 * S5 - ROcp0_85 * (RLcp0_111 + RLcp0_112 + RLcp0_113 + RLcp0_114 + RLcp0_19) + S5 * (RLcp0_211 + RLcp0_29) + S5 * (
310 RLcp0_212 + RLcp0_213) - RLcp0_116 * ROcp0_85 + RLcp0_216 * S5;
311 JTcp0_116_8 = ROcp0_27 * (RLcp0_311 + RLcp0_312 + RLcp0_313 + RLcp0_314 + RLcp0_316 + RLcp0_39) - ROcp0_37 * (RLcp0_211 + RLcp0_29) -
312 ROcp0_37 * (RLcp0_212 + RLcp0_213) - ROcp0_37 * (RLcp0_214 + RLcp0_216);
313 JTcp0_216_8 = -(ROcp0_17 * (RLcp0_311 + RLcp0_312 + RLcp0_313 + RLcp0_314 + RLcp0_316 + RLcp0_39) - ROcp0_37 * (RLcp0_111 + RLcp0_19) -
314 ROcp0_37 * (RLcp0_112 + RLcp0_113) - ROcp0_37 * (RLcp0_114 + RLcp0_116));
315 JTcp0_316_8 = ROcp0_17 * (RLcp0_211 + RLcp0_212 + RLcp0_213 + RLcp0_214 + RLcp0_216 + RLcp0_29) - ROcp0_27 * (RLcp0_111 + RLcp0_19) -
316 ROcp0_27 * (RLcp0_112 + RLcp0_113) - ROcp0_27 * (RLcp0_114 + RLcp0_116);
317 JTcp0_116_9 = ROcp0_27 * (RLcp0_311 + RLcp0_312 + RLcp0_313 + RLcp0_314) - ROcp0_37 * (RLcp0_211 + RLcp0_212) - ROcp0_37 * (RLcp0_213 +
318 RLcp0_214) - RLcp0_216 * ROcp0_37 + RLcp0_316 * ROcp0_27;
319 JTcp0_216_9 = RLcp0_116 * ROcp0_37 - RLcp0_316 * ROcp0_17 - ROcp0_17 * (RLcp0_311 + RLcp0_312 + RLcp0_313 + RLcp0_314) + ROcp0_37 * (
320 RLcp0_111 + RLcp0_112) + ROcp0_37 * (RLcp0_113 + RLcp0_114);
321 JTcp0_316_9 = ROcp0_17 * (RLcp0_211 + RLcp0_212 + RLcp0_213 + RLcp0_214) - ROcp0_27 * (RLcp0_111 + RLcp0_112) - ROcp0_27 * (RLcp0_113 +
322 RLcp0_114) - RLcp0_116 * ROcp0_27 + RLcp0_216 * ROcp0_17;
323 JTcp0_116_10 = ROcp0_59 * (RLcp0_311 + RLcp0_312 + RLcp0_313 + RLcp0_314) - ROcp0_69 * (RLcp0_211 + RLcp0_212) - ROcp0_69 * (RLcp0_213 +
324 RLcp0_214) - RLcp0_216 * ROcp0_69 + RLcp0_316 * ROcp0_59;
325 JTcp0_216_10 = RLcp0_116 * ROcp0_69 - RLcp0_316 * ROcp0_49 - ROcp0_49 * (RLcp0_311 + RLcp0_312 + RLcp0_313 + RLcp0_314) + ROcp0_69 * (
326 RLcp0_111 + RLcp0_112) + ROcp0_69 * (RLcp0_113 + RLcp0_114);
327 JTcp0_316_10 = ROcp0_49 * (RLcp0_211 + RLcp0_212 + RLcp0_213 + RLcp0_214) - ROcp0_59 * (RLcp0_111 + RLcp0_112) - ROcp0_59 * (RLcp0_113 +
328 RLcp0_114) - RLcp0_116 * ROcp0_59 + RLcp0_216 * ROcp0_49;
329 JTcp0_116_11 = ROcp0_810 * (RLcp0_312 + RLcp0_313 + RLcp0_314 + RLcp0_316) - ROcp0_910 * (RLcp0_212 + RLcp0_213) - ROcp0_910 * (
330 RLcp0_214 + RLcp0_216);
331 JTcp0_216_11 = -(ROcp0_710 * (RLcp0_312 + RLcp0_313 + RLcp0_314 + RLcp0_316) - ROcp0_910 * (RLcp0_112 + RLcp0_113) - ROcp0_910 * (
332 RLcp0_114 + RLcp0_116));
333 JTcp0_316_11 = ROcp0_710 * (RLcp0_212 + RLcp0_213 + RLcp0_214 + RLcp0_216) - ROcp0_810 * (RLcp0_112 + RLcp0_113) - ROcp0_810 * (
334 RLcp0_114 + RLcp0_116);
335 JTcp0_116_12 = ROcp0_211 * (RLcp0_313 + RLcp0_314) - ROcp0_311 * (RLcp0_213 + RLcp0_214) - RLcp0_216 * ROcp0_311 + RLcp0_316 * ROcp0_211;
336 JTcp0_216_12 = RLcp0_116 * ROcp0_311 - RLcp0_316 * ROcp0_111 - ROcp0_111 * (RLcp0_313 + RLcp0_314) + ROcp0_311 * (RLcp0_113 + RLcp0_114);
337 JTcp0_316_12 = ROcp0_111 * (RLcp0_213 + RLcp0_214) - ROcp0_211 * (RLcp0_113 + RLcp0_114) - RLcp0_116 * ROcp0_211 + RLcp0_216 * ROcp0_111;
338 JTcp0_116_13 = ROcp0_512 * (RLcp0_314 + RLcp0_316) - ROcp0_612 * (RLcp0_214 + RLcp0_216);
339 JTcp0_216_13 = -(ROcp0_412 * (RLcp0_314 + RLcp0_316) - ROcp0_612 * (RLcp0_114 + RLcp0_116));
340 JTcp0_316_13 = ROcp0_412 * (RLcp0_214 + RLcp0_216) - ROcp0_512 * (RLcp0_114 + RLcp0_116);
341 JTcp0_116_14 = -(RLcp0_216 * ROcp0_313 - RLcp0_316 * ROcp0_213);
342 JTcp0_216_14 = RLcp0_116 * ROcp0_313 - RLcp0_316 * ROcp0_113;
343 JTcp0_316_14 = -(RLcp0_116 * ROcp0_213 - RLcp0_216 * ROcp0_113);
344 JTcp0_116_15 = -(RLcp0_216 * ROcp0_914 - RLcp0_316 * ROcp0_814);
345 JTcp0_216_15 = RLcp0_116 * ROcp0_914 - RLcp0_316 * ROcp0_714;
346 JTcp0_316_15 = -(RLcp0_116 * ROcp0_814 - RLcp0_216 * ROcp0_714);
347 ORcp0_116 = OMcp0_215 * RLcp0_316 - OMcp0_315 * RLcp0_216;
348 ORcp0_216 = -(OMcp0_115 * RLcp0_316 - OMcp0_315 * RLcp0_116);
349 ORcp0_316 = OMcp0_115 * RLcp0_216 - OMcp0_215 * RLcp0_116;
350 VIcp0_116 = ORcp0_111 + ORcp0_112 + ORcp0_113 + ORcp0_114 + ORcp0_116 + ORcp0_18 + ORcp0_19 + qd[1] + ROcp0_415 * qd[16];
351 VIcp0_216 = ORcp0_211 + ORcp0_212 + ORcp0_213 + ORcp0_214 + ORcp0_216 + ORcp0_28 + ORcp0_29 + qd[2] + ROcp0_515 * qd[16];
352 VIcp0_316 = ORcp0_311 + ORcp0_312 + ORcp0_313 + ORcp0_314 + ORcp0_316 + ORcp0_38 + ORcp0_39 + qd[3] + ROcp0_615 * qd[16];
353 ACcp0_116 = qdd[1] + OMcp0_210 * ORcp0_311 + OMcp0_211 * ORcp0_312 + OMcp0_212 * ORcp0_313 + OMcp0_213 * ORcp0_314 + OMcp0_215 * ORcp0_316
354 + OMcp0_27 * ORcp0_38 + OMcp0_28 * ORcp0_39 - OMcp0_310 * ORcp0_211 - OMcp0_311 * ORcp0_212 - OMcp0_312 * ORcp0_213 - OMcp0_313 * ORcp0_214 -
355 OMcp0_315 * ORcp0_216 - OMcp0_37 * ORcp0_28 - OMcp0_38 * ORcp0_29 + OPcp0_210 * RLcp0_311 + OPcp0_211 * RLcp0_312 + OPcp0_212 * RLcp0_313 +
356 OPcp0_213 * RLcp0_314 + OPcp0_215 * RLcp0_316 + OPcp0_27 * RLcp0_38 + OPcp0_28 * RLcp0_39 - OPcp0_310 * RLcp0_211 - OPcp0_311 * RLcp0_212 -
357 OPcp0_312 * RLcp0_213 - OPcp0_313 * RLcp0_214 - OPcp0_315 * RLcp0_216 - OPcp0_37 * RLcp0_28 - OPcp0_38 * RLcp0_29 + ROcp0_415 * qdd[16] + (2.0) * qd[16] * (
358 OMcp0_215 * ROcp0_615 - OMcp0_315 * ROcp0_515);
359 ACcp0_216 = qdd[2] - OMcp0_110 * ORcp0_311 - OMcp0_111 * ORcp0_312 - OMcp0_112 * ORcp0_313 - OMcp0_113 * ORcp0_314 - OMcp0_115 * ORcp0_316
360 - OMcp0_17 * ORcp0_38 - OMcp0_18 * ORcp0_39 + OMcp0_310 * ORcp0_111 + OMcp0_311 * ORcp0_112 + OMcp0_312 * ORcp0_113 + OMcp0_313 * ORcp0_114 +
361 OMcp0_315 * ORcp0_116 + OMcp0_37 * ORcp0_18 + OMcp0_38 * ORcp0_19 - OPcp0_110 * RLcp0_311 - OPcp0_111 * RLcp0_312 - OPcp0_112 * RLcp0_313 -
362 OPcp0_113 * RLcp0_314 - OPcp0_115 * RLcp0_316 - OPcp0_17 * RLcp0_38 - OPcp0_18 * RLcp0_39 + OPcp0_310 * RLcp0_111 + OPcp0_311 * RLcp0_112 +
363 OPcp0_312 * RLcp0_113 + OPcp0_313 * RLcp0_114 + OPcp0_315 * RLcp0_116 + OPcp0_37 * RLcp0_18 + OPcp0_38 * RLcp0_19 + ROcp0_515 * qdd[16] - (2.0) * qd[16] * (
364 OMcp0_115 * ROcp0_615 - OMcp0_315 * ROcp0_415);
365 ACcp0_316 = qdd[3] + OMcp0_110 * ORcp0_211 + OMcp0_111 * ORcp0_212 + OMcp0_112 * ORcp0_213 + OMcp0_113 * ORcp0_214 + OMcp0_115 * ORcp0_216
366 + OMcp0_17 * ORcp0_28 + OMcp0_18 * ORcp0_29 - OMcp0_210 * ORcp0_111 - OMcp0_211 * ORcp0_112 - OMcp0_212 * ORcp0_113 - OMcp0_213 * ORcp0_114 -
367 OMcp0_215 * ORcp0_116 - OMcp0_27 * ORcp0_18 - OMcp0_28 * ORcp0_19 + OPcp0_110 * RLcp0_211 + OPcp0_111 * RLcp0_212 + OPcp0_112 * RLcp0_213 +
368 OPcp0_113 * RLcp0_214 + OPcp0_115 * RLcp0_216 + OPcp0_17 * RLcp0_28 + OPcp0_18 * RLcp0_29 - OPcp0_210 * RLcp0_111 - OPcp0_211 * RLcp0_112 -
369 OPcp0_212 * RLcp0_113 - OPcp0_213 * RLcp0_114 - OPcp0_215 * RLcp0_116 - OPcp0_27 * RLcp0_18 - OPcp0_28 * RLcp0_19 + ROcp0_615 * qdd[16] + (2.0) * qd[16] * (
370 OMcp0_115 * ROcp0_515 - OMcp0_215 * ROcp0_415);
371 OMcp0_117 = OMcp0_115 + ROcp0_115 * qd[17];
372 OMcp0_217 = OMcp0_215 + ROcp0_215 * qd[17];
373 OMcp0_317 = OMcp0_315 + ROcp0_315 * qd[17];
374 OMcp0_118 = OMcp0_117 + ROcp0_717 * qd[18];
375 OMcp0_218 = OMcp0_217 + ROcp0_817 * qd[18];
376 OMcp0_318 = OMcp0_317 + ROcp0_917 * qd[18];
377 OPcp0_118 = OPcp0_115 + ROcp0_115 * qdd[17] + ROcp0_717 * qdd[18] + qd[17] * (OMcp0_215 * ROcp0_315 - OMcp0_315 * ROcp0_215) + qd[18] * (
378 OMcp0_217 * ROcp0_917 - OMcp0_317 * ROcp0_817);
379 OPcp0_218 = OPcp0_215 + ROcp0_215 * qdd[17] + ROcp0_817 * qdd[18] - qd[17] * (OMcp0_115 * ROcp0_315 - OMcp0_315 * ROcp0_115) - qd[18] * (
380 OMcp0_117 * ROcp0_917 - OMcp0_317 * ROcp0_717);
381 OPcp0_318 = OPcp0_315 + ROcp0_315 * qdd[17] + ROcp0_917 * qdd[18] + qd[17] * (OMcp0_115 * ROcp0_215 - OMcp0_215 * ROcp0_115) + qd[18] * (
382 OMcp0_117 * ROcp0_817 - OMcp0_217 * ROcp0_717);
383
384 // = = Block_1_0_0_1_1_0 = =
385
386 // Symbolic Outputs
387
388 sens->P[1] = POcp0_116;
389 sens->P[2] = POcp0_216;
390 sens->P[3] = POcp0_316;
391 sens->R[1][1] = ROcp0_118;
392 sens->R[1][2] = ROcp0_218;
393 sens->R[1][3] = ROcp0_318;
394 sens->R[2][1] = ROcp0_418;
395 sens->R[2][2] = ROcp0_518;
396 sens->R[2][3] = ROcp0_618;
397 sens->R[3][1] = ROcp0_717;
398 sens->R[3][2] = ROcp0_817;
399 sens->R[3][3] = ROcp0_917;
400 sens->V[1] = VIcp0_116;
401 sens->V[2] = VIcp0_216;
402 sens->V[3] = VIcp0_316;
403 sens->OM[1] = OMcp0_118;
404 sens->OM[2] = OMcp0_218;
405 sens->OM[3] = OMcp0_318;
406 sens->J[1][1] = (1.0);
407 sens->J[1][5] = JTcp0_116_5;
408 sens->J[1][6] = JTcp0_116_6;
409 sens->J[1][7] = JTcp0_116_7;
410 sens->J[1][8] = JTcp0_116_8;
411 sens->J[1][9] = JTcp0_116_9;
412 sens->J[1][10] = JTcp0_116_10;
413 sens->J[1][11] = JTcp0_116_11;
414 sens->J[1][12] = JTcp0_116_12;
415 sens->J[1][13] = JTcp0_116_13;
416 sens->J[1][14] = JTcp0_116_14;
417 sens->J[1][15] = JTcp0_116_15;
418 sens->J[1][16] = ROcp0_415;
419 sens->J[2][2] = (1.0);
420 sens->J[2][4] = JTcp0_216_4;
421 sens->J[2][5] = JTcp0_216_5;
422 sens->J[2][6] = JTcp0_216_6;
423 sens->J[2][7] = JTcp0_216_7;
424 sens->J[2][8] = JTcp0_216_8;
425 sens->J[2][9] = JTcp0_216_9;
426 sens->J[2][10] = JTcp0_216_10;
427 sens->J[2][11] = JTcp0_216_11;
428 sens->J[2][12] = JTcp0_216_12;
429 sens->J[2][13] = JTcp0_216_13;
430 sens->J[2][14] = JTcp0_216_14;
431 sens->J[2][15] = JTcp0_216_15;
432 sens->J[2][16] = ROcp0_515;
433 sens->J[3][3] = (1.0);
434 sens->J[3][4] = JTcp0_316_4;
435 sens->J[3][5] = JTcp0_316_5;
436 sens->J[3][6] = JTcp0_316_6;
437 sens->J[3][7] = JTcp0_316_7;
438 sens->J[3][8] = JTcp0_316_8;
439 sens->J[3][9] = JTcp0_316_9;
440 sens->J[3][10] = JTcp0_316_10;
441 sens->J[3][11] = JTcp0_316_11;
442 sens->J[3][12] = JTcp0_316_12;
443 sens->J[3][13] = JTcp0_316_13;
444 sens->J[3][14] = JTcp0_316_14;
445 sens->J[3][15] = JTcp0_316_15;
446 sens->J[3][16] = ROcp0_615;
447 sens->J[4][4] = (1.0);
448 sens->J[4][6] = S5;
449 sens->J[4][7] = S5;
450 sens->J[4][8] = ROcp0_17;
451 sens->J[4][9] = ROcp0_17;
452 sens->J[4][10] = ROcp0_49;
453 sens->J[4][11] = ROcp0_710;
454 sens->J[4][12] = ROcp0_111;
455 sens->J[4][13] = ROcp0_412;
456 sens->J[4][14] = ROcp0_113;
457 sens->J[4][15] = ROcp0_714;
458 sens->J[4][17] = ROcp0_115;
459 sens->J[4][18] = ROcp0_717;
460 sens->J[5][5] = C4;
461 sens->J[5][6] = ROcp0_85;
462 sens->J[5][7] = ROcp0_85;
463 sens->J[5][8] = ROcp0_27;
464 sens->J[5][9] = ROcp0_27;
465 sens->J[5][10] = ROcp0_59;
466 sens->J[5][11] = ROcp0_810;
467 sens->J[5][12] = ROcp0_211;
468 sens->J[5][13] = ROcp0_512;
469 sens->J[5][14] = ROcp0_213;
470 sens->J[5][15] = ROcp0_814;
471 sens->J[5][17] = ROcp0_215;
472 sens->J[5][18] = ROcp0_817;
473 sens->J[6][5] = S4;
474 sens->J[6][6] = ROcp0_95;
475 sens->J[6][7] = ROcp0_95;
476 sens->J[6][8] = ROcp0_37;
477 sens->J[6][9] = ROcp0_37;
478 sens->J[6][10] = ROcp0_69;
479 sens->J[6][11] = ROcp0_910;
480 sens->J[6][12] = ROcp0_311;
481 sens->J[6][13] = ROcp0_612;
482 sens->J[6][14] = ROcp0_313;
483 sens->J[6][15] = ROcp0_914;
484 sens->J[6][17] = ROcp0_315;
485 sens->J[6][18] = ROcp0_917;
486 sens->A[1] = ACcp0_116;
487 sens->A[2] = ACcp0_216;
488 sens->A[3] = ACcp0_316;
489 sens->OMP[1] = OPcp0_118;
490 sens->OMP[2] = OPcp0_218;
491 sens->OMP[3] = OPcp0_318;
492
493 break;
494 default:
495 break;
496 }
497
498
499 // ====== END Task 1 ======
500
501
502}
503
504
C5
double C5
Definition mbs_sensor_ArmarIII_simplified_virt.h:5
ROcp0_318
double ROcp0_318
Definition mbs_sensor_ArmarIII_simplified_virt.h:147
C14
double C14
Definition mbs_sensor_ArmarIII_simplified_virt.h:19
S17
double S17
Definition mbs_sensor_ArmarIII_simplified_virt.h:24
ORcp0_38
double ORcp0_38
Definition mbs_sensor_ArmarIII_simplified_virt.h:290
ROcp0_99
double ROcp0_99
Definition mbs_sensor_ArmarIII_simplified_virt.h:108
ROcp0_518
double ROcp0_518
Definition mbs_sensor_ArmarIII_simplified_virt.h:149
ROcp0_35
double ROcp0_35
Definition mbs_sensor_ArmarIII_simplified_virt.h:88
S18
double S18
Definition mbs_sensor_ArmarIII_simplified_virt.h:26
ROcp0_910
double ROcp0_910
Definition mbs_sensor_ArmarIII_simplified_virt.h:114
ORcp0_112
double ORcp0_112
Definition mbs_sensor_ArmarIII_simplified_virt.h:294
OMcp0_35
double OMcp0_35
Definition mbs_sensor_ArmarIII_simplified_virt.h:278
ROcp0_69
double ROcp0_69
Definition mbs_sensor_ArmarIII_simplified_virt.h:105
S7
double S7
Definition mbs_sensor_ArmarIII_simplified_virt.h:10
C18
double C18
Definition mbs_sensor_ArmarIII_simplified_virt.h:25
ROcp0_813
double ROcp0_813
Definition mbs_sensor_ArmarIII_simplified_virt.h:125
RLcp0_38
double RLcp0_38
Definition mbs_sensor_ArmarIII_simplified_virt.h:287
ROcp0_85
double ROcp0_85
Definition mbs_sensor_ArmarIII_simplified_virt.h:89
OPcp0_118
double OPcp0_118
Definition mbs_sensor_ArmarIII_simplified_virt.h:306
RLcp0_112
double RLcp0_112
Definition mbs_sensor_ArmarIII_simplified_virt.h:291
ROcp0_814
double ROcp0_814
Definition mbs_sensor_ArmarIII_simplified_virt.h:131
ROcp0_215
double ROcp0_215
Definition mbs_sensor_ArmarIII_simplified_virt.h:134
ROcp0_25
double ROcp0_25
Definition mbs_sensor_ArmarIII_simplified_virt.h:87
RLcp0_28
double RLcp0_28
Definition mbs_sensor_ArmarIII_simplified_virt.h:286
OPcp0_218
double OPcp0_218
Definition mbs_sensor_ArmarIII_simplified_virt.h:307
ROcp0_710
double ROcp0_710
Definition mbs_sensor_ArmarIII_simplified_virt.h:112
RLcp0_312
double RLcp0_312
Definition mbs_sensor_ArmarIII_simplified_virt.h:293
ROcp0_57
double ROcp0_57
Definition mbs_sensor_ArmarIII_simplified_virt.h:98
ROcp0_415
double ROcp0_415
Definition mbs_sensor_ArmarIII_simplified_virt.h:136
ROcp0_89
double ROcp0_89
Definition mbs_sensor_ArmarIII_simplified_virt.h:107
ROcp0_714
double ROcp0_714
Definition mbs_sensor_ArmarIII_simplified_virt.h:130
ROcp0_417
double ROcp0_417
Definition mbs_sensor_ArmarIII_simplified_virt.h:142
ROcp0_311
double ROcp0_311
Definition mbs_sensor_ArmarIII_simplified_virt.h:117
ROcp0_218
double ROcp0_218
Definition mbs_sensor_ArmarIII_simplified_virt.h:146
C4
double C4
Definition mbs_sensor_ArmarIII_simplified_virt.h:3
ROcp0_111
double ROcp0_111
Definition mbs_sensor_ArmarIII_simplified_virt.h:115
ORcp0_18
double ORcp0_18
Definition mbs_sensor_ArmarIII_simplified_virt.h:288
OMcp0_25
double OMcp0_25
Definition mbs_sensor_ArmarIII_simplified_virt.h:277
ORcp0_212
double ORcp0_212
Definition mbs_sensor_ArmarIII_simplified_virt.h:295
ROcp0_315
double ROcp0_315
Definition mbs_sensor_ArmarIII_simplified_virt.h:135
OMcp0_26
double OMcp0_26
Definition mbs_sensor_ArmarIII_simplified_virt.h:280
OMcp0_118
double OMcp0_118
Definition mbs_sensor_ArmarIII_simplified_virt.h:303
OMcp0_218
double OMcp0_218
Definition mbs_sensor_ArmarIII_simplified_virt.h:304
ROcp0_515
double ROcp0_515
Definition mbs_sensor_ArmarIII_simplified_virt.h:137
C7
double C7
Definition mbs_sensor_ArmarIII_simplified_virt.h:9
ROcp0_115
double ROcp0_115
Definition mbs_sensor_ArmarIII_simplified_virt.h:133
ROcp0_517
double ROcp0_517
Definition mbs_sensor_ArmarIII_simplified_virt.h:143
ROcp0_95
double ROcp0_95
Definition mbs_sensor_ArmarIII_simplified_virt.h:90
S5
double S5
Definition mbs_sensor_ArmarIII_simplified_virt.h:6
ROcp0_79
double ROcp0_79
Definition mbs_sensor_ArmarIII_simplified_virt.h:106
RLcp0_116
double RLcp0_116
Definition mbs_sensor_ArmarIII_simplified_virt.h:297
ROcp0_59
double ROcp0_59
Definition mbs_sensor_ArmarIII_simplified_virt.h:104
ROcp0_511
double ROcp0_511
Definition mbs_sensor_ArmarIII_simplified_virt.h:119
ROcp0_310
double ROcp0_310
Definition mbs_sensor_ArmarIII_simplified_virt.h:111
ROcp0_617
double ROcp0_617
Definition mbs_sensor_ArmarIII_simplified_virt.h:144
ROcp0_49
double ROcp0_49
Definition mbs_sensor_ArmarIII_simplified_virt.h:103
ROcp0_47
double ROcp0_47
Definition mbs_sensor_ArmarIII_simplified_virt.h:97
ROcp0_67
double ROcp0_67
Definition mbs_sensor_ArmarIII_simplified_virt.h:99
S10
double S10
Definition mbs_sensor_ArmarIII_simplified_virt.h:14
RLcp0_18
double RLcp0_18
Definition mbs_sensor_ArmarIII_simplified_virt.h:285
ROcp0_210
double ROcp0_210
Definition mbs_sensor_ArmarIII_simplified_virt.h:110
C15
double C15
Definition mbs_sensor_ArmarIII_simplified_virt.h:21
C10
double C10
Definition mbs_sensor_ArmarIII_simplified_virt.h:13
ORcp0_116
double ORcp0_116
Definition mbs_sensor_ArmarIII_simplified_virt.h:300
OMcp0_36
double OMcp0_36
Definition mbs_sensor_ArmarIII_simplified_virt.h:281
S15
double S15
Definition mbs_sensor_ArmarIII_simplified_virt.h:22
ROcp0_26
double ROcp0_26
Definition mbs_sensor_ArmarIII_simplified_virt.h:92
RLcp0_316
double RLcp0_316
Definition mbs_sensor_ArmarIII_simplified_virt.h:299
S9
double S9
Definition mbs_sensor_ArmarIII_simplified_virt.h:12
C6
double C6
Definition mbs_sensor_ArmarIII_simplified_virt.h:7
ROcp0_411
double ROcp0_411
Definition mbs_sensor_ArmarIII_simplified_virt.h:118
ROcp0_713
double ROcp0_713
Definition mbs_sensor_ArmarIII_simplified_virt.h:124
ROcp0_810
double ROcp0_810
Definition mbs_sensor_ArmarIII_simplified_virt.h:113
OPcp0_318
double OPcp0_318
Definition mbs_sensor_ArmarIII_simplified_virt.h:308
ROcp0_36
double ROcp0_36
Definition mbs_sensor_ArmarIII_simplified_virt.h:93
ROcp0_118
double ROcp0_118
Definition mbs_sensor_ArmarIII_simplified_virt.h:145
ROcp0_914
double ROcp0_914
Definition mbs_sensor_ArmarIII_simplified_virt.h:132
ROcp0_66
double ROcp0_66
Definition mbs_sensor_ArmarIII_simplified_virt.h:96
OMcp0_318
double OMcp0_318
Definition mbs_sensor_ArmarIII_simplified_virt.h:305
OMcp0_16
double OMcp0_16
Definition mbs_sensor_ArmarIII_simplified_virt.h:279
S13
double S13
Definition mbs_sensor_ArmarIII_simplified_virt.h:18
ROcp0_611
double ROcp0_611
Definition mbs_sensor_ArmarIII_simplified_virt.h:120
C13
double C13
Definition mbs_sensor_ArmarIII_simplified_virt.h:17
ROcp0_615
double ROcp0_615
Definition mbs_sensor_ArmarIII_simplified_virt.h:138
ORcp0_216
double ORcp0_216
Definition mbs_sensor_ArmarIII_simplified_virt.h:301
ROcp0_110
double ROcp0_110
Definition mbs_sensor_ArmarIII_simplified_virt.h:109
RLcp0_216
double RLcp0_216
Definition mbs_sensor_ArmarIII_simplified_virt.h:298
ROcp0_418
double ROcp0_418
Definition mbs_sensor_ArmarIII_simplified_virt.h:148
ORcp0_312
double ORcp0_312
Definition mbs_sensor_ArmarIII_simplified_virt.h:296
C17
double C17
Definition mbs_sensor_ArmarIII_simplified_virt.h:23
C9
double C9
Definition mbs_sensor_ArmarIII_simplified_virt.h:11
ROcp0_56
double ROcp0_56
Definition mbs_sensor_ArmarIII_simplified_virt.h:95
ROcp0_913
double ROcp0_913
Definition mbs_sensor_ArmarIII_simplified_virt.h:126
C11
double C11
Definition mbs_sensor_ArmarIII_simplified_virt.h:15
RLcp0_212
double RLcp0_212
Definition mbs_sensor_ArmarIII_simplified_virt.h:292
S6
double S6
Definition mbs_sensor_ArmarIII_simplified_virt.h:8
S11
double S11
Definition mbs_sensor_ArmarIII_simplified_virt.h:16
ORcp0_316
double ORcp0_316
Definition mbs_sensor_ArmarIII_simplified_virt.h:302
S4
double S4
Definition mbs_sensor_ArmarIII_simplified_virt.h:4
ROcp0_211
double ROcp0_211
Definition mbs_sensor_ArmarIII_simplified_virt.h:116
ROcp0_618
double ROcp0_618
Definition mbs_sensor_ArmarIII_simplified_virt.h:150
ORcp0_28
double ORcp0_28
Definition mbs_sensor_ArmarIII_simplified_virt.h:289
S14
double S14
Definition mbs_sensor_ArmarIII_simplified_virt.h:20
ROcp0_612
double ROcp0_612
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:67
ROcp0_812
double ROcp0_812
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:69
RLcp0_114
double RLcp0_114
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:154
ROcp0_213
double ROcp0_213
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:72
S8
double S8
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:10
ROcp0_412
double ROcp0_412
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:65
OMcp0_38
double OMcp0_38
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:99
OMcp0_19
double OMcp0_19
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:106
ORcp0_214
double ORcp0_214
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:190
RLcp0_214
double RLcp0_214
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:155
OPcp0_213
double OPcp0_213
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:152
OMcp0_27
double OMcp0_27
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:95
ORcp0_29
double ORcp0_29
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:110
OMcp0_39
double OMcp0_39
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:108
OMcp0_310
double OMcp0_310
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:120
ORcp0_311
double ORcp0_311
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:135
OPcp0_113
double OPcp0_113
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:151
OMcp0_17
double OMcp0_17
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:94
OPcp0_313
double OPcp0_313
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:153
ROcp0_512
double ROcp0_512
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:66
OMcp0_210
double OMcp0_210
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:119
OPcp0_18
double OPcp0_18
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:100
RLcp0_39
double RLcp0_39
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:105
C12
double C12
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:17
ROcp0_712
double ROcp0_712
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:68
OMcp0_18
double OMcp0_18
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:97
OMcp0_29
double OMcp0_29
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:107
OMcp0_213
double OMcp0_213
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:149
ORcp0_314
double ORcp0_314
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:191
OMcp0_312
double OMcp0_312
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:144
OMcp0_313
double OMcp0_313
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:150
OMcp0_215
double OMcp0_215
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:199
OMcp0_111
double OMcp0_111
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:130
OPcp0_311
double OPcp0_311
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:138
OMcp0_37
double OMcp0_37
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:96
ORcp0_114
double ORcp0_114
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:189
ORcp0_111
double ORcp0_111
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:133
OPcp0_310
double OPcp0_310
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:126
ROcp0_113
double ROcp0_113
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:71
ROcp0_88
double ROcp0_88
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:45
OMcp0_315
double OMcp0_315
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:200
ORcp0_19
double ORcp0_19
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:109
OPcp0_110
double OPcp0_110
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:124
OPcp0_111
double OPcp0_111
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:136
OMcp0_115
double OMcp0_115
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:198
RLcp0_111
double RLcp0_111
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:127
ROcp0_98
double ROcp0_98
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:46
ORcp0_39
double ORcp0_39
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:111
RLcp0_311
double RLcp0_311
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:129
OMcp0_28
double OMcp0_28
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:98
C8
double C8
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:9
RLcp0_314
double RLcp0_314
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:156
ROcp0_78
double ROcp0_78
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:44
OPcp0_38
double OPcp0_38
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:102
OMcp0_112
double OMcp0_112
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:142
OMcp0_113
double OMcp0_113
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:148
OMcp0_211
double OMcp0_211
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:131
OMcp0_212
double OMcp0_212
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:143
RLcp0_211
double RLcp0_211
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:128
RLcp0_19
double RLcp0_19
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:103
RLcp0_29
double RLcp0_29
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:104
ROcp0_313
double ROcp0_313
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:73
ROcp0_912
double ROcp0_912
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:70
S12
double S12
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:18
ORcp0_211
double ORcp0_211
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:134
OPcp0_210
double OPcp0_210
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:125
OMcp0_311
double OMcp0_311
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:132
OPcp0_211
double OPcp0_211
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:137
OPcp0_28
double OPcp0_28
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:101
OMcp0_110
double OMcp0_110
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:118
q
#define q
virt
void mbs_sensor_ArmarIV W Torso virt(MbsSensor *sens, MbsData *s, int isens)
Definition mbs_sensor_ArmarIV-W-Torso-virt.c:36
qdd
#define qdd
qd
#define qd
mbs_sensor_ArmarIV-W-Torso-virt.h
ROcp0_614
double ROcp0_614
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:83
ACcp0_316
double ACcp0_316
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:243
OPcp0_17
double OPcp0_17
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:113
JTcp0_316_12
double JTcp0_316_12
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:225
S6p7
double S6p7
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:41
ROcp0_17
double ROcp0_17
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:38
OMcp0_317
double OMcp0_317
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:246
JTcp0_216_11
double JTcp0_216_11
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:221
JTcp0_316_9
double JTcp0_316_9
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:216
VIcp0_316
double VIcp0_316
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:240
RLcp0_213
double RLcp0_213
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:168
ROcp0_37
double ROcp0_37
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:40
JTcp0_216_13
double JTcp0_216_13
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:227
JTcp0_116_9
double JTcp0_116_9
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:214
OMcp0_117
double OMcp0_117
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:244
ACcp0_216
double ACcp0_216
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:242
OPcp0_315
double OPcp0_315
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:193
OMcp0_217
double OMcp0_217
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:245
JTcp0_216_15
double JTcp0_216_15
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:233
ROcp0_27
double ROcp0_27
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:39
JTcp0_116_7
double JTcp0_116_7
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:208
JTcp0_116_13
double JTcp0_116_13
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:226
JTcp0_216_12
double JTcp0_216_12
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:224
OPcp0_37
double OPcp0_37
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:115
JTcp0_216_10
double JTcp0_216_10
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:218
JTcp0_116_11
double JTcp0_116_11
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:220
POcp0_216
double POcp0_216
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:198
OPcp0_112
double OPcp0_112
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:164
JTcp0_216_14
double JTcp0_216_14
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:230
OMcp0_314
double OMcp0_314
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:184
JTcp0_116_14
double JTcp0_116_14
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:229
C6p7
double C6p7
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:37
ORcp0_213
double ORcp0_213
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:174
ROcp0_414
double ROcp0_414
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:81
VIcp0_216
double VIcp0_216
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:239
JTcp0_316_10
double JTcp0_316_10
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:219
ROcp0_717
double ROcp0_717
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:96
JTcp0_116_10
double JTcp0_116_10
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:217
ROcp0_817
double ROcp0_817
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:97
JTcp0_116_12
double JTcp0_116_12
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:223
ACcp0_116
double ACcp0_116
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:241
JTcp0_316_11
double JTcp0_316_11
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:222
OPcp0_212
double OPcp0_212
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:165
ORcp0_313
double ORcp0_313
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:175
ORcp0_113
double ORcp0_113
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:173
JTcp0_316_7
double JTcp0_316_7
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:210
OMcp0_214
double OMcp0_214
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:183
JTcp0_316_5
double JTcp0_316_5
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:204
JTcp0_216_7
double JTcp0_216_7
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:209
JTcp0_116_6
double JTcp0_116_6
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:205
JTcp0_316_15
double JTcp0_316_15
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:234
OPcp0_215
double OPcp0_215
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:192
OPcp0_312
double OPcp0_312
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:166
JTcp0_116_8
double JTcp0_116_8
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:211
OPcp0_27
double OPcp0_27
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:114
RLcp0_313
double RLcp0_313
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:169
JTcp0_216_6
double JTcp0_216_6
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:206
JTcp0_316_14
double JTcp0_316_14
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:231
VIcp0_116
double VIcp0_116
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:238
ROcp0_58
double ROcp0_58
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:46
POcp0_116
double POcp0_116
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:197
JTcp0_316_13
double JTcp0_316_13
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:228
JTcp0_316_8
double JTcp0_316_8
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:213
ROcp0_917
double ROcp0_917
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:98
JTcp0_216_8
double JTcp0_216_8
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:212
JTcp0_116_15
double JTcp0_116_15
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:232
JTcp0_116_5
double JTcp0_116_5
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:202
RLcp0_113
double RLcp0_113
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:167
JTcp0_216_5
double JTcp0_216_5
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:203
JTcp0_216_4
double JTcp0_216_4
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:200
ROcp0_68
double ROcp0_68
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:47
JTcp0_316_6
double JTcp0_316_6
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:207
OMcp0_114
double OMcp0_114
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:182
ROcp0_514
double ROcp0_514
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:82
JTcp0_216_9
double JTcp0_216_9
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:215
ROcp0_48
double ROcp0_48
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:45
OPcp0_115
double OPcp0_115
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:191
JTcp0_316_4
double JTcp0_316_4
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:201
POcp0_316
double POcp0_316
Definition mbs_sensor_ArmarIV-W-Torso-virt.h:199
MbsSensor::OM
double OM[4]
Angular velocity vector of the sensor expressed in the inertial frame: .
Definition mbs_sensor2.h:28
MbsSensor::V
double V[4]
Velocity vector of the sensor expressed in the inertial frame: .
Definition mbs_sensor2.h:26
MbsSensor::OMP
double OMP[4]
Angular acceleration vector of the sensor expressed in the inertial frame: .
Definition mbs_sensor2.h:32
MbsSensor::P
double P[4]
Position vector of the sensor expressed in the inertial frame: .
Definition mbs_sensor2.h:21
MbsSensor::A
double A[4]
Acceleration vector of the sensor expressed in the inertial frame: .
Definition mbs_sensor2.h:30
MbsSensor::J
double * J[7]
Jacobian matrix of the sensor: .
Definition mbs_sensor2.h:37
MbsSensor::R
double R[4][4]
Rotation matrix from the inertial frame to the sensor frame: .
Definition mbs_sensor2.h:24