RdlMultiDofTests.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 
3 #include "UnitTestUtils.hpp"
4 #include "rdl_dynamics/Kinematics.h"
5 
6 #include "Fixtures.h"
7 #include "Human36Fixture.h"
8 #include "rdl_dynamics/Dynamics.h"
9 
10 using namespace std;
11 using namespace RobotDynamics;
12 using namespace RobotDynamics::Math;
13 
14 const double TEST_PREC = 1.0e-12;
15 
16 struct SphericalJointFixture : testing::Test
17 {
18  SphericalJointFixture()
19  {
20  }
21 
22  void SetUp()
23  {
24  emulated_model.gravity = SpatialVector(0., 0., 0., 0., 0., -9.81);
25  multdof3_model.gravity = SpatialVector(0., 0., 0., 0., 0., -9.81);
26  eulerzyx_model.gravity = SpatialVector(0., 0., 0., 0., 0., -9.81);
27 
28  body = Body(1., Vector3d(1., 0., 0.), Vector3d(1., 1., 1.));
29 
30  joint_rot_zyx = Joint(SpatialVector(0., 0., 1., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.));
31  joint_spherical = Joint(JointTypeSpherical);
32  joint_eulerzyx = Joint(JointTypeEulerZYX);
33 
34  joint_rot_y = Joint(SpatialVector(0., 1., 0., 0., 0., 0.));
35 
36  emulated_model.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, body);
37  emu_body_id = emulated_model.appendBody(Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx, body);
38  emu_child_id = emulated_model.appendBody(Xtrans(Vector3d(1., 0., 0.)), joint_rot_y, body);
39 
40  multdof3_model.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, body);
41  sph_body_id = multdof3_model.appendBody(Xtrans(Vector3d(1., 0., 0.)), joint_spherical, body);
42  sph_child_id = multdof3_model.appendBody(Xtrans(Vector3d(1., 0., 0.)), joint_rot_y, body);
43 
44  eulerzyx_model.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, body);
45  eulerzyx_body_id = eulerzyx_model.appendBody(Xtrans(Vector3d(1., 0., 0.)), joint_eulerzyx, body);
46  eulerzyx_child_id = eulerzyx_model.appendBody(Xtrans(Vector3d(1., 0., 0.)), joint_rot_y, body);
47 
48  emuQ = VectorNd::Zero((size_t)emulated_model.q_size);
49  emuQDot = VectorNd::Zero((size_t)emulated_model.qdot_size);
50  emuQDDot = VectorNd::Zero((size_t)emulated_model.qdot_size);
51  emuTau = VectorNd::Zero((size_t)emulated_model.qdot_size);
52 
53  sphQ = VectorNd::Zero((size_t)multdof3_model.q_size);
54  sphQDot = VectorNd::Zero((size_t)multdof3_model.qdot_size);
55  sphQDDot = VectorNd::Zero((size_t)multdof3_model.qdot_size);
56  sphTau = VectorNd::Zero((size_t)multdof3_model.qdot_size);
57 
58  eulerzyxQ = VectorNd::Zero((size_t)eulerzyx_model.q_size);
59  eulerzyxQDot = VectorNd::Zero((size_t)eulerzyx_model.qdot_size);
60  eulerzyxQDDot = VectorNd::Zero((size_t)eulerzyx_model.qdot_size);
61  eulerzyxTau = VectorNd::Zero((size_t)eulerzyx_model.qdot_size);
62  }
63 
64  Joint joint_rot_zyx;
65  Joint joint_spherical;
66  Joint joint_eulerzyx;
67  Joint joint_rot_y;
68  Body body;
69 
70  unsigned int emu_body_id, emu_child_id, sph_body_id, sph_child_id, eulerzyx_body_id, eulerzyx_child_id;
71 
72  Model emulated_model;
73  Model multdof3_model;
74  Model eulerzyx_model;
75 
76  VectorNd emuQ;
77  VectorNd emuQDot;
78  VectorNd emuQDDot;
79  VectorNd emuTau;
80 
81  VectorNd sphQ;
82  VectorNd sphQDot;
83  VectorNd sphQDDot;
84  VectorNd sphTau;
85 
86  VectorNd eulerzyxQ;
87  VectorNd eulerzyxQDot;
88  VectorNd eulerzyxQDDot;
89  VectorNd eulerzyxTau;
90 };
91 
92 void ConvertQAndQDotFromEmulated(const Model& emulated_model, const VectorNd& q_emulated, const VectorNd& qdot_emulated, const Model& multdof3_model,
93  VectorNd* q_spherical, VectorNd* qdot_spherical)
94 {
95  for (unsigned int i = 1; i < multdof3_model.mJoints.size(); i++)
96  {
97  unsigned int q_index = multdof3_model.mJoints[i].q_index;
98 
99  if (multdof3_model.mJoints[i].mJointType == JointTypeSpherical)
100  {
101  Quaternion quat = Math::intrinsicZYXAnglesToQuaternion(Vector3d(q_emulated[q_index + 0], q_emulated[q_index + 1], q_emulated[q_index + 2]));
102  multdof3_model.SetQuaternion(i, quat, (*q_spherical));
103 
104  Vector3d omega = angular_velocity_from_angle_rates(Vector3d(q_emulated[q_index], q_emulated[q_index + 1], q_emulated[q_index + 2]),
105  Vector3d(qdot_emulated[q_index], qdot_emulated[q_index + 1], qdot_emulated[q_index + 2]));
106 
107  (*qdot_spherical)[q_index] = omega[0];
108  (*qdot_spherical)[q_index + 1] = omega[1];
109  (*qdot_spherical)[q_index + 2] = omega[2];
110  }
111  else
112  {
113  (*q_spherical)[q_index] = q_emulated[q_index];
114  (*qdot_spherical)[q_index] = qdot_emulated[q_index];
115  }
116  }
117 }
118 
119 TEST_F(SphericalJointFixture, TestQuaternionIntegration)
120 {
121  double timestep = 0.001;
122 
123  Vector3d zyx_angles_t0(0.1, 0.2, 0.3);
124  Vector3d zyx_rates(3., 5., 2.);
125  Vector3d zyx_angles_t1 = zyx_angles_t0 + timestep * zyx_rates;
126  Quaternion q_zyx_t1 = Math::intrinsicZYXAnglesToQuaternion(zyx_angles_t1);
127 
128  Quaternion q_t0 = Math::intrinsicZYXAnglesToQuaternion(zyx_angles_t0);
129  Vector3d w_base = global_angular_velocity_from_rates(zyx_angles_t0, zyx_rates);
130  Quaternion q_t1 = q_t0.timeStep(w_base, timestep);
131 
132  // Note: we test with a rather crude precision. My guess for the error is
133  // that we compare two different things:
134  // A) integration under the assumption that the euler rates are
135  // constant
136  // B) integration under the assumption that the angular velocity is
137  // constant
138  // However I am not entirely sure about this...
139 
140  EXPECT_NEAR(q_zyx_t1.x(), q_t1.x(), 1.0e-5);
141  EXPECT_NEAR(q_zyx_t1.y(), q_t1.y(), 1.0e-5);
142  EXPECT_NEAR(q_zyx_t1.z(), q_t1.z(), 1.0e-5);
143  EXPECT_NEAR(q_zyx_t1.w(), q_t1.w(), 1.0e-5);
144 }
145 
146 TEST_F(SphericalJointFixture, TestQIndices)
147 {
148  EXPECT_EQ(0u, multdof3_model.mJoints[1].q_index);
149  EXPECT_EQ(1u, multdof3_model.mJoints[2].q_index);
150  EXPECT_EQ(4u, multdof3_model.mJoints[3].q_index);
151 
152  EXPECT_EQ(5u, emulated_model.q_size);
153  EXPECT_EQ(5u, emulated_model.qdot_size);
154 
155  EXPECT_EQ(6u, multdof3_model.q_size);
156  EXPECT_EQ(5u, multdof3_model.qdot_size);
157  EXPECT_EQ(5u, multdof3_model.multdof3_w_index[2]);
158 }
159 
160 TEST_F(SphericalJointFixture, TestGetQuaternion)
161 {
162  multdof3_model.appendBody(Xtrans(Vector3d(1., 0., 0.)), joint_spherical, body);
163 
164  sphQ = VectorNd::Zero((size_t)multdof3_model.q_size);
165  sphQDot = VectorNd::Zero((size_t)multdof3_model.qdot_size);
166  sphQDDot = VectorNd::Zero((size_t)multdof3_model.qdot_size);
167  sphTau = VectorNd::Zero((size_t)multdof3_model.qdot_size);
168 
169  EXPECT_EQ(10u, multdof3_model.q_size);
170  EXPECT_EQ(8u, multdof3_model.qdot_size);
171 
172  EXPECT_EQ(0u, multdof3_model.mJoints[1].q_index);
173  EXPECT_EQ(1u, multdof3_model.mJoints[2].q_index);
174  EXPECT_EQ(4u, multdof3_model.mJoints[3].q_index);
175  EXPECT_EQ(5u, multdof3_model.mJoints[4].q_index);
176 
177  EXPECT_EQ(8u, multdof3_model.multdof3_w_index[2]);
178  EXPECT_EQ(9u, multdof3_model.multdof3_w_index[4]);
179 
180  sphQ[0] = 100.;
181  sphQ[1] = 0.;
182  sphQ[2] = 1.;
183  sphQ[3] = 2.;
184  sphQ[4] = 100.;
185  sphQ[5] = -6.;
186  sphQ[6] = -7.;
187  sphQ[7] = -8;
188  sphQ[8] = 4.;
189  sphQ[9] = -9.;
190 
191  Quaternion reference_1(0., 1., 2., 4.);
192  Quaternion quat_1 = multdof3_model.GetQuaternion(2, sphQ);
193 
194  EXPECT_EQ(reference_1.x(), quat_1.x());
195  EXPECT_EQ(reference_1.y(), quat_1.y());
196  EXPECT_EQ(reference_1.z(), quat_1.z());
197  EXPECT_EQ(reference_1.w(), quat_1.w());
198 
199  Quaternion reference_3(-6., -7., -8., -9.);
200  Quaternion quat_3 = multdof3_model.GetQuaternion(4, sphQ);
201 
202  EXPECT_EQ(reference_3.x(), quat_3.x());
203  EXPECT_EQ(reference_3.y(), quat_3.y());
204  EXPECT_EQ(reference_3.z(), quat_3.z());
205  EXPECT_EQ(reference_3.w(), quat_3.w());
206 }
207 
208 TEST_F(SphericalJointFixture, TestSetQuaternion)
209 {
210  multdof3_model.appendBody(Xtrans(Vector3d(1., 0., 0.)), joint_spherical, body);
211 
212  sphQ = VectorNd::Zero((size_t)multdof3_model.q_size);
213  sphQDot = VectorNd::Zero((size_t)multdof3_model.qdot_size);
214  sphQDDot = VectorNd::Zero((size_t)multdof3_model.qdot_size);
215  sphTau = VectorNd::Zero((size_t)multdof3_model.qdot_size);
216 
217  Quaternion reference_1(0., 1., 2., 3.);
218  multdof3_model.SetQuaternion(2, reference_1, sphQ);
219  Quaternion test = multdof3_model.GetQuaternion(2, sphQ);
220 
221  EXPECT_EQ(reference_1.x(), test.x());
222  EXPECT_EQ(reference_1.y(), test.y());
223  EXPECT_EQ(reference_1.z(), test.z());
224  EXPECT_EQ(reference_1.w(), test.w());
225 
226  Quaternion reference_2(11., 22., 33., 44.);
227  multdof3_model.SetQuaternion(4, reference_2, sphQ);
228  test = multdof3_model.GetQuaternion(4, sphQ);
229 
230  EXPECT_EQ(reference_2.x(), test.x());
231  EXPECT_EQ(reference_2.y(), test.y());
232  EXPECT_EQ(reference_2.z(), test.z());
233  EXPECT_EQ(reference_2.w(), test.w());
234 }
235 
236 TEST_F(SphericalJointFixture, TestOrientation)
237 {
238  emuQ[0] = 1.1;
239  emuQ[1] = 1.1;
240  emuQ[2] = 1.1;
241  emuQ[3] = 1.1;
242 
243  for (unsigned int i = 0; i < emuQ.size(); i++)
244  {
245  sphQ[i] = emuQ[i];
246  }
247 
248  Quaternion quat =
249  Math::toQuaternion(Vector3d(0., 0., 1.), emuQ[0]) * Math::toQuaternion(Vector3d(0., 1., 0.), emuQ[1]) * Math::toQuaternion(Vector3d(1., 0., 0.), emuQ[2]);
250  multdof3_model.SetQuaternion(2, quat, sphQ);
251 
252  updateKinematicsCustom(emulated_model, &emuQ, nullptr, nullptr);
253  updateKinematicsCustom(multdof3_model, &sphQ, nullptr, nullptr);
254 
255  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(emulated_model.bodyFrames[emu_child_id]->getInverseTransformToRoot().E,
256  multdof3_model.bodyFrames[sph_child_id]->getInverseTransformToRoot().E, TEST_PREC));
257 
258  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(emulated_model.bodyFrames[emu_child_id]->getTransformToRoot().E,
259  multdof3_model.bodyFrames[sph_child_id]->getTransformToRoot().E, TEST_PREC));
260 }
261 
262 TEST_F(SphericalJointFixture, TestUpdateKinematics)
263 {
264  emuQ[0] = 1.;
265  emuQ[1] = 1.;
266  emuQ[2] = 1.;
267  emuQ[3] = 1.;
268  emuQ[4] = 1.;
269 
270  emuQDot[0] = 1.;
271  emuQDot[1] = 1.;
272  emuQDot[2] = 1.;
273  emuQDot[3] = 1.;
274  emuQDot[4] = 1.;
275 
276  emuQDDot[0] = 1.;
277  emuQDDot[1] = 1.;
278  emuQDDot[2] = 1.;
279  emuQDDot[3] = 1.;
280  emuQDDot[4] = 1.;
281 
282  ConvertQAndQDotFromEmulated(emulated_model, emuQ, emuQDot, multdof3_model, &sphQ, &sphQDot);
283  ConvertQAndQDotFromEmulated(emulated_model, emuQ, emuQDDot, multdof3_model, &sphQ, &sphQDDot);
284 
285  Vector3d a = angular_acceleration_from_angle_rates(Vector3d(emuQ[3], emuQ[2], emuQ[1]), Vector3d(emuQDot[3], emuQDot[2], emuQDot[1]),
286  Vector3d(emuQDDot[3], emuQDDot[2], emuQDDot[1]));
287 
288  sphQDDot[0] = emuQDDot[0];
289  sphQDDot[1] = a[0];
290  sphQDDot[2] = a[1];
291  sphQDDot[3] = a[2];
292  sphQDDot[4] = emuQDDot[4];
293 
294  updateKinematicsCustom(emulated_model, &emuQ, &emuQDot, &emuQDDot);
295  updateKinematicsCustom(multdof3_model, &sphQ, &sphQDot, &sphQDDot);
296 
297  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(emulated_model.v[emu_body_id], multdof3_model.v[sph_body_id], TEST_PREC));
298  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(emulated_model.a[emu_body_id], multdof3_model.a[sph_body_id], TEST_PREC));
299 
300  updateKinematics(multdof3_model, sphQ, sphQDot, sphQDDot);
301 
302  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(emulated_model.v[emu_child_id], multdof3_model.v[sph_child_id], TEST_PREC));
303  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(emulated_model.a[emu_child_id], multdof3_model.a[sph_child_id], TEST_PREC));
304 }
305 
306 TEST_F(SphericalJointFixture, TestSpatialVelocities)
307 {
308  emuQ[0] = 1.;
309  emuQ[1] = 2.;
310  emuQ[2] = 3.;
311  emuQ[3] = 4.;
312 
313  emuQDot[0] = 4.;
314  emuQDot[1] = 2.;
315  emuQDot[2] = 3.;
316  emuQDot[3] = 6.;
317 
318  ConvertQAndQDotFromEmulated(emulated_model, emuQ, emuQDot, multdof3_model, &sphQ, &sphQDot);
319 
320  updateKinematicsCustom(emulated_model, &emuQ, &emuQDot, NULL);
321  updateKinematicsCustom(multdof3_model, &sphQ, &sphQDot, NULL);
322 
323  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(emulated_model.v[emu_child_id], multdof3_model.v[sph_child_id], TEST_PREC));
324 }
325 
326 TEST_F(SphericalJointFixture, TestForwardDynamicsQAndQDot)
327 {
328  emuQ[0] = 1.1;
329  emuQ[1] = 1.2;
330  emuQ[2] = 1.3;
331  emuQ[3] = 1.4;
332 
333  emuQDot[0] = 2.2;
334  emuQDot[1] = 2.3;
335  emuQDot[2] = 2.4;
336  emuQDot[3] = 2.5;
337 
338  ConvertQAndQDotFromEmulated(emulated_model, emuQ, emuQDot, multdof3_model, &sphQ, &sphQDot);
339 
340  forwardDynamics(emulated_model, emuQ, emuQDot, emuTau, emuQDDot);
341  forwardDynamics(multdof3_model, sphQ, sphQDot, sphTau, sphQDDot);
342 
343  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(emulated_model.a[emu_child_id], multdof3_model.a[sph_child_id], TEST_PREC));
344 }
345 
346 TEST_F(SphericalJointFixture, TestDynamicsConsistencyRNEA_ABA)
347 {
348  emuQ[0] = 1.1;
349  emuQ[1] = 1.2;
350  emuQ[2] = 1.3;
351  emuQ[3] = 1.4;
352  emuQ[4] = 1.5;
353 
354  emuQDot[0] = 1.;
355  emuQDot[1] = 2.;
356  emuQDot[2] = 3.;
357  emuQDot[3] = 4.;
358  emuQDot[4] = 5.;
359 
360  sphTau[0] = 5.;
361  sphTau[1] = 4.;
362  sphTau[2] = 7.;
363  sphTau[3] = 3.;
364  sphTau[4] = 2.;
365 
366  ConvertQAndQDotFromEmulated(emulated_model, emuQ, emuQDot, multdof3_model, &sphQ, &sphQDot);
367 
368  forwardDynamics(multdof3_model, sphQ, sphQDot, sphTau, sphQDDot);
369 
370  VectorNd tau_id(VectorNd::Zero(multdof3_model.qdot_size));
371  inverseDynamics(multdof3_model, sphQ, sphQDot, sphQDDot, tau_id);
372 
373  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(sphTau, tau_id, TEST_PREC));
374 }
375 
376 TEST_F(SphericalJointFixture, TestCRBA)
377 {
378  emuQ[0] = 1.1;
379  emuQ[1] = 1.2;
380  emuQ[2] = 1.3;
381  emuQ[3] = 1.4;
382  emuQ[4] = 1.5;
383 
384  emuQDot[0] = 1.;
385  emuQDot[1] = 2.;
386  emuQDot[2] = 3.;
387  emuQDot[3] = 4.;
388  emuQDot[4] = 5.;
389 
390  sphTau[0] = 5.;
391  sphTau[1] = 4.;
392  sphTau[2] = 7.;
393  sphTau[3] = 3.;
394  sphTau[4] = 2.;
395 
396  ConvertQAndQDotFromEmulated(emulated_model, emuQ, emuQDot, multdof3_model, &sphQ, &sphQDot);
397 
398  MatrixNd H_crba(MatrixNd::Zero(multdof3_model.qdot_size, multdof3_model.qdot_size));
399 
400  updateKinematicsCustom(multdof3_model, &sphQ, NULL, NULL);
401  compositeRigidBodyAlgorithm(multdof3_model, sphQ, H_crba, false);
402 
403  MatrixNd H_id(MatrixNd::Zero(multdof3_model.qdot_size, multdof3_model.qdot_size));
404  VectorNd H_col = VectorNd::Zero(multdof3_model.qdot_size);
405  VectorNd QDDot_zero = VectorNd::Zero(multdof3_model.qdot_size);
406 
407  for (unsigned int i = 0; i < multdof3_model.qdot_size; i++)
408  {
409  // compute each column
410  VectorNd delta_a = VectorNd::Zero(multdof3_model.qdot_size);
411  delta_a[i] = 1.;
412 
413  // compute ID (model, q, qdot, delta_a)
414  VectorNd id_delta = VectorNd::Zero(multdof3_model.qdot_size);
415  inverseDynamics(multdof3_model, sphQ, sphQDot, delta_a, id_delta);
416 
417  // compute ID (model, q, qdot, zero)
418  VectorNd id_zero = VectorNd::Zero(multdof3_model.qdot_size);
419  inverseDynamics(multdof3_model, sphQ, sphQDot, QDDot_zero, id_zero);
420 
421  H_col = id_delta - id_zero;
422  H_id.block(0, i, multdof3_model.qdot_size, 1) = H_col;
423  }
424 
425  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(H_id, H_crba, TEST_PREC));
426 }
427 
428 TEST_F(SphericalJointFixture, TestForwardDynamicsLagrangianVsABA)
429 {
430  emuQ[0] = 1.1;
431  emuQ[1] = 1.2;
432  emuQ[2] = 1.3;
433  emuQ[3] = 1.4;
434  emuQ[4] = 1.5;
435 
436  emuQDot[0] = 1.;
437  emuQDot[1] = 2.;
438  emuQDot[2] = 3.;
439  emuQDot[3] = 4.;
440  emuQDot[4] = 5.;
441 
442  sphTau[0] = 5.;
443  sphTau[1] = 4.;
444  sphTau[2] = 7.;
445  sphTau[3] = 3.;
446  sphTau[4] = 2.;
447 
448  ConvertQAndQDotFromEmulated(emulated_model, emuQ, emuQDot, multdof3_model, &sphQ, &sphQDot);
449 
450  VectorNd QDDot_aba = VectorNd::Zero(multdof3_model.qdot_size);
451  VectorNd QDDot_lag = VectorNd::Zero(multdof3_model.qdot_size);
452 
453  RobotDynamics::Math::MatrixNd H = RobotDynamics::Math::MatrixNd::Zero(multdof3_model.qdot_size, multdof3_model.qdot_size);
454  RobotDynamics::Math::VectorNd C = RobotDynamics::Math::VectorNd::Zero(multdof3_model.qdot_size);
455  forwardDynamicsLagrangian(multdof3_model, sphQ, sphQDot, sphTau, QDDot_lag, H, C);
456  forwardDynamics(multdof3_model, sphQ, sphQDot, sphTau, QDDot_aba);
457 
458  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_lag, QDDot_aba, TEST_PREC));
459 }
460 
461 TEST_F(SphericalJointFixture, TestContactsLagrangian)
462 {
463  ConstraintSet constraint_set_emu;
464 
465  constraint_set_emu.addConstraint(emu_child_id, Vector3d(0., 0., -1.), Vector3d(1., 0., 0.));
466  constraint_set_emu.addConstraint(emu_child_id, Vector3d(0., 0., -1.), Vector3d(0., 1., 0.));
467  constraint_set_emu.addConstraint(emu_child_id, Vector3d(0., 0., -1.), Vector3d(0., 0., 1.));
468 
469  constraint_set_emu.bind(emulated_model);
470 
471  ConstraintSet constraint_set_sph;
472 
473  constraint_set_sph.addConstraint(sph_child_id, Vector3d(0., 0., -1.), Vector3d(1., 0., 0.));
474  constraint_set_sph.addConstraint(sph_child_id, Vector3d(0., 0., -1.), Vector3d(0., 1., 0.));
475  constraint_set_sph.addConstraint(sph_child_id, Vector3d(0., 0., -1.), Vector3d(0., 0., 1.));
476 
477  constraint_set_sph.bind(multdof3_model);
478 
479  forwardDynamicsContactsDirect(emulated_model, emuQ, emuQDot, emuTau, constraint_set_emu, emuQDDot);
480  VectorNd emu_force_lagrangian = constraint_set_emu.force;
481  forwardDynamicsContactsDirect(multdof3_model, sphQ, sphQDot, sphTau, constraint_set_sph, sphQDDot);
482  VectorNd sph_force_lagrangian = constraint_set_sph.force;
483 
484  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(emu_force_lagrangian, sph_force_lagrangian, TEST_PREC));
485 }
486 
487 TEST_F(SphericalJointFixture, TestContacts)
488 {
489  ConstraintSet constraint_set_emu;
490 
491  constraint_set_emu.addConstraint(emu_child_id, Vector3d(0., 0., -1.), Vector3d(1., 0., 0.));
492  constraint_set_emu.addConstraint(emu_child_id, Vector3d(0., 0., -1.), Vector3d(0., 1., 0.));
493  constraint_set_emu.addConstraint(emu_child_id, Vector3d(0., 0., -1.), Vector3d(0., 0., 1.));
494 
495  constraint_set_emu.bind(emulated_model);
496 
497  ConstraintSet constraint_set_sph;
498 
499  constraint_set_sph.addConstraint(sph_child_id, Vector3d(0., 0., -1.), Vector3d(1., 0., 0.));
500  constraint_set_sph.addConstraint(sph_child_id, Vector3d(0., 0., -1.), Vector3d(0., 1., 0.));
501  constraint_set_sph.addConstraint(sph_child_id, Vector3d(0., 0., -1.), Vector3d(0., 0., 1.));
502 
503  constraint_set_sph.bind(multdof3_model);
504 
505  forwardDynamicsContactsKokkevis(emulated_model, emuQ, emuQDot, emuTau, constraint_set_emu, emuQDDot);
506  VectorNd emu_force_kokkevis = constraint_set_emu.force;
507  forwardDynamicsContactsKokkevis(multdof3_model, sphQ, sphQDot, sphTau, constraint_set_sph, sphQDDot);
508  VectorNd sph_force_kokkevis = constraint_set_sph.force;
509 
510  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(emu_force_kokkevis, sph_force_kokkevis, TEST_PREC));
511 }
512 
513 TEST_F(SphericalJointFixture, TestEulerZYXvsEmulatedLagrangian)
514 {
515  emuQ[0] = 1.1;
516  emuQ[1] = 1.2;
517  emuQ[2] = 1.3;
518  emuQ[3] = 1.4;
519  emuQ[4] = 1.5;
520 
521  emuQDot[0] = 1.;
522  emuQDot[1] = 2.;
523  emuQDot[2] = 3.;
524  emuQDot[3] = 4.;
525  emuQDot[4] = 5.;
526 
527  emuTau[0] = 5.;
528  emuTau[1] = 4.;
529  emuTau[2] = 7.;
530  emuTau[3] = 3.;
531  emuTau[4] = 2.;
532 
533  VectorNd QDDot_emu = VectorNd::Zero(emulated_model.qdot_size);
534  VectorNd QDDot_eulerzyx = VectorNd::Zero(eulerzyx_model.qdot_size);
535 
536  RobotDynamics::Math::MatrixNd H = RobotDynamics::Math::MatrixNd::Zero(emulated_model.qdot_size, emulated_model.qdot_size);
537  RobotDynamics::Math::VectorNd C = RobotDynamics::Math::VectorNd::Zero(emulated_model.qdot_size);
538  forwardDynamicsLagrangian(emulated_model, emuQ, emuQDot, emuTau, QDDot_emu, H, C);
539 
540  H = RobotDynamics::Math::MatrixNd::Zero(eulerzyx_model.qdot_size, eulerzyx_model.qdot_size);
541  C = RobotDynamics::Math::VectorNd::Zero(eulerzyx_model.qdot_size);
542  forwardDynamicsLagrangian(eulerzyx_model, emuQ, emuQDot, emuTau, QDDot_eulerzyx, H, C);
543 
544  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_emu, QDDot_eulerzyx, TEST_PREC));
545 }
546 
547 TEST_F(SphericalJointFixture, TestEulerZYXvsEmulatedArticulatedBodyAlgorithm)
548 {
549  emuQ[0] = 1.1;
550  emuQ[1] = 1.2;
551  emuQ[2] = 1.3;
552  emuQ[3] = 1.4;
553  emuQ[4] = 1.5;
554 
555  emuQDot[0] = 1.;
556  emuQDot[1] = 2.;
557  emuQDot[2] = 3.;
558  emuQDot[3] = 4.;
559  emuQDot[4] = 5.;
560 
561  emuTau[0] = 5.;
562  emuTau[1] = 4.;
563  emuTau[2] = 7.;
564  emuTau[3] = 3.;
565  emuTau[4] = 2.;
566 
567  VectorNd QDDot_emu = VectorNd::Zero(emulated_model.qdot_size);
568  VectorNd QDDot_eulerzyx = VectorNd::Zero(eulerzyx_model.qdot_size);
569 
570  forwardDynamics(emulated_model, emuQ, emuQDot, emuTau, QDDot_emu);
571  forwardDynamics(eulerzyx_model, emuQ, emuQDot, emuTau, QDDot_eulerzyx);
572 
573  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_emu, QDDot_eulerzyx, TEST_PREC));
574 }
575 
576 TEST_F(SphericalJointFixture, TestEulerZYXvsEmulatedContacts)
577 {
578  emuQ[0] = 1.1;
579  emuQ[1] = 1.2;
580  emuQ[2] = 1.3;
581  emuQ[3] = 1.4;
582  emuQ[4] = 1.5;
583 
584  emuQDot[0] = 1.;
585  emuQDot[1] = 2.;
586  emuQDot[2] = 3.;
587  emuQDot[3] = 4.;
588  emuQDot[4] = 5.;
589 
590  emuTau[0] = 5.;
591  emuTau[1] = 4.;
592  emuTau[2] = 7.;
593  emuTau[3] = 3.;
594  emuTau[4] = 2.;
595 
596  VectorNd QDDot_emu = VectorNd::Zero(emulated_model.qdot_size);
597  VectorNd QDDot_eulerzyx = VectorNd::Zero(eulerzyx_model.qdot_size);
598 
599  ConstraintSet CS_euler;
600  CS_euler.addConstraint(eulerzyx_child_id, Vector3d(1., 1., 1.), Vector3d(1., 0., 0.));
601  CS_euler.addConstraint(eulerzyx_child_id, Vector3d(0., 0., 0.), Vector3d(0., 1., 0.));
602  CS_euler.addConstraint(eulerzyx_child_id, Vector3d(0., 0., 0.), Vector3d(0., 0., 1.));
603  CS_euler.bind(eulerzyx_model);
604 
605  ConstraintSet CS_emulated;
606  CS_emulated.addConstraint(emu_child_id, Vector3d(1., 1., 1.), Vector3d(1., 0., 0.));
607  CS_emulated.addConstraint(emu_child_id, Vector3d(0., 0., 0.), Vector3d(0., 1., 0.));
608  CS_emulated.addConstraint(emu_child_id, Vector3d(0., 0., 0.), Vector3d(0., 0., 1.));
609  CS_emulated.bind(emulated_model);
610 
611  forwardDynamicsContactsDirect(emulated_model, emuQ, emuQDot, emuTau, CS_emulated, QDDot_emu);
612  forwardDynamicsContactsDirect(eulerzyx_model, emuQ, emuQDot, emuTau, CS_euler, QDDot_eulerzyx);
613 
614  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_emu, QDDot_eulerzyx, TEST_PREC));
615 
616  forwardDynamicsContactsKokkevis(emulated_model, emuQ, emuQDot, emuTau, CS_emulated, QDDot_emu);
617  forwardDynamicsContactsKokkevis(eulerzyx_model, emuQ, emuQDot, emuTau, CS_euler, QDDot_eulerzyx);
618 
619  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_emu, QDDot_eulerzyx, TEST_PREC * QDDot_emu.norm()));
620 
621  forwardDynamicsContactsKokkevis(emulated_model, emuQ, emuQDot, emuTau, CS_emulated, QDDot_emu);
622  forwardDynamicsContactsDirect(eulerzyx_model, emuQ, emuQDot, emuTau, CS_euler, QDDot_eulerzyx);
623 
624  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_emu, QDDot_eulerzyx, TEST_PREC * QDDot_emu.norm()));
625 }
626 
627 TEST_F(SphericalJointFixture, TestEulerZYXvsEmulatedCRBA)
628 {
629  emuQ[0] = 1.1;
630  emuQ[1] = 1.2;
631  emuQ[2] = 1.3;
632  emuQ[3] = 1.4;
633  emuQ[4] = 1.5;
634 
635  MatrixNd H_emulated(MatrixNd::Zero(emulated_model.q_size, emulated_model.q_size));
636  MatrixNd H_eulerzyx(MatrixNd::Zero(eulerzyx_model.q_size, eulerzyx_model.q_size));
637 
638  compositeRigidBodyAlgorithm(emulated_model, emuQ, H_emulated);
639  compositeRigidBodyAlgorithm(eulerzyx_model, emuQ, H_eulerzyx);
640 
641  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(H_emulated, H_eulerzyx, TEST_PREC));
642 }
643 
644 TEST_F(SphericalJointFixture, TestJointTypeTranslationZYX)
645 {
646  Model model_emulated;
647  Model model_3dof;
648 
649  Body body(1., Vector3d(1., 2., 1.), Matrix3d(1., 0., 0, 0., 1., 0., 0., 0., 1.));
650  Joint joint_emulated(SpatialVector(0., 0., 0., 1., 0., 0.), SpatialVector(0., 0., 0., 0., 1., 0.), SpatialVector(0., 0., 0., 0., 0., 1.));
651  Joint joint_3dof(JointTypeTranslationXYZ);
652 
653  model_emulated.appendBody(SpatialTransform(), joint_emulated, body);
654  model_3dof.appendBody(SpatialTransform(), joint_3dof, body);
655 
656  VectorNd q(VectorNd::Zero(model_emulated.q_size));
657  VectorNd qdot(VectorNd::Zero(model_emulated.qdot_size));
658  VectorNd qddot_emulated(VectorNd::Zero(model_emulated.qdot_size));
659  VectorNd qddot_3dof(VectorNd::Zero(model_emulated.qdot_size));
660  VectorNd tau(VectorNd::Zero(model_emulated.qdot_size));
661 
662  for (int i = 0; i < q.size(); i++)
663  {
664  q[i] = 1.1 * (static_cast<double>(i + 1));
665  qdot[i] = 0.1 * (static_cast<double>(i + 1));
666  qddot_3dof[i] = 0.21 * (static_cast<double>(i + 1));
667  tau[i] = 2.1 * (static_cast<double>(i + 1));
668  }
669 
670  qddot_emulated = qddot_3dof;
671  VectorNd id_emulated(tau);
672  VectorNd id_3dof(tau);
673  inverseDynamics(model_emulated, q, qdot, qddot_emulated, id_emulated);
674  inverseDynamics(model_3dof, q, qdot, qddot_emulated, id_3dof);
675  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(id_emulated, id_3dof, TEST_PREC * id_emulated.norm()));
676 
677  RobotDynamics::Math::MatrixNd H = RobotDynamics::Math::MatrixNd::Zero(model_emulated.qdot_size, model_emulated.qdot_size);
678  RobotDynamics::Math::VectorNd C = RobotDynamics::Math::VectorNd::Zero(model_emulated.qdot_size);
679  forwardDynamicsLagrangian(model_emulated, q, qdot, tau, qddot_emulated, H, C);
680 
681  H = RobotDynamics::Math::MatrixNd::Zero(model_3dof.qdot_size, model_3dof.qdot_size);
682  C = RobotDynamics::Math::VectorNd::Zero(model_3dof.qdot_size);
683  forwardDynamicsLagrangian(model_3dof, q, qdot, tau, qddot_3dof, H, C);
684 
685  EXPECT_TRUE(unit_test_utils::checkVector3dEq(qddot_emulated, qddot_3dof));
686 
687  MatrixNd H_emulated(MatrixNd::Zero(q.size(), q.size()));
688  MatrixNd H_3dof(MatrixNd::Zero(q.size(), q.size()));
689 
690  compositeRigidBodyAlgorithm(model_emulated, q, H_emulated);
691  compositeRigidBodyAlgorithm(model_3dof, q, H_3dof);
692 
693  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(H_emulated, H_3dof, TEST_PREC));
694 }
695 
696 TEST_F(SphericalJointFixture, TestJointTypeEulerXYZ)
697 {
698  Model model_emulated;
699  Model model_3dof;
700 
701  Body body(1., Vector3d(1., 2., 1.), Matrix3d(1., 0., 0, 0., 1., 0., 0., 0., 1.));
702  Joint joint_emulated(SpatialVector(1., 0., 0., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(0., 0., 1., 0., 0., 0.));
703  Joint joint_3dof(JointTypeEulerXYZ);
704 
705  model_emulated.appendBody(SpatialTransform(), joint_emulated, body);
706  model_3dof.appendBody(SpatialTransform(), joint_3dof, body);
707 
708  VectorNd q(VectorNd::Zero(model_emulated.q_size));
709  VectorNd qdot(VectorNd::Zero(model_emulated.qdot_size));
710  VectorNd qddot_emulated(VectorNd::Zero(model_emulated.qdot_size));
711  VectorNd qddot_3dof(VectorNd::Zero(model_emulated.qdot_size));
712  VectorNd tau(VectorNd::Zero(model_emulated.qdot_size));
713 
714  for (int i = 0; i < q.size(); i++)
715  {
716  q[i] = 1.1 * (static_cast<double>(i + 1));
717  qdot[i] = 0.55 * (static_cast<double>(i + 1));
718  qddot_emulated[i] = 0.23 * (static_cast<double>(i + 1));
719  qddot_3dof[i] = 0.22 * (static_cast<double>(i + 1));
720  tau[i] = 2.1 * (static_cast<double>(i + 1));
721  }
722 
723  updateKinematicsCustom(model_emulated, &q, &qdot, &qddot_emulated);
724  updateKinematicsCustom(model_3dof, &q, &qdot, &qddot_emulated);
725 
726  EXPECT_TRUE(unit_test_utils::checkMatrix3dEq(model_emulated.bodyFrames[3]->getTransformToRoot().E, model_3dof.bodyFrames[1]->getTransformToRoot().E));
727  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEq(model_emulated.v[3], model_3dof.v[1]));
728 
729  RobotDynamics::Math::MatrixNd H = RobotDynamics::Math::MatrixNd::Zero(model_emulated.qdot_size, model_emulated.qdot_size);
730  RobotDynamics::Math::VectorNd C = RobotDynamics::Math::VectorNd::Zero(model_emulated.qdot_size);
731  forwardDynamicsLagrangian(model_emulated, q, qdot, tau, qddot_emulated, H, C);
732 
733  H = RobotDynamics::Math::MatrixNd::Zero(model_3dof.qdot_size, model_3dof.qdot_size);
734  C = RobotDynamics::Math::VectorNd::Zero(model_3dof.qdot_size);
735  forwardDynamicsLagrangian(model_3dof, q, qdot, tau, qddot_3dof, H, C);
736 
737  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_emulated, qddot_3dof, TEST_PREC));
738 
739  MatrixNd H_emulated(MatrixNd::Zero(q.size(), q.size()));
740  MatrixNd H_3dof(MatrixNd::Zero(q.size(), q.size()));
741 
742  compositeRigidBodyAlgorithm(model_emulated, q, H_emulated);
743  compositeRigidBodyAlgorithm(model_3dof, q, H_3dof);
744 
745  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(H_emulated, H_3dof, TEST_PREC));
746 }
747 
748 TEST_F(SphericalJointFixture, TestJointTypeEulerYXZ)
749 {
750  Model model_emulated;
751  Model model_3dof;
752 
753  Body body(1., Vector3d(1., 2., 1.), Matrix3d(1., 0., 0, 0., 1., 0., 0., 0., 1.));
754  Joint joint_emulated(SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.), SpatialVector(0., 0., 1., 0., 0., 0.));
755  Joint joint_3dof(JointTypeEulerYXZ);
756 
757  model_emulated.appendBody(SpatialTransform(), joint_emulated, body);
758  model_3dof.appendBody(SpatialTransform(), joint_3dof, body);
759 
760  VectorNd q(VectorNd::Zero(model_emulated.q_size));
761  VectorNd qdot(VectorNd::Zero(model_emulated.qdot_size));
762  VectorNd qddot_emulated(VectorNd::Zero(model_emulated.qdot_size));
763  VectorNd qddot_3dof(VectorNd::Zero(model_emulated.qdot_size));
764  VectorNd tau(VectorNd::Zero(model_emulated.qdot_size));
765 
766  for (int i = 0; i < q.size(); i++)
767  {
768  q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
769  qdot[i] = 0.5 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
770  tau[i] = 0.5 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
771  qddot_3dof[i] = 0.5 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
772  }
773 
774  qddot_emulated = qddot_3dof;
775 
776  updateKinematicsCustom(model_emulated, &q, &qdot, &qddot_emulated);
777  updateKinematicsCustom(model_3dof, &q, &qdot, &qddot_emulated);
778 
779  EXPECT_TRUE(
780  unit_test_utils::checkMatrix3dEpsilonClose(model_emulated.bodyFrames[3]->getTransformToRoot().E, model_3dof.bodyFrames[1]->getTransformToRoot().E, TEST_PREC));
781  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(model_emulated.v[3], model_3dof.v[1], TEST_PREC));
782 
783  VectorNd id_emulated(tau);
784  VectorNd id_3dof(tau);
785  inverseDynamics(model_emulated, q, qdot, qddot_emulated, id_emulated);
786  inverseDynamics(model_3dof, q, qdot, qddot_emulated, id_3dof);
787 
788  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(id_emulated, id_3dof, TEST_PREC));
789 
790  RobotDynamics::Math::MatrixNd H = RobotDynamics::Math::MatrixNd::Zero(model_emulated.qdot_size, model_emulated.qdot_size);
791  RobotDynamics::Math::VectorNd C = RobotDynamics::Math::VectorNd::Zero(model_emulated.qdot_size);
792  forwardDynamicsLagrangian(model_emulated, q, qdot, tau, qddot_emulated, H, C);
793 
794  H = RobotDynamics::Math::MatrixNd::Zero(model_3dof.qdot_size, model_3dof.qdot_size);
795  C = RobotDynamics::Math::VectorNd::Zero(model_3dof.qdot_size);
796  forwardDynamicsLagrangian(model_3dof, q, qdot, tau, qddot_3dof, H, C);
797 
798  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_emulated, qddot_3dof, TEST_PREC));
799 
800  MatrixNd H_emulated(MatrixNd::Zero(q.size(), q.size()));
801  MatrixNd H_3dof(MatrixNd::Zero(q.size(), q.size()));
802 
803  compositeRigidBodyAlgorithm(model_emulated, q, H_emulated);
804  compositeRigidBodyAlgorithm(model_3dof, q, H_3dof);
805 
806  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(H_emulated, H_3dof, TEST_PREC));
807 }
808 
809 TEST_F(Human36, TestUpdateKinematics)
810 {
811  randomizeStates();
812 
813  VectorNd id_emulated(tau);
814  VectorNd id_3dof(tau);
815 
816  updateKinematics(*model_emulated, q, qdot, qddot);
817  updateKinematics(*model_3dof, q, qdot, qddot);
818 
819  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(model_emulated->v[body_id_emulated[BodyPelvis]], model_3dof->v[body_id_3dof[BodyPelvis]], TEST_PREC));
820  EXPECT_TRUE(
821  unit_test_utils::checkSpatialVectorsEpsilonClose(model_emulated->v[body_id_emulated[BodyThighRight]], model_3dof->v[body_id_3dof[BodyThighRight]], TEST_PREC));
822  EXPECT_TRUE(
823  unit_test_utils::checkSpatialVectorsEpsilonClose(model_emulated->v[body_id_emulated[BodyShankRight]], model_3dof->v[body_id_3dof[BodyShankRight]], TEST_PREC));
824 }
825 
826 TEST_F(Human36, TestInverseDynamics)
827 {
828  randomizeStates();
829 
830  VectorNd id_emulated(tau);
831  VectorNd id_3dof(tau);
832 
833  inverseDynamics(*model_emulated, q, qdot, qddot, id_emulated);
834  inverseDynamics(*model_3dof, q, qdot, qddot, id_3dof);
835 
836  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(id_emulated, id_3dof, TEST_PREC * id_emulated.norm()));
837 }
838 
839 TEST_F(Human36, TestNonlinearEffects)
840 {
841  randomizeStates();
842 
843  VectorNd nle_emulated(tau);
844  VectorNd nle_3dof(tau);
845 
846  nonlinearEffects(*model_emulated, q, qdot, nle_emulated);
847  nonlinearEffects(*model_3dof, q, qdot, nle_3dof);
848 
849  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(nle_emulated, nle_3dof, TEST_PREC * nle_emulated.norm()));
850 }
851 
852 TEST_F(Human36, TestContactsEmulatedLagrangianKokkevis)
853 {
854  randomizeStates();
855 
856  VectorNd qddot_lagrangian(qddot_emulated);
857  VectorNd qddot_kokkevis(qddot_emulated);
858 
859  forwardDynamicsContactsDirect(*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_lagrangian);
860  forwardDynamicsContactsKokkevis(*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_kokkevis);
861  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_lagrangian, qddot_kokkevis, TEST_PREC * qddot_lagrangian.norm()));
862 
863  forwardDynamicsContactsDirect(*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_lagrangian);
864  forwardDynamicsContactsKokkevis(*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_kokkevis);
865  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_lagrangian, qddot_kokkevis, TEST_PREC * qddot_lagrangian.norm()));
866 
867  forwardDynamicsContactsDirect(*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_lagrangian);
868  forwardDynamicsContactsKokkevis(*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_kokkevis);
869 
870  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_lagrangian, qddot_kokkevis, 1000 * TEST_PREC * qddot_lagrangian.norm()));
871 }
872 
873 TEST_F(Human36, TestContactsEmulatedLagrangianSparse)
874 {
875  randomizeStates();
876 
877  VectorNd qddot_lagrangian(qddot_emulated);
878  VectorNd qddot_sparse(qddot_emulated);
879 
880  forwardDynamicsContactsDirect(*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_lagrangian);
881  forwardDynamicsContactsRangeSpaceSparse(*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_sparse);
882  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_lagrangian, qddot_sparse, TEST_PREC * qddot_lagrangian.norm()));
883 
884  forwardDynamicsContactsDirect(*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_lagrangian);
885  forwardDynamicsContactsRangeSpaceSparse(*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_sparse);
886  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_lagrangian, qddot_sparse, TEST_PREC * qddot_lagrangian.norm()));
887 
888  forwardDynamicsContactsDirect(*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_lagrangian);
889  forwardDynamicsContactsRangeSpaceSparse(*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_sparse);
890  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_lagrangian, qddot_sparse, TEST_PREC * qddot_lagrangian.norm()));
891 }
892 
893 TEST_F(Human36, TestContactsEmulatedLagrangianNullSpaceLinearSolverPartialPivLU)
894 {
895  randomizeStates();
896 
897  VectorNd qddot_lagrangian(qddot_emulated);
898  VectorNd qddot_nullspace(qddot_emulated);
899 
900  constraints_1B1C_emulated.linear_solver = RobotDynamics::Math::LinearSolver::LinearSolverPartialPivLU;
901 
902  forwardDynamicsContactsDirect(*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_lagrangian);
903  forwardDynamicsContactsNullSpace(*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_nullspace);
904  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_lagrangian, qddot_nullspace, TEST_PREC * qddot_lagrangian.norm()));
905 
906  constraints_1B4C_emulated.linear_solver = RobotDynamics::Math::LinearSolver::LinearSolverPartialPivLU;
907 
908  forwardDynamicsContactsDirect(*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_lagrangian);
909  forwardDynamicsContactsNullSpace(*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_nullspace);
910  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_lagrangian, qddot_nullspace, TEST_PREC * qddot_lagrangian.norm()));
911 
912  constraints_4B4C_emulated.linear_solver = RobotDynamics::Math::LinearSolver::LinearSolverPartialPivLU;
913 
914  forwardDynamicsContactsDirect(*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_lagrangian);
915  forwardDynamicsContactsNullSpace(*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_nullspace);
916  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_lagrangian, qddot_nullspace, TEST_PREC * qddot_lagrangian.norm()));
917 }
918 
919 TEST_F(Human36, TestContactsEmulatedLagrangianNullSpaceLinearSolverHouseholderQR)
920 {
921  randomizeStates();
922 
923  VectorNd qddot_lagrangian(qddot_emulated);
924  VectorNd qddot_nullspace(qddot_emulated);
925 
926  constraints_1B1C_emulated.linear_solver = RobotDynamics::Math::LinearSolver::LinearSolverHouseholderQR;
927 
928  forwardDynamicsContactsDirect(*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_lagrangian);
929  forwardDynamicsContactsNullSpace(*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_nullspace);
930  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_lagrangian, qddot_nullspace, TEST_PREC * qddot_lagrangian.norm()));
931 
932  constraints_1B4C_emulated.linear_solver = RobotDynamics::Math::LinearSolver::LinearSolverHouseholderQR;
933 
934  forwardDynamicsContactsDirect(*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_lagrangian);
935  forwardDynamicsContactsNullSpace(*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_nullspace);
936  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_lagrangian, qddot_nullspace, TEST_PREC * qddot_lagrangian.norm()));
937 
938  constraints_4B4C_emulated.linear_solver = RobotDynamics::Math::LinearSolver::LinearSolverHouseholderQR;
939 
940  forwardDynamicsContactsDirect(*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_lagrangian);
941  forwardDynamicsContactsNullSpace(*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_nullspace);
942  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_lagrangian, qddot_nullspace, TEST_PREC * qddot_lagrangian.norm()));
943 }
944 
945 TEST_F(Human36, TestContactsEmulatedLagrangianNullSpace)
946 {
947  randomizeStates();
948 
949  VectorNd qddot_lagrangian(qddot_emulated);
950  VectorNd qddot_nullspace(qddot_emulated);
951 
952  forwardDynamicsContactsDirect(*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_lagrangian);
953  forwardDynamicsContactsNullSpace(*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_nullspace);
954  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_lagrangian, qddot_nullspace, TEST_PREC * qddot_lagrangian.norm()));
955 
956  forwardDynamicsContactsDirect(*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_lagrangian);
957  forwardDynamicsContactsNullSpace(*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_nullspace);
958  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_lagrangian, qddot_nullspace, TEST_PREC * qddot_lagrangian.norm()));
959 
960  forwardDynamicsContactsDirect(*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_lagrangian);
961  forwardDynamicsContactsNullSpace(*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_nullspace);
962  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_lagrangian, qddot_nullspace, TEST_PREC * qddot_lagrangian.norm()));
963 }
964 
965 TEST_F(Human36, TestContactsEmulatedMultdofLagrangian)
966 {
967  randomizeStates();
968 
969  forwardDynamicsContactsDirect(*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_emulated);
970  forwardDynamicsContactsDirect(*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_3dof);
971  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_emulated, qddot_3dof, TEST_PREC * qddot_emulated.norm()));
972 
973  forwardDynamicsContactsDirect(*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_emulated);
974  forwardDynamicsContactsDirect(*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_3dof);
975  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_emulated, qddot_3dof, TEST_PREC * qddot_emulated.norm()));
976 
977  forwardDynamicsContactsDirect(*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_emulated);
978  forwardDynamicsContactsDirect(*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_3dof);
979  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_emulated, qddot_3dof, TEST_PREC * qddot_emulated.norm()));
980 }
981 
982 TEST_F(Human36, TestContactsEmulatedMultdofSparse)
983 {
984  for (unsigned int i = 0; i < q.size(); i++)
985  {
986  q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
987  qdot[i] = 0.5 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
988  tau[i] = 0.5 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
989  }
990 
991  forwardDynamicsContactsRangeSpaceSparse(*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_emulated);
992 
993  for (unsigned int i = 0; i < q.size(); i++)
994  {
995  EXPECT_EQ(model_emulated->lambda_q[i], model_3dof->lambda_q[i]);
996  }
997 
998  forwardDynamicsContactsRangeSpaceSparse(*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_3dof);
999  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_emulated, qddot_3dof, TEST_PREC * qddot_emulated.norm()));
1000 
1001  forwardDynamicsContactsRangeSpaceSparse(*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_emulated);
1002  forwardDynamicsContactsRangeSpaceSparse(*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_3dof);
1003  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_emulated, qddot_3dof, TEST_PREC * qddot_emulated.norm()));
1004 
1005  forwardDynamicsContactsRangeSpaceSparse(*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_emulated);
1006  forwardDynamicsContactsRangeSpaceSparse(*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_3dof);
1007  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_emulated, qddot_3dof, TEST_PREC * qddot_emulated.norm()));
1008 }
1009 
1010 TEST_F(Human36, TestContactsEmulatedMultdofKokkevisSparse)
1011 {
1012  randomizeStates();
1013 
1014  forwardDynamicsContactsRangeSpaceSparse(*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_emulated);
1015 
1016  for (unsigned int i = 0; i < q.size(); i++)
1017  {
1018  EXPECT_EQ(model_emulated->lambda_q[i], model_3dof->lambda_q[i]);
1019  }
1020 
1021  VectorNd qddot_sparse(qddot_emulated);
1022  VectorNd qddot_kokkevis(qddot_emulated);
1023 
1024  forwardDynamicsContactsRangeSpaceSparse(*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_sparse);
1025  forwardDynamicsContactsKokkevis(*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_kokkevis);
1026  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_sparse, qddot_kokkevis, TEST_PREC * qddot_sparse.norm()));
1027 
1028  forwardDynamicsContactsRangeSpaceSparse(*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_sparse);
1029  forwardDynamicsContactsKokkevis(*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_kokkevis);
1030  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_sparse, qddot_kokkevis, TEST_PREC * qddot_sparse.norm()));
1031 
1032  forwardDynamicsContactsRangeSpaceSparse(*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_sparse);
1033  forwardDynamicsContactsKokkevis(*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_kokkevis);
1034  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_sparse, qddot_kokkevis, 1000 * TEST_PREC * qddot_sparse.norm()));
1035 }
1036 
1037 TEST_F(Human36, TestContactsEmulatedMultdofKokkevisMultiple)
1038 {
1039  randomizeStates();
1040 
1041  VectorNd qddot_kokkevis(qddot_emulated);
1042  VectorNd qddot_kokkevis_2(qddot_emulated);
1043 
1044  forwardDynamicsContactsKokkevis(*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_kokkevis);
1045  forwardDynamicsContactsKokkevis(*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_kokkevis_2);
1046  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_kokkevis, qddot_kokkevis_2, TEST_PREC * qddot_kokkevis.norm()));
1047 
1048  forwardDynamicsContactsKokkevis(*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_kokkevis);
1049  forwardDynamicsContactsKokkevis(*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_kokkevis_2);
1050  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_kokkevis, qddot_kokkevis_2, TEST_PREC * qddot_kokkevis.norm()));
1051 
1052  forwardDynamicsContactsKokkevis(*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_kokkevis);
1053  forwardDynamicsContactsKokkevis(*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_kokkevis_2);
1054  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_kokkevis, qddot_kokkevis_2, TEST_PREC * qddot_kokkevis.norm()));
1055 }
1056 
1057 TEST_F(SphericalJointFixture, TestEulerZYXvsEmulated)
1058 {
1059  emuQ[0] = 1.1;
1060  emuQ[1] = 1.2;
1061  emuQ[2] = 1.3;
1062  emuQ[3] = 1.4;
1063  emuQ[4] = 1.5;
1064 
1065  emuQDot[0] = 1.;
1066  emuQDot[1] = 2.;
1067  emuQDot[2] = 3.;
1068  emuQDot[3] = 4.;
1069  emuQDot[4] = 5.;
1070 
1071  emuTau[0] = 5.;
1072  emuTau[1] = 4.;
1073  emuTau[2] = 7.;
1074  emuTau[3] = 3.;
1075  emuTau[4] = 2.;
1076 
1077  VectorNd QDDot_emu = VectorNd::Zero(emulated_model.qdot_size);
1078  VectorNd QDDot_eulerzyx = VectorNd::Zero(eulerzyx_model.qdot_size);
1079 
1080  RobotDynamics::Math::MatrixNd H = RobotDynamics::Math::MatrixNd::Zero(emulated_model.qdot_size, emulated_model.qdot_size);
1081  RobotDynamics::Math::VectorNd C = RobotDynamics::Math::VectorNd::Zero(emulated_model.qdot_size);
1082  forwardDynamicsLagrangian(emulated_model, emuQ, emuQDot, emuTau, QDDot_emu, H, C);
1083 
1084  H = RobotDynamics::Math::MatrixNd::Zero(eulerzyx_model.qdot_size, eulerzyx_model.qdot_size);
1085  C = RobotDynamics::Math::VectorNd::Zero(eulerzyx_model.qdot_size);
1086  forwardDynamicsLagrangian(eulerzyx_model, emuQ, emuQDot, emuTau, QDDot_eulerzyx, H, C);
1087 
1088  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_emu, QDDot_eulerzyx, TEST_PREC));
1089 }
1090 
1091 TEST_F(Human36, SolveMInvTimesTau)
1092 {
1093  for (unsigned int i = 0; i < model->dof_count; i++)
1094  {
1095  q[i] = rand() / static_cast<double>(RAND_MAX);
1096  tau[i] = rand() / static_cast<double>(RAND_MAX);
1097  }
1098 
1099  MatrixNd M(MatrixNd::Zero(model->dof_count, model->dof_count));
1100  compositeRigidBodyAlgorithm(*model, q, M);
1101 
1102  VectorNd qddot_solve_llt = M.llt().solve(tau);
1103 
1104  VectorNd qddot_minv(q);
1105  calcMInvTimesTau(*model, q, tau, qddot_minv);
1106 
1107  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_solve_llt, qddot_minv, TEST_PREC * qddot_minv.norm()));
1108 }
1109 
1110 TEST_F(Human36, SolveMInvTimesTauReuse)
1111 {
1112  for (unsigned int i = 0; i < model->dof_count; i++)
1113  {
1114  q[i] = rand() / static_cast<double>(RAND_MAX);
1115  tau[i] = rand() / static_cast<double>(RAND_MAX);
1116  }
1117 
1118  MatrixNd M(MatrixNd::Zero(model->dof_count, model->dof_count));
1119  compositeRigidBodyAlgorithm(*model, q, M);
1120 
1121  VectorNd qddot_solve_llt = M.llt().solve(tau);
1122 
1123  VectorNd qddot_minv(q);
1124  calcMInvTimesTau(*model, q, tau, qddot_minv);
1125 
1126  for (unsigned int j = 0; j < 1; j++)
1127  {
1128  for (unsigned int i = 0; i < model->dof_count; i++)
1129  {
1130  tau[i] = rand() / static_cast<double>(RAND_MAX);
1131  }
1132 
1133  compositeRigidBodyAlgorithm(*model, q, M);
1134  qddot_solve_llt = M.llt().solve(tau);
1135 
1136  calcMInvTimesTau(*model, q, tau, qddot_minv);
1137 
1138  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_solve_llt, qddot_minv, TEST_PREC * qddot_solve_llt.norm()));
1139  }
1140 }
1141 
1142 int main(int argc, char** argv)
1143 {
1144  ::testing::InitGoogleTest(&argc, argv);
1145  return RUN_ALL_TESTS();
1146 }
unit_test_utils::checkVectorNdEpsilonClose
static bool checkVectorNdEpsilonClose(const RobotDynamics::Math::VectorNd &v1, const RobotDynamics::Math::VectorNd &v2, const double epsilon)
Definition: UnitTestUtils.hpp:288
RobotDynamics::compositeRigidBodyAlgorithm
void compositeRigidBodyAlgorithm(Model &model, const Math::VectorNd &Q, Math::MatrixNd &H, bool update_kinematics=true)
Computes the joint space inertia matrix by using the Composite Rigid Body Algorithm.
Definition: Dynamics.cc:302
RobotDynamics::Model::mJoints
std::vector< Joint > mJoints
All joints.
Definition: Model.h:209
RobotDynamics::Math::Quaternion::w
EIGEN_STRONG_INLINE double & w()
Definition: Quaternion.h:106
RobotDynamics::JointTypeEulerZYX
3 DoF joint that uses Euler ZYX convention (faster than emulated
Definition: Joint.h:197
RobotDynamics::Body
Describes all properties of a single body.
Definition: Body.h:30
RobotDynamics::Math::Quaternion::timeStep
Quaternion timeStep(const Vector3d &omega, double dt)
Definition: Quaternion.h:290
RobotDynamics::Math::angular_velocity_from_angle_rates
Vector3d angular_velocity_from_angle_rates(const Vector3d &zyx_angles, const Vector3d &zyx_angle_rates)
Definition: rdl_mathutils.h:204
RobotDynamics::Math::global_angular_velocity_from_rates
Vector3d global_angular_velocity_from_rates(const Vector3d &zyx_angles, const Vector3d &zyx_rates)
Definition: rdl_mathutils.h:215
RobotDynamics::Model::SetQuaternion
void SetQuaternion(unsigned int i, const Math::Quaternion &quat, Math::VectorNd &Q) const
Sets the quaternion for body i (only valid if body i is connected by a JointTypeSpherical joint) ...
Definition: Model.h:609
RobotDynamics::Math::Xtrans
SpatialTransform Xtrans(const Vector3d &r)
Get pure translation transform.
Definition: SpatialAlgebraOperators.h:369
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
RobotDynamics::Math::toQuaternion
static Quaternion toQuaternion(const Vector3d &axis, double angle_rad)
Get quaternion representation of axis and angle.
Definition: SpatialAlgebraOperators.h:63
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::forwardDynamicsContactsRangeSpaceSparse
void forwardDynamicsContactsRangeSpaceSparse(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDot)
Definition: Contacts.cc:362
unit_test_utils::checkMatrix3dEq
static bool checkMatrix3dEq(const RobotDynamics::Math::Matrix3d &t1, const RobotDynamics::Math::Matrix3d &t2)
Definition: UnitTestUtils.hpp:196
RobotDynamics::Model::v
Math::SpatialMotionV v
Definition: Model.h:202
unit_test_utils::checkVector3dEq
static bool checkVector3dEq(const RobotDynamics::Math::Vector3d &v1, const RobotDynamics::Math::Vector3d &v2)
Definition: UnitTestUtils.hpp:212
RobotDynamics::forwardDynamicsContactsDirect
void forwardDynamicsContactsDirect(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDot)
Computes forward dynamics with contact by constructing and solving the full lagrangian equation...
Definition: Contacts.cc:343
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::ConstraintSet
Structure that contains both constraint information and workspace memory.
Definition: Contacts.h:168
RobotDynamics::Math::Quaternion::y
EIGEN_STRONG_INLINE double & y()
Definition: Quaternion.h:86
TEST_PREC
const double TEST_PREC
Definition: RdlMultiDofTests.cpp:14
RobotDynamics::Joint
Describes a joint relative to the predecessor body.
Definition: Joint.h:228
RobotDynamics::JointTypeEulerXYZ
3 DoF joint that uses Euler XYZ convention (faster than emulated
Definition: Joint.h:199
RobotDynamics::Math::angular_acceleration_from_angle_rates
Vector3d angular_acceleration_from_angle_rates(const Vector3d &zyx_angles, const Vector3d &zyx_angle_rates, const Vector3d &zyx_angle_rates_dot)
Definition: rdl_mathutils.h:223
RobotDynamics::Math::Quaternion
Quaternion that are used for singularity free joints.
Definition: Quaternion.h:25
RobotDynamics::Math::VectorNd
Eigen::VectorXd VectorNd
Definition: rdl_eigenmath.h:25
TEST_F
TEST_F(SphericalJointFixture, TestQuaternionIntegration)
Definition: RdlMultiDofTests.cpp:119
unit_test_utils::checkMatrixNdEpsilonClose
static bool checkMatrixNdEpsilonClose(const RobotDynamics::Math::MatrixNd &t1, const RobotDynamics::Math::MatrixNd &t2, const double epsilon)
Definition: UnitTestUtils.hpp:154
RobotDynamics::forwardDynamicsContactsNullSpace
void forwardDynamicsContactsNullSpace(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDot)
Definition: Contacts.cc:370
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::calcMInvTimesTau
void calcMInvTimesTau(Model &model, const Math::VectorNd &Q, const Math::VectorNd &Tau, Math::VectorNd &QDDot, bool update_kinematics=true)
Computes the effect of multiplying the inverse of the joint space inertia matrix with a vector in lin...
Definition: Dynamics.cc:596
RobotDynamics::Model
Contains all information about the rigid body model.
Definition: Model.h:121
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
RobotDynamics::ConstraintSet::bind
bool bind(const Model &model)
Initializes and allocates memory for the constraint set.
Definition: Contacts.cc:62
RobotDynamics::Math::Quaternion::x
EIGEN_STRONG_INLINE double & x()
Definition: Quaternion.h:76
main
int main(int argc, char **argv)
Definition: RdlMultiDofTests.cpp:1142
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
RobotDynamics::Math::MatrixNd
Eigen::MatrixXd MatrixNd
Definition: rdl_eigenmath.h:26
unit_test_utils::checkSpatialVectorsEq
static bool checkSpatialVectorsEq(const RobotDynamics::Math::SpatialVector &v1, const RobotDynamics::Math::SpatialVector &v2)
Definition: UnitTestUtils.hpp:319
RobotDynamics::Math::Quaternion::z
EIGEN_STRONG_INLINE double & z()
Definition: Quaternion.h:96
RobotDynamics::inverseDynamics
void inverseDynamics(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, Math::VectorNd &Tau, Math::SpatialForceV *f_ext=nullptr, bool update_kinematics=true)
Definition: Dynamics.cc:23
RobotDynamics::JointTypeSpherical
3 DoF joint using Quaternions for joint positional variables and
Definition: Joint.h:195
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::forwardDynamicsLagrangian
void forwardDynamicsLagrangian(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, Math::VectorNd &QDDot, Math::MatrixNd &H, Math::VectorNd &C, Math::LinearSolver linear_solver=Math::LinearSolverColPivHouseholderQR, Math::SpatialForceV *f_ext=nullptr, bool update_kinematics=true)
Computes forward dynamics by building and solving the full Lagrangian equation.
Definition: Dynamics.cc:579
RobotDynamics::Math::intrinsicZYXAnglesToQuaternion
static Quaternion intrinsicZYXAnglesToQuaternion(double yaw, double pitch, double roll)
Convert YPR angles to quaternion.
Definition: SpatialAlgebraOperators.h:82
ConvertQAndQDotFromEmulated
void ConvertQAndQDotFromEmulated(const Model &emulated_model, const VectorNd &q_emulated, const VectorNd &qdot_emulated, const Model &multdof3_model, VectorNd *q_spherical, VectorNd *qdot_spherical)
Definition: RdlMultiDofTests.cpp:92
RobotDynamics::nonlinearEffects
void nonlinearEffects(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::VectorNd &Tau, bool update_kinematics=true)
Computes the coriolis forces.
Definition: Dynamics.cc:236
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
RobotDynamics::Model::qdot_size
unsigned int qdot_size
The size of the.
Definition: Model.h:187
RobotDynamics::forwardDynamicsContactsKokkevis
void forwardDynamicsContactsKokkevis(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDot)
Computes forward dynamics that accounts for active contacts in ConstraintSet.
Definition: Contacts.cc:682
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:28