Human36Fixture.h
Go to the documentation of this file.
1 #ifndef __RDL_HUMAN36_FIXTURE_H__
2 #define __RDL_HUMAN36_FIXTURE_H__
3 
4 #include "UnitTestUtils.hpp"
5 #include "rdl_dynamics/Model.h"
6 #include "rdl_dynamics/Contacts.h"
7 #include <gtest/gtest.h>
8 
9 struct Human36 : public testing::Test
10 {
11  RobotDynamics::Model* model;
12  RobotDynamics::Model* model_emulated;
13  RobotDynamics::Model* model_3dof;
14 
15  RobotDynamics::Math::VectorNd q;
16  RobotDynamics::Math::VectorNd qdot;
17  RobotDynamics::Math::VectorNd qddot;
18  RobotDynamics::Math::VectorNd tau;
19 
20  RobotDynamics::Math::VectorNd qddot_emulated;
21  RobotDynamics::Math::VectorNd qddot_3dof;
22 
23  RobotDynamics::ConstraintSet constraints_1B1C_emulated;
24  RobotDynamics::ConstraintSet constraints_1B4C_emulated;
25  RobotDynamics::ConstraintSet constraints_4B4C_emulated;
26 
27  RobotDynamics::ConstraintSet constraints_1B1C_3dof;
28  RobotDynamics::ConstraintSet constraints_1B4C_3dof;
29  RobotDynamics::ConstraintSet constraints_4B4C_3dof;
30 
31  enum SegmentName
32  {
33  SegmentPelvis = 0,
34  SegmentThigh,
35  SegmentShank,
36  SegmentFoot,
37  SegmentMiddleTrunk,
38  SegmentUpperTrunk,
39  SegmentUpperArm,
40  SegmentLowerArm,
41  SegmentHand,
42  SegmentHead,
43  SegmentNameLast
44  };
45 
46  enum BodyName
47  {
48  BodyPelvis,
49  BodyThighRight,
50  BodyShankRight,
51  BodyFootRight,
52  BodyThighLeft,
53  BodyShankLeft,
54  BodyFootLeft,
55  BodyMiddleTrunk,
56  BodyUpperTrunk,
57  BodyUpperArmRight,
58  BodyLowerArmRight,
59  BodyHandRight,
60  BodyUpperArmLeft,
61  BodyLowerArmLeft,
62  BodyHandLeft,
63  BodyHead,
64  BodyNameLast
65  };
66 
67  double SegmentLengths[SegmentNameLast];
68  double SegmentMass[SegmentNameLast];
69  double SegmentCOM[SegmentNameLast][3];
70  double SegmentRadiiOfGyration[SegmentNameLast][3];
71 
72  unsigned int body_id_emulated[BodyNameLast];
73  unsigned int body_id_3dof[BodyNameLast];
74 
75  void initParameters()
76  {
77  SegmentLengths[SegmentPelvis] = 0.1457;
78  SegmentLengths[SegmentThigh] = 0.4222;
79  SegmentLengths[SegmentShank] = 0.4403;
80  SegmentLengths[SegmentFoot] = 0.1037;
81  SegmentLengths[SegmentMiddleTrunk] = 0.2155;
82  SegmentLengths[SegmentUpperTrunk] = 0.2421;
83  SegmentLengths[SegmentUpperArm] = 0.2817;
84  SegmentLengths[SegmentLowerArm] = 0.2689;
85  SegmentLengths[SegmentHand] = 0.0862;
86  SegmentLengths[SegmentHead] = 0.2429;
87 
88  SegmentMass[SegmentPelvis] = 0.8154;
89  SegmentMass[SegmentThigh] = 10.3368;
90  SegmentMass[SegmentShank] = 3.1609;
91  SegmentMass[SegmentFoot] = 1.001;
92  SegmentMass[SegmentMiddleTrunk] = 16.33;
93  SegmentMass[SegmentUpperTrunk] = 15.96;
94  SegmentMass[SegmentUpperArm] = 1.9783;
95  SegmentMass[SegmentLowerArm] = 1.1826;
96  SegmentMass[SegmentHand] = 0.4453;
97  SegmentMass[SegmentHead] = 5.0662;
98 
99  SegmentCOM[SegmentPelvis][0] = 0.;
100  SegmentCOM[SegmentPelvis][1] = 0.;
101  SegmentCOM[SegmentPelvis][2] = 0.0891;
102 
103  SegmentCOM[SegmentThigh][0] = 0.;
104  SegmentCOM[SegmentThigh][1] = 0.;
105  SegmentCOM[SegmentThigh][2] = -0.1729;
106 
107  SegmentCOM[SegmentShank][0] = 0.;
108  SegmentCOM[SegmentShank][1] = 0.;
109  SegmentCOM[SegmentShank][2] = -0.1963;
110 
111  SegmentCOM[SegmentFoot][0] = 0.1254;
112  SegmentCOM[SegmentFoot][1] = 0.;
113  SegmentCOM[SegmentFoot][2] = -0.0516;
114 
115  SegmentCOM[SegmentMiddleTrunk][0] = 0.;
116  SegmentCOM[SegmentMiddleTrunk][1] = 0.;
117  SegmentCOM[SegmentMiddleTrunk][2] = 0.1185;
118 
119  SegmentCOM[SegmentUpperTrunk][0] = 0.;
120  SegmentCOM[SegmentUpperTrunk][1] = 0.;
121  SegmentCOM[SegmentUpperTrunk][2] = 0.1195;
122 
123  SegmentCOM[SegmentUpperArm][0] = 0.;
124  SegmentCOM[SegmentUpperArm][1] = 0.;
125  SegmentCOM[SegmentUpperArm][2] = -0.1626;
126 
127  SegmentCOM[SegmentLowerArm][0] = 0.;
128  SegmentCOM[SegmentLowerArm][1] = 0.;
129  SegmentCOM[SegmentLowerArm][2] = -0.1230;
130 
131  SegmentCOM[SegmentHand][0] = 0.;
132  SegmentCOM[SegmentHand][1] = 0.;
133  SegmentCOM[SegmentHand][2] = -0.0680;
134 
135  SegmentCOM[SegmentHead][0] = 0.;
136  SegmentCOM[SegmentHead][1] = 0.;
137  SegmentCOM[SegmentHead][2] = 1.1214;
138 
139  SegmentRadiiOfGyration[SegmentPelvis][0] = 0.0897;
140  SegmentRadiiOfGyration[SegmentPelvis][1] = 0.0855;
141  SegmentRadiiOfGyration[SegmentPelvis][2] = 0.0803;
142 
143  SegmentRadiiOfGyration[SegmentThigh][0] = 0.1389;
144  SegmentRadiiOfGyration[SegmentThigh][1] = 0.0629;
145  SegmentRadiiOfGyration[SegmentThigh][2] = 0.1389;
146 
147  SegmentRadiiOfGyration[SegmentShank][0] = 0.1123;
148  SegmentRadiiOfGyration[SegmentShank][1] = 0.0454;
149  SegmentRadiiOfGyration[SegmentShank][2] = 0.1096;
150 
151  SegmentRadiiOfGyration[SegmentFoot][0] = 0.0267;
152  SegmentRadiiOfGyration[SegmentFoot][1] = 0.0129;
153  SegmentRadiiOfGyration[SegmentFoot][2] = 0.0254;
154 
155  SegmentRadiiOfGyration[SegmentMiddleTrunk][0] = 0.0970;
156  SegmentRadiiOfGyration[SegmentMiddleTrunk][1] = 0.1009;
157  SegmentRadiiOfGyration[SegmentMiddleTrunk][2] = 0.0825;
158 
159  SegmentRadiiOfGyration[SegmentUpperTrunk][0] = 0.1273;
160  SegmentRadiiOfGyration[SegmentUpperTrunk][1] = 0.1172;
161  SegmentRadiiOfGyration[SegmentUpperTrunk][2] = 0.0807;
162 
163  SegmentRadiiOfGyration[SegmentUpperArm][0] = 0.0803;
164  SegmentRadiiOfGyration[SegmentUpperArm][1] = 0.0758;
165  SegmentRadiiOfGyration[SegmentUpperArm][2] = 0.0445;
166 
167  SegmentRadiiOfGyration[SegmentLowerArm][0] = 0.0742;
168  SegmentRadiiOfGyration[SegmentLowerArm][1] = 0.0713;
169  SegmentRadiiOfGyration[SegmentLowerArm][2] = 0.0325;
170 
171  SegmentRadiiOfGyration[SegmentHand][0] = 0.0541;
172  SegmentRadiiOfGyration[SegmentHand][1] = 0.0442;
173  SegmentRadiiOfGyration[SegmentHand][2] = 0.0346;
174 
175  SegmentRadiiOfGyration[SegmentHead][0] = 0.0736;
176  SegmentRadiiOfGyration[SegmentHead][1] = 0.0634;
177  SegmentRadiiOfGyration[SegmentHead][2] = 0.0765;
178  };
179 
180  RobotDynamics::Body create_body(SegmentName segment)
181  {
182  using namespace RobotDynamics;
183  using namespace RobotDynamics::Math;
184 
185  Matrix3d inertia_C(Matrix3d::Zero());
186  inertia_C(0, 0) = pow(SegmentRadiiOfGyration[segment][0] * SegmentLengths[segment], 2) * SegmentMass[segment];
187  inertia_C(1, 1) = pow(SegmentRadiiOfGyration[segment][1] * SegmentLengths[segment], 2) * SegmentMass[segment];
188  inertia_C(2, 2) = pow(SegmentRadiiOfGyration[segment][2] * SegmentLengths[segment], 2) * SegmentMass[segment];
189 
190  return RobotDynamics::Body(SegmentMass[segment], RobotDynamics::Math::Vector3d(SegmentCOM[segment][0], SegmentCOM[segment][1], SegmentCOM[segment][2]),
191  inertia_C);
192  }
193 
194  void generate()
195  {
196  using namespace RobotDynamics;
197  using namespace RobotDynamics::Math;
198 
199  Body pelvis_body = create_body(SegmentPelvis);
200  Body thigh_body = create_body(SegmentThigh);
201  Body shank_body = create_body(SegmentShank);
202  Body foot_body = create_body(SegmentFoot);
203  Body middle_trunk_body = create_body(SegmentMiddleTrunk);
204  Body upper_trunk_body = create_body(SegmentUpperTrunk);
205  Body upperarm_body = create_body(SegmentUpperArm);
206  Body lowerarm_body = create_body(SegmentLowerArm);
207  Body hand_body = create_body(SegmentHand);
208  Body head_body = create_body(SegmentHead);
209 
210  Matrix3d zero_matrix(Matrix3d::Zero(3, 3));
211  Body null_body(0., Vector3d(0., 0., 0.), zero_matrix);
212  null_body.mIsVirtual = true;
213 
214  Joint free_flyer(SpatialVector(0., 0., 0., 1., 0., 0.), SpatialVector(0., 0., 0., 0., 1., 0.), SpatialVector(0., 0., 0., 0., 0., 1.),
215  SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.), SpatialVector(0., 0., 1., 0., 0., 0.));
216 
217  Joint rot_yxz_emulated(SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.), SpatialVector(0., 0., 1., 0., 0., 0.));
218 
219  Joint trans_xyz = Joint(JointTypeTranslationXYZ);
220 
221  Joint rot_yxz_3dof = Joint(JointTypeEulerYXZ);
222 
223  Joint rot_yz(SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(0., 0., 1., 0., 0., 0.));
224 
225  Joint rot_y(SpatialVector(0., 1., 0., 0., 0., 0.));
226 
227  Joint fixed(JointTypeFixed);
228 
229  // Generate emulated model
230  model_emulated->gravity = MotionVector(0., 0., 0., 0., 0., -9.81);
231 
232  body_id_emulated[BodyPelvis] = model_emulated->addBody(0, Xtrans(Vector3d(0., 0., 0.)), free_flyer, pelvis_body, "pelvis");
233 
234  // right leg
235  body_id_emulated[BodyThighRight] =
236  model_emulated->addBody(body_id_emulated[BodyPelvis], Xtrans(Vector3d(0., -0.0872, 0.)), rot_yxz_emulated, thigh_body, "thigh_r");
237  body_id_emulated[BodyShankRight] = model_emulated->appendBody(Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_r");
238  body_id_emulated[BodyFootRight] = model_emulated->appendBody(Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_r");
239 
240  // left leg
241  body_id_emulated[BodyThighLeft] =
242  model_emulated->addBody(body_id_emulated[BodyPelvis], Xtrans(Vector3d(0., 0.0872, 0.)), rot_yxz_emulated, thigh_body, "thigh_l");
243  body_id_emulated[BodyShankLeft] = model_emulated->appendBody(Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_l");
244  body_id_emulated[BodyFootLeft] = model_emulated->appendBody(Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_l");
245 
246  // trunk
247  body_id_emulated[BodyMiddleTrunk] = model_emulated->addBody(body_id_emulated[BodyPelvis], Xtrans(Vector3d(0., 0., SegmentLengths[SegmentPelvis])),
248  rot_yxz_emulated, middle_trunk_body, "middletrunk");
249  body_id_emulated[BodyUpperTrunk] =
250  model_emulated->appendBody(Xtrans(Vector3d(0., 0., SegmentLengths[SegmentMiddleTrunk])), fixed, upper_trunk_body, "uppertrunk");
251 
252  // right arm
253  body_id_emulated[BodyUpperArmRight] = model_emulated->addBody(body_id_emulated[BodyUpperTrunk], Xtrans(Vector3d(0., -0.1900, SegmentLengths[SegmentUpperTrunk])),
254  rot_yxz_emulated, upperarm_body, "upperarm_r");
255  body_id_emulated[BodyLowerArmRight] = model_emulated->appendBody(Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_r");
256  body_id_emulated[BodyHandRight] = model_emulated->appendBody(Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_r");
257 
258  // left arm
259  body_id_emulated[BodyUpperArmLeft] = model_emulated->addBody(body_id_emulated[BodyUpperTrunk], Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])),
260  rot_yxz_emulated, upperarm_body, "upperarm_l");
261  body_id_emulated[BodyLowerArmLeft] = model_emulated->appendBody(Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_l");
262  body_id_emulated[BodyHandLeft] = model_emulated->appendBody(Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_l");
263 
264  // head
265  body_id_emulated[BodyHead] = model_emulated->addBody(body_id_emulated[BodyUpperTrunk], Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])),
266  rot_yxz_emulated, upperarm_body, "head");
267 
268  // Generate 3dof model
269  model_3dof->gravity = MotionVector(0., 0., 0., 0., 0., -9.81);
270 
271  unsigned int pelvis_trans = model_3dof->addBody(0, Xtrans(Vector3d(0., 0., 0.)), trans_xyz, null_body, "pelvis_trans_xyz");
272 
273  body_id_3dof[BodyPelvis] = model_3dof->addBody(pelvis_trans, Xtrans(Vector3d(0., 0., 0.)), rot_yxz_3dof, pelvis_body, "pelvis");
274  // body_id_3dof[BodyPelvis] = model_3dof->addBody (0, Xtrans (Vector3d (0., 0., 0.)), free_flyer, pelvis_body, "pelvis");
275 
276  // right leg
277  body_id_3dof[BodyThighRight] = model_3dof->addBody(body_id_3dof[BodyPelvis], Xtrans(Vector3d(0., -0.0872, 0.)), rot_yxz_3dof, thigh_body, "thigh_r");
278  body_id_3dof[BodyShankRight] = model_3dof->appendBody(Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_r");
279  body_id_3dof[BodyFootRight] = model_3dof->appendBody(Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_r");
280 
281  // left leg
282  body_id_3dof[BodyThighLeft] = model_3dof->addBody(body_id_3dof[BodyPelvis], Xtrans(Vector3d(0., 0.0872, 0.)), rot_yxz_3dof, thigh_body, "thigh_l");
283  body_id_3dof[BodyShankLeft] = model_3dof->appendBody(Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_l");
284  body_id_3dof[BodyFootLeft] = model_3dof->appendBody(Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_l");
285 
286  // trunk
287  body_id_3dof[BodyMiddleTrunk] =
288  model_3dof->addBody(body_id_3dof[BodyPelvis], Xtrans(Vector3d(0., 0., SegmentLengths[SegmentPelvis])), rot_yxz_3dof, middle_trunk_body, "middletrunk");
289  body_id_3dof[BodyUpperTrunk] = model_3dof->appendBody(Xtrans(Vector3d(0., 0., SegmentLengths[SegmentMiddleTrunk])), fixed, upper_trunk_body, "uppertrunk");
290 
291  // right arm
292  body_id_3dof[BodyUpperArmRight] = model_3dof->addBody(body_id_3dof[BodyUpperTrunk], Xtrans(Vector3d(0., -0.1900, SegmentLengths[SegmentUpperTrunk])),
293  rot_yxz_3dof, upperarm_body, "upperarm_r");
294  body_id_3dof[BodyLowerArmRight] = model_3dof->appendBody(Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_r");
295  body_id_3dof[BodyHandRight] = model_3dof->appendBody(Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_r");
296 
297  // left arm
298  body_id_3dof[BodyUpperArmLeft] =
299  model_3dof->addBody(body_id_3dof[BodyUpperTrunk], Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz_3dof, upperarm_body, "upperarm_l");
300  body_id_3dof[BodyLowerArmLeft] = model_3dof->appendBody(Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_l");
301  body_id_3dof[BodyHandLeft] = model_3dof->appendBody(Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_l");
302 
303  // head
304  body_id_3dof[BodyHead] =
305  model_3dof->addBody(body_id_3dof[BodyUpperTrunk], Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz_3dof, upperarm_body, "head");
306  }
307 
308  void initConstraintSets()
309  {
310  using namespace RobotDynamics;
311  using namespace RobotDynamics::Math;
312 
313  unsigned int foot_r_emulated = model_emulated->GetBodyId("foot_r");
314  unsigned int foot_l_emulated = model_emulated->GetBodyId("foot_l");
315  unsigned int hand_r_emulated = model_emulated->GetBodyId("hand_r");
316  unsigned int hand_l_emulated = model_emulated->GetBodyId("hand_l");
317 
318  constraints_1B1C_emulated.addConstraint(foot_r_emulated, Vector3d(0.1, 0., -0.05), Vector3d(1., 0., 0.));
319  constraints_1B1C_emulated.bind(*model_emulated);
320 
321  constraints_1B4C_emulated.addConstraint(foot_r_emulated, Vector3d(0.1, 0., -0.05), Vector3d(1., 0., 0.));
322  constraints_1B4C_emulated.addConstraint(foot_r_emulated, Vector3d(0.1, 0., -0.05), Vector3d(0., 1., 0.));
323  constraints_1B4C_emulated.addConstraint(foot_r_emulated, Vector3d(0.1, 0., -0.05), Vector3d(0., 0., 1.));
324  constraints_1B4C_emulated.addConstraint(foot_r_emulated, Vector3d(-0.1, 0., -0.05), Vector3d(1., 0., 0.));
325  constraints_1B4C_emulated.bind(*model_emulated);
326 
327  constraints_4B4C_emulated.addConstraint(foot_r_emulated, Vector3d(0.1, 0., -0.05), Vector3d(1., 0., 0.));
328  constraints_4B4C_emulated.addConstraint(foot_r_emulated, Vector3d(0.1, 0., -0.05), Vector3d(0., 1., 0.));
329  constraints_4B4C_emulated.addConstraint(foot_r_emulated, Vector3d(0.1, 0., -0.05), Vector3d(0., 0., 1.));
330  constraints_4B4C_emulated.addConstraint(foot_r_emulated, Vector3d(-0.1, 0., -0.05), Vector3d(1., 0., 0.));
331 
332  constraints_4B4C_emulated.addConstraint(foot_l_emulated, Vector3d(0.1, 0., -0.05), Vector3d(1., 0., 0.));
333  constraints_4B4C_emulated.addConstraint(foot_l_emulated, Vector3d(0.1, 0., -0.05), Vector3d(0., 1., 0.));
334  constraints_4B4C_emulated.addConstraint(foot_l_emulated, Vector3d(0.1, 0., -0.05), Vector3d(0., 0., 1.));
335  constraints_4B4C_emulated.addConstraint(foot_l_emulated, Vector3d(-0.1, 0., -0.05), Vector3d(1., 0., 0.));
336 
337  constraints_4B4C_emulated.addConstraint(hand_r_emulated, Vector3d(0.1, 0., -0.05), Vector3d(1., 0., 0.));
338  constraints_4B4C_emulated.addConstraint(hand_r_emulated, Vector3d(0.1, 0., -0.05), Vector3d(0., 1., 0.));
339  constraints_4B4C_emulated.addConstraint(hand_r_emulated, Vector3d(0.1, 0., -0.05), Vector3d(0., 0., 1.));
340  constraints_4B4C_emulated.addConstraint(hand_r_emulated, Vector3d(-0.1, 0., -0.05), Vector3d(1., 0., 0.));
341 
342  constraints_4B4C_emulated.addConstraint(hand_l_emulated, Vector3d(0.1, 0., -0.05), Vector3d(1., 0., 0.));
343  constraints_4B4C_emulated.addConstraint(hand_l_emulated, Vector3d(0.1, 0., -0.05), Vector3d(0., 1., 0.));
344  constraints_4B4C_emulated.addConstraint(hand_l_emulated, Vector3d(0.1, 0., -0.05), Vector3d(0., 0., 1.));
345  constraints_4B4C_emulated.addConstraint(hand_l_emulated, Vector3d(-0.1, 0., -0.05), Vector3d(1., 0., 0.));
346  constraints_4B4C_emulated.bind(*model);
347 
348  unsigned int foot_r_3dof = model_3dof->GetBodyId("foot_r");
349  unsigned int foot_l_3dof = model_3dof->GetBodyId("foot_l");
350  unsigned int hand_r_3dof = model_3dof->GetBodyId("hand_r");
351  unsigned int hand_l_3dof = model_3dof->GetBodyId("hand_l");
352 
353  constraints_1B1C_3dof.addConstraint(foot_r_3dof, Vector3d(0.1, 0., -0.05), Vector3d(1., 0., 0.));
354  constraints_1B1C_3dof.bind(*model_3dof);
355 
356  constraints_1B4C_3dof.addConstraint(foot_r_3dof, Vector3d(0.1, 0., -0.05), Vector3d(1., 0., 0.));
357  constraints_1B4C_3dof.addConstraint(foot_r_3dof, Vector3d(0.1, 0., -0.05), Vector3d(0., 1., 0.));
358  constraints_1B4C_3dof.addConstraint(foot_r_3dof, Vector3d(0.1, 0., -0.05), Vector3d(0., 0., 1.));
359  constraints_1B4C_3dof.addConstraint(foot_r_3dof, Vector3d(-0.1, 0., -0.05), Vector3d(1., 0., 0.));
360  constraints_1B4C_3dof.bind(*model_3dof);
361 
362  constraints_4B4C_3dof.addConstraint(foot_r_3dof, Vector3d(0.1, 0., -0.05), Vector3d(1., 0., 0.));
363  constraints_4B4C_3dof.addConstraint(foot_r_3dof, Vector3d(0.1, 0., -0.05), Vector3d(0., 1., 0.));
364  constraints_4B4C_3dof.addConstraint(foot_r_3dof, Vector3d(0.1, 0., -0.05), Vector3d(0., 0., 1.));
365  constraints_4B4C_3dof.addConstraint(foot_r_3dof, Vector3d(-0.1, 0., -0.05), Vector3d(1., 0., 0.));
366 
367  constraints_4B4C_3dof.addConstraint(foot_l_3dof, Vector3d(0.1, 0., -0.05), Vector3d(1., 0., 0.));
368  constraints_4B4C_3dof.addConstraint(foot_l_3dof, Vector3d(0.1, 0., -0.05), Vector3d(0., 1., 0.));
369  constraints_4B4C_3dof.addConstraint(foot_l_3dof, Vector3d(0.1, 0., -0.05), Vector3d(0., 0., 1.));
370  constraints_4B4C_3dof.addConstraint(foot_l_3dof, Vector3d(-0.1, 0., -0.05), Vector3d(1., 0., 0.));
371 
372  constraints_4B4C_3dof.addConstraint(hand_r_3dof, Vector3d(0.1, 0., -0.05), Vector3d(1., 0., 0.));
373  constraints_4B4C_3dof.addConstraint(hand_r_3dof, Vector3d(0.1, 0., -0.05), Vector3d(0., 1., 0.));
374  constraints_4B4C_3dof.addConstraint(hand_r_3dof, Vector3d(0.1, 0., -0.05), Vector3d(0., 0., 1.));
375  constraints_4B4C_3dof.addConstraint(hand_r_3dof, Vector3d(-0.1, 0., -0.05), Vector3d(1., 0., 0.));
376 
377  constraints_4B4C_3dof.addConstraint(hand_l_3dof, Vector3d(0.1, 0., -0.05), Vector3d(1., 0., 0.));
378  constraints_4B4C_3dof.addConstraint(hand_l_3dof, Vector3d(0.1, 0., -0.05), Vector3d(0., 1., 0.));
379  constraints_4B4C_3dof.addConstraint(hand_l_3dof, Vector3d(0.1, 0., -0.05), Vector3d(0., 0., 1.));
380  constraints_4B4C_3dof.addConstraint(hand_l_3dof, Vector3d(-0.1, 0., -0.05), Vector3d(1., 0., 0.));
381  constraints_4B4C_3dof.bind(*model_3dof);
382  }
383 
384  void randomizeStates()
385  {
386  for (int i = 0; i < q.size(); i++)
387  {
388  q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
389  qdot[i] = 0.5 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
390  tau[i] = 0.5 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
391  qddot[i] = 0.5 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
392  }
393 
394  qddot_emulated = qddot;
395  qddot_3dof = qddot;
396  }
397 
398  Human36()
399  {
400  srand(time(NULL));
401  }
402 
403  void SetUp()
404  {
405  using namespace RobotDynamics;
406  using namespace RobotDynamics::Math;
407 
408  initParameters();
409  model_emulated = new RobotDynamics::Model();
410  model_3dof = new RobotDynamics::Model();
411  model = model_emulated;
412  generate();
413  initConstraintSets();
414 
415  q = VectorNd::Zero(model_emulated->q_size);
416  qdot = VectorNd::Zero(model_emulated->qdot_size);
417  qddot = VectorNd::Zero(model_emulated->qdot_size);
418  tau = VectorNd::Zero(model_emulated->qdot_size);
419 
420  qddot_emulated = VectorNd::Zero(model_emulated->qdot_size);
421  qddot_3dof = VectorNd::Zero(model_emulated->qdot_size);
422  };
423 
424  void TearDown()
425  {
426  EXPECT_TRUE(unit_test_utils::checkModelZeroVectorsAndMatrices(*model_emulated));
427  EXPECT_TRUE(unit_test_utils::checkModelZeroVectorsAndMatrices(*model_3dof));
428  delete model_emulated;
429  delete model_3dof;
430  }
431 };
432 
433 #endif
RobotDynamics::Body
Describes all properties of a single body.
Definition: Body.h:30
Human36::model_3dof
RobotDynamics::Model * model_3dof
Definition: Human36Fixture.h:13
Human36::qdot
RobotDynamics::Math::VectorNd qdot
Definition: Human36Fixture.h:16
Human36::randomizeStates
void randomizeStates()
Definition: Human36Fixture.h:384
Human36::body_id_3dof
unsigned int body_id_3dof[BodyNameLast]
Definition: Human36Fixture.h:73
Human36::constraints_4B4C_3dof
RobotDynamics::ConstraintSet constraints_4B4C_3dof
Definition: Human36Fixture.h:29
Human36::SegmentLengths
double SegmentLengths[SegmentNameLast]
Definition: Human36Fixture.h:67
Human36::initConstraintSets
void initConstraintSets()
Definition: Human36Fixture.h:308
Human36::TearDown
void TearDown()
Definition: Human36Fixture.h:424
Human36::constraints_4B4C_emulated
RobotDynamics::ConstraintSet constraints_4B4C_emulated
Definition: Human36Fixture.h:25
RobotDynamics::Math::Xtrans
SpatialTransform Xtrans(const Vector3d &r)
Get pure translation transform.
Definition: SpatialAlgebraOperators.h:369
RobotDynamics::JointTypeFixed
Fixed joint which causes the inertial properties to be merged with.
Definition: Joint.h:205
RobotDynamics::ConstraintSet::addConstraint
unsigned int addConstraint(unsigned int body_id, const Math::Vector3d &body_point, const Math::Vector3d &world_normal, const char *constraint_name=NULL, double normal_acceleration=0.)
Adds a constraint to the constraint set.
Definition: Contacts.cc:27
unit_test_utils::checkModelZeroVectorsAndMatrices
bool checkModelZeroVectorsAndMatrices(RobotDynamics::Model &model)
Definition: UnitTestUtils.hpp:27
RobotDynamics::Model::gravity
Math::MotionVector gravity
the cartesian vector of the gravity
Definition: Model.h:196
RobotDynamics::Model::appendBody
unsigned int appendBody(const Math::SpatialTransform &joint_frame, const Joint &joint, const Body &body, std::string body_name="")
Adds a Body to the model such that the previously added Body is the Parent.
Definition: Model.cc:517
Human36::tau
RobotDynamics::Math::VectorNd tau
Definition: Human36Fixture.h:18
Human36::SegmentRadiiOfGyration
double SegmentRadiiOfGyration[SegmentNameLast][3]
Definition: Human36Fixture.h:70
RobotDynamics::Model::addBody
unsigned int addBody(const unsigned int parent_id, const Math::SpatialTransform &joint_frame, const Joint &joint, const Body &body, std::string body_name="")
Connects a given body to the model.
Definition: Model.cc:271
Human36::create_body
RobotDynamics::Body create_body(SegmentName segment)
Definition: Human36Fixture.h:180
RobotDynamics::Model::q_size
unsigned int q_size
The size of the -vector. For models without spherical joints the value is the same as Model::dof_coun...
Definition: Model.h:178
RobotDynamics::ConstraintSet
Structure that contains both constraint information and workspace memory.
Definition: Contacts.h:168
RobotDynamics::Joint
Describes a joint relative to the predecessor body.
Definition: Joint.h:228
RobotDynamics::Model::GetBodyId
unsigned int GetBodyId(const char *body_name) const
Returns the id of a body that was passed to addBody()
Definition: Model.h:432
Human36::constraints_1B4C_3dof
RobotDynamics::ConstraintSet constraints_1B4C_3dof
Definition: Human36Fixture.h:28
RobotDynamics::Math::VectorNd
Eigen::VectorXd VectorNd
Definition: rdl_eigenmath.h:25
Human36::body_id_emulated
unsigned int body_id_emulated[BodyNameLast]
Definition: Human36Fixture.h:72
Human36::SegmentMass
double SegmentMass[SegmentNameLast]
Definition: Human36Fixture.h:68
RobotDynamics::Model
Contains all information about the rigid body model.
Definition: Model.h:121
RobotDynamics::ConstraintSet::bind
bool bind(const Model &model)
Initializes and allocates memory for the constraint set.
Definition: Contacts.cc:62
RobotDynamics::Math
Math types such as vectors and matrices and utility functions.
Definition: ForceVector.hpp:21
Human36::generate
void generate()
Definition: Human36Fixture.h:194
Human36::initParameters
void initParameters()
Definition: Human36Fixture.h:75
Human36::constraints_1B4C_emulated
RobotDynamics::ConstraintSet constraints_1B4C_emulated
Definition: Human36Fixture.h:24
Human36::constraints_1B1C_3dof
RobotDynamics::ConstraintSet constraints_1B1C_3dof
Definition: Human36Fixture.h:27
Human36::qddot_3dof
RobotDynamics::Math::VectorNd qddot_3dof
Definition: Human36Fixture.h:21
Human36::model
RobotDynamics::Model * model
Definition: Human36Fixture.h:11
Human36::constraints_1B1C_emulated
RobotDynamics::ConstraintSet constraints_1B1C_emulated
Definition: Human36Fixture.h:23
Human36::qddot
RobotDynamics::Math::VectorNd qddot
Definition: Human36Fixture.h:17
Human36::qddot_emulated
RobotDynamics::Math::VectorNd qddot_emulated
Definition: Human36Fixture.h:20
Human36::q
RobotDynamics::Math::VectorNd q
Definition: Human36Fixture.h:15
Human36::SetUp
void SetUp()
Definition: Human36Fixture.h:403
RobotDynamics
Namespace for all structures of the RobotDynamics library.
Definition: Body.h:21
RobotDynamics::Model::qdot_size
unsigned int qdot_size
The size of the.
Definition: Model.h:187
Human36::SegmentCOM
double SegmentCOM[SegmentNameLast][3]
Definition: Human36Fixture.h:69
Human36::model_emulated
RobotDynamics::Model * model_emulated
Definition: Human36Fixture.h:12
RobotDynamics::JointTypeEulerYXZ
3 DoF joint that uses Euler YXZ convention (faster than emulated
Definition: Joint.h:201

rdl_dynamics
Author(s):
autogenerated on Tue Apr 20 2021 02:25:27