RdlCustomJointMultiBodyTests.cpp
Go to the documentation of this file.
1 /*
2  * RBDL - Rigid Body Dynamics Library
3  * Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
4  * Copyright (c) 2016 Matthew Millard <matthew.millard@iwr.uni-heidelberg.de>
5  */
6 
7 #include <gtest/gtest.h>
8 #include <iostream>
9 
10 #include "Fixtures.h"
11 #include "Human36Fixture.h"
12 #include "rdl_dynamics/rdl_mathutils.h"
13 #include "rdl_dynamics/Model.h"
14 #include "rdl_dynamics/Kinematics.h"
15 #include "rdl_dynamics/Dynamics.h"
16 #include "rdl_dynamics/Contacts.h"
17 #include "rdl_dynamics/rdl_utils.h"
18 #include <vector>
19 
20 #include "UnitTestUtils.hpp"
21 
22 using namespace std;
23 using namespace RobotDynamics;
24 using namespace RobotDynamics::Math;
25 
26 const double TEST_PREC = 1.0e-11;
27 const int NUMBER_OF_MODELS = 3;
28 const int NUMBER_OF_BODIES = 3;
29 
30 //==============================================================================
31 /*
32 
33  The purpose of this test is to test that all of the code in RDL
34  related to a multibody mechanism that includes a custom joint functions.
35  Specifically this test is for the multi-pass algorithms in rdl ... namely
36  the CompositeRigidBodyAlgorithm. However, because these tests have already
37  been written for CustomJointSingleBodyTests.cc, we'll run them all on the
38  multibody models that we will be testing.
39 
40  We will be testing 3 models to get good coverage of the
41  CompositeRigidBodyAlgorithm:
42 
43  1. Rx - multidof - custom
44  2. Rx - custom - multidof
45  3. custom - multidof - Rx
46 
47  As before, to test that the model works, we will create a model using
48  standard RDL versions (the reference model), and then we will create
49  a model using a custom joint in the correct location. The following
50  algorithms will be tested in this manner:
51 
52  UpdateKinematicsCustom
53  Jacobians
54  InverseDynamics
55  CompositeRigidBodyAlgorithm
56  ForwardDynamics
57  CalcMInvTimestau
58  ForwardDynamicsContactsKokkevis
59 
60 */
61 //==============================================================================
62 
63 struct CustomJointTypeRevoluteX : public CustomJoint
64 {
65  CustomJointTypeRevoluteX()
66  {
67  mDoFCount = 1;
68  S = MatrixNd::Zero(6, 1);
69  S(0, 0) = 1.0;
70  S_o = MatrixNd::Zero(6, 1);
71  d_u = MatrixNd::Zero(mDoFCount, 1);
72  }
73 
74  virtual void jcalc(Model& model, unsigned int joint_id, const Math::VectorNd& q, const Math::VectorNd& qdot)
75  {
76  model.X_J[joint_id] = Xrotx(q[model.mJoints[joint_id].q_index]);
77  model.v_J[joint_id][0] = qdot[model.mJoints[joint_id].q_index];
78  }
79 
80  virtual void jcalc_X_lambda_S(Model& model, unsigned int joint_id, const Math::VectorNd& q)
81  {
82  model.X_J[joint_id] = Xrotx(q[model.mJoints[joint_id].q_index]);
83 
84  const Joint& joint = model.mJoints[joint_id];
85  model.mCustomJoints[joint.custom_joint_index]->S = S;
86  }
87 };
88 
89 struct CustomEulerZYXJoint : public CustomJoint
90 {
91  CustomEulerZYXJoint()
92  {
93  mDoFCount = 3;
94  S = MatrixNd::Zero(6, 3);
95  S_o = MatrixNd::Zero(6, 3);
96  d_u = MatrixNd::Zero(mDoFCount, 1);
97  }
98 
99  virtual void jcalc(Model& model, unsigned int joint_id, const Math::VectorNd& q, const Math::VectorNd& qdot)
100  {
101  double q0 = q[model.mJoints[joint_id].q_index];
102  double q1 = q[model.mJoints[joint_id].q_index + 1];
103  double q2 = q[model.mJoints[joint_id].q_index + 2];
104 
105  double s0 = sin(q0);
106  double c0 = cos(q0);
107  double s1 = sin(q1);
108  double c1 = cos(q1);
109  double s2 = sin(q2);
110  double c2 = cos(q2);
111 
112  model.X_J[joint_id].E =
113  Matrix3d(c0 * c1, s0 * c1, -s1, c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2, c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2);
114 
115  S.setZero();
116  S(0, 0) = -s1;
117  S(0, 2) = 1.;
118 
119  S(1, 0) = c1 * s2;
120  S(1, 1) = c2;
121 
122  S(2, 0) = c1 * c2;
123  S(2, 1) = -s2;
124 
125  double qdot0 = qdot[model.mJoints[joint_id].q_index];
126  double qdot1 = qdot[model.mJoints[joint_id].q_index + 1];
127  double qdot2 = qdot[model.mJoints[joint_id].q_index + 2];
128 
129  Vector3d qdotv(qdot0, qdot1, qdot2);
130  model.v_J[joint_id].set(S * qdotv);
131 
132  S_o(0, 0) = -c1 * qdot1;
133  S_o(1, 0) = -s1 * s2 * qdot1 + c1 * c2 * qdot2;
134  S_o(1, 1) = -s2 * qdot2;
135  S_o(2, 0) = -s1 * c2 * qdot1 - c1 * s2 * qdot2;
136  S_o(2, 1) = -c2 * qdot2;
137 
138  model.c_J[joint_id] = S_o * qdotv;
139  }
140 
141  virtual void jcalc_X_lambda_S(Model& model, unsigned int joint_id, const Math::VectorNd& q)
142  {
143  double q0 = q[model.mJoints[joint_id].q_index];
144  double q1 = q[model.mJoints[joint_id].q_index + 1];
145  double q2 = q[model.mJoints[joint_id].q_index + 2];
146 
147  double s0 = sin(q0);
148  double c0 = cos(q0);
149  double s1 = sin(q1);
150  double c1 = cos(q1);
151  double s2 = sin(q2);
152  double c2 = cos(q2);
153 
154  model.X_J[joint_id] = SpatialTransform(
155  Matrix3d(c0 * c1, s0 * c1, -s1, c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2, c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2),
156  Vector3d(0., 0., 0.));
157 
158  S.setZero();
159  S(0, 0) = -s1;
160  S(0, 2) = 1.;
161 
162  S(1, 0) = c1 * s2;
163  S(1, 1) = c2;
164 
165  S(2, 0) = c1 * c2;
166  S(2, 1) = -s2;
167 
168  const Joint& joint = model.mJoints[joint_id];
169  model.mCustomJoints[joint.custom_joint_index]->S = S;
170  }
171 };
172 
173 //==============================================================================
174 // Test Fixture
175 //==============================================================================
176 
177 struct RdlCustomJointMultiBodyFixture : public testing::Test
178 {
179  RdlCustomJointMultiBodyFixture()
180  {
181  }
182 
183  void SetUp()
184  {
185  reference_model.resize(NUMBER_OF_MODELS);
186  custom_model.resize(NUMBER_OF_MODELS);
187 
188  body.resize(NUMBER_OF_MODELS);
189  custom_joint.resize(NUMBER_OF_MODELS);
190 
191  reference_body_id.resize(NUMBER_OF_MODELS);
192  custom_body_id.resize(NUMBER_OF_MODELS);
193 
194  for (int i = 0; i < NUMBER_OF_MODELS; ++i)
195  {
196  body.at(i).resize(3);
197  custom_joint.at(i).resize(1);
198 
199  reference_body_id.at(i).resize(3);
200  custom_body_id.at(i).resize(3);
201  }
202 
203  q.resize(NUMBER_OF_MODELS);
204  qdot.resize(NUMBER_OF_MODELS);
205  qddot.resize(NUMBER_OF_MODELS);
206  tau.resize(NUMBER_OF_MODELS);
207 
208  //========================================================
209  // Test Model 1: Rx - multidof3 - custom
210  //========================================================
211 
212  custom_rx_joint1 = CustomJointTypeRevoluteX();
213 
214  Matrix3d inertia1 = Matrix3d::Identity(3, 3);
215 
216  Body body11 = Body(1., Vector3d(1.1, 1.2, 1.3), inertia1);
217  Body body12 = Body(2., Vector3d(2.1, 2.2, 2.3), inertia1);
218  Body body13 = Body(3., Vector3d(3.1, 3.2, 3.3), inertia1);
219 
220  ModelPtr reference1(new Model()), custom1(new Model());
221 
222  Vector3d r1 = Vector3d(0.78, -0.125, 0.37);
223 
224  double th1 = M_PI / 6.0;
225  double sinTh1 = sin(th1);
226  double cosTh1 = cos(th1);
227 
228  Matrix3d rm1 = Matrix3d(1.0, 0., 0., 0., cosTh1, -sinTh1, 0., sinTh1, cosTh1);
229 
230  Vector3d r2 = Vector3d(-0.178, 0.2125, -0.937);
231 
232  double th2 = M_PI / 2.15;
233  double sinTh2 = sin(th2);
234  double cosTh2 = cos(th2);
235 
236  Matrix3d rm2 = Matrix3d(cosTh2, 0., sinTh2, 0., 1., 0., -sinTh2, 0., cosTh2);
237 
238  unsigned int reference_body_id10 = reference1->addBody(0, SpatialTransform(), Joint(JointTypeRevoluteX), body11);
239 
240  unsigned int reference_body_id11 = reference1->addBody(reference_body_id10, SpatialTransform(rm1, r1), Joint(JointTypeEulerZYX), body12);
241 
242  unsigned int reference_body_id12 = reference1->addBody(reference_body_id11, SpatialTransform(rm2, r2), Joint(JointTypeRevoluteX), body13);
243 
244  unsigned int custom_body_id10 = custom1->addBody(0, SpatialTransform(), Joint(JointTypeRevoluteX), body11);
245 
246  unsigned int custom_body_id11 = custom1->addBody(custom_body_id10, SpatialTransform(rm1, r1), Joint(JointTypeEulerZYX), body12);
247 
248  unsigned int custom_body_id12 = custom1->addBodyCustomJoint(custom_body_id11, SpatialTransform(rm2, r2), &custom_rx_joint1, body13);
249 
250  VectorNd q1 = VectorNd::Zero(reference1->q_size);
251  VectorNd qdot1 = VectorNd::Zero(reference1->qdot_size);
252  VectorNd qddot1 = VectorNd::Zero(reference1->qdot_size);
253  VectorNd tau1 = VectorNd::Zero(reference1->qdot_size);
254 
255  int idx = 0;
256  reference_model.at(idx) = (reference1);
257  custom_model.at(idx) = (custom1);
258 
259  reference_body_id.at(idx).at(0) = (reference_body_id10);
260  reference_body_id.at(idx).at(1) = (reference_body_id11);
261  reference_body_id.at(idx).at(2) = (reference_body_id12);
262 
263  custom_body_id.at(idx).at(0) = (custom_body_id10);
264  custom_body_id.at(idx).at(1) = (custom_body_id11);
265  custom_body_id.at(idx).at(2) = (custom_body_id12);
266 
267  body.at(idx).at(0) = (body11);
268  body.at(idx).at(1) = (body12);
269  body.at(idx).at(2) = (body13);
270  custom_joint.at(idx).at(0) = (&custom_rx_joint1);
271 
272  q.at(idx) = (q1);
273  qdot.at(idx) = (qdot1);
274  qddot.at(idx) = (qddot1);
275  tau.at(idx) = (tau1);
276 
277  //========================================================
278  // Test Model 2: Rx - custom - multidof3
279  //========================================================
280 
281  ModelPtr reference2(new Model()), custom2(new Model());
282 
283  unsigned int reference_body_id20 = reference2->addBody(0, SpatialTransform(), Joint(JointTypeRevoluteX), body11);
284 
285  unsigned int reference_body_id21 = reference2->addBody(reference_body_id20, SpatialTransform(rm2, r2), Joint(JointTypeRevoluteX), body12);
286 
287  unsigned int reference_body_id22 = reference2->addBody(reference_body_id21, SpatialTransform(rm1, r1), Joint(JointTypeEulerZYX), body13);
288 
289  unsigned int custom_body_id20 = custom2->addBody(0, SpatialTransform(), Joint(JointTypeRevoluteX), body11);
290 
291  unsigned int custom_body_id21 = custom2->addBodyCustomJoint(custom_body_id20, SpatialTransform(rm2, r2), &custom_rx_joint1, body12);
292 
293  unsigned int custom_body_id22 = custom2->addBody(custom_body_id21, SpatialTransform(rm1, r1), Joint(JointTypeEulerZYX), body13);
294 
295  VectorNd q2 = VectorNd::Zero(reference2->q_size);
296  VectorNd qdot2 = VectorNd::Zero(reference2->qdot_size);
297  VectorNd qddot2 = VectorNd::Zero(reference2->qdot_size);
298  VectorNd tau2 = VectorNd::Zero(reference2->qdot_size);
299 
300  idx = 1;
301  reference_model.at(idx) = (reference2);
302  custom_model.at(idx) = (custom2);
303 
304  reference_body_id.at(idx).at(0) = (reference_body_id20);
305  reference_body_id.at(idx).at(1) = (reference_body_id21);
306  reference_body_id.at(idx).at(2) = (reference_body_id22);
307 
308  custom_body_id.at(idx).at(0) = (custom_body_id20);
309  custom_body_id.at(idx).at(1) = (custom_body_id21);
310  custom_body_id.at(idx).at(2) = (custom_body_id22);
311 
312  body.at(idx).at(0) = (body11);
313  body.at(idx).at(1) = (body12);
314  body.at(idx).at(2) = (body13);
315  custom_joint.at(idx).at(0) = (&custom_rx_joint1);
316 
317  q.at(idx) = (q2);
318  qdot.at(idx) = (qdot2);
319  qddot.at(idx) = (qddot2);
320  tau.at(idx) = (tau2);
321 
322  //========================================================
323  // Test Model 3: custom - multidof3 - Rx
324  //========================================================
325 
326  ModelPtr reference3(new Model()), custom3(new Model());
327 
328  unsigned int reference_body_id30 = reference3->addBody(0, SpatialTransform(), Joint(JointTypeRevoluteX), body11);
329 
330  unsigned int reference_body_id31 = reference3->addBody(reference_body_id30, SpatialTransform(rm1, r1), Joint(JointTypeEulerZYX), body12);
331 
332  unsigned int reference_body_id32 = reference3->addBody(reference_body_id31, SpatialTransform(rm2, r2), Joint(JointTypeRevoluteX), body13);
333 
334  unsigned int custom_body_id30 = custom3->addBodyCustomJoint(0, SpatialTransform(), &custom_rx_joint1, body11);
335 
336  unsigned int custom_body_id31 = custom3->addBody(custom_body_id30, SpatialTransform(rm1, r1), Joint(JointTypeEulerZYX), body12);
337 
338  unsigned int custom_body_id32 = custom3->addBody(custom_body_id31, SpatialTransform(rm2, r2), Joint(JointTypeRevoluteX), body13);
339 
340  VectorNd q3 = VectorNd::Zero(reference3->q_size);
341  VectorNd qdot3 = VectorNd::Zero(reference3->qdot_size);
342  VectorNd qddot3 = VectorNd::Zero(reference3->qdot_size);
343  VectorNd tau3 = VectorNd::Zero(reference3->qdot_size);
344 
345  idx = 2;
346  reference_model.at(idx) = (reference3);
347  custom_model.at(idx) = (custom3);
348 
349  reference_body_id.at(idx).at(0) = (reference_body_id30);
350  reference_body_id.at(idx).at(1) = (reference_body_id31);
351  reference_body_id.at(idx).at(2) = (reference_body_id32);
352 
353  custom_body_id.at(idx).at(0) = (custom_body_id30);
354  custom_body_id.at(idx).at(1) = (custom_body_id31);
355  custom_body_id.at(idx).at(2) = (custom_body_id32);
356 
357  body.at(idx).at(0) = (body11);
358  body.at(idx).at(1) = (body12);
359  body.at(idx).at(2) = (body13);
360  custom_joint.at(idx).at(0) = (&custom_rx_joint1);
361 
362  q.at(idx) = (q3);
363  qdot.at(idx) = (qdot3);
364  qddot.at(idx) = (qddot3);
365  tau.at(idx) = (tau3);
366  }
367 
368  void TearDown()
369  {
370  for (unsigned int i = 0; i < reference_model.size(); i++)
371  {
372  EXPECT_TRUE(unit_test_utils::checkModelZeroVectorsAndMatrices(*reference_model[i]));
373  }
374 
375  for (unsigned int i = 0; i < custom_model.size(); i++)
376  {
377  EXPECT_TRUE(unit_test_utils::checkModelZeroVectorsAndMatrices(*custom_model[i]));
378  }
379  }
380 
381  vector<ModelPtr> reference_model;
382  vector<ModelPtr> custom_model;
383 
384  vector<vector<Body>> body;
385  vector<vector<CustomJoint*>> custom_joint;
386 
387  vector<vector<unsigned int>> reference_body_id;
388  vector<vector<unsigned int>> custom_body_id;
389 
390  vector<VectorNd> q;
391  vector<VectorNd> qdot;
392  vector<VectorNd> qddot;
393  vector<VectorNd> tau;
394 
395  CustomJointTypeRevoluteX custom_rx_joint1;
396  CustomJointTypeRevoluteX custom_rx_joint2;
397  CustomJointTypeRevoluteX custom_rx_joint3;
398 };
399 
400 //==============================================================================
401 //
402 // Tests
403 // UpdateKinematicsCustom
404 // Jacobians
405 // InverseDynamics
406 // CompositeRigidBodyAlgorithm
407 // ForwardDynamics
408 // CalcMInvTimestau
409 // ForwardDynamicsContactsKokkevis
410 //
411 //==============================================================================
412 
413 TEST_F(RdlCustomJointMultiBodyFixture, UpdateKinematics)
414 {
415  VectorNd test;
416 
417  for (int idx = 0; idx < NUMBER_OF_MODELS; ++idx)
418  {
419  int dof = reference_model.at(idx)->dof_count;
420  for (unsigned int i = 0; i < dof; i++)
421  {
422  q.at(idx)[i] = i * 0.1;
423  qdot.at(idx)[i] = i * 0.15;
424  qddot.at(idx)[i] = i * 0.17;
425  }
426 
427  updateKinematics(*reference_model.at(idx), q.at(idx), qdot.at(idx), qddot.at(idx));
428  updateKinematics(*custom_model.at(idx), q.at(idx), qdot.at(idx), qddot.at(idx));
429 
430  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(
431  reference_model.at(idx)->bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)]->getTransformToRoot().E,
432  custom_model.at(idx)->bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)]->getTransformToRoot().E, TEST_PREC));
433  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(
434  reference_model.at(idx)->bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)]->getTransformToRoot().r,
435  custom_model.at(idx)->bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)]->getTransformToRoot().r, TEST_PREC));
436  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(reference_model.at(idx)->v[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)],
437  custom_model.at(idx)->v[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)], TEST_PREC));
438  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(reference_model.at(idx)->a[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)],
439  custom_model.at(idx)->a[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)], TEST_PREC));
440  }
441 }
442 
443 TEST_F(RdlCustomJointMultiBodyFixture, updateKinematicsCustom)
444 {
445  for (int idx = 0; idx < NUMBER_OF_MODELS; ++idx)
446  {
447  int dof = reference_model.at(idx)->dof_count;
448  for (unsigned int i = 0; i < dof; i++)
449  {
450  q.at(idx)[i] = i * 9.133758561390194e-01;
451  qdot.at(idx)[i] = i * 6.323592462254095e-01;
452  qddot.at(idx)[i] = i * 9.754040499940952e-02;
453  }
454 
455  updateKinematicsCustom(*reference_model.at(idx), &q.at(idx), NULL, NULL);
456  updateKinematicsCustom(*custom_model.at(idx), &q.at(idx), NULL, NULL);
457 
458  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(
459  reference_model.at(idx)->bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)]->getTransformToRoot().E,
460  custom_model.at(idx)->bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)]->getTransformToRoot().E, TEST_PREC));
461  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(
462  reference_model.at(idx)->bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)]->getTransformToRoot().r,
463  custom_model.at(idx)->bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)]->getTransformToRoot().r, TEST_PREC));
464 
465  // velocity
466  updateKinematicsCustom(*reference_model.at(idx), &q.at(idx), &qdot.at(idx), NULL);
467  updateKinematicsCustom(*custom_model.at(idx), &q.at(idx), &qdot.at(idx), NULL);
468 
469  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(reference_model.at(idx)->v[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)],
470  custom_model.at(idx)->v[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)], TEST_PREC));
471 
472  // All
473  updateKinematicsCustom(*reference_model.at(idx), &q.at(idx), &qdot.at(idx), &qddot.at(idx));
474  updateKinematicsCustom(*custom_model.at(idx), &q.at(idx), &qdot.at(idx), &qddot.at(idx));
475 
476  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(reference_model.at(idx)->a[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)],
477  custom_model.at(idx)->a[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)], TEST_PREC));
478  }
479 }
480 
481 TEST_F(RdlCustomJointMultiBodyFixture, Jacobians)
482 {
483  for (int idx = 0; idx < NUMBER_OF_MODELS; ++idx)
484  {
485  int dof = reference_model.at(idx)->dof_count;
486 
487  for (unsigned int i = 0; i < dof; i++)
488  {
489  q.at(idx)[i] = i * 9.133758561390194e-01;
490  qdot.at(idx)[i] = i * 6.323592462254095e-01;
491  qddot.at(idx)[i] = i * 9.754040499940952e-02;
492  }
493 
494  // position
495  updateKinematics(*reference_model.at(idx), q.at(idx), qdot.at(idx), qddot.at(idx));
496  updateKinematics(*custom_model.at(idx), q.at(idx), qdot.at(idx), qddot.at(idx));
497 
498  // Check the Spatial Jacobian
499  MatrixNd Gref = MatrixNd(MatrixNd::Zero(6, reference_model.at(idx)->dof_count));
500  MatrixNd Gcus = MatrixNd(MatrixNd::Zero(6, reference_model.at(idx)->dof_count));
501  MatrixNd Gcus2 = MatrixNd(MatrixNd::Zero(6, reference_model.at(idx)->dof_count));
502  MatrixNd GDotref = MatrixNd(MatrixNd::Zero(6, reference_model.at(idx)->dof_count));
503  MatrixNd GDotcus = MatrixNd(MatrixNd::Zero(6, reference_model.at(idx)->dof_count));
504  MatrixNd GDotcus2 = MatrixNd(MatrixNd::Zero(6, reference_model.at(idx)->dof_count));
505 
506  calcBodySpatialJacobian(*reference_model.at(idx), q.at(idx), reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1), Gref);
507  calcBodySpatialJacobian(*custom_model.at(idx), q.at(idx), custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1), Gcus);
508 
509  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(Gref, Gcus, TEST_PREC));
510 
511  // Check the Spatial Jacobian
512  Gref.setZero();
513  Gcus.setZero();
514 
515  ReferenceFramePtr b1_ref_frame = reference_model.at(idx)->bodyFrames[reference_body_id.at(idx)[0]];
516  ReferenceFramePtr b2_ref_frame = reference_model.at(idx)->bodyFrames[reference_body_id.at(idx)[1]];
517  ReferenceFramePtr b3_ref_frame = reference_model.at(idx)->bodyFrames[reference_body_id.at(idx)[2]];
518 
519  ReferenceFramePtr b1_cus_frame = custom_model.at(idx)->bodyFrames[custom_body_id.at(idx)[0]];
520  ReferenceFramePtr b2_cus_frame = custom_model.at(idx)->bodyFrames[custom_body_id.at(idx)[1]];
521  ReferenceFramePtr b3_cus_frame = custom_model.at(idx)->bodyFrames[custom_body_id.at(idx)[2]];
522 
523  calcRelativeBodySpatialJacobian(*reference_model.at(idx), q.at(idx), Gref, b1_ref_frame, b2_ref_frame, b3_ref_frame);
524  calcRelativeBodySpatialJacobian(*custom_model.at(idx), q.at(idx), Gcus, b1_cus_frame, b2_cus_frame, b3_cus_frame);
525 
526  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(Gref, Gcus, unit_test_utils::TEST_PREC));
527 
528  GDotref.setZero();
529  GDotcus.setZero();
530  GDotcus2.setZero();
531  Gcus2.setZero();
532  Gcus.setZero();
533 
534  calcRelativeBodySpatialJacobianDot(*reference_model.at(idx), q.at(idx), qdot.at(idx), GDotref, b1_ref_frame, b2_ref_frame, b3_ref_frame);
535  calcRelativeBodySpatialJacobianDot(*custom_model.at(idx), q.at(idx), qdot.at(idx), GDotcus, b1_cus_frame, b2_cus_frame, b3_cus_frame);
536 
537  calcRelativeBodySpatialJacobian(*custom_model.at(idx), q.at(idx), Gcus, b1_cus_frame, b2_cus_frame, b3_cus_frame);
538  calcRelativeBodySpatialJacobianAndJacobianDot(*custom_model.at(idx), q.at(idx), qdot.at(idx), Gcus2, GDotcus2, b1_cus_frame, b2_cus_frame, b3_cus_frame);
539 
540  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(GDotref, GDotcus, TEST_PREC));
541  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(GDotcus, GDotcus2, TEST_PREC));
542  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(Gcus, Gcus2, TEST_PREC));
543 
544  GDotref.setZero();
545  GDotcus.setZero();
546 
547  calcBodySpatialJacobianDot(*reference_model.at(idx), q.at(idx), qdot.at(idx), reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1), GDotref);
548  calcBodySpatialJacobianDot(*custom_model.at(idx), q.at(idx), qdot.at(idx), custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1), GDotcus);
549 
550  for (int i = 0; i < 6; ++i)
551  {
552  for (int j = 0; j < dof; ++j)
553  {
554  EXPECT_NEAR(GDotref(i, j), GDotcus(i, j), TEST_PREC);
555  }
556  }
557 
558  // Check the 6d point Jacobian
559  Vector3d point_position(1.1, 1.2, 2.1);
560 
561  calcPointJacobian6D(*reference_model.at(idx), q.at(idx), reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1), point_position, Gref);
562  calcPointJacobian6D(*custom_model.at(idx), q.at(idx), custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1), point_position, Gcus);
563 
564  for (int i = 0; i < 6; ++i)
565  {
566  for (int j = 0; j < dof; ++j)
567  {
568  EXPECT_NEAR(Gref(i, j), Gcus(i, j), TEST_PREC);
569  }
570  }
571 
572  Gref.setZero();
573  Gcus.setZero();
574  calcPointJacobianDot6D(*reference_model.at(idx), q.at(idx), qdot.at(idx), reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1), point_position, Gref);
575  calcPointJacobianDot6D(*custom_model.at(idx), q.at(idx), qdot.at(idx), custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1), point_position, Gcus);
576 
577  for (int i = 0; i < 6; ++i)
578  {
579  for (int j = 0; j < dof; ++j)
580  {
581  EXPECT_NEAR(Gref(i, j), Gcus(i, j), TEST_PREC);
582  }
583  }
584 
585  Gref = MatrixNd::Constant(3, reference_model.at(idx)->dof_count, 0.);
586  Gcus = MatrixNd::Constant(3, reference_model.at(idx)->dof_count, 0.);
587 
588  calcPointJacobianDot(*reference_model.at(idx), q.at(idx), qdot.at(idx), reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1), point_position, Gref);
589  calcPointJacobianDot(*custom_model.at(idx), q.at(idx), qdot.at(idx), custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1), point_position, Gcus);
590 
591  for (int i = 0; i < 3; ++i)
592  {
593  for (int j = 0; j < dof; ++j)
594  {
595  EXPECT_NEAR(Gref(i, j), Gcus(i, j), TEST_PREC);
596  }
597  }
598 
599  // Check the 3d point Jacobian
600  MatrixNd GrefPt = MatrixNd::Constant(3, reference_model.at(idx)->dof_count, 0.);
601  MatrixNd GcusPt = MatrixNd::Constant(3, reference_model.at(idx)->dof_count, 0.);
602 
603  MatrixNd GDotrefPt = MatrixNd::Constant(3, reference_model.at(idx)->dof_count, 0.);
604  MatrixNd GDotcusPt = MatrixNd::Constant(3, reference_model.at(idx)->dof_count, 0.);
605 
606  calcPointJacobian(*reference_model.at(idx), q.at(idx), reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1), point_position, GrefPt);
607  calcPointJacobian(*custom_model.at(idx), q.at(idx), custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1), point_position, GcusPt);
608 
609  for (int i = 0; i < 3; ++i)
610  {
611  for (int j = 0; j < dof; ++j)
612  {
613  EXPECT_NEAR(GrefPt(i, j), GcusPt(i, j), TEST_PREC);
614  }
615  }
616 
617  GrefPt = MatrixNd::Constant(6, reference_model.at(idx)->qdot_size, 0.);
618  GcusPt = MatrixNd::Constant(6, reference_model.at(idx)->qdot_size, 0.);
619 
620  RobotDynamics::ReferenceFramePtr frame1 = reference_model.at(idx)->bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)];
621  RobotDynamics::ReferenceFramePtr frame2 = custom_model.at(idx)->bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)];
622 
623  calcPointJacobian6D(*reference_model.at(idx), q.at(idx), GrefPt, frame1);
624  calcPointJacobian6D(*custom_model.at(idx), q.at(idx), GcusPt, frame2);
625 
626  for (int i = 0; i < 6; ++i)
627  {
628  for (int j = 0; j < dof; ++j)
629  {
630  EXPECT_NEAR(GrefPt(i, j), GcusPt(i, j), TEST_PREC);
631  }
632  }
633 
634  GrefPt = MatrixNd::Constant(6, reference_model.at(idx)->qdot_size, 0.);
635  GcusPt = MatrixNd::Constant(6, reference_model.at(idx)->qdot_size, 0.);
636 
637  frame1 = reference_model.at(idx)->bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)];
638  frame2 = custom_model.at(idx)->bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)];
639 
640  calcPointJacobianDot6D(*reference_model.at(idx), q.at(idx), qdot.at(idx), frame1, GrefPt);
641  calcPointJacobianDot6D(*custom_model.at(idx), q.at(idx), qdot.at(idx), frame2, GcusPt);
642 
643  for (int i = 0; i < 6; ++i)
644  {
645  for (int j = 0; j < dof; ++j)
646  {
647  EXPECT_NEAR(GrefPt(i, j), GcusPt(i, j), TEST_PREC);
648  }
649  }
650 
651  GrefPt = MatrixNd::Constant(3, reference_model.at(idx)->qdot_size, 0.);
652  GcusPt = MatrixNd::Constant(3, reference_model.at(idx)->qdot_size, 0.);
653 
654  frame1 = reference_model.at(idx)->bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)];
655  frame2 = custom_model.at(idx)->bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)];
656 
657  calcPointJacobianDot(*reference_model.at(idx), q.at(idx), qdot.at(idx), frame1, GrefPt);
658  calcPointJacobianDot(*custom_model.at(idx), q.at(idx), qdot.at(idx), frame2, GcusPt);
659 
660  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(GrefPt, GcusPt, TEST_PREC));
661 
662  GrefPt = MatrixNd::Constant(3, reference_model.at(idx)->qdot_size, 0.);
663  GcusPt = MatrixNd::Constant(3, reference_model.at(idx)->qdot_size, 0.);
664 
665  frame1 = reference_model.at(idx)->bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)];
666  frame2 = custom_model.at(idx)->bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)];
667 
668  calcPointJacobian(*reference_model.at(idx), q.at(idx), GrefPt, frame1);
669  calcPointJacobian(*custom_model.at(idx), q.at(idx), GcusPt, frame2);
670 
671  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(GrefPt, GcusPt, TEST_PREC));
672 
673  GrefPt = MatrixNd::Constant(6, reference_model.at(idx)->qdot_size, 0.);
674  GcusPt = MatrixNd::Constant(6, reference_model.at(idx)->qdot_size, 0.);
675 
676  GDotrefPt = MatrixNd::Constant(6, reference_model.at(idx)->qdot_size, 0.);
677  GDotcusPt = MatrixNd::Constant(6, reference_model.at(idx)->qdot_size, 0.);
678 
679  frame1 = reference_model.at(idx)->bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1)];
680  RobotDynamics::ReferenceFramePtr frame1Rel = reference_model.at(idx)->bodyFrames[reference_body_id.at(idx).at(NUMBER_OF_BODIES - 2)];
681  frame2 = custom_model.at(idx)->bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1)];
682  RobotDynamics::ReferenceFramePtr frame2Rel = custom_model.at(idx)->bodyFrames[custom_body_id.at(idx).at(NUMBER_OF_BODIES - 2)];
683 
684  calcRelativePointJacobian6D(*reference_model.at(idx), q.at(idx), GrefPt, frame1, frame1Rel, frame1);
685  calcRelativePointJacobian6D(*custom_model.at(idx), q.at(idx), GcusPt, frame2, frame2Rel, frame2);
686 
687  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(GrefPt, GcusPt, TEST_PREC));
688 
689  calcRelativePointJacobianDot6D(*reference_model.at(idx), q.at(idx), qdot.at(idx), GDotrefPt, frame1, frame1Rel, frame1);
690  calcRelativePointJacobianDot6D(*custom_model.at(idx), q.at(idx), qdot.at(idx), GDotcusPt, frame2, frame2Rel, frame2);
691 
692  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(GDotrefPt, GDotcusPt, TEST_PREC));
693 
694  MatrixNd GcusPt2 = MatrixNd::Constant(6, custom_model.at(idx)->qdot_size, 0.);
695  MatrixNd GDotcusPt2 = MatrixNd::Constant(6, custom_model.at(idx)->qdot_size, 0.);
696  GcusPt = MatrixNd::Constant(6, custom_model.at(idx)->qdot_size, 0.);
697  GDotcusPt = MatrixNd::Constant(6, custom_model.at(idx)->qdot_size, 0.);
698 
699  calcRelativePointJacobian6D(*custom_model.at(idx), q.at(idx), GcusPt, frame2, frame2Rel, frame2);
700  calcRelativePointJacobianDot6D(*custom_model.at(idx), q.at(idx), qdot.at(idx), GDotcusPt, frame2, frame2Rel, frame2);
701  calcRelativePointJacobianAndJacobianDot6D(*custom_model.at(idx), q.at(idx), qdot.at(idx), GcusPt2, GDotcusPt2, frame2, frame2Rel, frame2);
702 
703  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(GcusPt, GcusPt2, TEST_PREC));
704  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(GDotcusPt, GDotcusPt2, TEST_PREC));
705 
706  MatrixNd G_com_ref = MatrixNd::Constant(3, reference_model.at(idx)->dof_count, 0.);
707  MatrixNd G_com_cus = MatrixNd::Constant(3, reference_model.at(idx)->dof_count, 0.);
708 
709  Utils::calcCenterOfMassJacobian(*reference_model.at(idx), q.at(idx), G_com_ref);
710  Utils::calcCenterOfMassJacobian(*custom_model.at(idx), q.at(idx), G_com_cus);
711 
712  unit_test_utils::checkMatrixNdEpsilonClose(G_com_ref, G_com_cus, TEST_PREC);
713 
714  MatrixNd Aref = MatrixNd::Constant(6, reference_model.at(idx)->dof_count, 0.);
715  MatrixNd Acus = MatrixNd::Constant(6, reference_model.at(idx)->dof_count, 0.);
716 
717  Utils::calcCentroidalMomentumMatrix(*reference_model.at(idx), q.at(idx), Aref);
718  Utils::calcCentroidalMomentumMatrix(*custom_model.at(idx), q.at(idx), Acus);
719 
720  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(Aref, Acus, TEST_PREC));
721  }
722 }
723 
724 TEST_F(RdlCustomJointMultiBodyFixture, calcCentroidalMomentumMatrixDot)
725 {
726  for (int idx = 0; idx < NUMBER_OF_MODELS; ++idx)
727  {
728  int dof = reference_model.at(idx)->dof_count;
729 
730  for (unsigned int i = 0; i < dof; i++)
731  {
732  q.at(idx)[i] = i * 9.133758561390194e-01;
733  qdot.at(idx)[i] = i * 6.323592462254095e-01;
734  qddot.at(idx)[i] = i * 9.754040499940952e-02;
735  }
736 
737  // Check the Spatial Jacobian
738  MatrixNd ADotref = MatrixNd(MatrixNd::Zero(6, reference_model.at(idx)->qdot_size));
739  MatrixNd ADotcus = MatrixNd(MatrixNd::Zero(6, reference_model.at(idx)->qdot_size));
740 
741  Utils::calcCentroidalMomentumMatrixDot(*reference_model.at(idx), q.at(idx), qdot.at(idx), ADotref);
742  Utils::calcCentroidalMomentumMatrixDot(*custom_model.at(idx), q.at(idx), qdot.at(idx), ADotcus);
743 
744  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(ADotref, ADotcus, unit_test_utils::TEST_PREC * 10.));
745  }
746 }
747 
748 TEST_F(RdlCustomJointMultiBodyFixture, GravityEffects)
749 {
750  for (int idx = 0; idx < NUMBER_OF_MODELS; ++idx)
751  {
752  int dof = reference_model.at(idx)->dof_count;
753 
754  for (unsigned int i = 0; i < dof; i++)
755  {
756  q.at(idx)[i] = i * 9.133758561390194e-01;
757  qdot.at(idx)[i] = i * 6.323592462254095e-01;
758  qddot.at(idx)[i] = i * 9.754040499940952e-02;
759  }
760 
761  // position
762  VectorNd tauRef = VectorNd::Zero(reference_model.at(idx)->qdot_size);
763  VectorNd tauCus = VectorNd::Zero(reference_model.at(idx)->qdot_size);
764 
765  updateKinematics(*reference_model.at(idx), q.at(idx), qdot.at(idx), qddot.at(idx));
766  updateKinematics(*custom_model.at(idx), q.at(idx), qdot.at(idx), qddot.at(idx));
767 
768  qdot.at(idx).setZero();
769  nonlinearEffects(*reference_model.at(idx), q.at(idx), qdot.at(idx), tauRef);
770  gravityEffects(*custom_model.at(idx), tauCus);
771 
772  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(tauRef, tauCus, TEST_PREC));
773  }
774 }
775 
776 TEST_F(RdlCustomJointMultiBodyFixture, InverseDynamics)
777 {
778  for (int idx = 0; idx < NUMBER_OF_MODELS; ++idx)
779  {
780  int dof = reference_model.at(idx)->dof_count;
781 
782  for (unsigned int i = 0; i < dof; i++)
783  {
784  q.at(idx)[i] = i * 9.133758561390194e-01;
785  qdot.at(idx)[i] = i * 6.323592462254095e-01;
786  qddot.at(idx)[i] = i * 9.754040499940952e-02;
787  }
788 
789  // position
790  VectorNd tauRef = VectorNd::Zero(reference_model.at(idx)->qdot_size);
791  VectorNd tauCus = VectorNd::Zero(reference_model.at(idx)->qdot_size);
792 
793  inverseDynamics(*reference_model.at(idx), q.at(idx), qdot.at(idx), qddot.at(idx), tauRef);
794  inverseDynamics(*custom_model.at(idx), q.at(idx), qdot.at(idx), qddot.at(idx), tauCus);
795 
796  VectorNd tauErr = tauRef - tauCus;
797 
798  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(tauRef, tauCus, TEST_PREC));
799  }
800 }
801 
802 TEST_F(RdlCustomJointMultiBodyFixture, CompositeRigidBodyAlgorithm)
803 {
804  for (int idx = 0; idx < 2; ++idx)
805  {
806  int dof = reference_model.at(idx)->dof_count;
807 
808  for (unsigned int i = 0; i < dof; i++)
809  {
810  q.at(idx)[i] = (i + 0.1) * 9.133758561390194e-01;
811  qdot.at(idx)[i] = (i + 0.1) * 6.323592462254095e-01;
812  tau.at(idx)[i] = (i + 0.1) * 9.754040499940952e-02;
813  }
814 
815  MatrixNd h_ref = MatrixNd::Constant(dof, dof, 0.);
816  VectorNd c_ref = VectorNd::Constant(dof, 0.);
817  VectorNd qddot_zero_ref = VectorNd::Constant(dof, 0.);
818  VectorNd qddot_crba_ref = VectorNd::Constant(dof, 0.);
819 
820  MatrixNd h_cus = MatrixNd::Constant(dof, dof, 0.);
821  VectorNd c_cus = VectorNd::Constant(dof, 0.);
822  VectorNd qddot_zero_cus = VectorNd::Constant(dof, 0.);
823  VectorNd qddot_crba_cus = VectorNd::Constant(dof, 0.);
824 
825  VectorNd qddotRef = VectorNd::Zero(dof);
826  VectorNd qddotCus = VectorNd::Zero(dof);
827 
828  // Ref
829  forwardDynamics(*reference_model.at(idx), q.at(idx), qdot.at(idx), tau.at(idx), qddotRef);
830  compositeRigidBodyAlgorithm(*reference_model.at(idx), q.at(idx), h_ref);
831  inverseDynamics(*reference_model.at(idx), q.at(idx), qdot.at(idx), qddot_zero_ref, c_ref);
832  linSolveGaussElimPivot(h_ref, c_ref * -1. + tau.at(idx), qddot_crba_ref);
833 
834  // Custom
835  forwardDynamics(*custom_model.at(idx), q.at(idx), qdot.at(idx), tau.at(idx), qddotCus);
836  compositeRigidBodyAlgorithm(*custom_model.at(idx), q.at(idx), h_cus);
837  inverseDynamics(*custom_model.at(idx), q.at(idx), qdot.at(idx), qddot_zero_cus, c_cus);
838  linSolveGaussElimPivot(h_cus, c_cus * -1. + tau.at(idx), qddot_crba_cus);
839 
840  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_crba_ref, qddot_crba_cus, TEST_PREC));
841  }
842 }
843 
844 TEST_F(RdlCustomJointMultiBodyFixture, ForwardDynamics)
845 {
846  for (int idx = 0; idx < NUMBER_OF_MODELS; ++idx)
847  {
848  int dof = reference_model.at(idx)->dof_count;
849 
850  for (unsigned int i = 0; i < dof; i++)
851  {
852  q.at(idx)[i] = (i + 0.1) * 9.133758561390194e-01;
853  qdot.at(idx)[i] = (i + 0.1) * 6.323592462254095e-01;
854  qddot.at(idx)[i] = (i + 0.1) * 2.323592499940952e-01;
855  tau.at(idx)[i] = (i + 0.1) * 9.754040499940952e-02;
856  }
857 
858  VectorNd qddotRef = VectorNd::Zero(reference_model.at(idx)->qdot_size);
859  VectorNd qddotCus = VectorNd::Zero(reference_model.at(idx)->qdot_size);
860 
861  forwardDynamics(*reference_model.at(idx), q.at(idx), qdot.at(idx), tau.at(idx), qddotRef);
862  forwardDynamics(*custom_model.at(idx), q.at(idx), qdot.at(idx), tau.at(idx), qddotCus);
863 
864  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddotRef, qddotCus, TEST_PREC));
865  }
866 }
867 
868 TEST_F(RdlCustomJointMultiBodyFixture, CalcMInvTimestau)
869 {
870  for (int idx = 0; idx < NUMBER_OF_MODELS; ++idx)
871  {
872  int dof = reference_model.at(idx)->dof_count;
873 
874  for (unsigned int i = 0; i < dof; i++)
875  {
876  q.at(idx)[i] = (i + 0.1) * 9.133758561390194e-01;
877  tau.at(idx)[i] = (i + 0.1) * 9.754040499940952e-02;
878  }
879 
880  // reference
881  VectorNd qddot_minv_ref = VectorNd::Zero(dof);
882 
883  calcMInvTimesTau(*reference_model.at(idx), q.at(idx), tau.at(idx), qddot_minv_ref, true);
884 
885  // custom
886  VectorNd qddot_minv_cus = VectorNd::Zero(dof);
887  calcMInvTimesTau(*custom_model.at(idx), q.at(idx), tau.at(idx), qddot_minv_cus, true);
888 
889  // check.
890  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_minv_ref, qddot_minv_cus, TEST_PREC));
891  }
892 }
893 
894 TEST_F(RdlCustomJointMultiBodyFixture, ForwardDynamicsContactsKokkevis)
895 {
896  for (int idx = 0; idx < NUMBER_OF_MODELS; ++idx)
897  {
898  int dof = reference_model.at(idx)->dof_count;
899 
900  // Adding a 1 constraint to a system with 1 dof is
901  // a no-no
902  if (dof > 1)
903  {
904  for (unsigned int i = 0; i < dof; i++)
905  {
906  q.at(idx)[i] = (i + 0.1) * 9.133758561390194e-01;
907  qdot.at(idx)[i] = (i + 0.1) * 6.323592462254095e-01;
908  tau.at(idx)[i] = (i + 0.1) * 9.754040499940952e-02;
909  }
910 
911  VectorNd qddot_ref = VectorNd::Zero(dof);
912  VectorNd qddot_cus = VectorNd::Zero(dof);
913 
914  VectorNd qdot_plus_ref = VectorNd::Zero(dof);
915  VectorNd qdot_plus_cus = VectorNd::Zero(dof);
916 
917  Vector3d contact_point(0., 1., 0.);
918 
919  ConstraintSet constraint_set_ref;
920  ConstraintSet constraint_set_cus;
921 
922  // Reference
923  constraint_set_ref.addConstraint(reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1), contact_point, Vector3d(1., 0., 0.), "ground_x");
924 
925  constraint_set_ref.addConstraint(reference_body_id.at(idx).at(NUMBER_OF_BODIES - 1), contact_point, Vector3d(0., 1., 0.), "ground_y");
926 
927  constraint_set_ref.bind(*reference_model.at(idx));
928 
929  // Custom
930  constraint_set_cus.addConstraint(custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1), contact_point, Vector3d(1., 0., 0.), "ground_x");
931 
932  constraint_set_cus.addConstraint(custom_body_id.at(idx).at(NUMBER_OF_BODIES - 1), contact_point, Vector3d(0., 1., 0.), "ground_y");
933 
934  constraint_set_cus.bind(*custom_model.at(idx));
935 
936  computeContactImpulsesDirect(*reference_model.at(idx), q.at(idx), qdot.at(idx), constraint_set_ref, qdot_plus_ref);
937  forwardDynamicsContactsKokkevis(*reference_model.at(idx), q.at(idx), qdot_plus_ref, tau.at(idx), constraint_set_ref, qddot_ref);
938 
939  computeContactImpulsesDirect(*custom_model.at(idx), q.at(idx), qdot.at(idx), constraint_set_cus, qdot_plus_cus);
940  forwardDynamicsContactsKokkevis(*custom_model.at(idx), q.at(idx), qdot_plus_cus, tau.at(idx), constraint_set_cus, qddot_cus);
941 
942  VectorNd qdot_plus_error = qdot_plus_ref - qdot_plus_cus;
943  VectorNd qddot_error = qddot_ref - qddot_cus;
944 
945  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qdot_plus_ref, qdot_plus_cus, TEST_PREC));
946 
947  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_ref, qddot_cus, TEST_PREC));
948  }
949  }
950 }
951 
952 int main(int argc, char** argv)
953 {
954  ::testing::InitGoogleTest(&argc, argv);
955  return RUN_ALL_TESTS();
956 }
957 
958 //
959 // Completed?
960 // x : implement test for UpdateKinematicsCustom
961 // x : implement test for Jacobians
962 // x : implement test for InverseDynamics
963 // x : implement test for CompositeRigidBodyAlgorithm
964 // x : implement test for ForwardDynamics
965 // x : implement test for CalcMInvTimestau
966 // x : implement test for ForwardDynamicsContactsKokkevis
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::computeContactImpulsesDirect
void computeContactImpulsesDirect(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
Computes contact gain by constructing and solving the full lagrangian equation.
Definition: Contacts.cc:383
RobotDynamics::Utils::calcCenterOfMassJacobian
void calcCenterOfMassJacobian(Model &model, const Math::VectorNd &q, Math::MatrixNd &jCom, bool update_kinematics=true)
Computes the matrix such that .
Definition: rdl_utils.cc:464
RobotDynamics::calcPointJacobian
void calcPointJacobian(Model &model, const Math::VectorNd &Q, unsigned int body_id, const Math::Vector3d &point_position, Math::MatrixNd &G, bool update_kinematics=true)
Computes the 3D point jacobian for a point on a body.
Definition: Kinematics.cc:510
RobotDynamics::Model::mJoints
std::vector< Joint > mJoints
All joints.
Definition: Model.h:209
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::Model::c_J
std::vector< Math::SpatialVector > c_J
Definition: Model.h:217
RobotDynamics::calcRelativeBodySpatialJacobianAndJacobianDot
void calcRelativeBodySpatialJacobianAndJacobianDot(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, Math::MatrixNd &GDot, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=nullptr, bool update_kinematics=true)
Computes both the body spatial jacobian and its time derivative. This function will be a bit more eff...
Definition: Kinematics.cc:402
rdl_utils.h
RobotDynamics::CustomJoint
CustomJoint is a struct used to create a joint with user defined parameters. This is accomplished by ...
Definition: Joint.h:662
RobotDynamics::ModelPtr
std::shared_ptr< Model > ModelPtr
Definition: Model.h:731
RdlCustomJointMultiBodyFixture::custom_joint
vector< vector< CustomJoint * > > custom_joint
Definition: RdlCustomJointMultiBodyTests.cpp:385
RobotDynamics::Math::linSolveGaussElimPivot
bool linSolveGaussElimPivot(MatrixNd A, VectorNd b, VectorNd &x)
Solves a linear system using gaussian elimination with pivoting.
Definition: rdl_mathutils.cc:36
RobotDynamics::calcRelativeBodySpatialJacobian
void calcRelativeBodySpatialJacobian(Model &model, const Math::VectorNd &Q, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=nullptr, bool update_kinematics=true)
Computes the jacobian of a frame with being the "base" frame, being the "relative" frame...
Definition: Kinematics.cc:234
unit_test_utils::TEST_PREC
const double TEST_PREC
Definition: UnitTestUtils.hpp:22
RobotDynamics::calcPointJacobianDot
void calcPointJacobianDot(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, Math::MatrixNd &G, bool update_kinematics=true)
Computes the time derivative of the linear components the a point jacobian on a body.
Definition: Kinematics.cc:751
RobotDynamics::ConstraintSet::addConstraint
unsigned int addConstraint(unsigned int body_id, const Math::Vector3d &body_point, const Math::Vector3d &world_normal, const char *constraint_name=NULL, double normal_acceleration=0.)
Adds a constraint to the constraint set.
Definition: Contacts.cc:27
unit_test_utils::checkModelZeroVectorsAndMatrices
bool checkModelZeroVectorsAndMatrices(RobotDynamics::Model &model)
Definition: UnitTestUtils.hpp:27
RobotDynamics::calcRelativePointJacobian6D
void calcRelativePointJacobian6D(Model &model, const Math::VectorNd &Q, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics=true)
Compute the 6D jacobian of the origin of a reference frame relative to the origin of another referenc...
Definition: Kinematics.cc:643
rdl_mathutils.h
main
int main(int argc, char **argv)
Definition: RdlCustomJointMultiBodyTests.cpp:952
CustomJointTypeRevoluteX::jcalc
virtual void jcalc(Model &model, unsigned int joint_id, const Math::VectorNd &q, const Math::VectorNd &qdot)
Definition: RdlCustomJointMultiBodyTests.cpp:74
RobotDynamics::calcRelativePointJacobianAndJacobianDot6D
void calcRelativePointJacobianAndJacobianDot6D(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, Math::MatrixNd &GDot, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics=true)
Compute the point Jacobian of the origin of baseFrame, relative to the origin of relativeFrame, expressed in expressedInFrame. Also compute that point Jacobians time derivative.
Definition: Kinematics.cc:1033
CustomJointTypeRevoluteX::jcalc_X_lambda_S
virtual void jcalc_X_lambda_S(Model &model, unsigned int joint_id, const Math::VectorNd &q)
Definition: RdlCustomJointMultiBodyTests.cpp:80
RobotDynamics::calcPointJacobian6D
void calcPointJacobian6D(Model &model, const Math::VectorNd &Q, Math::MatrixNd &G, ReferenceFramePtr frame, bool update_kinematics=true)
Compute the 6D point jacobian of the origin of a reference frame If a position of a point is computed...
Definition: Kinematics.cc:604
CustomEulerZYXJoint::jcalc_X_lambda_S
virtual void jcalc_X_lambda_S(Model &model, unsigned int joint_id, const Math::VectorNd &q)
Definition: RdlCustomJointMultiBodyTests.cpp:141
RobotDynamics::ReferenceFramePtr
std::shared_ptr< ReferenceFrame > ReferenceFramePtr
Definition: ReferenceFrame.hpp:72
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::calcRelativePointJacobianDot6D
void calcRelativePointJacobianDot6D(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics=true)
Compute the time derivative of the 6D jacobian of the origin of a reference frame relative to the ori...
Definition: Kinematics.cc:877
RdlCustomJointMultiBodyFixture::reference_body_id
vector< vector< unsigned int > > reference_body_id
Definition: RdlCustomJointMultiBodyTests.cpp:387
CustomEulerZYXJoint::jcalc
virtual void jcalc(Model &model, unsigned int joint_id, const Math::VectorNd &q, const Math::VectorNd &qdot)
Definition: RdlCustomJointMultiBodyTests.cpp:99
RobotDynamics::calcPointJacobianDot6D
void calcPointJacobianDot6D(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, RobotDynamics::ReferenceFramePtr frame, Math::MatrixNd &G, bool update_kinematics=true)
Computes the time derivative of a point jacobian of a point on a body.
Definition: Kinematics.cc:826
RobotDynamics::Joint::custom_joint_index
unsigned int custom_joint_index
Definition: Joint.h:635
RobotDynamics::Utils::calcCentroidalMomentumMatrixDot
void calcCentroidalMomentumMatrixDot(Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, Math::MatrixNd &Adot, bool update_kinematics=true)
Calculates the time derivative of the centroidal momentum matrix, i.e. the matrix computed by RobotDy...
Definition: rdl_utils.cc:613
RobotDynamics::Joint
Describes a joint relative to the predecessor body.
Definition: Joint.h:228
RobotDynamics::calcBodySpatialJacobianDot
void calcBodySpatialJacobianDot(Model &model, const Math::VectorNd &Q, const Math::VectorNd QDot, unsigned int body_id, Math::MatrixNd &G, const bool update_kinematics=true)
Computes the time derivative of the spatial jacobian for a body. The result will be returned via the ...
Definition: Kinematics.cc:1367
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
unit_test_utils::checkMatrixNdEpsilonClose
static bool checkMatrixNdEpsilonClose(const RobotDynamics::Math::MatrixNd &t1, const RobotDynamics::Math::MatrixNd &t2, const double epsilon)
Definition: UnitTestUtils.hpp:154
TEST_PREC
const double TEST_PREC
Definition: RdlCustomJointMultiBodyTests.cpp:26
RobotDynamics::calcBodySpatialJacobian
void calcBodySpatialJacobian(Model &model, const Math::VectorNd &Q, unsigned int body_id, Math::MatrixNd &G, bool update_kinematics=true)
Computes the spatial jacobian for a body. The result will be returned via the G argument and represen...
Definition: Kinematics.cc:1310
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::Model::mCustomJoints
std::vector< CustomJoint * > mCustomJoints
Definition: Model.h:242
RobotDynamics::Utils::calcCentroidalMomentumMatrix
void calcCentroidalMomentumMatrix(Model &model, const Math::VectorNd &q, Math::MatrixNd &A, bool update_kinematics=true)
Calculates the centroidal momentum matrix, for a model. The centroidal momentum matrix is a matrix ...
Definition: rdl_utils.cc:560
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::calcRelativeBodySpatialJacobianDot
void calcRelativeBodySpatialJacobianDot(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=nullptr, bool update_kinematics=true)
Computes the rate of change of the jacobian, , with being the "base" frame, being the relative" fra...
Definition: Kinematics.cc:301
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
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
NUMBER_OF_BODIES
const int NUMBER_OF_BODIES
Definition: RdlCustomJointMultiBodyTests.cpp:28
RdlCustomJointMultiBodyFixture::custom_body_id
vector< vector< unsigned int > > custom_body_id
Definition: RdlCustomJointMultiBodyTests.cpp:388
RobotDynamics::gravityEffects
void gravityEffects(Model &model, Math::VectorNd &Tau)
Computes the gravity vector.
Definition: Dynamics.cc:100
TEST_F
TEST_F(RdlCustomJointMultiBodyFixture, UpdateKinematics)
Definition: RdlCustomJointMultiBodyTests.cpp:413
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::v_J
Math::SpatialMotionV v_J
Definition: Model.h:219
NUMBER_OF_MODELS
const int NUMBER_OF_MODELS
Definition: RdlCustomJointMultiBodyTests.cpp:27
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::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::X_J
std::vector< Math::SpatialTransform > X_J
Definition: Model.h:215
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::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

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