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
37void 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
mbs_sensor2.h
C5
double C5
Definition mbs_sensor_ArmarIII_simplified_virt.h:5
ROcp0_35
double ROcp0_35
Definition mbs_sensor_ArmarIII_simplified_virt.h:88
ROcp0_910
double ROcp0_910
Definition mbs_sensor_ArmarIII_simplified_virt.h:114
ROcp0_16
double ROcp0_16
Definition mbs_sensor_ArmarIII_simplified_virt.h:91
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
ROcp0_85
double ROcp0_85
Definition mbs_sensor_ArmarIII_simplified_virt.h:89
RLcp0_112
double RLcp0_112
Definition mbs_sensor_ArmarIII_simplified_virt.h:291
ROcp0_25
double ROcp0_25
Definition mbs_sensor_ArmarIII_simplified_virt.h:87
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_46
double ROcp0_46
Definition mbs_sensor_ArmarIII_simplified_virt.h:94
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_311
double ROcp0_311
Definition mbs_sensor_ArmarIII_simplified_virt.h:117
ROcp0_513
double ROcp0_513
Definition mbs_sensor_ArmarIII_simplified_virt.h:122
C4
double C4
Definition mbs_sensor_ArmarIII_simplified_virt.h:3
ROcp0_111
double ROcp0_111
Definition mbs_sensor_ArmarIII_simplified_virt.h:115
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
OMcp0_26
double OMcp0_26
Definition mbs_sensor_ArmarIII_simplified_virt.h:280
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_95
double ROcp0_95
Definition mbs_sensor_ArmarIII_simplified_virt.h:90
S5
double S5
Definition mbs_sensor_ArmarIII_simplified_virt.h:6
ROcp0_59
double ROcp0_59
Definition mbs_sensor_ArmarIII_simplified_virt.h:104
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
C15
double C15
Definition mbs_sensor_ArmarIII_simplified_virt.h:21
C10
double C10
Definition mbs_sensor_ArmarIII_simplified_virt.h:13
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
S9
double S9
Definition mbs_sensor_ArmarIII_simplified_virt.h:12
C6
double C6
Definition mbs_sensor_ArmarIII_simplified_virt.h:7
ROcp0_810
double ROcp0_810
Definition mbs_sensor_ArmarIII_simplified_virt.h:113
ROcp0_36
double ROcp0_36
Definition mbs_sensor_ArmarIII_simplified_virt.h:93
ROcp0_66
double ROcp0_66
Definition mbs_sensor_ArmarIII_simplified_virt.h:96
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_77
double ROcp0_77
Definition mbs_sensor_ArmarIII_simplified_virt.h:100
ROcp0_87
double ROcp0_87
Definition mbs_sensor_ArmarIII_simplified_virt.h:101
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_312
double ORcp0_312
Definition mbs_sensor_ArmarIII_simplified_virt.h:296
C9
double C9
Definition mbs_sensor_ArmarIII_simplified_virt.h:11
ROcp0_56
double ROcp0_56
Definition mbs_sensor_ArmarIII_simplified_virt.h:95
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
ROcp0_413
double ROcp0_413
Definition mbs_sensor_ArmarIII_simplified_virt.h:121
S4
double S4
Definition mbs_sensor_ArmarIII_simplified_virt.h:4
ROcp0_97
double ROcp0_97
Definition mbs_sensor_ArmarIII_simplified_virt.h:102
ROcp0_211
double ROcp0_211
Definition mbs_sensor_ArmarIII_simplified_virt.h:116
ROcp0_613
double ROcp0_613
Definition mbs_sensor_ArmarIII_simplified_virt.h:123
mbs_sensor_ArmarIV-GazeStab-virt.h
JTcp0_314_5
double JTcp0_314_5
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:164
ROcp0_28
double ROcp0_28
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:42
OMcp0_216
double OMcp0_216
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:202
ROcp0_612
double ROcp0_612
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:67
POcp0_314
double POcp0_314
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:159
ROcp0_812
double ROcp0_812
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:69
OPcp0_19
double OPcp0_19
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:112
JTcp0_114_7
double JTcp0_114_7
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:168
RLcp0_114
double RLcp0_114
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:154
JTcp0_214_4
double JTcp0_214_4
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:160
ROcp0_213
double ROcp0_213
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:72
JTcp0_214_6
double JTcp0_214_6
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:166
JTcp0_114_10
double JTcp0_114_10
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:177
JTcp0_214_8
double JTcp0_214_8
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:172
ROcp0_38
double ROcp0_38
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:43
ORcp0_310
double ORcp0_310
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:123
S8
double S8
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:10
ACcp0_114
double ACcp0_114
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:195
S16
double S16
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:24
ROcp0_412
double ROcp0_412
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:65
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_114_11
double JTcp0_114_11
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:180
ACcp0_314
double ACcp0_314
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:197
ROcp0_715
double ROcp0_715
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:80
POcp0_214
double POcp0_214
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:158
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
VIcp0_114
double VIcp0_114
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:192
ROcp0_416
double ROcp0_416
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:86
OPcp0_216
double OPcp0_216
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:205
RLcp0_214
double RLcp0_214
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:155
ROcp0_29
double ROcp0_29
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:48
OPcp0_213
double OPcp0_213
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:152
JTcp0_214_10
double JTcp0_214_10
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:178
JTcp0_314_11
double JTcp0_314_11
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:182
OMcp0_27
double OMcp0_27
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:95
JTcp0_214_5
double JTcp0_214_5
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:163
ROcp0_911
double ROcp0_911
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:64
ORcp0_29
double ORcp0_29
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:110
ROcp0_116
double ROcp0_116
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:83
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_110
double ORcp0_110
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:121
JTcp0_314_6
double JTcp0_314_6
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:167
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
JTcp0_114_9
double JTcp0_114_9
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:174
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
ROcp0_516
double ROcp0_516
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:87
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
JTcp0_114_8
double JTcp0_114_8
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:171
JTcp0_214_12
double JTcp0_214_12
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:184
ROcp0_711
double ROcp0_711
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:62
ROcp0_712
double ROcp0_712
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:68
JTcp0_114_12
double JTcp0_114_12
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:183
VIcp0_214
double VIcp0_214
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:193
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
ROcp0_815
double ROcp0_815
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:81
ROcp0_915
double ROcp0_915
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:82
JTcp0_214_11
double JTcp0_214_11
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:181
ROcp0_216
double ROcp0_216
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:84
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_316
double OMcp0_316
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:203
ROcp0_39
double ROcp0_39
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:49
ROcp0_410
double ROcp0_410
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:53
OPcp0_116
double OPcp0_116
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:204
JTcp0_214_9
double JTcp0_214_9
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:175
OMcp0_215
double OMcp0_215
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:199
ACcp0_214
double ACcp0_214
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:196
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
ROcp0_18
double ROcp0_18
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:41
RLcp0_111
double RLcp0_111
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:127
OPcp0_29
double OPcp0_29
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:113
POcp0_114
double POcp0_114
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:157
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
ROcp0_19
double ROcp0_19
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:47
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
OPcp0_316
double OPcp0_316
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:206
C8
double C8
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:9
JTcp0_314_13
double JTcp0_314_13
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:188
OPcp0_39
double OPcp0_39
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:114
ROcp0_316
double ROcp0_316
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:85
RLcp0_314
double RLcp0_314
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:156
JTcp0_314_7
double JTcp0_314_7
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:170
JTcp0_314_9
double JTcp0_314_9
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:176
ROcp0_78
double ROcp0_78
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:44
JTcp0_214_13
double JTcp0_214_13
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:187
C16
double C16
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:23
OMcp0_116
double OMcp0_116
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:201
VIcp0_314
double VIcp0_314
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:194
ROcp0_510
double ROcp0_510
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:54
OPcp0_38
double OPcp0_38
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:102
JTcp0_214_7
double JTcp0_214_7
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:169
OMcp0_112
double OMcp0_112
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:142
JTcp0_114_6
double JTcp0_114_6
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:165
OMcp0_113
double OMcp0_113
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:148
JTcp0_314_12
double JTcp0_314_12
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:185
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
JTcp0_114_13
double JTcp0_114_13
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:186
JTcp0_114_5
double JTcp0_114_5
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:162
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
RLcp0_210
double RLcp0_210
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:116
ROcp0_313
double ROcp0_313
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:73
ROcp0_811
double ROcp0_811
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:63
ROcp0_912
double ROcp0_912
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:70
JTcp0_314_4
double JTcp0_314_4
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:161
ROcp0_616
double ROcp0_616
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:88
JTcp0_314_10
double JTcp0_314_10
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:179
S12
double S12
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:18
RLcp0_110
double RLcp0_110
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:115
ORcp0_211
double ORcp0_211
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:134
ROcp0_610
double ROcp0_610
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:55
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
RLcp0_310
double RLcp0_310
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:117
OPcp0_28
double OPcp0_28
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:101
JTcp0_314_8
double JTcp0_314_8
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:173
OMcp0_110
double OMcp0_110
Definition mbs_sensor_ArmarIV-GazeStab-virt.h:118
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
q
#define q
qdd
#define qdd
qd
#define qd
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
user_hard_param_WO_torso.h
DPT_1_4
#define DPT_1_4
Definition user_hard_param_WO_torso.h:75
DPT_2_3
#define DPT_2_3
Definition user_hard_param_WO_torso.h:74
DPT_2_4
#define DPT_2_4
Definition user_hard_param_WO_torso.h:76
DPT_3_1
#define DPT_3_1
Definition user_hard_param_W_torso.h:85
DPT_3_2
#define DPT_3_2
Definition user_hard_param_W_torso.h:86
DPT_3_4
#define DPT_3_4
Definition user_hard_param_W_torso.h:88