RdlCustomJointTests.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 
3 #include <iostream>
4 
5 #include "UnitTestUtils.hpp"
6 #include "Fixtures.h"
7 #include "rdl_dynamics/Kinematics.h"
8 #include "rdl_dynamics/Dynamics.h"
9 #include "rdl_dynamics/rdl_utils.h"
10 
11 using namespace std;
12 using namespace RobotDynamics;
13 using namespace RobotDynamics::Math;
14 
15 const double TEST_PREC = 1.0e-12;
16 
17 struct RdlCustomEulerZYXJoint : public CustomJoint
18 {
19  RdlCustomEulerZYXJoint()
20  {
21  mDoFCount = 3;
22  S = MatrixNd::Zero(6, 3);
23  S_o = MatrixNd::Zero(6, 3);
24  };
25 
26  virtual void jcalc(Model& model, unsigned int joint_id, const Math::VectorNd& q, const Math::VectorNd& qdot)
27  {
28  double q0 = q[model.mJoints[joint_id].q_index];
29  double q1 = q[model.mJoints[joint_id].q_index + 1];
30  double q2 = q[model.mJoints[joint_id].q_index + 2];
31 
32  double s0 = sin(q0);
33  double c0 = cos(q0);
34  double s1 = sin(q1);
35  double c1 = cos(q1);
36  double s2 = sin(q2);
37  double c2 = cos(q2);
38 
39  model.X_J[joint_id].E =
40  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);
41  model.bodyFrames[joint_id]->setTransformFromParent(SpatialTransform(Matrix3d(c0 * c1, s0 * c1, -s1, c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2,
42  c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2),
43  Vector3d(0., 0., 0.)) *
44  model.X_T[joint_id]);
45 
46  S(0, 0) = -s1;
47  S(0, 2) = 1.;
48 
49  S(1, 0) = c1 * s2;
50  S(1, 1) = c2;
51 
52  S(2, 0) = c1 * c2;
53  S(2, 1) = -s2;
54 
55  double qdot0 = qdot[model.mJoints[joint_id].q_index];
56  double qdot1 = qdot[model.mJoints[joint_id].q_index + 1];
57  double qdot2 = qdot[model.mJoints[joint_id].q_index + 2];
58 
59  Vector3d qdotv(qdot0, qdot1, qdot2);
60  model.v_J[joint_id].set(S * qdotv);
61 
62  S_o(0, 0) = -c1 * qdot1;
63  S_o(1, 0) = -s1 * s2 * qdot1 + c1 * c2 * qdot2;
64  S_o(1, 1) = -s2 * qdot2;
65  S_o(2, 0) = -s1 * c2 * qdot1 - c1 * s2 * qdot2;
66  S_o(2, 1) = -c2 * qdot2;
67 
68  model.c_J[joint_id] = S_o * qdotv; //.set(-c1 * qdot0 * qdot1, -s1 * s2 * qdot0 * qdot1 + c1 * c2 * qdot0 * qdot2 - s2 * qdot1 * qdot2, -s1 * c2 * qdot0 * qdot1
69  //- c1 * s2 * qdot0 * qdot2 - c2 * qdot1 * qdot2, 0., 0., 0.);
70 
71  model.bodyFrames[joint_id]->update();
72  }
73 
74  virtual void jcalc_X_lambda_S(Model& model, unsigned int joint_id, const Math::VectorNd& q)
75  {
76  double q0 = q[model.mJoints[joint_id].q_index];
77  double q1 = q[model.mJoints[joint_id].q_index + 1];
78  double q2 = q[model.mJoints[joint_id].q_index + 2];
79 
80  double s0 = sin(q0);
81  double c0 = cos(q0);
82  double s1 = sin(q1);
83  double c1 = cos(q1);
84  double s2 = sin(q2);
85  double c2 = cos(q2);
86 
87  model.bodyFrames[joint_id]->setTransformFromParent(SpatialTransform(Matrix3d(c0 * c1, s0 * c1, -s1, c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2,
88  c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2),
89  Vector3d(0., 0., 0.)) *
90  model.X_T[joint_id]);
91  model.bodyFrames[joint_id]->update();
92 
93  S.setZero();
94  S(0, 0) = -s1;
95  S(0, 2) = 1.;
96  S(1, 0) = c1 * s2;
97  S(1, 1) = c2;
98  S(2, 0) = c1 * c2;
99  S(2, 1) = -s2;
100  }
101 };
102 
103 struct RdlCustomJointFixture : public testing::Test
104 {
105  RdlCustomJointFixture()
106  {
107  }
108 
109  void SetUp()
110  {
111  custom_joint = new RdlCustomEulerZYXJoint();
112 
113  Matrix3d inertia = Matrix3d::Identity(3, 3);
114  body = Body(1., Vector3d(1.1, 1.2, 1.3), inertia);
115  reference_body_id = reference_model.addBody(0, SpatialTransform(), Joint(JointTypeEulerZYX), body);
116  custom_body_id = custom_model.addBodyCustomJoint(0, SpatialTransform(), custom_joint, body);
117 
118  q = VectorNd::Zero(reference_model.q_size);
119  qdot = VectorNd::Zero(reference_model.qdot_size);
120  qddot = VectorNd::Zero(reference_model.qdot_size);
121  tau = VectorNd::Zero(reference_model.qdot_size);
122  }
123 
124  void TearDown()
125  {
126  EXPECT_TRUE(unit_test_utils::checkModelZeroVectorsAndMatrices(reference_model));
127  EXPECT_TRUE(unit_test_utils::checkModelZeroVectorsAndMatrices(custom_model));
128  delete custom_joint;
129  }
130 
131  Model reference_model;
132  Model custom_model;
133 
134  Body body;
135  CustomJoint* custom_joint;
136 
137  unsigned int reference_body_id;
138  unsigned int custom_body_id;
139 
140  VectorNd q;
141  VectorNd qdot;
142  VectorNd qddot;
143  VectorNd tau;
144 };
145 
146 TEST_F(RdlCustomJointFixture, updateKinematics)
147 {
148  for (unsigned int i = 0; i < 3; i++)
149  {
150  q[i] = i * 0.1;
151  qdot[i] = i * 0.15;
152  qddot[i] = i * 0.17;
153  }
154 
155  updateKinematics(reference_model, q, qdot, qddot);
156  updateKinematics(custom_model, q, qdot, qddot);
157 
158  EXPECT_TRUE(unit_test_utils::checkMatrix3dEq(reference_model.bodyFrames[reference_body_id]->getTransformToRoot().E,
159  custom_model.bodyFrames[custom_body_id]->getTransformToRoot().E));
160 
161  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEq(reference_model.v[reference_body_id], custom_model.v[custom_body_id]));
162 
163  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEq(reference_model.a[reference_body_id], custom_model.a[custom_body_id]));
164 }
165 
166 TEST_F(RdlCustomJointFixture, updateKinematicsCustom)
167 {
168  for (unsigned int i = 0; i < 3; i++)
169  {
170  q[i] = i * 0.1;
171  qdot[i] = i * 0.15;
172  qddot[i] = i * 0.17;
173  }
174 
175  updateKinematicsCustom(reference_model, &q, nullptr, nullptr);
176  updateKinematicsCustom(custom_model, &q, nullptr, nullptr);
177 
178  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(reference_model.bodyFrames[reference_body_id]->getTransformToRoot().toMatrix(),
179  custom_model.bodyFrames[custom_body_id]->getTransformToRoot().toMatrix(), unit_test_utils::TEST_PREC));
180 
181  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEq(Math::SpatialVectorZero, custom_model.v[custom_body_id]));
182  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEq(Math::SpatialVectorZero, custom_model.a[custom_body_id]));
183 
184  updateKinematicsCustom(reference_model, &q, &qdot, nullptr);
185  updateKinematicsCustom(custom_model, &q, &qdot, nullptr);
186 
187  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(reference_model.bodyFrames[reference_body_id]->getTransformToRoot().toMatrix(),
188  custom_model.bodyFrames[custom_body_id]->getTransformToRoot().toMatrix(), unit_test_utils::TEST_PREC));
189 
190  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEq(reference_model.v[reference_body_id], custom_model.v[custom_body_id]));
191  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEq(Math::SpatialVectorZero, custom_model.a[custom_body_id]));
192 
193  updateKinematicsCustom(reference_model, &q, &qdot, &qddot);
194  updateKinematicsCustom(custom_model, &q, &qdot, &qddot);
195 
196  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(reference_model.bodyFrames[reference_body_id]->getTransformToRoot().toMatrix(),
197  custom_model.bodyFrames[custom_body_id]->getTransformToRoot().toMatrix(), unit_test_utils::TEST_PREC));
198 
199  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEq(reference_model.v[reference_body_id], custom_model.v[custom_body_id]));
200 
201  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEq(reference_model.a[reference_body_id], custom_model.a[custom_body_id]));
202 }
203 
204 TEST_F(RdlCustomJointFixture, inverseDynamics)
205 {
206  for (unsigned int i = 0; i < 3; i++)
207  {
208  q[i] = i * 0.1;
209  qdot[i] = i * 0.15;
210  qddot[i] = i * 0.17;
211  }
212 
213  VectorNd tau_ref = VectorNd::Zero(tau.size());
214  VectorNd tau_cust = VectorNd::Zero(tau.size());
215 
216  inverseDynamics(reference_model, q, qdot, qddot, tau_ref);
217  inverseDynamics(custom_model, q, qdot, qddot, tau_cust);
218 
219  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(tau_ref, tau_cust, unit_test_utils::TEST_PREC));
220 }
221 
222 TEST_F(RdlCustomJointFixture, compositeRigidBodyAlgorithm)
223 {
224  for (unsigned int i = 0; i < 3; i++)
225  {
226  q[i] = i * 0.1;
227  qdot[i] = i * 0.15;
228  qddot[i] = i * 0.17;
229  }
230 
231  MatrixNd H_ref(reference_model.qdot_size, reference_model.qdot_size);
232  MatrixNd H_cust(custom_model.qdot_size, custom_model.qdot_size);
233 
234  compositeRigidBodyAlgorithm(reference_model, q, H_ref);
235  compositeRigidBodyAlgorithm(custom_model, q, H_cust);
236 
237  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(H_ref, H_cust, unit_test_utils::TEST_PREC));
238 }
239 
240 TEST_F(RdlCustomJointFixture, forwardDynamics)
241 {
242  for (unsigned int i = 0; i < 3; i++)
243  {
244  q[i] = i * 0.1;
245  qdot[i] = i * 0.15;
246  qddot[i] = i * 0.17;
247  }
248 
249  VectorNd qddot_ref = VectorNd::Zero(qdot.size());
250  VectorNd qddot_cust = VectorNd::Zero(qdot.size());
251 
252  forwardDynamics(reference_model, q, qdot, tau, qddot_ref);
253  forwardDynamics(custom_model, q, qdot, tau, qddot_cust);
254 
255  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_ref, qddot_cust, unit_test_utils::TEST_PREC));
256 }
257 
258 TEST_F(RdlCustomJointFixture, calcMInvTimesTau)
259 {
260  for (unsigned int i = 0; i < 3; i++)
261  {
262  q[i] = i * 0.1;
263  qdot[i] = i * 0.15;
264  qddot[i] = i * 0.17;
265  tau[i] = i * 0.12;
266  }
267 
268  VectorNd qddot_ref = VectorNd::Zero(qdot.size());
269  VectorNd qddot_cust = VectorNd::Zero(qdot.size());
270 
271  calcMInvTimesTau(custom_model, q, tau, qddot_cust);
272  calcMInvTimesTau(reference_model, q, tau, qddot_ref);
273 
274  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_cust, qddot_ref, unit_test_utils::TEST_PREC));
275 }
276 
277 TEST_F(RdlCustomJointFixture, nonlinearEffects)
278 {
279  for (unsigned int i = 0; i < 3; i++)
280  {
281  q[i] = i * 0.1;
282  qdot[i] = i * 0.15;
283  qddot[i] = i * 0.17;
284  tau[i] = i * 0.12;
285  }
286 
287  VectorNd tau_ref = VectorNd::Zero(qdot.size());
288  VectorNd tau_cust = VectorNd::Zero(qdot.size());
289 
290  nonlinearEffects(custom_model, q, qdot, tau_cust);
291  nonlinearEffects(reference_model, q, qdot, tau_ref);
292 
293  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(tau_cust, tau_ref, unit_test_utils::TEST_PREC));
294 }
295 
296 TEST_F(RdlCustomJointFixture, calcPointJacobian)
297 {
298  for (unsigned int i = 0; i < 3; i++)
299  {
300  q[i] = i * 0.1;
301  qdot[i] = i * 0.15;
302  qddot[i] = i * 0.17;
303  tau[i] = i * 0.12;
304  }
305 
306  MatrixNd G_cust(3, custom_model.qdot_size);
307  MatrixNd G_ref(3, reference_model.qdot_size);
308 
309  Math::Vector3d point_position(0.1, -0.2, 0.1);
310 
311  calcPointJacobian(custom_model, q, custom_body_id, point_position, G_cust);
312  calcPointJacobian(reference_model, q, reference_body_id, point_position, G_ref);
313 
314  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(G_cust, G_ref, unit_test_utils::TEST_PREC));
315 }
316 
317 TEST_F(RdlCustomJointFixture, calcPointJacobian6D)
318 {
319  for (unsigned int i = 0; i < 3; i++)
320  {
321  q[i] = i * 0.1;
322  qdot[i] = i * 0.15;
323  qddot[i] = i * 0.17;
324  tau[i] = i * 0.12;
325  }
326 
327  MatrixNd G_cust(6, custom_model.qdot_size);
328  MatrixNd G_ref(6, reference_model.qdot_size);
329 
330  Math::Vector3d point_position(0.1, -0.2, 0.1);
331 
332  calcPointJacobian6D(custom_model, q, custom_body_id, point_position, G_cust);
333  calcPointJacobian6D(reference_model, q, reference_body_id, point_position, G_ref);
334 
335  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(G_cust, G_ref, unit_test_utils::TEST_PREC));
336 }
337 
338 TEST_F(RdlCustomJointFixture, calcBodySpatialJacobian)
339 {
340  for (unsigned int i = 0; i < 3; i++)
341  {
342  q[i] = i * 0.1;
343  qdot[i] = i * 0.15;
344  qddot[i] = i * 0.17;
345  tau[i] = i * 0.12;
346  }
347 
348  MatrixNd G_cust(6, custom_model.qdot_size);
349  MatrixNd G_ref(6, reference_model.qdot_size);
350 
351  Math::Vector3d point_position(0.1, -0.2, 0.1);
352 
353  calcBodySpatialJacobian(custom_model, q, custom_body_id, G_cust);
354  calcBodySpatialJacobian(reference_model, q, reference_body_id, G_ref);
355 
356  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(G_cust, G_ref, unit_test_utils::TEST_PREC));
357 }
358 
359 TEST_F(RdlCustomJointFixture, calcBodySpatialJacobianDot)
360 {
361  for (unsigned int i = 0; i < 3; i++)
362  {
363  q[i] = i * 0.1;
364  qdot[i] = i * 0.15;
365  qddot[i] = i * 0.17;
366  tau[i] = i * 0.12;
367  }
368 
369  MatrixNd GDot_cust(6, custom_model.qdot_size), GDot_ref(6, custom_model.qdot_size);
370  GDot_cust.setZero();
371  GDot_ref.setZero();
372 
373  calcBodySpatialJacobianDot(custom_model, q, qdot, custom_body_id, GDot_cust);
374  calcBodySpatialJacobianDot(reference_model, q, qdot, custom_body_id, GDot_ref);
375 
376  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(GDot_cust, GDot_ref, unit_test_utils::TEST_PREC));
377 }
378 
379 TEST_F(RdlCustomJointFixture, calcPointJacobianDot)
380 {
381  for (unsigned int i = 0; i < 3; i++)
382  {
383  q[i] = i * 0.1;
384  qdot[i] = i * 0.15;
385  qddot[i] = i * 0.17;
386  tau[i] = i * 0.12;
387  }
388 
389  MatrixNd GDot_cust(3, custom_model.qdot_size), GDot_ref(3, custom_model.qdot_size);
390  GDot_cust.setZero();
391  GDot_ref.setZero();
392 
393  Vector3d p(-0.1, -0.2, 1.1);
394  calcPointJacobianDot(custom_model, q, qdot, custom_body_id, p, GDot_cust);
395  calcPointJacobianDot(reference_model, q, qdot, custom_body_id, p, GDot_ref);
396 
397  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(GDot_cust, GDot_ref, unit_test_utils::TEST_PREC));
398 }
399 
400 TEST_F(RdlCustomJointFixture, calcCentroidalMomentumMatrix)
401 {
402  for (unsigned int i = 0; i < 3; i++)
403  {
404  q[i] = i * 0.1;
405  qdot[i] = i * 0.15;
406  }
407 
408  MatrixNd A(6, custom_model.qdot_size);
409  A.setZero();
410 
411  Vector3d com, com_velocity, ang_momentum;
412  double mass;
413 
414  Vector3d p(-0.1, -0.2, 1.1);
415  Utils::calcCentroidalMomentumMatrix(custom_model, q, A);
416  Utils::calcCenterOfMass(custom_model, q, qdot, mass, com, &com_velocity, &ang_momentum);
417 
418  SpatialVector m_exp;
419  m_exp.setLinearPart(com_velocity * mass);
420  m_exp.setAngularPart(ang_momentum);
421 
422  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(SpatialVector(A * qdot), m_exp, unit_test_utils::TEST_PREC));
423 }
424 
425 TEST_F(RdlCustomJointFixture, calcCentroidalMomentumMatrixDot)
426 {
427  for (unsigned int i = 0; i < 3; i++)
428  {
429  q[i] = i * 0.1;
430  qdot[i] = i * 0.15;
431  qddot[i] = i * 0.17;
432  tau[i] = i * 0.12;
433  }
434 
435  MatrixNd ADot_cust(6, custom_model.qdot_size), ADot_ref(6, custom_model.qdot_size);
436  ADot_cust.setZero();
437  ADot_ref.setZero();
438 
439  Utils::calcCentroidalMomentumMatrixDot(reference_model, q, qdot, ADot_ref);
440  Utils::calcCentroidalMomentumMatrixDot(custom_model, q, qdot, ADot_cust);
441 
442  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(ADot_ref, ADot_cust, unit_test_utils::TEST_PREC));
443 }
444 
445 TEST_F(RdlCustomJointFixture, calcPointJacobianDot6D)
446 {
447  for (unsigned int i = 0; i < 3; i++)
448  {
449  q[i] = i * 0.1;
450  qdot[i] = i * 0.15;
451  qddot[i] = i * 0.17;
452  tau[i] = i * 0.12;
453  }
454 
455  MatrixNd GDot_cust(6, custom_model.qdot_size), GDot_ref(6, custom_model.qdot_size);
456  GDot_cust.setZero();
457  GDot_ref.setZero();
458 
459  Vector3d p(-0.1, -0.2, 1.1);
460  calcPointJacobianDot6D(custom_model, q, qdot, custom_body_id, p, GDot_cust);
461  calcPointJacobianDot6D(reference_model, q, qdot, custom_body_id, p, GDot_ref);
462 
463  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(GDot_cust, GDot_ref, unit_test_utils::TEST_PREC));
464 }
465 
466 TEST_F(RdlCustomJointFixture, calcPointVelocity)
467 {
468  for (unsigned int i = 0; i < 3; i++)
469  {
470  q[i] = i * 0.1;
471  qdot[i] = i * 0.15;
472  qddot[i] = i * 0.17;
473  tau[i] = i * 0.12;
474  }
475 
476  MatrixNd G_cust(6, custom_model.qdot_size);
477  MatrixNd G_ref(6, reference_model.qdot_size);
478 
479  Math::Vector3d point_position(0.1, -0.2, 0.1);
480 
481  FrameVector v_cust = calcPointVelocity(custom_model, q, qdot, custom_body_id, point_position);
482  FrameVector v_ref = calcPointVelocity(reference_model, q, qdot, reference_body_id, point_position);
483 
484  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(v_cust, v_ref, unit_test_utils::TEST_PREC));
485 }
486 
487 TEST_F(RdlCustomJointFixture, calcPointVelocity6D)
488 {
489  for (unsigned int i = 0; i < 3; i++)
490  {
491  q[i] = i * 0.1;
492  qdot[i] = i * 0.15;
493  qddot[i] = i * 0.17;
494  tau[i] = i * 0.12;
495  }
496 
497  MatrixNd G_cust(6, custom_model.qdot_size);
498  MatrixNd G_ref(6, reference_model.qdot_size);
499 
500  Math::Vector3d point_position(0.1, -0.2, 0.1);
501 
502  FrameVectorPair v_cust = calcPointVelocity6D(custom_model, q, qdot, custom_body_id, point_position);
503  FrameVectorPair v_ref = calcPointVelocity6D(reference_model, q, qdot, reference_body_id, point_position);
504 
505  EXPECT_TRUE(v_cust.linear().isApprox(v_ref.linear(), TEST_PREC));
506  EXPECT_TRUE(v_cust.angular().isApprox(v_ref.angular(), TEST_PREC));
507 }
508 
509 TEST_F(RdlCustomJointFixture, forwardDynamicsContacts)
510 {
511  ConstraintSet c_ref, c_cust;
512 
513  for (unsigned int i = 0; i < 3; i++)
514  {
515  q[i] = i * 0.1;
516  qdot[i] = i * 0.15;
517  qddot[i] = i * 0.17;
518  tau[i] = i * 0.12;
519  }
520 
521  Math::VectorNd qddot_ref(qddot.size());
522  Math::VectorNd qddot_cust(qddot.size());
523 
524  c_ref.addConstraint(1, Math::Vector3d(0., 0.05, -0.12), Math::Vector3d(1., 0., 0.));
525  c_cust.addConstraint(1, Math::Vector3d(0., 0.05, -0.12), Math::Vector3d(1., 0., 0.));
526  c_cust.bind(custom_model);
527  c_ref.bind(reference_model);
528 
529  forwardDynamicsContactsDirect(custom_model, q, qdot, tau, c_cust, qddot_cust);
530  forwardDynamicsContactsDirect(reference_model, q, qdot, tau, c_ref, qddot_ref);
531 
532  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_cust, qddot_ref, unit_test_utils::TEST_PREC));
533 
534  forwardDynamicsContactsRangeSpaceSparse(custom_model, q, qdot, tau, c_cust, qddot_cust);
535  forwardDynamicsContactsRangeSpaceSparse(reference_model, q, qdot, tau, c_ref, qddot_ref);
536 
537  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_cust, qddot_ref, unit_test_utils::TEST_PREC));
538 
539  forwardDynamicsContactsNullSpace(custom_model, q, qdot, tau, c_cust, qddot_cust);
540  forwardDynamicsContactsNullSpace(reference_model, q, qdot, tau, c_ref, qddot_ref);
541 
542  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_cust, qddot_ref, unit_test_utils::TEST_PREC));
543 
544  forwardDynamicsContactsKokkevis(custom_model, q, qdot, tau, c_cust, qddot_cust);
545  forwardDynamicsContactsKokkevis(reference_model, q, qdot, tau, c_ref, qddot_ref);
546 
547  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_cust, qddot_ref, unit_test_utils::TEST_PREC));
548 }
549 
550 int main(int argc, char** argv)
551 {
552  ::testing::InitGoogleTest(&argc, argv);
553  return RUN_ALL_TESTS();
554 }
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::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::Math::SpatialVectorZero
SpatialVector SpatialVectorZero
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
TEST_PREC
const double TEST_PREC
Definition: RdlCustomJointTests.cpp:15
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::Math::FrameVectorPair::linear
FrameVector linear() const
Get copy of linear component.
Definition: FrameVectorPair.hpp:222
RobotDynamics::Math::SpatialVector::setAngularPart
void setAngularPart(const Vector3d &v)
Definition: rdl_eigenmath.h:217
unit_test_utils::TEST_PREC
const double TEST_PREC
Definition: UnitTestUtils.hpp:22
RobotDynamics::Math::SpatialVector::setLinearPart
void setLinearPart(const Vector3d &v)
Definition: rdl_eigenmath.h:224
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::Math::FrameVectorPair::angular
FrameVector angular() const
Get copy of angular component.
Definition: FrameVectorPair.hpp:231
main
int main(int argc, char **argv)
Definition: RdlCustomJointTests.cpp:550
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
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
unit_test_utils::checkMatrix3dEq
static bool checkMatrix3dEq(const RobotDynamics::Math::Matrix3d &t1, const RobotDynamics::Math::Matrix3d &t2)
Definition: UnitTestUtils.hpp:196
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::calcPointVelocity
Math::FrameVector calcPointVelocity(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
Computes the velocity of a point on a body.
Definition: Kinematics.cc:1422
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::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::Math::FrameVector
A FrameVector is a 3D vector with a ReferenceFrame, and all operations between FrameVectors and other...
Definition: FrameVector.hpp:33
RdlCustomEulerZYXJoint::jcalc_X_lambda_S
virtual void jcalc_X_lambda_S(Model &model, unsigned int joint_id, const Math::VectorNd &q)
Definition: RdlCustomJointTests.cpp:74
RdlCustomEulerZYXJoint::jcalc
virtual void jcalc(Model &model, unsigned int joint_id, const Math::VectorNd &q, const Math::VectorNd &qdot)
Definition: RdlCustomJointTests.cpp:26
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::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::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::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::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
unit_test_utils::checkSpatialMatrixEpsilonClose
static bool checkSpatialMatrixEpsilonClose(const RobotDynamics::Math::SpatialMatrix &t1, const RobotDynamics::Math::SpatialMatrix &t2, const double epsilon)
Definition: UnitTestUtils.hpp:122
RobotDynamics::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::Utils::calcCenterOfMass
void calcCenterOfMass(Model &model, const Math::VectorNd &q, Math::Vector3d &com, bool update_kinematics=true)
Computes the Center of Mass (COM) position.
Definition: rdl_utils.cc:202
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::FrameVectorPair
A FrameVector is a pair of 3D vector with a ReferenceFrame.
Definition: FrameVectorPair.hpp:33
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
RobotDynamics::calcPointVelocity6D
Math::FrameVectorPair calcPointVelocity6D(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
Computes angular and linear velocity of a point that is fixed on a body.
Definition: Kinematics.cc:1459
TEST_F
TEST_F(RdlCustomJointFixture, updateKinematics)
Definition: RdlCustomJointTests.cpp:146
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
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
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