RdlContactsTests.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 
3 #include <iostream>
4 
5 #include "rdl_dynamics/Model.h"
6 #include "rdl_dynamics/Contacts.h"
7 #include "rdl_dynamics/Dynamics.h"
8 #include "rdl_dynamics/Kinematics.h"
9 
10 #include "UnitTestUtils.hpp"
11 
12 #include "Fixtures.h"
13 #include "Human36Fixture.h"
14 
15 using namespace std;
16 using namespace RobotDynamics;
17 using namespace RobotDynamics::Math;
18 
19 const double TEST_PREC = 1.0e-11;
20 
21 struct FixedBase6DoF9DoFFixture : public testing::Test
22 {
23  FixedBase6DoF9DoFFixture()
24  {
25  }
26 
27  void SetUp()
28  {
29  model = new Model;
30 
31  model->gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
32 
33  /* 3 DoF (rot.) joint at base
34  * 3 DoF (rot.) joint child origin
35  *
36  * X Contact point (ref child)
37  * |
38  * Base |
39  * / body |
40  * O-------*
41  * \
42  * Child body
43  */
44 
45  // base body (3 DoF)
46  base = Body(1., Vector3d(0.5, 0., 0.), Vector3d(1., 1., 1.));
47  joint_rotzyx = Joint(SpatialVector(0., 0., 1., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.));
48  base_id = model->addBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_rotzyx, base);
49 
50  // child body 1 (3 DoF)
51  child = Body(1., Vector3d(0., 0.5, 0.), Vector3d(1., 1., 1.));
52  child_id = model->addBody(base_id, Xtrans(Vector3d(0., 0., 0.)), joint_rotzyx, child);
53 
54  // child body (3 DoF)
55  child_2 = Body(1., Vector3d(0., 0.5, 0.), Vector3d(1., 1., 1.));
56  child_2_id = model->addBody(child_id, Xtrans(Vector3d(0., 0., 0.)), joint_rotzyx, child_2);
57 
58  Q = VectorNd::Constant(model->mBodies.size() - 1, 0.);
59  QDot = VectorNd::Constant(model->mBodies.size() - 1, 0.);
60  QDDot = VectorNd::Constant(model->mBodies.size() - 1, 0.);
61  Tau = VectorNd::Constant(model->mBodies.size() - 1, 0.);
62 
63  contact_body_id = child_id;
64  contact_point = Vector3d(0.5, 0.5, 0.);
65  contact_normal = Vector3d(0., 1., 0.);
66  }
67 
68  void TearDown()
69  {
70  EXPECT_TRUE(unit_test_utils::checkModelZeroVectorsAndMatrices(*model));
71  delete model;
72  }
73 
74  Model* model;
75 
76  unsigned int base_id, child_id, child_2_id;
77 
78  Body base, child, child_2;
79 
80  Joint joint_rotzyx;
81 
82  VectorNd Q;
83  VectorNd QDot;
84  VectorNd QDDot;
85  VectorNd Tau;
86 
87  unsigned int contact_body_id;
88  Vector3d contact_point;
89  Vector3d contact_normal;
90  ConstraintSet constraint_set;
91 };
92 
93 //
94 // ForwardDynamicsContactsDirect
95 //
96 TEST_F(FixedBase6DoF9DoFFixture, TestForwardDynamicsContactsDirectSimple)
97 {
98  Model model;
99  model.gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
100  Body base_body(1., Vector3d(0., 0., 0.), Vector3d(1., 1., 1.));
101  unsigned int base_body_id = model.addBody(0, SpatialTransform(),
102  Joint(SpatialVector(0., 0., 0., 1., 0., 0.), SpatialVector(0., 0., 0., 0., 1., 0.), SpatialVector(0., 0., 0., 0., 0., 1.),
103  SpatialVector(0., 0., 1., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.)),
104  base_body);
105 
106  VectorNd Q = VectorNd::Constant((size_t)model.dof_count, 0.);
107  VectorNd QDot = VectorNd::Constant((size_t)model.dof_count, 0.);
108  VectorNd QDDot = VectorNd::Constant((size_t)model.dof_count, 0.);
109  VectorNd Tau = VectorNd::Constant((size_t)model.dof_count, 0.);
110 
111  Q[1] = 1.;
112  QDot[0] = 1.;
113  QDot[3] = -1.;
114 
115  unsigned int contact_body_id = base_body_id;
116  Vector3d contact_point(0., -1., 0.);
117 
118  ConstraintSet constraint_set;
119 
120  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(1., 0., 0.), "ground_x");
121  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 1., 0.), "ground_y");
122  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 0., 1.), "ground_z");
123 
124  constraint_set.bind(model);
125 
126  forwardDynamicsContactsDirect(model, Q, QDot, Tau, constraint_set, QDDot);
127 
128  Vector3d point_acceleration = calcPointAcceleration(model, Q, QDot, QDDot, contact_body_id, contact_point);
129 
130  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(0., 0., 0.), point_acceleration, TEST_PREC));
131  EXPECT_TRUE(unit_test_utils::checkModelZeroVectorsAndMatrices(model));
132 
133  constraint_set.clear();
134 
135  EXPECT_TRUE(unit_test_utils::checkVectorNdEq(constraint_set.acceleration, VectorNd::Zero(constraint_set.acceleration.rows())));
136  EXPECT_TRUE(unit_test_utils::checkVectorNdEq(constraint_set.a, VectorNd::Zero(constraint_set.a.rows())));
137  EXPECT_TRUE(unit_test_utils::checkVectorNdEq(constraint_set.force, VectorNd::Zero(constraint_set.force.rows())));
138  EXPECT_TRUE(unit_test_utils::checkVectorNdEq(constraint_set.impulse, VectorNd::Zero(constraint_set.force.rows())));
139  EXPECT_TRUE(unit_test_utils::checkVectorNdEq(constraint_set.gamma, VectorNd::Zero(constraint_set.gamma.rows())));
140  EXPECT_TRUE(unit_test_utils::checkVectorNdEq(constraint_set.b, VectorNd::Zero(constraint_set.b.rows())));
141  EXPECT_TRUE(unit_test_utils::checkVectorNdEq(constraint_set.x, VectorNd::Zero(constraint_set.x.rows())));
142  EXPECT_TRUE(unit_test_utils::checkVectorNdEq(constraint_set.QDDot_0, VectorNd::Zero(constraint_set.QDDot_0.rows())));
143  EXPECT_TRUE(unit_test_utils::checkVectorNdEq(constraint_set.QDDot_t, VectorNd::Zero(constraint_set.QDDot_t.rows())));
144  EXPECT_TRUE(unit_test_utils::checkVectorNdEq(constraint_set.d_u, VectorNd::Zero(constraint_set.d_u.rows())));
145 
146  EXPECT_TRUE(unit_test_utils::checkMatrixNdEq(constraint_set.H, MatrixNd::Zero(constraint_set.H.rows(), constraint_set.H.cols())));
147  EXPECT_TRUE(unit_test_utils::checkMatrixNdEq(constraint_set.C, MatrixNd::Zero(constraint_set.C.rows(), constraint_set.C.cols())));
148  EXPECT_TRUE(unit_test_utils::checkMatrixNdEq(constraint_set.G, MatrixNd::Zero(constraint_set.G.rows(), constraint_set.G.cols())));
149  EXPECT_TRUE(unit_test_utils::checkMatrixNdEq(constraint_set.A, MatrixNd::Zero(constraint_set.A.rows(), constraint_set.A.cols())));
150  EXPECT_TRUE(unit_test_utils::checkMatrixNdEq(constraint_set.K, MatrixNd::Zero(constraint_set.K.rows(), constraint_set.K.cols())));
151  EXPECT_TRUE(unit_test_utils::checkModelZeroVectorsAndMatrices(model));
152 
153  unsigned int i;
154  for (i = 0; i < constraint_set.f_t.size(); i++)
155  {
156  EXPECT_TRUE(unit_test_utils::checkVectorNdEq(constraint_set.f_t[i], VectorNd::Zero(constraint_set.f_t[i].rows())));
157  }
158 
159  for (i = 0; i < constraint_set.f_ext_constraints.size(); i++)
160  {
161  EXPECT_TRUE(unit_test_utils::checkVectorNdEq(constraint_set.f_ext_constraints[i], VectorNd::Zero(constraint_set.f_ext_constraints[i].rows())));
162  }
163 
164  for (i = 0; i < constraint_set.point_accel_0.size(); i++)
165  {
166  EXPECT_TRUE(unit_test_utils::checkVectorNdEq(constraint_set.point_accel_0[i], VectorNd::Zero(constraint_set.point_accel_0[i].rows())));
167  }
168 
169  for (i = 0; i < constraint_set.d_pA.size(); i++)
170  {
171  EXPECT_TRUE(unit_test_utils::checkVectorNdEq(constraint_set.d_pA[i], VectorNd::Zero(constraint_set.d_pA[i].rows())));
172  }
173 
174  for (i = 0; i < constraint_set.d_a.size(); i++)
175  {
176  EXPECT_TRUE(unit_test_utils::checkVectorNdEq(constraint_set.d_a[i], VectorNd::Zero(constraint_set.d_a[i].rows())));
177  }
178 }
179 
180 TEST_F(FixedBase6DoF9DoFFixture, TestForwardDynamicsContactsDirectMoving)
181 {
182  Model model;
183  model.gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
184  Body base_body(1., Vector3d(0., 0., 0.), Vector3d(1., 1., 1.));
185  unsigned int base_body_id = model.addBody(0, SpatialTransform(),
186  Joint(SpatialVector(0., 0., 0., 1., 0., 0.), SpatialVector(0., 0., 0., 0., 1., 0.), SpatialVector(0., 0., 0., 0., 0., 1.),
187  SpatialVector(0., 0., 1., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.)),
188  base_body);
189 
190  VectorNd Q = VectorNd::Constant((size_t)model.dof_count, 0.);
191  VectorNd QDot = VectorNd::Constant((size_t)model.dof_count, 0.);
192  VectorNd QDDot = VectorNd::Constant((size_t)model.dof_count, 0.);
193  VectorNd Tau = VectorNd::Constant((size_t)model.dof_count, 0.);
194 
195  Q[0] = 0.1;
196  Q[1] = 0.2;
197  Q[2] = 0.3;
198  Q[3] = 0.4;
199  Q[4] = 0.5;
200  Q[5] = 0.6;
201  QDot[0] = 1.1;
202  QDot[1] = 1.2;
203  QDot[2] = 1.3;
204  QDot[3] = -1.4;
205  QDot[4] = -1.5;
206  QDot[5] = -1.6;
207 
208  unsigned int contact_body_id = base_body_id;
209  Vector3d contact_point(0., -1., 0.);
210 
211  ConstraintSet constraint_set;
212 
213  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(1., 0., 0.), "ground_x");
214  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 1., 0.), "ground_y");
215  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 0., 1.), "ground_z");
216 
217  constraint_set.bind(model);
218 
219  forwardDynamicsContactsDirect(model, Q, QDot, Tau, constraint_set, QDDot);
220 
221  Vector3d point_acceleration = calcPointAcceleration(model, Q, QDot, QDDot, contact_body_id, contact_point);
222 
223  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(0., 0., 0.), point_acceleration, TEST_PREC));
224  EXPECT_TRUE(unit_test_utils::checkModelZeroVectorsAndMatrices(model));
225 }
226 
227 //
228 // ForwardDynamicsContacts
229 //
230 TEST_F(FixedBase6DoF, ForwardDynamicsContactsSingleContact)
231 {
232  contact_normal.set(0., 1., 0.);
233  constraint_set.addConstraint(contact_body_id, contact_point, contact_normal);
234  ConstraintSet constraint_set_lagrangian = constraint_set.Copy();
235 
236  constraint_set_lagrangian.bind(*model);
237  constraint_set.bind(*model);
238 
239  Vector3d point_accel_lagrangian, point_accel_contacts;
240 
241  VectorNd QDDot_lagrangian = VectorNd::Constant(model->mBodies.size() - 1, 0.);
242  VectorNd QDDot_contacts = VectorNd::Constant(model->mBodies.size() - 1, 0.);
243 
244  forwardDynamicsContactsDirect(*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
245 
246  forwardDynamicsContactsKokkevis(*model, Q, QDot, Tau, constraint_set, QDDot_contacts);
247 
248  point_accel_lagrangian = calcPointAcceleration(*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true);
249  point_accel_contacts = calcPointAcceleration(*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true);
250 
251  EXPECT_NEAR(constraint_set_lagrangian.force[0], constraint_set.force[0], TEST_PREC);
252  EXPECT_NEAR(contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts), TEST_PREC);
253  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(point_accel_lagrangian, point_accel_contacts, TEST_PREC));
254  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_lagrangian, QDDot_contacts, TEST_PREC));
255 }
256 
257 TEST_F(FixedBase6DoF, ForwardDynamicsContactsSingleContactRotated)
258 {
259  Q[0] = 0.6;
260  Q[3] = M_PI * 0.6;
261  Q[4] = 0.1;
262 
263  contact_normal.set(0., 1., 0.);
264 
265  constraint_set.addConstraint(contact_body_id, contact_point, contact_normal);
266  ConstraintSet constraint_set_lagrangian = constraint_set.Copy();
267 
268  constraint_set_lagrangian.bind(*model);
269  constraint_set.bind(*model);
270 
271  Vector3d point_accel_lagrangian, point_accel_contacts, point_accel_contacts_opt;
272 
273  VectorNd QDDot_lagrangian = VectorNd::Constant(model->mBodies.size() - 1, 0.);
274  VectorNd QDDot_contacts = VectorNd::Constant(model->mBodies.size() - 1, 0.);
275  VectorNd QDDot_contacts_opt = VectorNd::Constant(model->mBodies.size() - 1, 0.);
276 
277  forwardDynamicsContactsDirect(*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
278  forwardDynamicsContactsKokkevis(*model, Q, QDot, Tau, constraint_set, QDDot_contacts_opt);
279 
280  point_accel_lagrangian = calcPointAcceleration(*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true);
281  point_accel_contacts_opt = calcPointAcceleration(*model, Q, QDot, QDDot_contacts_opt, contact_body_id, contact_point, true);
282 
283  EXPECT_NEAR(constraint_set_lagrangian.force[0], constraint_set.force[0], TEST_PREC);
284  EXPECT_NEAR(contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts_opt), TEST_PREC);
285  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(point_accel_lagrangian, point_accel_contacts_opt, TEST_PREC));
286  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_lagrangian, QDDot_contacts_opt, TEST_PREC));
287 }
288 
289 // Similiar to the previous test, this test compares the results of
290 // - ForwardDynamicsContactsDirect
291 // - ForwardDynamcsContactsOpt
292 // for the example model in FixedBase6DoF and a moving state (i.e. a
293 // nonzero QDot)
294 //
295 TEST_F(FixedBase6DoF, ForwardDynamicsContactsSingleContactRotatedMoving)
296 {
297  Q[0] = 0.6;
298  Q[3] = M_PI * 0.6;
299  Q[4] = 0.1;
300 
301  QDot[0] = -0.3;
302  QDot[1] = 0.1;
303  QDot[2] = -0.5;
304  QDot[3] = 0.8;
305 
306  contact_normal.set(0., 1., 0.);
307  constraint_set.addConstraint(contact_body_id, contact_point, contact_normal);
308  ConstraintSet constraint_set_lagrangian = constraint_set.Copy();
309 
310  constraint_set_lagrangian.bind(*model);
311  constraint_set.bind(*model);
312 
313  Vector3d point_accel_lagrangian, point_accel_contacts;
314 
315  VectorNd QDDot_lagrangian = VectorNd::Constant(model->mBodies.size() - 1, 0.);
316  VectorNd QDDot_contacts = VectorNd::Constant(model->mBodies.size() - 1, 0.);
317 
318  forwardDynamicsContactsDirect(*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
319 
320  forwardDynamicsContactsKokkevis(*model, Q, QDot, Tau, constraint_set, QDDot_contacts);
321 
322  point_accel_lagrangian = calcPointAcceleration(*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true);
323  point_accel_contacts = calcPointAcceleration(*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true);
324 
325  // check whether FDContactsLagrangian and FDContactsOld match
326  EXPECT_NEAR(constraint_set_lagrangian.force[0], constraint_set.force[0], TEST_PREC);
327 
328  EXPECT_NEAR(contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts), TEST_PREC);
329  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(point_accel_lagrangian, point_accel_contacts, TEST_PREC));
330  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_lagrangian, QDDot_contacts, TEST_PREC));
331 }
332 
333 TEST_F(FixedBase6DoF, ForwardDynamicsContactsOptDoubleContact)
334 {
335  ConstraintSet constraint_set_lagrangian;
336 
337  constraint_set.addConstraint(contact_body_id, Vector3d(1., 0., 0.), contact_normal);
338  constraint_set.addConstraint(contact_body_id, Vector3d(0., 1., 0.), contact_normal);
339 
340  constraint_set_lagrangian = constraint_set.Copy();
341  constraint_set_lagrangian.bind(*model);
342  constraint_set.bind(*model);
343 
344  Vector3d point_accel_lagrangian, point_accel_contacts;
345 
346  VectorNd QDDot_lagrangian = VectorNd::Constant(model->mBodies.size() - 1, 0.);
347  VectorNd QDDot_contacts = VectorNd::Constant(model->mBodies.size() - 1, 0.);
348 
349  forwardDynamicsContactsDirect(*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
350  forwardDynamicsContactsKokkevis(*model, Q, QDot, Tau, constraint_set, QDDot_contacts);
351 
352  point_accel_lagrangian = calcPointAcceleration(*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true);
353  point_accel_contacts = calcPointAcceleration(*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true);
354 
355  // check whether FDContactsLagrangian and FDContacts match
356  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(constraint_set_lagrangian.force, constraint_set.force, TEST_PREC));
357  // check whether the point accelerations match
358  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(point_accel_lagrangian, point_accel_contacts, TEST_PREC));
359  // check whether the generalized accelerations match
360  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_lagrangian, QDDot_contacts, TEST_PREC));
361 }
362 
363 TEST_F(FixedBase6DoF, ForwardDynamicsContactsOptDoubleContactRepeated)
364 {
365  // makes sure that all variables in the constraint set gets reset
366  // properly when making repeated calls to ForwardDynamicsContacts.
367  ConstraintSet constraint_set_lagrangian;
368 
369  constraint_set.addConstraint(contact_body_id, Vector3d(1., 0., 0.), contact_normal);
370  constraint_set.addConstraint(contact_body_id, Vector3d(0., 1., 0.), contact_normal);
371 
372  constraint_set_lagrangian = constraint_set.Copy();
373  constraint_set_lagrangian.bind(*model);
374  constraint_set.bind(*model);
375 
376  Vector3d point_accel_lagrangian, point_accel_contacts;
377 
378  VectorNd QDDot_lagrangian = VectorNd::Constant(model->mBodies.size() - 1, 0.);
379  VectorNd QDDot_contacts = VectorNd::Constant(model->mBodies.size() - 1, 0.);
380 
381  forwardDynamicsContactsDirect(*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
382  // Call ForwardDynamicsContacts multiple times such that old values might
383  // be re-used and thus cause erroneus values.
384  forwardDynamicsContactsKokkevis(*model, Q, QDot, Tau, constraint_set, QDDot_contacts);
385  forwardDynamicsContactsKokkevis(*model, Q, QDot, Tau, constraint_set, QDDot_contacts);
386  forwardDynamicsContactsKokkevis(*model, Q, QDot, Tau, constraint_set, QDDot_contacts);
387 
388  point_accel_lagrangian = calcPointAcceleration(*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true);
389  point_accel_contacts = calcPointAcceleration(*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true);
390 
391  // check whether FDContactsLagrangian and FDContacts match
392  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(constraint_set_lagrangian.force, constraint_set.force, TEST_PREC));
393  // check whether the point accelerations match
394  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(point_accel_lagrangian, point_accel_contacts, TEST_PREC));
395  // check whether the generalized accelerations match
396  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_lagrangian, QDDot_contacts, TEST_PREC));
397 }
398 
399 TEST_F(FixedBase6DoF, ForwardDynamicsContactsOptMultipleContact)
400 {
401  ConstraintSet constraint_set_lagrangian;
402 
403  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(1., 0., 0.));
404  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 1., 0.));
405 
406  constraint_set_lagrangian = constraint_set.Copy();
407  constraint_set_lagrangian.bind(*model);
408  constraint_set.bind(*model);
409 
410  // we rotate the joints so that we have full mobility at the contact
411  // point:
412  //
413  // O X (contact point)
414  // \ /
415  // \ /
416  // \ /
417  // *
418  //
419 
420  Q[0] = M_PI * 0.25;
421  Q[1] = 0.2;
422  Q[3] = M_PI * 0.5;
423 
424  VectorNd QDDot_lagrangian = VectorNd::Constant(model->mBodies.size() - 1, 0.);
425  VectorNd QDDot_contacts = VectorNd::Constant(model->mBodies.size() - 1, 0.);
426 
427  forwardDynamicsContactsDirect(*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
428  forwardDynamicsContactsKokkevis(*model, Q, QDot, Tau, constraint_set, QDDot_contacts);
429 
430  Vector3d point_accel_c = calcPointAcceleration(*model, Q, QDot, QDDot, contact_body_id, contact_point);
431 
432  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_lagrangian, QDDot_contacts, TEST_PREC));
433  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(constraint_set_lagrangian.force, constraint_set.force, TEST_PREC));
434 
435  EXPECT_NEAR(0., point_accel_c[0], TEST_PREC);
436  EXPECT_NEAR(0., point_accel_c[1], TEST_PREC);
437 }
438 
439 TEST_F(FixedBase6DoF9DoFFixture, ForwardDynamicsContactsOptMultipleContactsMultipleBodiesMoving)
440 {
441  ConstraintSet constraint_set_lagrangian;
442 
443  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(1., 0., 0.));
444  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 1., 0.));
445  constraint_set.addConstraint(child_2_id, contact_point, Vector3d(0., 1., 0.));
446 
447  constraint_set_lagrangian = constraint_set.Copy();
448  constraint_set_lagrangian.bind(*model);
449  constraint_set.bind(*model);
450 
451  Q[0] = 0.1;
452  Q[1] = -0.1;
453  Q[2] = 0.1;
454  Q[3] = -0.1;
455  Q[4] = -0.1;
456  Q[5] = 0.1;
457 
458  QDot[0] = 1.;
459  QDot[1] = -1.;
460  QDot[2] = 1;
461  QDot[3] = -1.5;
462  QDot[4] = 1.5;
463  QDot[5] = -1.5;
464 
465  VectorNd QDDot_lagrangian = VectorNd::Constant(model->mBodies.size() - 1, 0.);
466 
467  forwardDynamicsContactsKokkevis(*model, Q, QDot, Tau, constraint_set, QDDot);
468 
469  Vector3d point_accel_c, point_accel_2_c;
470 
471  point_accel_c = calcPointAcceleration(*model, Q, QDot, QDDot, contact_body_id, contact_point);
472  point_accel_2_c = calcPointAcceleration(*model, Q, QDot, QDDot, child_2_id, contact_point);
473 
474  forwardDynamicsContactsDirect(*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
475 
476  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(constraint_set_lagrangian.force, constraint_set.force, TEST_PREC));
477 
478  EXPECT_NEAR(0., point_accel_c[0], TEST_PREC);
479  EXPECT_NEAR(0., point_accel_c[1], TEST_PREC);
480  EXPECT_NEAR(0., point_accel_2_c[1], TEST_PREC);
481 
482  point_accel_c = calcPointAcceleration(*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point);
483  point_accel_2_c = calcPointAcceleration(*model, Q, QDot, QDDot_lagrangian, child_2_id, contact_point);
484 
485  EXPECT_NEAR(0., point_accel_c[0], TEST_PREC);
486  EXPECT_NEAR(0., point_accel_c[1], TEST_PREC);
487  EXPECT_NEAR(0., point_accel_2_c[1], TEST_PREC);
488 
489  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_lagrangian, QDDot, TEST_PREC));
490 }
491 
492 TEST_F(FixedBase6DoF9DoFFixture, ForwardDynamicsContactsOptMultipleContactsMultipleBodiesMovingLinearSolverPartialPivLU)
493 {
494  ConstraintSet constraint_set_lagrangian;
495 
496  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(1., 0., 0.));
497  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 1., 0.));
498  constraint_set.addConstraint(child_2_id, contact_point, Vector3d(0., 1., 0.));
499 
500  constraint_set_lagrangian = constraint_set.Copy();
501  constraint_set_lagrangian.linear_solver = RobotDynamics::Math::LinearSolver::LinearSolverPartialPivLU;
502  constraint_set_lagrangian.bind(*model);
503  constraint_set.linear_solver = RobotDynamics::Math::LinearSolver::LinearSolverPartialPivLU;
504  constraint_set.bind(*model);
505 
506  Q[0] = 0.1;
507  Q[1] = -0.1;
508  Q[2] = 0.1;
509  Q[3] = -0.1;
510  Q[4] = -0.1;
511  Q[5] = 0.1;
512 
513  QDot[0] = 1.;
514  QDot[1] = -1.;
515  QDot[2] = 1;
516  QDot[3] = -1.5;
517  QDot[4] = 1.5;
518  QDot[5] = -1.5;
519 
520  VectorNd QDDot_lagrangian = VectorNd::Constant(model->mBodies.size() - 1, 0.);
521 
522  forwardDynamicsContactsKokkevis(*model, Q, QDot, Tau, constraint_set, QDDot);
523 
524  Vector3d point_accel_c, point_accel_2_c;
525 
526  point_accel_c = calcPointAcceleration(*model, Q, QDot, QDDot, contact_body_id, contact_point);
527  point_accel_2_c = calcPointAcceleration(*model, Q, QDot, QDDot, child_2_id, contact_point);
528 
529  forwardDynamicsContactsDirect(*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
530 
531  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(constraint_set_lagrangian.force, constraint_set.force, TEST_PREC));
532 
533  EXPECT_NEAR(0., point_accel_c[0], TEST_PREC);
534  EXPECT_NEAR(0., point_accel_c[1], TEST_PREC);
535  EXPECT_NEAR(0., point_accel_2_c[1], TEST_PREC);
536 
537  point_accel_c = calcPointAcceleration(*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point);
538  point_accel_2_c = calcPointAcceleration(*model, Q, QDot, QDDot_lagrangian, child_2_id, contact_point);
539 
540  EXPECT_NEAR(0., point_accel_c[0], TEST_PREC);
541  EXPECT_NEAR(0., point_accel_c[1], TEST_PREC);
542  EXPECT_NEAR(0., point_accel_2_c[1], TEST_PREC);
543 
544  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_lagrangian, QDDot, TEST_PREC));
545 }
546 
547 TEST_F(FixedBase6DoF9DoFFixture, ForwardDynamicsContactsOptMultipleContactsMultipleBodiesMovingLinearSolverHouseholderQR)
548 {
549  ConstraintSet constraint_set_lagrangian;
550 
551  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(1., 0., 0.));
552  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 1., 0.));
553  constraint_set.addConstraint(child_2_id, contact_point, Vector3d(0., 1., 0.));
554 
555  constraint_set_lagrangian = constraint_set.Copy();
556  constraint_set_lagrangian.linear_solver = RobotDynamics::Math::LinearSolver::LinearSolverHouseholderQR;
557  constraint_set_lagrangian.bind(*model);
558  constraint_set.linear_solver = RobotDynamics::Math::LinearSolver::LinearSolverHouseholderQR;
559  constraint_set.bind(*model);
560 
561  Q[0] = 0.1;
562  Q[1] = -0.1;
563  Q[2] = 0.1;
564  Q[3] = -0.1;
565  Q[4] = -0.1;
566  Q[5] = 0.1;
567 
568  QDot[0] = 1.;
569  QDot[1] = -1.;
570  QDot[2] = 1;
571  QDot[3] = -1.5;
572  QDot[4] = 1.5;
573  QDot[5] = -1.5;
574 
575  VectorNd QDDot_lagrangian = VectorNd::Constant(model->mBodies.size() - 1, 0.);
576 
577  forwardDynamicsContactsKokkevis(*model, Q, QDot, Tau, constraint_set, QDDot);
578 
579  Vector3d point_accel_c, point_accel_2_c;
580 
581  point_accel_c = calcPointAcceleration(*model, Q, QDot, QDDot, contact_body_id, contact_point);
582  point_accel_2_c = calcPointAcceleration(*model, Q, QDot, QDDot, child_2_id, contact_point);
583 
584  forwardDynamicsContactsDirect(*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
585 
586  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(constraint_set_lagrangian.force, constraint_set.force, TEST_PREC));
587 
588  EXPECT_NEAR(0., point_accel_c[0], TEST_PREC);
589  EXPECT_NEAR(0., point_accel_c[1], TEST_PREC);
590  EXPECT_NEAR(0., point_accel_2_c[1], TEST_PREC);
591 
592  point_accel_c = calcPointAcceleration(*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point);
593  point_accel_2_c = calcPointAcceleration(*model, Q, QDot, QDDot_lagrangian, child_2_id, contact_point);
594 
595  EXPECT_NEAR(0., point_accel_c[0], TEST_PREC);
596  EXPECT_NEAR(0., point_accel_c[1], TEST_PREC);
597  EXPECT_NEAR(0., point_accel_2_c[1], TEST_PREC);
598 
599  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_lagrangian, QDDot, TEST_PREC));
600 }
601 
602 TEST_F(FixedBase6DoF9DoFFixture, ForwardDynamicsContactsOptMultipleContactsMultipleBodiesMovingAlternate)
603 {
604  ConstraintSet constraint_set_lagrangian;
605 
606  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(1., 0., 0.));
607  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 1., 0.));
608  constraint_set.addConstraint(child_2_id, contact_point, Vector3d(0., 1., 0.));
609 
610  constraint_set_lagrangian = constraint_set.Copy();
611  constraint_set_lagrangian.bind(*model);
612  constraint_set.bind(*model);
613 
614  Q[0] = 0.1;
615  Q[1] = -0.3;
616  Q[2] = 0.15;
617  Q[3] = -0.21;
618  Q[4] = -0.81;
619  Q[5] = 0.11;
620  Q[6] = 0.31;
621  Q[7] = -0.91;
622  Q[8] = 0.61;
623 
624  QDot[0] = 1.3;
625  QDot[1] = -1.7;
626  QDot[2] = 3;
627  QDot[3] = -2.5;
628  QDot[4] = 1.5;
629  QDot[5] = -5.5;
630  QDot[6] = 2.5;
631  QDot[7] = -1.5;
632  QDot[8] = -3.5;
633 
634  VectorNd QDDot_lagrangian = VectorNd::Constant(model->mBodies.size() - 1, 0.);
635 
636  forwardDynamicsContactsKokkevis(*model, Q, QDot, Tau, constraint_set, QDDot);
637 
638  Vector3d point_accel_c, point_accel_2_c;
639 
640  point_accel_c = calcPointAcceleration(*model, Q, QDot, QDDot, contact_body_id, contact_point);
641  point_accel_2_c = calcPointAcceleration(*model, Q, QDot, QDDot, child_2_id, contact_point);
642 
643  forwardDynamicsContactsDirect(*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
644 
645  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(constraint_set_lagrangian.force, constraint_set.force, TEST_PREC));
646 
647  EXPECT_NEAR(0., point_accel_c[0], TEST_PREC);
648  EXPECT_NEAR(0., point_accel_c[1], TEST_PREC);
649  EXPECT_NEAR(0., point_accel_2_c[1], TEST_PREC);
650 
651  point_accel_c = calcPointAcceleration(*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point);
652  point_accel_2_c = calcPointAcceleration(*model, Q, QDot, QDDot_lagrangian, child_2_id, contact_point);
653 
654  EXPECT_NEAR(0., point_accel_c[0], TEST_PREC);
655  EXPECT_NEAR(0., point_accel_c[1], TEST_PREC);
656  EXPECT_NEAR(0., point_accel_2_c[1], TEST_PREC);
657 
658  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_lagrangian, QDDot, TEST_PREC));
659 }
660 
661 TEST_F(FixedBase6DoF12DoFFloatingBase, ForwardDynamicsContactsMultipleContactsFloatingBase)
662 {
663  ConstraintSet constraint_set_lagrangian;
664 
665  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(1., 0., 0.));
666  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 1., 0.));
667  constraint_set.addConstraint(child_2_id, contact_point, Vector3d(0., 1., 0.));
668 
669  constraint_set_lagrangian = constraint_set.Copy();
670  constraint_set_lagrangian.bind(*model);
671  constraint_set.bind(*model);
672 
673  VectorNd QDDot_lagrangian = VectorNd::Constant(model->dof_count, 0.);
674 
675  Q[0] = 0.1;
676  Q[1] = -0.3;
677  Q[2] = 0.15;
678  Q[3] = -0.21;
679  Q[4] = -0.81;
680  Q[5] = 0.11;
681  Q[6] = 0.31;
682  Q[7] = -0.91;
683  Q[8] = 0.61;
684 
685  QDot[0] = 1.3;
686  QDot[1] = -1.7;
687  QDot[2] = 3;
688  QDot[3] = -2.5;
689  QDot[4] = 1.5;
690  QDot[5] = -5.5;
691  QDot[6] = 2.5;
692  QDot[7] = -1.5;
693  QDot[8] = -3.5;
694 
695  forwardDynamicsContactsKokkevis(*model, Q, QDot, Tau, constraint_set, QDDot);
696 
697  Vector3d point_accel_c, point_accel_2_c;
698 
699  point_accel_c = calcPointAcceleration(*model, Q, QDot, QDDot, contact_body_id, contact_point);
700  point_accel_2_c = calcPointAcceleration(*model, Q, QDot, QDDot, child_2_id, contact_point);
701 
702  forwardDynamicsContactsDirect(*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
703 
704  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(constraint_set_lagrangian.force, constraint_set.force, TEST_PREC));
705 
706  EXPECT_NEAR(0., point_accel_c[0], TEST_PREC);
707  EXPECT_NEAR(0., point_accel_c[1], TEST_PREC);
708  EXPECT_NEAR(0., point_accel_2_c[1], TEST_PREC);
709 
710  point_accel_c = calcPointAcceleration(*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point);
711  point_accel_2_c = calcPointAcceleration(*model, Q, QDot, QDDot_lagrangian, child_2_id, contact_point);
712 
713  EXPECT_NEAR(0., point_accel_c[0], TEST_PREC);
714  EXPECT_NEAR(0., point_accel_c[1], TEST_PREC);
715  EXPECT_NEAR(0., point_accel_2_c[1], TEST_PREC);
716 
717  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot_lagrangian, QDDot, TEST_PREC));
718 }
719 
720 TEST_F(Human36, ForwardDynamicsContactsFixedBody)
721 {
722  VectorNd qddot_lagrangian(VectorNd::Zero(qddot.size()));
723  VectorNd qddot_sparse(VectorNd::Zero(qddot.size()));
724 
725  randomizeStates();
726 
727  ConstraintSet constraint_upper_trunk;
728  constraint_upper_trunk.addConstraint(body_id_3dof[BodyUpperTrunk], Vector3d(1.1, 2.2, 3.3), Vector3d(1., 0., 0.));
729  constraint_upper_trunk.bind(*model_3dof);
730 
731  forwardDynamicsContactsDirect(*model_3dof, q, qdot, tau, constraint_upper_trunk, qddot_lagrangian);
732  forwardDynamicsContactsRangeSpaceSparse(*model_3dof, q, qdot, tau, constraint_upper_trunk, qddot_sparse);
733  forwardDynamicsContactsKokkevis(*model_3dof, q, qdot, tau, constraint_upper_trunk, qddot);
734 
735  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_lagrangian, qddot, TEST_PREC * qddot_lagrangian.norm() * 10.));
736  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qddot_lagrangian, qddot_sparse, TEST_PREC * qddot_lagrangian.norm() * 10.));
737 }
738 
739 TEST_F(Human36, ForwardDynamicsContactsImpulses)
740 {
741  VectorNd qddot_lagrangian(VectorNd::Zero(qddot.size()));
742 
743  randomizeStates();
744 
745  Vector3d heel_point(-0.03, 0., -0.03);
746 
747  ConstraintSet constraint_feet;
748  constraint_feet.addConstraint(body_id_3dof[BodyFootLeft], heel_point, Vector3d(1., 0., 0.));
749  constraint_feet.addConstraint(body_id_3dof[BodyFootLeft], heel_point, Vector3d(0., 1., 0.));
750  constraint_feet.addConstraint(body_id_3dof[BodyFootLeft], heel_point, Vector3d(0., 0., 1.));
751  constraint_feet.addConstraint(body_id_3dof[BodyFootRight], heel_point, Vector3d(1., 0., 0.));
752  constraint_feet.addConstraint(body_id_3dof[BodyFootRight], heel_point, Vector3d(0., 1., 0.));
753  constraint_feet.addConstraint(body_id_3dof[BodyFootRight], heel_point, Vector3d(0., 0., 1.));
754  constraint_feet.bind(*model_3dof);
755 
756  VectorNd qdotplus(VectorNd::Zero(qdot.size()));
757 
758  computeContactImpulsesDirect(*model_3dof, q, qdot, constraint_feet, qdotplus);
759 
760  Vector3d heel_left_velocity = calcPointVelocity(*model_3dof, q, qdotplus, body_id_3dof[BodyFootLeft], heel_point);
761  Vector3d heel_right_velocity = calcPointVelocity(*model_3dof, q, qdotplus, body_id_3dof[BodyFootRight], heel_point);
762 
763  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(0., 0., 0.), heel_left_velocity, TEST_PREC));
764  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(0., 0., 0.), heel_right_velocity, TEST_PREC));
765 }
766 
767 int main(int argc, char** argv)
768 {
769  ::testing::InitGoogleTest(&argc, argv);
770  return RUN_ALL_TESTS();
771 }
RobotDynamics::ConstraintSet::C
Math::VectorNd C
Workspace for the coriolis forces.
Definition: Contacts.h:260
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::ConstraintSet::f_t
Math::SpatialForceV f_t
Workspace for the test forces.
Definition: Contacts.h:299
RobotDynamics::ConstraintSet::f_ext_constraints
Math::SpatialForceV f_ext_constraints
Workspace for the actual spatial forces.
Definition: Contacts.h: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::ConstraintSet::b
Math::VectorNd b
Workspace for the Lagrangian right-hand-side.
Definition: Contacts.h:270
RobotDynamics::ConstraintSet::impulse
Math::VectorNd impulse
Definition: Contacts.h:248
RobotDynamics::Body
Describes all properties of a single body.
Definition: Body.h:30
Tau
VectorNd Tau
Definition: RdlTwoLegModelTests.cpp:32
RobotDynamics::ConstraintSet::clear
void clear()
Clears all variables in the constraint set.
Definition: Contacts.cc:127
RobotDynamics::ConstraintSet::a
Math::VectorNd a
Workspace for the accelerations of due to the test forces.
Definition: Contacts.h:290
QDDot
VectorNd QDDot
Definition: RdlTwoLegModelTests.cpp:31
RobotDynamics::ConstraintSet::Copy
ConstraintSet Copy()
Copies the constraints and resets its ConstraintSet::bound flag.
Definition: Contacts.h:191
QDot
VectorNd QDot
Definition: RdlTwoLegModelTests.cpp:30
Q
VectorNd Q
Definition: RdlTwoLegModelTests.cpp:29
RobotDynamics::Math::Xtrans
SpatialTransform Xtrans(const Vector3d &r)
Get pure translation transform.
Definition: SpatialAlgebraOperators.h:369
RobotDynamics::ConstraintSet::d_pA
Math::SpatialForceV d_pA
Workspace for the bias force due to the test force.
Definition: Contacts.h:308
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::ConstraintSet::A
Math::MatrixNd A
Workspace for the Lagrangian left-hand-side matrix.
Definition: Contacts.h:267
RobotDynamics::calcPointAcceleration
Math::FrameVector calcPointAcceleration(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
Computes the linear acceleration of a point on a body.
Definition: Kinematics.cc:1593
RobotDynamics::Model::gravity
Math::MotionVector gravity
the cartesian vector of the gravity
Definition: Model.h:196
RobotDynamics::ConstraintSet::acceleration
Math::VectorNd acceleration
Definition: Contacts.h:242
heel_point
Vector3d heel_point(0., 0., 0.)
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::Model::addBody
unsigned int addBody(const unsigned int parent_id, const Math::SpatialTransform &joint_frame, const Joint &joint, const Body &body, std::string body_name="")
Connects a given body to the model.
Definition: Model.cc:271
RobotDynamics::ConstraintSet::QDDot_t
Math::VectorNd QDDot_t
Workspace for the test accelerations.
Definition: Contacts.h:293
main
int main(int argc, char **argv)
Definition: RdlContactsTests.cpp:767
TEST_F
TEST_F(FixedBase6DoF9DoFFixture, TestForwardDynamicsContactsDirectSimple)
Definition: RdlContactsTests.cpp:96
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::ConstraintSet::gamma
Math::VectorNd gamma
Workspace of the lower part of b.
Definition: Contacts.h:263
RobotDynamics::Joint
Describes a joint relative to the predecessor body.
Definition: Joint.h:228
RobotDynamics::ConstraintSet::point_accel_0
std::vector< Math::Vector3d > point_accel_0
Workspace for the default point accelerations.
Definition: Contacts.h:305
RobotDynamics::Math::VectorNd
Eigen::VectorXd VectorNd
Definition: rdl_eigenmath.h:25
RobotDynamics::ConstraintSet::QDDot_0
Math::VectorNd QDDot_0
Workspace for the default accelerations.
Definition: Contacts.h:296
RobotDynamics::Model
Contains all information about the rigid body model.
Definition: Model.h:121
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
RobotDynamics::ConstraintSet::K
Math::MatrixNd K
Workspace for the Inverse Articulated-Body Inertia.
Definition: Contacts.h:287
RobotDynamics::ConstraintSet::d_a
std::vector< Math::SpatialVector > d_a
Workspace for the acceleration due to the test force.
Definition: Contacts.h:311
TEST_PREC
const double TEST_PREC
Definition: RdlContactsTests.cpp:19
unit_test_utils::checkVector3dEpsilonClose
static bool checkVector3dEpsilonClose(const RobotDynamics::Math::Vector3d &v1, const RobotDynamics::Math::Vector3d &v2, const double epsilon)
Definition: UnitTestUtils.hpp:256
unit_test_utils::checkMatrixNdEq
static bool checkMatrixNdEq(const RobotDynamics::Math::MatrixNd &t1, const RobotDynamics::Math::MatrixNd &t2)
Definition: UnitTestUtils.hpp:175
RobotDynamics::ConstraintSet::linear_solver
Math::LinearSolver linear_solver
Method that should be used to solve internal linear systems.
Definition: Contacts.h:230
unit_test_utils::checkVectorNdEq
static bool checkVectorNdEq(const RobotDynamics::Math::VectorNd &v1, const RobotDynamics::Math::VectorNd &v2)
Definition: UnitTestUtils.hpp:238
RobotDynamics::Model::dof_count
unsigned int dof_count
number of degrees of freedoms of the model
Definition: Model.h:169
RobotDynamics
Namespace for all structures of the RobotDynamics library.
Definition: Body.h:21
RobotDynamics::ConstraintSet::x
Math::VectorNd x
Workspace for the Lagrangian solution.
Definition: Contacts.h:273
RobotDynamics::ConstraintSet::H
Math::MatrixNd H
Workspace for the joint space inertia matrix.
Definition: Contacts.h:257
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