RdlModelTests.cpp
Go to the documentation of this file.
1 //
2 // Created by jordan on 3/24/17.
3 //
4 
5 #include <gtest/gtest.h>
6 #include <iostream>
7 
8 #include "Fixtures.h"
9 #include "rdl_dynamics/FramePoint.hpp"
10 #include "rdl_dynamics/Kinematics.h"
11 #include "rdl_dynamics/Dynamics.h"
12 #include "rdl_dynamics/Model.h"
13 #include "rdl_dynamics/rdl_mathutils.h"
14 #include "UnitTestUtils.hpp"
15 
16 using namespace std;
17 using namespace RobotDynamics;
18 using namespace RobotDynamics::Math;
19 
20 const double TEST_PREC = 1.0e-14;
21 
22 struct RdlModelFixture : public testing::Test
23 {
24  RdlModelFixture()
25  {
26  }
27 
28  void SetUp()
29  {
30  model = new Model;
31  model->gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
32  }
33 
34  void TearDown()
35  {
36  delete model;
37  }
38 
39  Model* model;
40 };
41 
42 TEST_F(RdlModelFixture, testCommonParentId)
43 {
44  Body b(1., Vector3d(1., 1., 1.), Vector3d(1., 1., 1.));
45  Joint j(JointTypeRevoluteX);
46 
47  unsigned int id1 = model->appendBody(SpatialTransform(), j, b, "b1");
48  unsigned int id2 = model->appendBody(SpatialTransform(), j, b, "b2");
49  unsigned int id3 = model->addBody(id1, SpatialTransform(), j, b, "b3");
50  unsigned int id4 = model->addBody(id2, SpatialTransform(), j, b, "b4");
51  unsigned int id5 = model->addBody(id2, SpatialTransform(), j, b, "b5");
52  unsigned int id6 = model->addBody(id2, SpatialTransform(), j, b, "b6");
53 
54  unsigned int id7 = model->addBody(id6, SpatialTransform(), j, b, "b7");
55  unsigned int id8 = model->addBody(id6, SpatialTransform(), j, b, "b8");
56 
57  EXPECT_EQ(model->getCommonMovableParentId(id8, id7), id6);
58  EXPECT_EQ(model->getCommonMovableParentId(id1, id4), id1);
59  EXPECT_EQ(model->getCommonMovableParentId(id7, id8), id6);
60  EXPECT_EQ(model->getCommonMovableParentId(id4, id1), id1);
61  EXPECT_EQ(model->getCommonMovableParentId(0, id8), 0);
62  EXPECT_EQ(model->getCommonMovableParentId(id4, id4), id4);
63  EXPECT_EQ(model->mass, 8.0);
64 
65  unsigned int id9 = model->addBody(0, SpatialTransform(), j, b, "b9");
66  unsigned int id10 = model->addBody(id9, SpatialTransform(), j, b, "b10");
67 
68  EXPECT_EQ(model->getCommonMovableParentId(id10, id8), 0);
69  EXPECT_EQ(model->getCommonMovableParentId(id8, id10), 0);
70 
71  EXPECT_EQ(model->mass, 10.0);
72 
73  Joint jf(JointTypeFixed);
74  unsigned int fid1 = model->addBody(id10, SpatialTransform(), jf, b, "fb1");
75  unsigned int fid2 = model->addBody(fid1, SpatialTransform(), jf, b, "fb2");
76  unsigned int fid3 = model->addBody(id2, SpatialTransform(), jf, b, "fb3");
77  unsigned int fid4 = model->addBody(id10, SpatialTransform(), jf, b, "fb4");
78 
79  EXPECT_EQ(model->mass, 14.0);
80 
81  EXPECT_EQ(model->getCommonMovableParentId(fid1, fid2), id10);
82  EXPECT_EQ(model->getCommonMovableParentId(fid2, fid1), id10);
83  EXPECT_EQ(model->getCommonMovableParentId(fid1, fid4), id10);
84  EXPECT_EQ(model->getCommonMovableParentId(fid4, fid1), id10);
85  EXPECT_EQ(model->getCommonMovableParentId(fid1, fid3), 0);
86  EXPECT_EQ(model->getCommonMovableParentId(fid1, id9), id9);
87 }
88 
89 TEST_F(RdlModelFixture, TestInit)
90 {
91  EXPECT_EQ(1u, model->lambda.size());
92  EXPECT_EQ(1u, model->mu.size());
93  EXPECT_EQ(0u, model->dof_count);
94 
95  EXPECT_EQ(0u, model->q_size);
96  EXPECT_EQ(0u, model->qdot_size);
97 
98  EXPECT_EQ(1u, model->v.size());
99  EXPECT_EQ(1u, model->a.size());
100 
101  EXPECT_EQ(1u, model->mJoints.size());
102 
103  EXPECT_EQ(1u, model->S.size());
104 
105  EXPECT_EQ(1u, model->c.size());
106  EXPECT_EQ(1u, model->IA.size());
107  EXPECT_EQ(1u, model->pA.size());
108  EXPECT_EQ(1u, model->U.size());
109  EXPECT_EQ(1u, model->d.size());
110  EXPECT_EQ(1u, model->u.size());
111  EXPECT_EQ(1u, model->Ic.size());
112  EXPECT_EQ(1u, model->I.size());
113 
114  EXPECT_EQ(1u, model->X_lambda.size());
115  EXPECT_EQ(1u, model->bodyFrames.size());
116  EXPECT_EQ(1u, model->bodyCenteredFrames.size());
117  EXPECT_EQ(0u, model->fixedBodyFrames.size());
118  EXPECT_EQ(1u, model->mBodies.size());
119 }
120 
121 TEST_F(RdlModelFixture, TestaddBodyDimensions)
122 {
123  Body body(1., Vector3d(1., 1., 1.), Vector3d(1., 1., 1.));
124  Joint joint(SpatialVector(0., 0., 1., 0., 0., 0.));
125 
126  unsigned int body_id = 0;
127  // Adding null body.
128  body_id = model->addBody(0, Xtrans(Vector3d(0., 0., 0.)), joint, body);
129 
130  EXPECT_EQ(1u, body_id);
131  EXPECT_EQ(2u, model->lambda.size());
132  EXPECT_EQ(2u, model->mu.size());
133  EXPECT_EQ(1u, model->dof_count);
134 
135  EXPECT_EQ(2u, model->v.size());
136  EXPECT_EQ(2u, model->a.size());
137 
138  EXPECT_EQ(2u, model->mJoints.size());
139  EXPECT_EQ(2u, model->S.size());
140 
141  EXPECT_EQ(2u, model->c.size());
142  EXPECT_EQ(2u, model->IA.size());
143  EXPECT_EQ(2u, model->pA.size());
144  EXPECT_EQ(2u, model->U.size());
145  EXPECT_EQ(2u, model->d.size());
146  EXPECT_EQ(2u, model->u.size());
147  EXPECT_EQ(2u, model->Ic.size());
148  EXPECT_EQ(2u, model->I.size());
149 
150  SpatialVector spatial_zero;
151  spatial_zero.setZero();
152 
153  EXPECT_EQ(2u, model->X_lambda.size());
154  EXPECT_EQ(2u, model->bodyFrames.size());
155  EXPECT_EQ(2u, model->bodyCenteredFrames.size());
156  EXPECT_EQ(0u, model->fixedBodyFrames.size());
157  EXPECT_EQ(2u, model->mBodies.size());
158 }
159 
160 TEST_F(RdlModelFixture, TestFloatingBodyDimensions)
161 {
162  Body body(1., Vector3d(1., 1., 1.), Vector3d(1., 1., 1.));
163  Joint float_base_joint(JointTypeFloatingBase);
164 
165  model->appendBody(SpatialTransform(), float_base_joint, body, "rootBody");
166 
167  EXPECT_EQ(3u, model->lambda.size());
168  EXPECT_EQ(3u, model->mu.size());
169  EXPECT_EQ(6u, model->dof_count);
170  EXPECT_EQ(7u, model->q_size);
171  EXPECT_EQ(6u, model->qdot_size);
172 
173  EXPECT_EQ(3u, model->v.size());
174  EXPECT_EQ(3u, model->a.size());
175 
176  EXPECT_EQ(3u, model->mJoints.size());
177  EXPECT_EQ(3u, model->S.size());
178 
179  EXPECT_EQ(3u, model->c.size());
180  EXPECT_EQ(3u, model->IA.size());
181  EXPECT_EQ(3u, model->pA.size());
182  EXPECT_EQ(3u, model->U.size());
183  EXPECT_EQ(3u, model->d.size());
184  EXPECT_EQ(3u, model->u.size());
185 
186  SpatialVector spatial_zero;
187  spatial_zero.setZero();
188 
189  EXPECT_EQ(3u, model->X_lambda.size());
190  EXPECT_EQ(3u, model->bodyFrames.size());
191  EXPECT_EQ(3u, model->bodyCenteredFrames.size());
192  EXPECT_EQ(0u, model->fixedBodyFrames.size());
193  EXPECT_EQ(3u, model->mBodies.size());
194 }
195 
198 TEST_F(RdlModelFixture, TestaddBodySpatialValues)
199 {
200  Body body(1., Vector3d(1., 1., 1.), Vector3d(1., 1., 1.));
201  Joint joint(SpatialVector(0., 0., 1., 0., 0., 0.));
202 
203  model->addBody(0, Xtrans(Vector3d(0., 0., 0.)), joint, body);
204 
205  SpatialVector spatial_joint_axis(0., 0., 1., 0., 0., 0.);
206  EXPECT_EQ(spatial_joint_axis, joint.mJointAxes[0]);
207 }
208 
209 TEST_F(RdlModelFixture, testIsBodyId)
210 {
211  Body body(1., Vector3d(1., 1., 1.), Vector3d(1., 1., 1.));
212  Joint joint(SpatialVector(0., 0., 1., 0., 0., 0.));
213 
214  Joint fixed(JointTypeFixed);
215 
216  unsigned int bodyId1 = model->addBody(0, Xtrans(Vector3d(0., 0., 0.)), joint, body, "mybody");
217 
218  unsigned int fb1 = model->addBody(0, Xtrans(Vector3d(0., 0., 0.)), fixed, body, "fixed");
219 
220  EXPECT_TRUE(model->IsBodyId(bodyId1));
221  EXPECT_TRUE(model->IsBodyId(fb1));
222 
223  EXPECT_FALSE(model->IsBodyId(bodyId1 + 1));
224  EXPECT_FALSE(model->IsBodyId(fb1 + 1));
225 }
226 
227 TEST_F(RdlModelFixture, TestaddBodyTestBodyName)
228 {
229  Body body(1., Vector3d(1., 1., 1.), Vector3d(1., 1., 1.));
230  Joint joint(SpatialVector(0., 0., 1., 0., 0., 0.));
231 
232  model->addBody(0, Xtrans(Vector3d(0., 0., 0.)), joint, body, "mybody");
233 
234  unsigned int body_id = model->GetBodyId("mybody");
235 
236  EXPECT_EQ(1u, body_id);
237  EXPECT_EQ(std::numeric_limits<unsigned int>::max(), model->GetBodyId("unknownbody"));
238 }
239 
240 TEST_F(RdlModelFixture, TestjcalcSimple)
241 {
242  Body body(1., Vector3d(1., 1., 1.), Vector3d(1., 1., 1.));
243  Joint joint(SpatialVector(0., 0., 1., 0., 0., 0.));
244 
245  model->addBody(0, Xtrans(Vector3d(1., 0., 0.)), joint, body);
246 
247  VectorNd Q = VectorNd::Zero(model->q_size);
248  VectorNd QDot = VectorNd::Zero(model->q_size);
249 
250  QDot[0] = 1.;
251  jcalc(*model, 1, Q, QDot);
252 
253  SpatialMatrix test_matrix(1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.,
254  1.);
255  SpatialVector test_vector(0., 0., 1., 0., 0., 0.);
256  SpatialVector test_joint_axis(0., 0., 1., 0., 0., 0.);
257 
258  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(test_matrix, model->X_J[1].toMatrix(), 1.0e-16));
259  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(test_vector, model->v_J[1], 1.0e-16));
260  EXPECT_EQ(test_joint_axis, model->S[1]);
261 
262  Q[0] = M_PI * 0.5;
263  QDot[0] = 1.;
264 
265  jcalc(*model, 1, Q, QDot);
266 
267  test_matrix.set(0., 1., 0., 0., 0., 0., -1., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., -1., 0., 0., 0., 0., 0., 0., 0., 1.);
268 
269  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(test_matrix, model->X_J[1].toMatrix(), 1.0e-16));
270  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(test_vector, model->v_J[1], 1.0e-16));
271  EXPECT_EQ(test_joint_axis, model->S[1]);
272 }
273 
274 TEST_F(RdlModelFixture, TestTransformBaseToLocal)
275 {
276  Body body(1., Vector3d(1., 1., 1.), Vector3d(1., 1., 1.));
277 
278  unsigned int body_id = model->addBody(0, SpatialTransform(),
279  Joint(SpatialVector(0., 0., 0., 1., 0., 0.), SpatialVector(0., 0., 0., 0., 1., 0.), SpatialVector(0., 0., 0., 0., 0., 1.),
280  SpatialVector(0., 0., 1., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.)),
281  body);
282 
283  VectorNd q = VectorNd::Zero(model->dof_count);
284  VectorNd qdot = VectorNd::Zero(model->dof_count);
285  VectorNd qddot = VectorNd::Zero(model->dof_count);
286  VectorNd tau = VectorNd::Zero(model->dof_count);
287 
288  Vector3d base_coords(0., 0., 0.);
289  Vector3d body_coords;
290  Vector3d base_coords_back;
291 
292  updateKinematics(*model, q, qdot, qddot);
293  FramePoint p_base_coords(model->bodyFrames[body_id], base_coords);
294  p_base_coords.changeFrame(model->worldFrame);
295 
296  p_base_coords.changeFrame(model->bodyFrames[body_id]);
297 
298  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(base_coords, p_base_coords.vec(), TEST_PREC));
299 
300  q[0] = 1.;
301  q[1] = 0.2;
302  q[2] = -2.3;
303  q[3] = -2.3;
304  q[4] = 0.03;
305  q[5] = -0.23;
306 
307  updateKinematics(*model, q, qdot, qddot);
308 
309  p_base_coords.setIncludingFrame(base_coords, model->worldFrame);
310  p_base_coords.changeFrame(model->bodyFrames[body_id]);
311  p_base_coords.changeFrame(model->worldFrame);
312 
313  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(base_coords, p_base_coords.vec(), TEST_PREC));
314 }
315 
316 TEST_F(RdlModelFixture, Model1DoFJoint)
317 {
318  // the standard modeling using a null body
319  Body null_body;
320  null_body.mIsVirtual = true;
321  Body body(1., Vector3d(1., 0.4, 0.4), Vector3d(1., 1., 1.));
322  Joint joint_rot_z(JointType1DoF);
323  joint_rot_z.mJointAxes[0] = SpatialVector(0., 0., 1., 0., 0., 0.);
324 
325  Model model_std;
326  model_std.gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
327 
328  model_std.addBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body);
329 
330  Model model_2;
331  model_2.gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
332 
333  Joint joint_rot_z_2(JointTypeRevoluteZ);
334  model_2.addBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z_2, body, "body1");
335 
336  VectorNd Q = VectorNd::Zero(model_std.dof_count);
337  VectorNd QDot = VectorNd::Zero(model_std.dof_count);
338  VectorNd Tau = VectorNd::Zero(model_std.dof_count);
339 
340  VectorNd QDDot_2 = VectorNd::Zero(model_std.dof_count);
341  VectorNd QDDot_std = VectorNd::Zero(model_std.dof_count);
342 
343  forwardDynamics(model_std, Q, QDot, Tau, QDDot_std);
344  forwardDynamics(model_2, Q, QDot, Tau, QDDot_2);
345 
346  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_std, QDDot_2, TEST_PREC));
347 }
348 
349 TEST_F(RdlModelFixture, Model2DoFJoint)
350 {
351  // the standard modeling using a null body
352  Body null_body;
353  null_body.mIsVirtual = true;
354  Body body(1., Vector3d(1., 0.4, 0.4), Vector3d(1., 1., 1.));
355  Joint joint_rot_z(SpatialVector(0., 0., 1., 0., 0., 0.));
356  Joint joint_rot_x(SpatialVector(1., 0., 0., 0., 0., 0.));
357 
358  Model model_std;
359  model_std.gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
360 
361  model_std.addBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body);
362  model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body, "body1");
363 
364  // using a model with a 2 DoF joint
365  Joint joint_rot_zx(SpatialVector(0., 0., 1., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.));
366 
367  Model model_2;
368  model_2.gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
369 
370  model_2.addBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zx, body, "body1");
371 
372  VectorNd Q = VectorNd::Zero(model_std.dof_count);
373  VectorNd QDot = VectorNd::Zero(model_std.dof_count);
374  VectorNd Tau = VectorNd::Zero(model_std.dof_count);
375 
376  VectorNd QDDot_2 = VectorNd::Zero(model_std.dof_count);
377  VectorNd QDDot_std = VectorNd::Zero(model_std.dof_count);
378 
379  forwardDynamics(model_std, Q, QDot, Tau, QDDot_std);
380  forwardDynamics(model_2, Q, QDot, Tau, QDDot_2);
381 
382  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_std, QDDot_2, TEST_PREC));
383 }
384 
385 TEST_F(RdlModelFixture, Model3DoFJoint)
386 {
387  // the standard modeling using a null body
388  Body null_body;
389  null_body.mIsVirtual = true;
390  Body body(1., Vector3d(1., 0.4, 0.4), Vector3d(1., 1., 1.));
391 
392  Joint joint_rot_z(SpatialVector(0., 0., 1., 0., 0., 0.));
393  Joint joint_rot_y(SpatialVector(0., 1., 0., 0., 0., 0.));
394  Joint joint_rot_x(SpatialVector(1., 0., 0., 0., 0., 0.));
395 
396  Model model_std;
397  model_std.gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
398 
399  unsigned int body_id;
400 
401  // in total we add two bodies to make sure that the transformations are
402  // correct.
403  model_std.addBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body);
404  model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, null_body);
405  body_id = model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body, "body1");
406 
407  model_std.addBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body);
408  model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, null_body);
409  body_id = model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body, "body2");
410  EXPECT_EQ(model_std.mass, 2.0);
411  // using a model with a 3 DoF joint
412  Joint joint_rot_zyx(SpatialVector(0., 0., 1., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.));
413 
414  Model model_2;
415  model_2.gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
416 
417  // in total we add two bodies to make sure that the transformations are
418  // correct.
419  body_id = model_2.addBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx, body);
420  body_id = model_2.addBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx, body);
421  EXPECT_EQ(model_2.mass, 2.0);
422 
423  VectorNd Q = VectorNd::Zero(model_std.dof_count);
424  VectorNd QDot = VectorNd::Zero(model_std.dof_count);
425  VectorNd Tau = VectorNd::Zero(model_std.dof_count);
426 
427  VectorNd QDDot_2 = VectorNd::Zero(model_std.dof_count);
428  VectorNd QDDot_std = VectorNd::Zero(model_std.dof_count);
429 
430  forwardDynamics(model_std, Q, QDot, Tau, QDDot_std);
431  forwardDynamics(model_2, Q, QDot, Tau, QDDot_2);
432 
433  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_std, QDDot_2, TEST_PREC));
434 }
435 
436 TEST_F(RdlModelFixture, Model4DoFJoint)
437 {
438  // the standard modeling using a null body
439  Body null_body;
440  null_body.mIsVirtual = true;
441  Body body(1., Vector3d(1., 0.4, 0.4), Vector3d(1., 1., 1.));
442 
443  Joint joint_rot_z(SpatialVector(0., 0., 1., 0., 0., 0.));
444  Joint joint_rot_y(SpatialVector(0., 1., 0., 0., 0., 0.));
445  Joint joint_rot_x(SpatialVector(1., 0., 0., 0., 0., 0.));
446  Joint joint_trans_x(SpatialVector(0., 0., 0., 1., 0., 0.));
447 
448  Model model_std;
449  model_std.gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
450 
451  unsigned int body_id;
452 
453  // in total we add two bodies to make sure that the transformations are
454  // correct.
455  model_std.addBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body);
456  model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, null_body);
457  model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, null_body);
458  body_id = model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_trans_x, body, "body1");
459 
460  model_std.addBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body);
461  model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, null_body);
462  model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, null_body);
463  body_id = model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_trans_x, body, "body2");
464 
465  // using a model with a 2 DoF joint
466  Joint joint_rot_zyx_tr_x(SpatialVector(0., 0., 1., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.),
467  SpatialVector(0., 0., 0., 1., 0., 0.));
468 
469  Model model_2;
470  model_2.gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
471 
472  // in total we add two bodies to make sure that the transformations are
473  // correct.
474  body_id = model_2.addBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx_tr_x, body);
475  body_id = model_2.addBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx_tr_x, body);
476 
477  VectorNd Q = VectorNd::Zero(model_std.dof_count);
478  VectorNd QDot = VectorNd::Zero(model_std.dof_count);
479  VectorNd Tau = VectorNd::Zero(model_std.dof_count);
480 
481  VectorNd QDDot_2 = VectorNd::Zero(model_std.dof_count);
482  VectorNd QDDot_std = VectorNd::Zero(model_std.dof_count);
483 
484  forwardDynamics(model_std, Q, QDot, Tau, QDDot_std);
485  forwardDynamics(model_2, Q, QDot, Tau, QDDot_2);
486 
487  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_std, QDDot_2, TEST_PREC));
488 }
489 
490 TEST_F(RdlModelFixture, Model5DoFJoint)
491 {
492  // the standard modeling using a null body
493  Body null_body;
494  null_body.mIsVirtual = true;
495  Body body(1., Vector3d(1., 0.4, 0.4), Vector3d(1., 1., 1.));
496 
497  Joint joint_rot_z(SpatialVector(0., 0., 1., 0., 0., 0.));
498  Joint joint_rot_y(SpatialVector(0., 1., 0., 0., 0., 0.));
499  Joint joint_rot_x(SpatialVector(1., 0., 0., 0., 0., 0.));
500  Joint joint_trans_x(SpatialVector(0., 0., 0., 1., 0., 0.));
501  Joint joint_trans_y(SpatialVector(0., 0., 0., 0., 1., 0.));
502 
503  Model model_std;
504  model_std.gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
505 
506  unsigned int body_id;
507 
508  // in total we add two bodies to make sure that the transformations are
509  // correct.
510  model_std.addBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body);
511  model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, null_body);
512  model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, null_body);
513  model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_trans_x, null_body);
514  body_id = model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_trans_y, body, "body1");
515 
516  model_std.addBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body);
517  model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, null_body);
518  model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, null_body);
519  model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_trans_x, null_body);
520  body_id = model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_trans_y, body, "body2");
521 
522  // using a model with a 2 DoF joint
523  Joint joint_rot_zyx_tr_xy(SpatialVector(0., 0., 1., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.),
524  SpatialVector(0., 0., 0., 1., 0., 0.), SpatialVector(0., 0., 0., 0., 1., 0.));
525 
526  Model model_2;
527  model_2.gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
528 
529  // in total we add two bodies to make sure that the transformations are
530  // correct.
531  body_id = model_2.addBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx_tr_xy, body);
532  body_id = model_2.addBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx_tr_xy, body);
533 
534  VectorNd Q = VectorNd::Zero(model_std.dof_count);
535  VectorNd QDot = VectorNd::Zero(model_std.dof_count);
536  VectorNd Tau = VectorNd::Zero(model_std.dof_count);
537 
538  VectorNd QDDot_2 = VectorNd::Zero(model_std.dof_count);
539  VectorNd QDDot_std = VectorNd::Zero(model_std.dof_count);
540 
541  forwardDynamics(model_std, Q, QDot, Tau, QDDot_std);
542  forwardDynamics(model_2, Q, QDot, Tau, QDDot_2);
543 
544  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_std, QDDot_2, TEST_PREC));
545 }
546 
547 TEST_F(RdlModelFixture, Model6DoFJoint)
548 {
549  // the standard modeling using a null body
550  Body null_body;
551  null_body.mIsVirtual = true;
552  Body body(1., Vector3d(1., 0.4, 0.4), Vector3d(1., 1., 1.));
553 
554  Joint joint_rot_z(SpatialVector(0., 0., 1., 0., 0., 0.));
555  Joint joint_rot_y(SpatialVector(0., 1., 0., 0., 0., 0.));
556  Joint joint_rot_x(SpatialVector(1., 0., 0., 0., 0., 0.));
557 
558  Model model_std;
559  model_std.gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
560 
561  unsigned int body_id;
562 
563  Joint joint_floating_base(SpatialVector(0., 0., 0., 1., 0., 0.), SpatialVector(0., 0., 0., 0., 1., 0.), SpatialVector(0., 0., 0., 0., 0., 1.),
564  SpatialVector(0., 0., 1., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.));
565  body_id = model_std.addBody(0, SpatialTransform(), joint_floating_base, body);
566 
567  model_std.addBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body);
568  model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, null_body);
569  body_id = model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body);
570 
571  // using a model with a 2 DoF joint
572  Joint joint_rot_zyx(SpatialVector(0., 0., 1., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.));
573 
574  Model model_2;
575  model_2.gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
576 
577  // in total we add two bodies to make sure that the transformations are
578  // correct.
579  body_id = model_2.addBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_floating_base, body);
580  body_id = model_2.addBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx, body);
581 
582  VectorNd Q = VectorNd::Zero(model_std.dof_count);
583  VectorNd QDot = VectorNd::Zero(model_std.dof_count);
584  VectorNd Tau = VectorNd::Zero(model_std.dof_count);
585 
586  VectorNd QDDot_2 = VectorNd::Zero(model_std.dof_count);
587  VectorNd QDDot_std = VectorNd::Zero(model_std.dof_count);
588 
589  assert(model_std.q_size == model_2.q_size);
590 
591  forwardDynamics(model_std, Q, QDot, Tau, QDDot_std);
592  forwardDynamics(model_2, Q, QDot, Tau, QDDot_2);
593 
594  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_std, QDDot_2, TEST_PREC));
595 }
596 
597 TEST_F(RdlModelFixture, ModelFixedJointQueryBodyId)
598 {
599  // the standard modeling using a null body
600  Body null_body;
601  null_body.mIsVirtual = true;
602  Body body(1., Vector3d(1., 0.4, 0.4), Vector3d(1., 1., 1.));
603  Body fixed_body(1., Vector3d(1., 0.4, 0.4), Vector3d(1., 1., 1.));
604 
605  Model model;
606 
607  Joint joint_rot_z(SpatialVector(0., 0., 1., 0., 0., 0.));
608 
609  model.addBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body);
610  unsigned int fixed_body_id = model.appendBody(Xtrans(Vector3d(0., 1., 0.)), Joint(JointTypeFixed), fixed_body, "fixed_body");
611 
612  EXPECT_EQ(fixed_body_id, model.GetBodyId("fixed_body"));
613 }
614 
615 /*
616  * Makes sure that when appending a body to a fixed joint the parent of the
617  * newly added parent is actually the moving body that the fixed body is
618  * attached to.
619  */
620 TEST_F(RdlModelFixture, ModelAppendToFixedBody)
621 {
622  Body null_body;
623  null_body.mIsVirtual = true;
624  Body body(1., Vector3d(1., 0.4, 0.4), Vector3d(1., 1., 1.));
625  Body fixed_body(1., Vector3d(1., 0.4, 0.4), Vector3d(1., 1., 1.));
626 
627  Model model;
628 
629  Joint joint_rot_z(SpatialVector(0., 0., 1., 0., 0., 0.));
630 
631  unsigned int movable_body = model.addBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body);
632  // unsigned int fixed_body_id = model.appendBody (Xtrans(Vector3d(0., 1., 0.)), Joint(JointTypeFixed), fixed_body, "fixed_body");
633  unsigned int appended_body_id = model.appendBody(Xtrans(Vector3d(0., 1., 0.)), joint_rot_z, body, "appended_body");
634 
635  EXPECT_EQ(movable_body + 1, appended_body_id);
636  EXPECT_EQ(movable_body, model.lambda[appended_body_id]);
637 }
638 
639 // Adds a fixed body to another fixed body.
640 TEST_F(RdlModelFixture, ModelAppendFixedToFixedBody)
641 {
642  Body null_body;
643  null_body.mIsVirtual = true;
644 
645  double movable_mass = 1.1;
646  Vector3d movable_com(1., 0.4, 0.4);
647 
648  double fixed_mass = 1.2;
649  Vector3d fixed_com(1.1, 0.5, 0.5);
650 
651  Vector3d fixed_displacement(0., 1., 0.);
652 
653  Body body(movable_mass, movable_com, Vector3d(1., 1., 1.));
654  Body fixed_body(fixed_mass, fixed_com, Vector3d(1., 1., 1.));
655 
656  Model model;
657 
658  Joint joint_rot_z(SpatialVector(0., 0., 1., 0., 0., 0.));
659 
660  unsigned int movable_body = model.addBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body);
661  unsigned int fixed_body_id = model.appendBody(Xtrans(fixed_displacement), Joint(JointTypeFixed), fixed_body, "fixed_body");
662  unsigned int fixed_body_2_id = model.appendBody(Xtrans(fixed_displacement), Joint(JointTypeFixed), fixed_body, "fixed_body_2");
663  unsigned int appended_body_id = model.appendBody(Xtrans(Vector3d(0., 1., 0.)), joint_rot_z, body, "appended_body");
664 
665  EXPECT_EQ(movable_body + 1, appended_body_id);
666  EXPECT_EQ(movable_body, model.lambda[appended_body_id]);
667  EXPECT_EQ(movable_mass + fixed_mass * 2., model.mBodies[movable_body].mMass);
668 
669  EXPECT_EQ(movable_body, model.mFixedBodies[fixed_body_id - model.fixed_body_discriminator].mMovableParent);
670  EXPECT_EQ(movable_body, model.mFixedBodies[fixed_body_2_id - model.fixed_body_discriminator].mMovableParent);
671 
672  double new_mass = 3.5;
673  Vector3d new_com =
674  (1. / new_mass) * (movable_mass * movable_com + fixed_mass * (fixed_com + fixed_displacement) + fixed_mass * (fixed_com + fixed_displacement * 2.));
675 
676  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(new_com, model.mBodies[movable_body].mCenterOfMass, TEST_PREC));
677 }
678 
679 // Ensures that the transformations of the movable parent and fixed joint
680 // frame is in proper order
681 TEST_F(RdlModelFixture, ModelFixedJointRotationOrderTranslationRotation)
682 {
683  // the standard modeling using a null body
684  Body null_body;
685  null_body.mIsVirtual = true;
686  Body body(1., Vector3d(1., 0.4, 0.4), Vector3d(1., 1., 1.));
687  Body fixed_body(1., Vector3d(1., 0.4, 0.4), Vector3d(1., 1., 1.));
688 
689  Model model;
690 
691  Joint joint_rot_z(SpatialVector(0., 0., 1., 0., 0., 0.));
692 
693  SpatialTransform trans_x = Xtrans(Vector3d(1., 0., 0.));
694  SpatialTransform rot_z = Xrotz(45. * M_PI / 180.);
695 
696  model.addBody(0, trans_x, joint_rot_z, body);
697  model.appendBody(rot_z, Joint(JointTypeFixed), fixed_body, "fixed_body");
698  unsigned int body_after_fixed = model.appendBody(trans_x, joint_rot_z, body);
699 
700  VectorNd Q(VectorNd::Zero(model.dof_count));
701  Q[0] = 45 * M_PI / 180.;
702  updateKinematicsCustom(model, &Q, nullptr, nullptr);
703  VectorNd QDot = VectorNd::Zero(model.dof_count);
704  VectorNd QDDot = VectorNd::Zero(model.dof_count);
705 
706  updateKinematics(model, Q, QDot, QDDot);
707  FramePoint p(model.bodyFrames[body_after_fixed], Vector3d(0., 1., 0.));
708  p.changeFrame(model.worldFrame);
709 
710  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(0., 1., 0.), p.vec(), TEST_PREC));
711 }
712 
713 // Ensures that the transformations of the movable parent and fixed joint
714 // frame is in proper order
715 TEST_F(RdlModelFixture, ModelFixedJointRotationOrderRotationTranslation)
716 {
717  // the standard modeling using a null body
718  Body null_body;
719  null_body.mIsVirtual = true;
720  Body body(1., Vector3d(1., 0.4, 0.4), Vector3d(1., 1., 1.));
721  Body fixed_body(1., Vector3d(1., 0.4, 0.4), Vector3d(1., 1., 1.));
722 
723  Model model;
724 
725  Joint joint_rot_z(SpatialVector(0., 0., 1., 0., 0., 0.));
726 
727  SpatialTransform rot_z = Xrotz(45. * M_PI / 180.);
728  SpatialTransform trans_x = Xtrans(Vector3d(1., 0., 0.));
729 
730  model.addBody(0, rot_z, joint_rot_z, body);
731  model.appendBody(trans_x, Joint(JointTypeFixed), fixed_body, "fixed_body");
732  unsigned int body_after_fixed = model.appendBody(trans_x, joint_rot_z, body);
733 
734  VectorNd Q(VectorNd::Zero(model.dof_count));
735  Q[0] = 45 * M_PI / 180.;
736  updateKinematicsCustom(model, &Q, nullptr, nullptr);
737  FramePoint p(model.bodyFrames[body_after_fixed], Vector3d(0., 1., 0.));
738  p.changeFrame(model.worldFrame);
739 
740  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(-1., 2., 0.), p.vec(), TEST_PREC));
741 }
742 
743 TEST_F(RdlModelFixture, ModelGetBodyName)
744 {
745  Body null_body;
746  null_body.mIsVirtual = true;
747  Body body(1., Vector3d(1., 0.4, 0.4), Vector3d(1., 1., 1.));
748  Body fixed_body(1., Vector3d(1., 0.4, 0.4), Vector3d(1., 1., 1.));
749 
750  Model model;
751 
752  Joint joint_rot_z(SpatialVector(0., 0., 1., 0., 0., 0.));
753 
754  model.addBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body);
755  unsigned int fixed_body_id = model.appendBody(Xtrans(Vector3d(0., 1., 0.)), Joint(JointTypeFixed), fixed_body, "fixed_body");
756  unsigned int appended_body_id = model.appendBody(Xtrans(Vector3d(0., 1., 0.)), joint_rot_z, body, "appended_body");
757 
758  EXPECT_EQ(string("fixed_body"), model.GetBodyName(fixed_body_id));
759  EXPECT_EQ(string("appended_body"), model.GetBodyName(appended_body_id));
760  EXPECT_EQ(string(""), model.GetBodyName(123));
761 }
762 
763 TEST_F(RotZRotZYXFixed, ModelGetParentBodyId)
764 {
765  EXPECT_EQ(0u, model->GetParentBodyId(0));
766  EXPECT_EQ(0u, model->GetParentBodyId(body_a_id));
767  EXPECT_EQ(body_a_id, model->GetParentBodyId(body_b_id));
768 }
769 
770 TEST_F(RotZRotZYXFixed, ModelGetParentIdFixed)
771 {
772  EXPECT_EQ(body_b_id, model->GetParentBodyId(body_fixed_id));
773 }
774 
775 TEST_F(RotZRotZYXFixed, ModelGetJointFrame)
776 {
777  SpatialTransform transform_a = model->GetJointFrame(body_a_id);
778  SpatialTransform transform_b = model->GetJointFrame(body_b_id);
779  SpatialTransform transform_root = model->GetJointFrame(0);
780 
781  EXPECT_TRUE(unit_test_utils::checkVector3dEq(fixture_transform_a.r, transform_a.r));
782  EXPECT_TRUE(unit_test_utils::checkVector3dEq(fixture_transform_b.r, transform_b.r));
783  EXPECT_TRUE(unit_test_utils::checkVector3dEq(Vector3d(0., 0., 0.), transform_root.r));
784 }
785 
786 TEST_F(RotZRotZYXFixed, ModelGetJointFrameFixed)
787 {
788  SpatialTransform transform_fixed = model->GetJointFrame(body_fixed_id);
789 
790  EXPECT_TRUE(unit_test_utils::checkVector3dEq(fixture_transform_fixed.r, transform_fixed.r));
791 }
792 
793 TEST_F(RotZRotZYXFixed, ModelSetJointFrame)
794 {
795  SpatialTransform new_transform_a = Xtrans(Vector3d(-1., -2., -3.));
796  SpatialTransform new_transform_b = Xtrans(Vector3d(-4., -5., -6.));
797  SpatialTransform new_transform_root = Xtrans(Vector3d(-99, -99., -99.));
798 
799  model->SetJointFrame(body_a_id, new_transform_a);
800  model->SetJointFrame(body_b_id, new_transform_b);
801  model->SetJointFrame(0, new_transform_root);
802 
803  SpatialTransform transform_a = model->GetJointFrame(body_a_id);
804  SpatialTransform transform_b = model->GetJointFrame(body_b_id);
805  SpatialTransform transform_root = model->GetJointFrame(0);
806 
807  EXPECT_TRUE(unit_test_utils::checkVector3dEq(new_transform_a.r, transform_a.r));
808  EXPECT_TRUE(unit_test_utils::checkVector3dEq(new_transform_b.r, transform_b.r));
809  EXPECT_TRUE(unit_test_utils::checkVector3dEq(Vector3d(0., 0., 0.), transform_root.r));
810 
811  try
812  {
813  model->SetJointFrame(model->fixed_body_discriminator + 1, new_transform_root);
814  FAIL();
815  }
816  catch (RobotDynamics::RdlException& e)
817  {
818  EXPECT_STREQ(e.what(), "Error: setting of parent transform not supported for fixed bodies!");
819  }
820 }
821 
822 TEST_F(RdlModelFixture, BodyOrientationFixedJoint)
823 {
824  Model model_fixed;
825  Model model_movable;
826 
827  Body body(1., Vector3d(1., 1., 1.), Vector3d(1., 1., 1.));
828  Joint joint_fixed(JointTypeFixed);
829  Joint joint_rot_x(SpatialVector(1., 0., 0., 0., 0., 0.));
830 
831  model_fixed.appendBody(Xrotx(45 * M_PI / 180), joint_rot_x, body);
832  unsigned int body_id_fixed = model_fixed.appendBody(Xroty(45 * M_PI / 180), joint_fixed, body);
833 
834  model_movable.appendBody(Xrotx(45 * M_PI / 180), joint_rot_x, body);
835  unsigned int body_id_movable = model_movable.appendBody(Xroty(45 * M_PI / 180), joint_rot_x, body);
836 
837  VectorNd q_fixed(VectorNd::Zero(model_fixed.q_size));
838  VectorNd q_movable(VectorNd::Zero(model_movable.q_size));
839 
840  updateKinematicsCustom(model_fixed, &q_fixed, nullptr, nullptr);
841  updateKinematicsCustom(model_movable, &q_movable, nullptr, nullptr);
842 
843  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(model_fixed.fixedBodyFrames[body_id_fixed - model->fixed_body_discriminator]->getInverseTransformToRoot().E,
844  model_movable.bodyFrames[body_id_movable]->getInverseTransformToRoot().E, TEST_PREC));
845  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(model_fixed.fixedBodyFrames[body_id_fixed - model->fixed_body_discriminator]->getTransformToRoot().E,
846  model_movable.bodyFrames[body_id_movable]->getTransformToRoot().E, TEST_PREC));
847 }
848 
849 TEST_F(RdlModelFixture, testAddingTwoBodiesWithSameNameThrows)
850 {
851  Model model;
852 
853  Body body(1., Vector3d(1., 1., 1.), Vector3d(1., 1., 1.));
854  Joint joint(JointTypeRevoluteX);
855 
856  unsigned int id = model.addBody(0, Xrotx(0.1), joint, body, "body_1");
857 
858  try
859  {
860  model.addBody(id, Xrotx(0.1), joint, body, "body_1");
861  FAIL();
862  }
863  catch (RobotDynamics::RdlException& e)
864  {
865  EXPECT_STREQ("Error: Body with name 'body_1' already exists!", e.what());
866  }
867 }
868 
869 TEST_F(RdlModelFixture, testAddingTwoFixedBodiesWithSameNameThrows)
870 {
871  Model model;
872 
873  Body body(1., Vector3d(1., 1., 1.), Vector3d(1., 1., 1.));
874  Joint joint_fixed(JointTypeFixed);
875 
876  unsigned int fbid = model.addBody(0, Xrotx(0.1), joint_fixed, body, "body_1");
877 
878  try
879  {
880  model.addBody(fbid, Xrotx(0.1), joint_fixed, body, "body_1");
881  FAIL();
882  }
883  catch (RobotDynamics::RdlException& e)
884  {
885  EXPECT_STREQ("Error: Fixed body with name 'body_1' already exists!", e.what());
886  }
887 }
888 
889 TEST_F(RdlModelFixture, transformsOnConstruction)
890 {
891  Model model;
892 
893  Body body(1., Vector3d(1., 1., 1.), Vector3d(1., 1., 1.));
894  Joint j(JointTypeRevoluteX);
895 
896  unsigned int id_1 = model.addBody(0, RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(0., 0., 1.)), j, body, "b1");
897  unsigned int id_2 = model.addBody(id_1, RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(0., 1., 1.)), j, body, "b2");
898  unsigned int id_3 = model.addBody(id_2, RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(1., 0., 1.)), j, body, "b3");
899 
900  EXPECT_TRUE(model.bodyFrames[id_1]->getTransformFromParent().toMatrix().isApprox(RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(0., 0., 1.)).toMatrix(),
901  unit_test_utils::TEST_PREC));
902  EXPECT_TRUE(model.bodyFrames[id_2]->getTransformFromParent().toMatrix().isApprox(RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(0., 1., 1.)).toMatrix(),
903  unit_test_utils::TEST_PREC));
904  EXPECT_TRUE(model.bodyFrames[id_3]->getTransformFromParent().toMatrix().isApprox(RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(1., 0., 1.)).toMatrix(),
905  unit_test_utils::TEST_PREC));
906 
907  EXPECT_TRUE(model.bodyFrames[id_3]->getTransformToRoot().toMatrix().isApprox(RobotDynamics::Math::Xtrans(RobotDynamics::Math::Vector3d(-1., -1., -3.)).toMatrix(),
908  unit_test_utils::TEST_PREC));
909 }
910 
911 int main(int argc, char** argv)
912 {
913  ::testing::InitGoogleTest(&argc, argv);
914  return RUN_ALL_TESTS();
915 }
joint_rot_z
Joint joint_rot_z(SpatialVector(0., 0., 1., 0., 0., 0.))
unit_test_utils::checkVectorNdEpsilonClose
static bool checkVectorNdEpsilonClose(const RobotDynamics::Math::VectorNd &v1, const RobotDynamics::Math::VectorNd &v2, const double epsilon)
Definition: UnitTestUtils.hpp:288
FramePoint.hpp
File containing the FramePoint<T> object definition.
RobotDynamics::Math::FramePoint
A FramePoint is a 3D point that is expressed in a ReferenceFrame. To change the ReferenceFrame a Fram...
Definition: FramePoint.hpp:40
RobotDynamics::Body
Describes all properties of a single body.
Definition: Body.h:30
Tau
VectorNd Tau
Definition: RdlTwoLegModelTests.cpp:32
QDDot
VectorNd QDDot
Definition: RdlTwoLegModelTests.cpp:31
RobotDynamics::Math::Xroty
SpatialTransform Xroty(const double &yrot)
Get transform with zero translation and pure rotation about y axis.
Definition: SpatialAlgebraOperators.h:286
RobotDynamics::Model::mFixedBodies
std::vector< FixedBody > mFixedBodies
All bodies that are attached to a body via a fixed joint.
Definition: Model.h:298
RobotDynamics::Model::lambda
std::vector< unsigned int > lambda
Definition: Model.h:156
QDot
VectorNd QDot
Definition: RdlTwoLegModelTests.cpp:30
unit_test_utils::TEST_PREC
const double TEST_PREC
Definition: UnitTestUtils.hpp:22
Q
VectorNd Q
Definition: RdlTwoLegModelTests.cpp:29
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::jcalc
void jcalc(Model &model, unsigned int joint_id, const Math::VectorNd &q, const Math::VectorNd &qdot)
Computes all variables for a joint model and updates the body frame as well as the body&#39;s center of m...
Definition: Joint.cc:23
rdl_mathutils.h
TEST_PREC
const double TEST_PREC
Definition: RdlModelTests.cpp:20
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
RobotDynamics::Model::bodyFrames
std::vector< ReferenceFramePtr > bodyFrames
Definition: Model.h:145
RobotDynamics::Math::SpatialMatrix::set
void set(const Scalar &m00, const Scalar &m01, const Scalar &m02, const Scalar &m03, const Scalar &m04, const Scalar &m05, const Scalar &m10, const Scalar &m11, const Scalar &m12, const Scalar &m13, const Scalar &m14, const Scalar &m15, const Scalar &m20, const Scalar &m21, const Scalar &m22, const Scalar &m23, const Scalar &m24, const Scalar &m25, const Scalar &m30, const Scalar &m31, const Scalar &m32, const Scalar &m33, const Scalar &m34, const Scalar &m35, const Scalar &m40, const Scalar &m41, const Scalar &m42, const Scalar &m43, const Scalar &m44, const Scalar &m45, const Scalar &m50, const Scalar &m51, const Scalar &m52, const Scalar &m53, const Scalar &m54, const Scalar &m55)
Definition: rdl_eigenmath.h:317
RobotDynamics::Model::worldFrame
ReferenceFramePtr worldFrame
Definition: Model.h:141
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
RobotDynamics::Math::Xrotz
SpatialTransform Xrotz(const double &zrot)
Get transform with zero translation and pure rotation about z axis.
Definition: SpatialAlgebraOperators.h:300
RobotDynamics::Math::FramePoint::setIncludingFrame
EIGEN_STRONG_INLINE void setIncludingFrame(const Math::Vector3d &v, ReferenceFramePtr referenceFrame)
Set both the ReferenceFrame this object is expressed in as well as the (x,y,z) coordinates of the poi...
Definition: FramePoint.hpp:138
RobotDynamics::Math::Point3d::vec
EIGEN_STRONG_INLINE Math::Vector3d vec() const
Definition: Point3.hpp:269
unit_test_utils::checkVector3dEq
static bool checkVector3dEq(const RobotDynamics::Math::Vector3d &v1, const RobotDynamics::Math::Vector3d &v2)
Definition: UnitTestUtils.hpp:212
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::Math::SpatialTransform
Compact representation of spatial transformations.
Definition: rdl_eigenmath.h:337
RobotDynamics::FrameObject::changeFrame
virtual void changeFrame(ReferenceFramePtr desiredFrame)
Change the ReferenceFrame this FrameObject is expressed in.
Definition: FrameObject.cpp:12
RobotDynamics::Model::mBodies
std::vector< Body > mBodies
All bodies 0 ... N_B, including the base.
Definition: Model.h:321
RobotDynamics::RdlException::what
virtual const char * what() const
Definition: RdlExceptions.hpp:31
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
RobotDynamics::Math::Xrotx
SpatialTransform Xrotx(const double &xrot)
Get transform with zero translation and pure rotation about x axis.
Definition: SpatialAlgebraOperators.h:272
RobotDynamics::Math::VectorNd
Eigen::VectorXd VectorNd
Definition: rdl_eigenmath.h:25
RobotDynamics::updateKinematics
void updateKinematics(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot)
Updates and computes velocities and accelerations of the bodies.
Definition: Kinematics.cc:25
RobotDynamics::Joint::mJointAxes
Math::SpatialVector * mJointAxes
The spatial axes of the joint.
Definition: Joint.h:625
unit_test_utils::checkSpatialMatrixEpsilonClose
static bool checkSpatialMatrixEpsilonClose(const RobotDynamics::Math::SpatialMatrix &t1, const RobotDynamics::Math::SpatialMatrix &t2, const double epsilon)
Definition: UnitTestUtils.hpp:122
RobotDynamics::Model
Contains all information about the rigid body model.
Definition: Model.h:121
RobotDynamics::JointTypeFloatingBase
A 6-DoF joint for floating-base (or freeflyer) systems.
Definition: Joint.h:204
RobotDynamics::updateKinematicsCustom
void updateKinematicsCustom(Model &model, const Math::VectorNd *Q, const Math::VectorNd *QDot=nullptr, const Math::VectorNd *QDDot=nullptr)
Selectively updates model internal states of body positions, velocities and/or accelerations.
Definition: Kinematics.cc:72
TEST_F
TEST_F(RdlModelFixture, testCommonParentId)
Definition: RdlModelTests.cpp:42
RobotDynamics::Math
Math types such as vectors and matrices and utility functions.
Definition: ForceVector.hpp:21
unit_test_utils::checkSpatialVectorsEpsilonClose
static bool checkSpatialVectorsEpsilonClose(const RobotDynamics::Math::SpatialVector &v1, const RobotDynamics::Math::SpatialVector &v2, const double epsilon)
Definition: UnitTestUtils.hpp:306
main
int main(int argc, char **argv)
Definition: RdlModelTests.cpp:911
RobotDynamics::Model::fixed_body_discriminator
unsigned int fixed_body_discriminator
Value that is used to discriminate between fixed and movable bodies.
Definition: Model.h:312
RobotDynamics::RdlException
A custom exception.
Definition: RdlExceptions.hpp:20
unit_test_utils::checkVector3dEpsilonClose
static bool checkVector3dEpsilonClose(const RobotDynamics::Math::Vector3d &v1, const RobotDynamics::Math::Vector3d &v2, const double epsilon)
Definition: UnitTestUtils.hpp:256
RobotDynamics::Model::GetBodyName
std::string GetBodyName(unsigned int body_id) const
Returns the name of a body for a given body id.
Definition: Model.h:464
RobotDynamics::Model::fixedBodyFrames
std::vector< ReferenceFramePtr > fixedBodyFrames
Definition: Model.h:154
RobotDynamics::forwardDynamics
void forwardDynamics(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, Math::VectorNd &QDDot, Math::SpatialForceV *f_ext=nullptr, bool update_kinematics=true)
Computes forward dynamics with the Articulated Body Algorithm.
Definition: Dynamics.cc:455
RobotDynamics::Model::dof_count
unsigned int dof_count
number of degrees of freedoms of the model
Definition: Model.h:169
RobotDynamics::Math::toMatrix
static Matrix3d toMatrix(const Quaternion &q)
Convert quaternion to rotation matrix.
Definition: SpatialAlgebraOperators.h:187
RobotDynamics
Namespace for all structures of the RobotDynamics library.
Definition: Body.h:21
unit_test_utils::checkMatrix3dEpsilonClose
static bool checkMatrix3dEpsilonClose(const RobotDynamics::Math::Matrix3d &t1, const RobotDynamics::Math::Matrix3d &t2, const double epsilon)
Definition: UnitTestUtils.hpp:138

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