RdlCustomJointSingleBodyTests.cpp
Go to the documentation of this file.
1 
2 
3 #include <gtest/gtest.h>
4 
5 #include "UnitTestUtils.hpp"
6 
7 #include <iostream>
8 
9 #include "Fixtures.h"
10 #include "rdl_dynamics/Kinematics.h"
11 #include "rdl_dynamics/Dynamics.h"
12 #include "rdl_dynamics/Contacts.h"
13 #include "rdl_dynamics/rdl_utils.h"
14 #include <vector>
15 
16 using namespace std;
17 using namespace RobotDynamics;
18 using namespace RobotDynamics::Math;
19 
20 const double TEST_PREC = 2.0e-12;
21 const int NUMBER_OF_MODELS = 2;
22 
23 //==============================================================================
24 /*
25 
26  The purpose of this test is to test that all of the code in RDL
27  related to a single CustomJoint functions. To do this we will implement
28  joints that already exist in RBDL but using the CustomJoint interface. The
29  test will then numerically compare the results produced by the CustomJoint
30  and the equivalent built-in joint in RBDL. The following algorithms will
31  be tested:
32 
33  UpdateKinematicsCustom
34  Jacobians
35  InverseDynamics
36  CompositeRigidBodyAlgorithm
37  ForwardDynamics
38  CalcMInvTimestau
39  ForwardDynamicsContactsKokkevis
40 
41 */
42 //==============================================================================
43 //==============================================================================
44 /*
45  As a note, below are the basic fields and functions that every CustomJoint
46  class member must provide. Refer to Featherstone's dynamics text if the field
47  names below don't make sense to you.
48 
49  1. Extend from CustomJoint:
50 
51  struct CustomJointClass: public CustomJoint
52 
53  2. Make a default constructor, and initialize member variables
54  mDoFCount
55  S
56  d_u
57 
58  e.g.
59 
60  CustomJointClass()
61 
62  3. Implement the method jcalc. This method must populate X_J, v_J, c_J, and S.
63 
64  virtual void jcalc
65  model.X_J[joint_id]
66  model.v_J
67  model.c_J
68  model.mCustomJoints[joint.custom_joint_index]->S = S
69 
70  4. Implement the method jcalc_X_lambda_S. This method must populate X_lambda
71  and S.
72 
73  virtual void jcalc_X_lambda_S
74  model.X_lambda
75  model.mCustomJoints[joint.custom_joint_index]->S = S;
76 
77  */
78 //==============================================================================
79 // Custom Joint Code
80 //==============================================================================
81 struct CustomJointTypeRevoluteX : public CustomJoint
82 {
83  CustomJointTypeRevoluteX()
84  {
85  mDoFCount = 1;
86  S = MatrixNd::Zero(6, 1);
87  S(0, 0) = 1.0;
88  d_u = MatrixNd::Zero(mDoFCount, 1);
89  S_o = MatrixNd::Zero(6, 1);
90  }
91 
92  virtual void jcalc(Model& model, unsigned int joint_id, const Math::VectorNd& q, const Math::VectorNd& qdot)
93  {
94  model.X_J[joint_id] = Xrotx(q[model.mJoints[joint_id].q_index]);
95  model.v_J[joint_id][0] = qdot[model.mJoints[joint_id].q_index];
96 
97  model.bodyFrames[joint_id]->setTransformFromParent(model.X_J[joint_id] * model.X_T[joint_id]);
98  model.bodyFrames[joint_id]->update();
99  }
100 
101  virtual void jcalc_X_lambda_S(Model& model, unsigned int joint_id, const Math::VectorNd& q)
102  {
103  model.bodyFrames[joint_id]->setTransformFromParent(Xrotx(q[model.mJoints[joint_id].q_index]) * model.X_T[joint_id]);
104  model.bodyFrames[joint_id]->update();
105 
106  const Joint& joint = model.mJoints[joint_id];
107  model.mCustomJoints[joint.custom_joint_index]->S = S;
108  }
109 };
110 
111 struct CustomEulerZYXJoint : public CustomJoint
112 {
113  CustomEulerZYXJoint()
114  {
115  mDoFCount = 3;
116  S = MatrixNd::Zero(6, 3);
117  S_o = MatrixNd::Zero(6, 3);
118  d_u = MatrixNd::Zero(mDoFCount, 1);
119  }
120 
121  virtual void jcalc(Model& model, unsigned int joint_id, const Math::VectorNd& q, const Math::VectorNd& qdot)
122  {
123  double q0 = q[model.mJoints[joint_id].q_index];
124  double q1 = q[model.mJoints[joint_id].q_index + 1];
125  double q2 = q[model.mJoints[joint_id].q_index + 2];
126 
127  double s0 = sin(q0);
128  double c0 = cos(q0);
129  double s1 = sin(q1);
130  double c1 = cos(q1);
131  double s2 = sin(q2);
132  double c2 = cos(q2);
133 
134  model.X_J[joint_id].E =
135  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);
136 
137  S.setZero();
138  S(0, 0) = -s1;
139  S(0, 2) = 1.;
140 
141  S(1, 0) = c1 * s2;
142  S(1, 1) = c2;
143 
144  S(2, 0) = c1 * c2;
145  S(2, 1) = -s2;
146 
147  double qdot0 = qdot[model.mJoints[joint_id].q_index];
148  double qdot1 = qdot[model.mJoints[joint_id].q_index + 1];
149  double qdot2 = qdot[model.mJoints[joint_id].q_index + 2];
150 
151  Vector3d qdotv(qdot0, qdot1, qdot2);
152  model.v_J[joint_id].set(S * qdotv);
153 
154  S_o(0, 0) = -c1 * qdot1;
155  S_o(1, 0) = -s1 * s2 * qdot1 + c1 * c2 * qdot2;
156  S_o(1, 1) = -s2 * qdot2;
157  S_o(2, 0) = -s1 * c2 * qdot1 - c1 * s2 * qdot2;
158  S_o(2, 1) = -c2 * qdot2;
159 
160  model.c_J[joint_id] = S_o * qdotv;
161 
162  model.bodyFrames[joint_id]->setTransformFromParent(model.X_J[joint_id] * model.X_T[joint_id]);
163  model.bodyFrames[joint_id]->update();
164  }
165 
166  virtual void jcalc_X_lambda_S(Model& model, unsigned int joint_id, const Math::VectorNd& q)
167  {
168  double q0 = q[model.mJoints[joint_id].q_index];
169  double q1 = q[model.mJoints[joint_id].q_index + 1];
170  double q2 = q[model.mJoints[joint_id].q_index + 2];
171 
172  double s0 = sin(q0);
173  double c0 = cos(q0);
174  double s1 = sin(q1);
175  double c1 = cos(q1);
176  double s2 = sin(q2);
177  double c2 = cos(q2);
178 
179  model.bodyFrames[joint_id]->setTransformFromParent(SpatialTransform(Matrix3d(c0 * c1, s0 * c1, -s1, c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2,
180  c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2),
181  Vector3d(0., 0., 0.)) *
182  model.X_T[joint_id]);
183  model.bodyFrames[joint_id]->update();
184 
185  S.setZero();
186  S(0, 0) = -s1;
187  S(0, 2) = 1.;
188 
189  S(1, 0) = c1 * s2;
190  S(1, 1) = c2;
191 
192  S(2, 0) = c1 * c2;
193  S(2, 1) = -s2;
194 
195  const Joint& joint = model.mJoints[joint_id];
196  model.mCustomJoints[joint.custom_joint_index]->S = S;
197  }
198 };
199 
200 //==============================================================================
201 // Test Fixture
202 //==============================================================================
203 
204 struct CustomJointSingleBodyFixture : public testing::Test
205 {
206  CustomJointSingleBodyFixture()
207  {
208  }
209 
210  void SetUp()
211  {
212  reference_model.resize(NUMBER_OF_MODELS);
213  custom_model.resize(NUMBER_OF_MODELS);
214 
215  body.resize(NUMBER_OF_MODELS);
216  custom_joint.resize(NUMBER_OF_MODELS);
217 
218  reference_body_id.resize(NUMBER_OF_MODELS);
219  custom_body_id.resize(NUMBER_OF_MODELS);
220 
221  q.resize(NUMBER_OF_MODELS);
222  qdot.resize(NUMBER_OF_MODELS);
223  qddot.resize(NUMBER_OF_MODELS);
224  tau.resize(NUMBER_OF_MODELS);
225 
226  //========================================================
227  // Test Model 0: 3dof rotational custom joint vs. standard.
228  //========================================================
229 
230  custom_joint0 = CustomEulerZYXJoint();
231 
232  Matrix3d inertia0 = Matrix3d::Identity(3, 3);
233  Body body0 = Body(1., Vector3d(1.1, 1.2, 1.3), inertia0);
234 
235  ModelPtr reference0(new Model()), custom0(new Model());
236 
237  unsigned int reference_body_id0 = reference0->addBody(0, SpatialTransform(), Joint(JointTypeEulerZYX), body0);
238 
239  unsigned int custom_body_id0 = custom0->addBodyCustomJoint(0, SpatialTransform(), &custom_joint0, body0);
240 
241  VectorNd q0 = VectorNd::Zero(reference0->q_size);
242  VectorNd qdot0 = VectorNd::Zero(reference0->qdot_size);
243  VectorNd qddot0 = VectorNd::Zero(reference0->qdot_size);
244  VectorNd tau0 = VectorNd::Zero(reference0->qdot_size);
245 
246  reference_model.at(0) = reference0;
247  custom_model.at(0) = custom0;
248 
249  reference_body_id.at(0) = (reference_body_id0);
250  custom_body_id.at(0) = (custom_body_id0);
251 
252  body.at(0) = (body0);
253  custom_joint.at(0) = (&custom_joint0);
254 
255  q.at(0) = (q0);
256  qdot.at(0) = (qdot0);
257  qddot.at(0) = (qddot0);
258  tau.at(0) = (tau0);
259 
260  //========================================================
261  // Test Model 1: 1 dof rotational custom joint vs. standard.
262  //========================================================
263 
264  custom_joint1 = CustomJointTypeRevoluteX();
265 
266  ModelPtr reference1(new Model()), custom1(new Model());
267 
268  unsigned int reference_body_id1 = reference1->addBody(0, SpatialTransform(), Joint(JointTypeRevoluteX), body0);
269 
270  unsigned int custom_body_id1 = custom1->addBodyCustomJoint(0, SpatialTransform(), &custom_joint1, body0);
271 
272  VectorNd q1 = VectorNd::Zero(reference1->q_size);
273  VectorNd qdot1 = VectorNd::Zero(reference1->qdot_size);
274  VectorNd qddot1 = VectorNd::Zero(reference1->qdot_size);
275  VectorNd tau1 = VectorNd::Zero(reference1->qdot_size);
276 
277  reference_model.at(1) = (reference1);
278  custom_model.at(1) = (custom1);
279 
280  reference_body_id.at(1) = (reference_body_id1);
281  custom_body_id.at(1) = (custom_body_id1);
282 
283  body.at(1) = (body0);
284  custom_joint.at(1) = (&custom_joint1);
285 
286  q.at(1) = (q1);
287  qdot.at(1) = (qdot1);
288  qddot.at(1) = (qddot1);
289  tau.at(1) = (tau1);
290  }
291 
292  void TearDown()
293  {
294  for (unsigned int i = 0; i < reference_model.size(); i++)
295  {
296  EXPECT_TRUE(unit_test_utils::checkModelZeroVectorsAndMatrices(*reference_model[i]));
297  }
298 
299  for (unsigned int i = 0; i < custom_model.size(); i++)
300  {
301  EXPECT_TRUE(unit_test_utils::checkModelZeroVectorsAndMatrices(*custom_model[i]));
302  }
303  }
304 
305  vector<ModelPtr> reference_model;
306  vector<ModelPtr> custom_model;
307 
308  vector<Body> body;
309  vector<CustomJoint*> custom_joint;
310 
311  vector<unsigned int> reference_body_id;
312  vector<unsigned int> custom_body_id;
313 
314  vector<VectorNd> q;
315  vector<VectorNd> qdot;
316  vector<VectorNd> qddot;
317  vector<VectorNd> tau;
318  CustomJointTypeRevoluteX custom_joint1;
319  CustomEulerZYXJoint custom_joint0;
320 };
321 
322 //==============================================================================
323 //
324 // Tests
325 // UpdateKinematicsCustom
326 // Jacobians
327 // InverseDynamics
328 // CompositeRigidBodyAlgorithm
329 // ForwardDynamics
330 // CalcMInvTimestau
331 // ForwardDynamicsContactsKokkevis
332 //
333 //==============================================================================
334 
335 TEST_F(CustomJointSingleBodyFixture, UpdateKinematics)
336 {
337  VectorNd test;
338 
339  for (int idx = 0; idx < NUMBER_OF_MODELS; ++idx)
340  {
341  int dof = reference_model.at(idx)->dof_count;
342  for (unsigned int i = 0; i < dof; i++)
343  {
344  q.at(idx)[i] = i * 0.1;
345  qdot.at(idx)[i] = i * 0.15;
346  qddot.at(idx)[i] = i * 0.17;
347  }
348 
349  updateKinematics(*reference_model.at(idx), q.at(idx), qdot.at(idx), qddot.at(idx));
350  updateKinematics(*custom_model.at(idx), q.at(idx), qdot.at(idx), qddot.at(idx));
351 
352  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(reference_model.at(idx)->bodyFrames[reference_body_id.at(idx)]->getInverseTransformToRoot().E,
353  custom_model.at(idx)->bodyFrames[custom_body_id.at(idx)]->getInverseTransformToRoot().E,
354  unit_test_utils::TEST_PREC));
355 
356  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(reference_model.at(idx)->v[reference_body_id.at(idx)],
357  custom_model.at(idx)->v[custom_body_id.at(idx)], unit_test_utils::TEST_PREC));
358 
359  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(reference_model.at(idx)->a[reference_body_id.at(idx)],
360  custom_model.at(idx)->a[custom_body_id.at(idx)], unit_test_utils::TEST_PREC));
361  }
362 }
363 
364 TEST_F(CustomJointSingleBodyFixture, UpdateKinematicsCustom)
365 {
366  for (int idx = 0; idx < NUMBER_OF_MODELS; ++idx)
367  {
368  int dof = reference_model.at(idx)->dof_count;
369  for (unsigned int i = 0; i < dof; i++)
370  {
371  q.at(idx)[i] = i * 9.133758561390194e-01;
372  qdot.at(idx)[i] = i * 6.323592462254095e-01;
373  qddot.at(idx)[i] = i * 9.754040499940952e-02;
374  }
375 
376  updateKinematicsCustom(*reference_model.at(idx), &q.at(idx), NULL, NULL);
377  updateKinematicsCustom(*custom_model.at(idx), &q.at(idx), NULL, NULL);
378 
379  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(reference_model.at(idx)->bodyFrames[reference_body_id.at(idx)]->getInverseTransformToRoot().E,
380  custom_model.at(idx)->bodyFrames[custom_body_id.at(idx)]->getInverseTransformToRoot().E,
381  unit_test_utils::TEST_PREC));
382 
383  // velocity
384  updateKinematicsCustom(*reference_model.at(idx), &q.at(idx), &qdot.at(idx), NULL);
385  updateKinematicsCustom(*custom_model.at(idx), &q.at(idx), &qdot.at(idx), NULL);
386 
387  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(reference_model.at(idx)->v[reference_body_id.at(idx)],
388  custom_model.at(idx)->v[custom_body_id.at(idx)], unit_test_utils::TEST_PREC));
389 
390  // All
391  updateKinematicsCustom(*reference_model.at(idx), &q.at(idx), &qdot.at(idx), &qddot.at(idx));
392  updateKinematicsCustom(*custom_model.at(idx), &q.at(idx), &qdot.at(idx), &qddot.at(idx));
393 
394  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(reference_model.at(idx)->a[reference_body_id.at(idx)],
395  custom_model.at(idx)->a[custom_body_id.at(idx)], unit_test_utils::TEST_PREC));
396  }
397 }
398 
399 TEST_F(CustomJointSingleBodyFixture, Jacobians)
400 {
401  for (int idx = 0; idx < NUMBER_OF_MODELS; ++idx)
402  {
403  int dof = reference_model.at(idx)->dof_count;
404 
405  for (unsigned int i = 0; i < dof; i++)
406  {
407  q.at(idx)[i] = i * 9.133758561390194e-01;
408  qdot.at(idx)[i] = i * 6.323592462254095e-01;
409  qddot.at(idx)[i] = i * 9.754040499940952e-02;
410  }
411 
412  // position
413  updateKinematics(*reference_model.at(idx), q.at(idx), qdot.at(idx), qddot.at(idx));
414  updateKinematics(*custom_model.at(idx), q.at(idx), qdot.at(idx), qddot.at(idx));
415 
416  // Check the Spatial Jacobian
417  MatrixNd Gref = MatrixNd(MatrixNd::Zero(6, reference_model.at(idx)->dof_count));
418 
419  MatrixNd Gcus = MatrixNd(MatrixNd::Zero(6, reference_model.at(idx)->dof_count));
420 
421  calcBodySpatialJacobian(*reference_model.at(idx), q.at(idx), reference_body_id.at(idx), Gref);
422  calcBodySpatialJacobian(*custom_model.at(idx), q.at(idx), custom_body_id.at(idx), Gcus);
423 
424  for (int i = 0; i < 6; ++i)
425  {
426  for (int j = 0; j < dof; ++j)
427  {
428  EXPECT_NEAR(Gref(i, j), Gcus(i, j), TEST_PREC);
429  }
430  }
431 
432  // Check the 6d point Jacobian
433  Vector3d point_position(1.1, 1.2, 2.1);
434 
435  calcPointJacobian6D(*reference_model.at(idx), q.at(idx), reference_body_id.at(idx), point_position, Gref);
436  calcPointJacobian6D(*custom_model.at(idx), q.at(idx), custom_body_id.at(idx), point_position, Gcus);
437 
438  for (int i = 0; i < 6; ++i)
439  {
440  for (int j = 0; j < dof; ++j)
441  {
442  EXPECT_NEAR(Gref(i, j), Gcus(i, j), TEST_PREC);
443  }
444  }
445 
446  // Check the 3d point Jacobian
447  MatrixNd GrefPt = MatrixNd::Constant(3, reference_model.at(idx)->dof_count, 0.);
448  MatrixNd GcusPt = MatrixNd::Constant(3, reference_model.at(idx)->dof_count, 0.);
449 
450  calcPointJacobian(*reference_model.at(idx), q.at(idx), reference_body_id.at(idx), point_position, GrefPt);
451  calcPointJacobian(*custom_model.at(idx), q.at(idx), custom_body_id.at(idx), point_position, GcusPt);
452 
453  for (int i = 0; i < 3; ++i)
454  {
455  for (int j = 0; j < dof; ++j)
456  {
457  EXPECT_NEAR(GrefPt(i, j), GcusPt(i, j), TEST_PREC);
458  }
459  }
460 
461  // Check the body spatial jacobian dot
462  MatrixNd GDotrefPt = MatrixNd::Constant(6, reference_model.at(idx)->dof_count, 0.);
463  MatrixNd GDotcusPt = MatrixNd::Constant(6, reference_model.at(idx)->dof_count, 0.);
464 
465  calcBodySpatialJacobianDot(*reference_model.at(idx), q.at(idx), qdot.at(idx), reference_body_id.at(idx), GDotrefPt);
466  calcBodySpatialJacobianDot(*custom_model.at(idx), q.at(idx), qdot.at(idx), custom_body_id.at(idx), GDotcusPt);
467 
468  for (int i = 0; i < GDotcusPt.rows(); ++i)
469  {
470  for (int j = 0; j < GDotcusPt.cols(); ++j)
471  {
472  EXPECT_NEAR(GDotrefPt(i, j), GDotcusPt(i, j), TEST_PREC);
473  }
474  }
475 
476  GDotrefPt = MatrixNd::Constant(6, reference_model.at(idx)->dof_count, 0.);
477  GDotcusPt = MatrixNd::Constant(6, reference_model.at(idx)->dof_count, 0.);
478 
479  calcPointJacobianDot6D(*reference_model.at(idx), q.at(idx), qdot.at(idx), reference_body_id.at(idx), point_position, GDotrefPt);
480  calcPointJacobianDot6D(*custom_model.at(idx), q.at(idx), qdot.at(idx), custom_body_id.at(idx), point_position, GDotcusPt);
481 
482  for (int i = 0; i < GDotcusPt.rows(); ++i)
483  {
484  for (int j = 0; j < GDotcusPt.cols(); ++j)
485  {
486  EXPECT_NEAR(GDotrefPt(i, j), GDotcusPt(i, j), TEST_PREC);
487  }
488  }
489 
490  GDotrefPt = MatrixNd::Constant(3, reference_model.at(idx)->dof_count, 0.);
491  GDotcusPt = MatrixNd::Constant(3, reference_model.at(idx)->dof_count, 0.);
492 
493  calcPointJacobianDot(*reference_model.at(idx), q.at(idx), qdot.at(idx), reference_body_id.at(idx), point_position, GDotrefPt);
494  calcPointJacobianDot(*custom_model.at(idx), q.at(idx), qdot.at(idx), custom_body_id.at(idx), point_position, GDotcusPt);
495 
496  for (int i = 0; i < GDotcusPt.rows(); ++i)
497  {
498  for (int j = 0; j < GDotcusPt.cols(); ++j)
499  {
500  EXPECT_NEAR(GDotrefPt(i, j), GDotcusPt(i, j), TEST_PREC);
501  }
502  }
503 
504  MatrixNd Aref = MatrixNd::Constant(6, reference_model.at(idx)->dof_count, 0.);
505  MatrixNd Acus = MatrixNd::Constant(6, reference_model.at(idx)->dof_count, 0.);
506 
507  Utils::calcCentroidalMomentumMatrix(*reference_model.at(idx), q.at(idx), Aref);
508  Utils::calcCentroidalMomentumMatrix(*custom_model.at(idx), q.at(idx), Acus);
509 
510  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(Aref, Acus, TEST_PREC));
511  }
512 }
513 
514 TEST_F(CustomJointSingleBodyFixture, CentroidalMomentum)
515 {
516  for (int idx = 0; idx < NUMBER_OF_MODELS; ++idx)
517  {
518  int dof = reference_model.at(idx)->dof_count;
519 
520  for (unsigned int i = 0; i < dof; i++)
521  {
522  q.at(idx)[i] = i * 9.133758561390194e-01;
523  qdot.at(idx)[i] = i * 6.323592462254095e-01;
524  qddot.at(idx)[i] = i * 9.754040499940952e-02;
525  }
526 
527  // Check the Spatial Jacobian
528  MatrixNd ADotref = MatrixNd(MatrixNd::Zero(6, reference_model.at(idx)->qdot_size));
529  MatrixNd ADotcus = MatrixNd(MatrixNd::Zero(6, reference_model.at(idx)->qdot_size));
530 
531  Utils::calcCentroidalMomentumMatrixDot(*reference_model.at(idx), q.at(idx), qdot.at(idx), ADotref);
532  Utils::calcCentroidalMomentumMatrixDot(*custom_model.at(idx), q.at(idx), qdot.at(idx), ADotcus);
533 
534  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(ADotref, ADotcus, unit_test_utils::TEST_PREC));
535  }
536 }
537 
538 TEST_F(CustomJointSingleBodyFixture, InverseDynamics)
539 {
540  for (int idx = 0; idx < NUMBER_OF_MODELS; ++idx)
541  {
542  int dof = reference_model.at(idx)->dof_count;
543 
544  for (unsigned int i = 0; i < dof; i++)
545  {
546  q.at(idx)[i] = i * 9.133758561390194e-01;
547  qdot.at(idx)[i] = i * 6.323592462254095e-01;
548  qddot.at(idx)[i] = i * 9.754040499940952e-02;
549  }
550 
551  // position
552 
553  VectorNd tauRef = VectorNd::Zero(reference_model.at(idx)->qdot_size);
554  VectorNd tauCus = VectorNd::Zero(reference_model.at(idx)->qdot_size);
555 
556  inverseDynamics(*reference_model.at(idx), q.at(idx), qdot.at(idx), qddot.at(idx), tauRef);
557  inverseDynamics(*custom_model.at(idx), q.at(idx), qdot.at(idx), qddot.at(idx), tauCus);
558 
559  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(tauRef, tauCus, unit_test_utils::TEST_PREC));
560  }
561 }
562 
563 TEST_F(CustomJointSingleBodyFixture, CompositeRigidBodyAlgorithm)
564 {
565  for (int idx = 0; idx < NUMBER_OF_MODELS; ++idx)
566  {
567  int dof = reference_model.at(idx)->dof_count;
568 
569  for (unsigned int i = 0; i < dof; i++)
570  {
571  q.at(idx)[i] = (i + 0.1) * 9.133758561390194e-01;
572  qdot.at(idx)[i] = (i + 0.1) * 6.323592462254095e-01;
573  tau.at(idx)[i] = (i + 0.1) * 9.754040499940952e-02;
574  }
575 
576  MatrixNd h_ref = MatrixNd::Constant(dof, dof, 0.);
577  VectorNd c_ref = VectorNd::Constant(dof, 0.);
578  VectorNd qddot_zero_ref = VectorNd::Constant(dof, 0.);
579  VectorNd qddot_crba_ref = VectorNd::Constant(dof, 0.);
580 
581  MatrixNd h_cus = MatrixNd::Constant(dof, dof, 0.);
582  VectorNd c_cus = VectorNd::Constant(dof, 0.);
583  VectorNd qddot_zero_cus = VectorNd::Constant(dof, 0.);
584  VectorNd qddot_crba_cus = VectorNd::Constant(dof, 0.);
585 
586  VectorNd qddotRef = VectorNd::Zero(dof);
587  VectorNd qddotCus = VectorNd::Zero(dof);
588 
589  // Ref
590  forwardDynamics(*reference_model.at(idx), q.at(idx), qdot.at(idx), tau.at(idx), qddotRef);
591  compositeRigidBodyAlgorithm(*reference_model.at(idx), q.at(idx), h_ref);
592  inverseDynamics(*reference_model.at(idx), q.at(idx), qdot.at(idx), qddot_zero_ref, c_ref);
593  linSolveGaussElimPivot(h_ref, c_ref * -1. + tau.at(idx), qddot_crba_ref);
594 
595  // Custom
596  forwardDynamics(*custom_model.at(idx), q.at(idx), qdot.at(idx), tau.at(idx), qddotCus);
597  compositeRigidBodyAlgorithm(*custom_model.at(idx), q.at(idx), h_cus);
598  inverseDynamics(*custom_model.at(idx), q.at(idx), qdot.at(idx), qddot_zero_cus, c_cus);
599  linSolveGaussElimPivot(h_cus, c_cus * -1. + tau.at(idx), qddot_crba_cus);
600 
601  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_crba_ref, qddot_crba_cus, unit_test_utils::TEST_PREC));
602  }
603 }
604 
605 TEST_F(CustomJointSingleBodyFixture, ForwardDynamics)
606 {
607  for (int idx = 0; idx < NUMBER_OF_MODELS; ++idx)
608  {
609  int dof = reference_model.at(idx)->dof_count;
610 
611  for (unsigned int i = 0; i < dof; i++)
612  {
613  q.at(idx)[i] = (i + 0.1) * 9.133758561390194e-01;
614  qdot.at(idx)[i] = (i + 0.1) * 6.323592462254095e-01;
615  qddot.at(idx)[i] = (i + 0.1) * 2.323592499940952e-01;
616  tau.at(idx)[i] = (i + 0.1) * 9.754040499940952e-02;
617  }
618 
619  VectorNd qddotRef = VectorNd::Zero(reference_model.at(idx)->qdot_size);
620  VectorNd qddotCus = VectorNd::Zero(reference_model.at(idx)->qdot_size);
621 
622  forwardDynamics(*reference_model.at(idx), q.at(idx), qdot.at(idx), tau.at(idx), qddotRef);
623  forwardDynamics(*custom_model.at(idx), q.at(idx), qdot.at(idx), tau.at(idx), qddotCus);
624 
625  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddotRef, qddotCus, unit_test_utils::TEST_PREC));
626  }
627 }
628 
629 TEST_F(CustomJointSingleBodyFixture, CalcMInvTimestau)
630 {
631  for (int idx = 0; idx < NUMBER_OF_MODELS; ++idx)
632  {
633  int dof = reference_model.at(idx)->dof_count;
634 
635  for (unsigned int i = 0; i < dof; i++)
636  {
637  q.at(idx)[i] = (i + 0.1) * 9.133758561390194e-01;
638  tau.at(idx)[i] = (i + 0.1) * 9.754040499940952e-02;
639  }
640 
641  // reference
642  VectorNd qddot_minv_ref = VectorNd::Zero(dof);
643  calcMInvTimesTau(*reference_model.at(idx), q.at(idx), tau.at(idx), qddot_minv_ref, true);
644 
645  // custom
646  VectorNd qddot_minv_cus = VectorNd::Zero(dof);
647  calcMInvTimesTau(*custom_model.at(idx), q.at(idx), tau.at(idx), qddot_minv_cus, true);
648  // check.
649  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_minv_ref, qddot_minv_cus, unit_test_utils::TEST_PREC));
650  }
651 }
652 
653 TEST_F(CustomJointSingleBodyFixture, ForwardDynamicsContactsKokkevis)
654 {
655  for (int idx = 0; idx < NUMBER_OF_MODELS; ++idx)
656  {
657  int dof = reference_model.at(idx)->dof_count;
658 
659  // Adding a 1 constraint to a system with 1 dof is
660  // a no-no
661  if (dof > 1)
662  {
663  for (unsigned int i = 0; i < dof; i++)
664  {
665  q.at(idx)[i] = (i + 0.1) * 9.133758561390194e-01;
666  qdot.at(idx)[i] = (i + 0.1) * 6.323592462254095e-01;
667 
668  tau.at(idx)[i] = (i + 0.1) * 9.754040499940952e-02;
669  }
670 
671  VectorNd qddot_ref = VectorNd::Zero(dof);
672  VectorNd qddot_cus = VectorNd::Zero(dof);
673 
674  VectorNd qdot_plus_ref = VectorNd::Zero(dof);
675  VectorNd qdot_plus_cus = VectorNd::Zero(dof);
676 
677  Vector3d contact_point(0., 1., 0.);
678 
679  ConstraintSet constraint_set_ref;
680  ConstraintSet constraint_set_cus;
681 
682  // Reference
683  constraint_set_ref.addConstraint(reference_body_id.at(idx), contact_point, Vector3d(1., 0., 0.), "ground_x");
684  constraint_set_ref.addConstraint(reference_body_id.at(idx), contact_point, Vector3d(0., 1., 0.), "ground_y");
685  constraint_set_ref.bind(*reference_model.at(idx));
686 
687  // Custom
688  constraint_set_cus.addConstraint(custom_body_id.at(idx), contact_point, Vector3d(1., 0., 0.), "ground_x");
689  constraint_set_cus.addConstraint(custom_body_id.at(idx), contact_point, Vector3d(0., 1., 0.), "ground_y");
690  constraint_set_cus.bind(*custom_model.at(idx));
691 
692  computeContactImpulsesDirect(*reference_model.at(idx), q.at(idx), qdot.at(idx), constraint_set_ref, qdot_plus_ref);
693  forwardDynamicsContactsKokkevis(*reference_model.at(idx), q.at(idx), qdot_plus_ref, tau.at(idx), constraint_set_ref, qddot_ref);
694 
695  computeContactImpulsesDirect(*custom_model.at(idx), q.at(idx), qdot.at(idx), constraint_set_cus, qdot_plus_cus);
696  forwardDynamicsContactsKokkevis(*custom_model.at(idx), q.at(idx), qdot_plus_cus, tau.at(idx), constraint_set_cus, qddot_cus);
697 
698  VectorNd qdot_plus_error = qdot_plus_ref - qdot_plus_cus;
699  VectorNd qddot_error = qddot_ref - qddot_cus;
700 
701  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qdot_plus_ref, qdot_plus_cus, unit_test_utils::TEST_PREC));
702 
703  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_ref, qddot_cus, unit_test_utils::TEST_PREC));
704  }
705  }
706 }
707 
708 int main(int argc, char** argv)
709 {
710  ::testing::InitGoogleTest(&argc, argv);
711  return RUN_ALL_TESTS();
712 }
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::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
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
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
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
CustomJointTypeRevoluteX::jcalc
virtual void jcalc(Model &model, unsigned int joint_id, const Math::VectorNd &q, const Math::VectorNd &qdot)
Definition: RdlCustomJointSingleBodyTests.cpp:92
CustomJointTypeRevoluteX::jcalc_X_lambda_S
virtual void jcalc_X_lambda_S(Model &model, unsigned int joint_id, const Math::VectorNd &q)
Definition: RdlCustomJointSingleBodyTests.cpp:101
RobotDynamics::Model::bodyFrames
std::vector< ReferenceFramePtr > bodyFrames
Definition: Model.h:145
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: RdlCustomJointSingleBodyTests.cpp:166
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
CustomEulerZYXJoint::jcalc
virtual void jcalc(Model &model, unsigned int joint_id, const Math::VectorNd &q, const Math::VectorNd &qdot)
Definition: RdlCustomJointSingleBodyTests.cpp:121
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
NUMBER_OF_MODELS
const int NUMBER_OF_MODELS
Definition: RdlCustomJointSingleBodyTests.cpp:21
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
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
TEST_PREC
const double TEST_PREC
Definition: RdlCustomJointSingleBodyTests.cpp:20
TEST_F
TEST_F(CustomJointSingleBodyFixture, UpdateKinematics)
Definition: RdlCustomJointSingleBodyTests.cpp:335
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
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
RobotDynamics::Model::v_J
Math::SpatialMotionV v_J
Definition: Model.h:219
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
main
int main(int argc, char **argv)
Definition: RdlCustomJointSingleBodyTests.cpp:708
RobotDynamics
Namespace for all structures of the RobotDynamics library.
Definition: Body.h:21
RobotDynamics::Model::X_T
std::vector< Math::SpatialTransform > X_T
Transformations from the parent body to the frame of the joint.
Definition: Model.h:225
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