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
RobotComponents
components
Feedforward
symbolic_routines
mbs_sensor_ArmarIV-GazeStab-virt_new.c
Generated on Sat Sep 28 2024 09:14:10 for armarx_documentation by
1.8.17