RdlUtilsTests.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 
3 #include "UnitTestUtils.hpp"
4 #include "Fixtures.h"
5 #include "Human36Fixture.h"
6 #include "rdl_dynamics/rdl_utils.h"
7 
8 using namespace std;
9 using namespace RobotDynamics;
10 using namespace RobotDynamics::Math;
11 
12 struct RdlUtilsTests : public testing::Test
13 {
14 };
15 
16 TEST_F(RdlUtilsTests, testGetDofName)
17 {
18  SpatialVector rx(1., 0., 0., 0., 0., 0.);
19  SpatialVector ry(0., 1., 0., 0., 0., 0.);
20  SpatialVector rz(0., 0., 1., 0., 0., 0.);
21  SpatialVector tx(0., 0., 0., 1., 0., 0.);
22  SpatialVector ty(0., 0., 0., 0., 1., 0.);
23  SpatialVector tz(0., 0., 0., 0., 0., 1.);
24 
25  EXPECT_STREQ("RX", Utils::getDofName(rx).c_str());
26  EXPECT_STREQ("RY", Utils::getDofName(ry).c_str());
27  EXPECT_STREQ("RZ", Utils::getDofName(rz).c_str());
28  EXPECT_STREQ("TX", Utils::getDofName(tx).c_str());
29  EXPECT_STREQ("TY", Utils::getDofName(ty).c_str());
30  EXPECT_STREQ("TZ", Utils::getDofName(tz).c_str());
31 
32  SpatialVector c(1., 0., 0., 0., 0., 1.);
33 
34  ostringstream dof_stream(ostringstream::out);
35  dof_stream << "custom (" << c.transpose() << ")";
36  EXPECT_STREQ(dof_stream.str().c_str(), Utils::getDofName(c).c_str());
37 }
38 
39 TEST_F(RdlUtilsTests, testGetBodyName)
40 {
41  Model model;
42  Body b1;
43  b1.mIsVirtual = true;
44  Joint j(JointType6DoF);
45  j.mJointAxes[0] = SpatialVector(1., 0., 0., 0., 0., 0.);
46  j.mJointAxes[1] = SpatialVector(0., 1., 0., 0., 0., 0.);
47  j.mJointAxes[2] = SpatialVector(0., 0., 1., 0., 0., 0.);
48  j.mJointAxes[3] = SpatialVector(0., 0., 0., 1., 0., 0.);
49  j.mJointAxes[4] = SpatialVector(0., 0., 0., 0., 1., 0.);
50  j.mJointAxes[5] = SpatialVector(0., 0., 0., 0., 0., 1.);
51  unsigned int b_id = model.addBody(0, SpatialTransform(), j, b1, "b1");
52  Body b2(1., Vector3d(1., 1., 1.), Vector3d(1., 1., 1.));
53  ;
54  unsigned int b_id2 = model.addBody(3, SpatialTransform(), Joint(JointTypeRevoluteY), b2, "b2");
55 
56  EXPECT_STREQ("b2", Utils::getBodyName(model, b_id2).c_str());
57  EXPECT_STREQ("", Utils::getBodyName(model, b_id).c_str());
58 }
59 
60 TEST_F(FloatingBase12DoF, TestKineticEnergy)
61 {
62  VectorNd q = VectorNd::Zero(model->q_size);
63  VectorNd qdot = VectorNd::Zero(model->q_size);
64 
65  for (unsigned int i = 0; i < q.size(); i++)
66  {
67  q[i] = 0.1 * i;
68  qdot[i] = 0.3 * i;
69  }
70 
71  MatrixNd H = MatrixNd::Zero(model->q_size, model->q_size);
72  compositeRigidBodyAlgorithm(*model, q, H, true);
73 
74  double kinetic_energy_ref = 0.5 * qdot.transpose() * H * qdot;
75  double ke = Utils::calcKineticEnergy(*model, q, qdot);
76 
77  EXPECT_EQ(ke, kinetic_energy_ref);
78 }
79 
80 TEST_F(RdlUtilsTests, TestPotentialEnergy)
81 {
82  Model model;
83  Matrix3d inertia = Matrix3d::Identity(3, 3);
84  Body body(0.5, Vector3d(0., 0., 0.), inertia);
85  Joint joint(SpatialVector(0., 0., 0., 1., 0., 0.), SpatialVector(0., 0., 0., 0., 1., 0.), SpatialVector(0., 0., 0., 0., 0., 1.));
86 
87  model.appendBody(Xtrans(Vector3d::Zero()), joint, body);
88 
89  VectorNd q = VectorNd::Zero(model.q_size);
90  double potential_energy_zero = Utils::calcPotentialEnergy(model, q);
91  EXPECT_EQ(0., potential_energy_zero);
92 
93  q[1] = 1.;
94  double potential_energy_lifted = Utils::calcPotentialEnergy(model, q);
95  EXPECT_EQ(4.905, potential_energy_lifted);
96 }
97 
98 TEST_F(RdlUtilsTests, TestCOMSimple)
99 {
100  Model model;
101  Matrix3d inertia = Matrix3d::Identity(3, 3);
102  Body body(123., Vector3d(1., 2., 3.), inertia);
103  Joint joint(SpatialVector(0., 0., 0., 1., 0., 0.), SpatialVector(0., 0., 0., 0., 1., 0.), SpatialVector(0., 0., 0., 0., 0., 1.));
104 
105  model.appendBody(Xtrans(Vector3d::Zero()), joint, body);
106 
107  VectorNd q = VectorNd::Zero(model.q_size);
108  VectorNd qdot = VectorNd::Zero(model.qdot_size);
109 
110  double mass;
111  Vector3d com;
112  Vector3d com_velocity;
113  FramePoint p_com, pcom_2;
114  FrameVector v_com;
115  Utils::calcCenterOfMass(model, q, qdot, mass, com, &com_velocity);
116  Utils::updateCenterOfMassFrame(model, com);
117  EXPECT_TRUE(model.comFrame->getTransformFromParent().r.isApprox(com, 1.e-10));
118  EXPECT_TRUE(model.comFrame->getTransformFromParent().E.isApprox(RobotDynamics::Math::Matrix3d::Identity(), 1.e-10));
119  Utils::calcCenterOfMass(model, q, qdot, mass, p_com, &v_com);
120  Utils::updateCenterOfMassFrame(model, p_com.vec());
121  EXPECT_TRUE(model.comFrame->getTransformFromParent().r.isApprox(p_com.vec(), 1.e-10));
122  EXPECT_TRUE(model.comFrame->getTransformFromParent().E.isApprox(RobotDynamics::Math::Matrix3d::Identity(), 1.e-10));
123  Utils::calcCenterOfMass(model, q, pcom_2);
124  Utils::updateCenterOfMassFrame(model, pcom_2.vec());
125  EXPECT_TRUE(model.comFrame->getTransformFromParent().r.isApprox(pcom_2.vec(), 1.e-10));
126  EXPECT_TRUE(model.comFrame->getTransformFromParent().E.isApprox(RobotDynamics::Math::Matrix3d::Identity(), 1.e-10));
127 
128  EXPECT_EQ(123., mass);
129  EXPECT_EQ(model.mass, 123.);
130  EXPECT_TRUE(unit_test_utils::checkVector3dEq(Vector3d(1., 2., 3.), com));
131  EXPECT_TRUE(unit_test_utils::checkVector3dEq(pcom_2.vec(), com));
132  EXPECT_TRUE(unit_test_utils::checkVector3dEq(p_com.vec(), Vector3d(1., 2., 3.)));
133  EXPECT_TRUE(unit_test_utils::checkVector3dEq(Vector3d(0., 0., 0.), com_velocity));
134 
135  q[1] = 1.;
136  Utils::calcCenterOfMass(model, q, qdot, mass, com, &com_velocity);
137  Utils::updateCenterOfMassFrame(model, com);
138  EXPECT_TRUE(model.comFrame->getTransformFromParent().r.isApprox(com, 1.e-10));
139  EXPECT_TRUE(model.comFrame->getTransformFromParent().E.isApprox(RobotDynamics::Math::Matrix3d::Identity(), 1.e-10));
140  Utils::calcCenterOfMass(model, q, qdot, p_com);
141  Utils::updateCenterOfMassFrame(model, p_com.vec());
142  EXPECT_TRUE(model.comFrame->getTransformFromParent().r.isApprox(p_com.vec(), 1.e-10));
143  EXPECT_TRUE(model.comFrame->getTransformFromParent().E.isApprox(RobotDynamics::Math::Matrix3d::Identity(), 1.e-10));
144  Utils::calcCenterOfMass(model, q, pcom_2);
145  Utils::updateCenterOfMassFrame(model, pcom_2.vec());
146  EXPECT_TRUE(model.comFrame->getTransformFromParent().r.isApprox(pcom_2.vec(), 1.e-10));
147  EXPECT_TRUE(model.comFrame->getTransformFromParent().E.isApprox(RobotDynamics::Math::Matrix3d::Identity(), 1.e-10));
148 
149  EXPECT_TRUE(unit_test_utils::checkVector3dEq(Vector3d(1., 3., 3.), p_com.vec()));
150  EXPECT_TRUE(unit_test_utils::checkVector3dEq(pcom_2.vec(), com));
151  EXPECT_TRUE(unit_test_utils::checkVector3dEq(Vector3d(1., 3., 3.), com));
152  EXPECT_TRUE(unit_test_utils::checkVector3dEq(Vector3d(0., 0., 0.), com_velocity));
153 
154  qdot[1] = 1.;
155  Utils::calcCenterOfMass(model, q, qdot, mass, com, &com_velocity);
156  Utils::calcCenterOfMass(model, q, qdot, p_com);
157  Utils::calcCenterOfMass(model, q, pcom_2);
158  EXPECT_TRUE(unit_test_utils::checkVector3dEq(Vector3d(1., 3., 3.), com));
159  EXPECT_TRUE(unit_test_utils::checkVector3dEq(pcom_2.vec(), com));
160  EXPECT_TRUE(unit_test_utils::checkVector3dEq(Vector3d(1., 3., 3.), p_com.vec()));
161  EXPECT_TRUE(unit_test_utils::checkVector3dEq(Vector3d(0., 1., 0.), com_velocity));
162 }
163 
164 TEST_F(RdlUtilsTests, TestCOMWrench)
165 {
166  Model model;
167  double g = -9.81;
168  model.gravity = MotionVector(0., 0., 0., 0., 0., g);
169  Matrix3d inertia = Matrix3d::Identity(3, 3);
170  Body b1(2., Vector3d(0., 0., 0.), inertia);
171  Joint tx(JointTypePrismatic, Math::Vector3d(1., 0., 0.));
172 
173  model.appendBody(Xtrans(Vector3d(1., 0., 0.)), tx, b1);
174 
175  VectorNd q = VectorNd::Zero(model.q_size);
176  VectorNd qdot = VectorNd::Zero(model.qdot_size);
177 
178  double mass;
179  Vector3d com;
180  Utils::calcCenterOfMass(model, q, qdot, mass, com, nullptr);
181  Utils::updateCenterOfMassFrame(model, com);
182  SpatialForce gravity_wrench = Utils::calcGravityWrenchOnCenterOfMass(model, q);
183  EXPECT_TRUE(gravity_wrench.isApprox(SpatialVector(0., 0., 0., 0., 0., 2. * g), 1.e-10));
184 
185  EXPECT_TRUE(com.isApprox(Vector3d(1., 0., 0.), 1.e-12));
186 
187  model.addBody(0, Xtrans(Vector3d(-1., 0., 0.)), tx, b1);
188  q = VectorNd::Zero(model.q_size);
189  qdot = VectorNd::Zero(model.qdot_size);
190 
191  Utils::calcCenterOfMass(model, q, qdot, mass, com, nullptr);
192  Utils::updateCenterOfMassFrame(model, com);
193  gravity_wrench = Utils::calcGravityWrenchOnCenterOfMass(model, q);
194  EXPECT_TRUE(gravity_wrench.isApprox(SpatialVector(0., 0., 0., 0., 0., 4. * g), 1.e-10));
195 
196  EXPECT_NEAR(com[0], 0., 1.e-12);
197  EXPECT_NEAR(com[1], 0., 1.e-12);
198  EXPECT_NEAR(com[2], 0., 1.e-12);
199 
200  model.addBody(0, Xtrans(Vector3d(0., 1., 0.)), tx, b1);
201  model.addBody(0, Xtrans(Vector3d(0., -1., 0.)), tx, b1);
202  q = VectorNd::Zero(model.q_size);
203  qdot = VectorNd::Zero(model.qdot_size);
204 
205  Utils::calcCenterOfMass(model, q, qdot, mass, com, nullptr);
206  Utils::updateCenterOfMassFrame(model, com);
207  gravity_wrench = Utils::calcGravityWrenchOnCenterOfMass(model, q);
208  EXPECT_TRUE(gravity_wrench.isApprox(SpatialVector(0., 0., 0., 0., 0., 8. * g), 1.e-10));
209 
210  EXPECT_NEAR(com[0], 0., 1.e-12);
211  EXPECT_NEAR(com[1], 0., 1.e-12);
212  EXPECT_NEAR(com[2], 0., 1.e-12);
213 }
214 
215 TEST_F(RdlUtilsTests, TestCOMWrenchYawRotatedCOM)
216 {
217  Model model;
218  double g = -9.81;
219  double mass = 2.;
220  model.gravity = MotionVector(0., 0., 0., 0., 0., g);
221  Matrix3d inertia = Matrix3d::Identity(3, 3);
222  Body b1(mass, Vector3d(0., 0., 0.), inertia);
223  Joint tx(JointTypePrismatic, Math::Vector3d(1., 0., 0.));
224 
225  model.appendBody(Xtrans(Vector3d(1., 0., 0.)), tx, b1);
226 
227  VectorNd q = VectorNd::Zero(model.q_size);
228  VectorNd qdot = VectorNd::Zero(model.qdot_size);
229 
230  double m;
231  Vector3d com;
232  Utils::calcCenterOfMass(model, q, qdot, m, com, nullptr);
233  Utils::updateCenterOfMassFrame(model, com, Vector3d(M_PI, 0., 0.));
234  SpatialForce gravity_wrench = Utils::calcGravityWrenchOnCenterOfMass(model, q);
235  EXPECT_TRUE(gravity_wrench.isApprox(SpatialVector(0., 0., 0., 0., 0., mass * g), 1.e-10));
236 }
237 
238 TEST_F(RdlUtilsTests, TestCOMWrenchRollRotatedCOM)
239 {
240  Model model;
241  double g = -9.81;
242  double mass = 2.;
243  model.gravity = MotionVector(0., 0., 0., 0., 0., g);
244  Matrix3d inertia = Matrix3d::Identity(3, 3);
245  Body b1(mass, Vector3d(0., 0., 0.), inertia);
246  Joint tx(JointTypePrismatic, Math::Vector3d(1., 0., 0.));
247 
248  model.appendBody(Xtrans(Vector3d(1., 0., 0.)), tx, b1);
249 
250  VectorNd q = VectorNd::Zero(model.q_size);
251  VectorNd qdot = VectorNd::Zero(model.qdot_size);
252 
253  double m;
254  Vector3d com;
255  Utils::calcCenterOfMass(model, q, qdot, m, com, nullptr);
256  Utils::updateCenterOfMassFrame(model, com, Vector3d(0., 0., M_PI));
257  SpatialForce gravity_wrench = Utils::calcGravityWrenchOnCenterOfMass(model, q);
258  EXPECT_TRUE(gravity_wrench.isApprox(SpatialVector(0., 0., 0., 0., 0., -mass * g), 1.e-10));
259 }
260 
261 TEST_F(RdlUtilsTests, TestCOMWrenchRollRotatedQuaternionCOM)
262 {
263  Model model;
264  double g = -9.81;
265  double mass = 2.;
266  model.gravity = MotionVector(0., 0., 0., 0., 0., g);
267  Matrix3d inertia = Matrix3d::Identity(3, 3);
268  Body b1(mass, Vector3d(0., 0., 0.), inertia);
269  Joint tx(JointTypePrismatic, Math::Vector3d(1., 0., 0.));
270 
271  model.appendBody(Xtrans(Vector3d(1., 0., 0.)), tx, b1);
272 
273  VectorNd q = VectorNd::Zero(model.q_size);
274  VectorNd qdot = VectorNd::Zero(model.qdot_size);
275 
276  double m;
277  Vector3d com;
278  Utils::calcCenterOfMass(model, q, qdot, m, com, nullptr);
279  Utils::updateCenterOfMassFrame(model, com, Math::intrinsicZYXAnglesToQuaternion(Vector3d(0., 0., M_PI)));
280  SpatialForce gravity_wrench = Utils::calcGravityWrenchOnCenterOfMass(model, q);
281  EXPECT_TRUE(gravity_wrench.isApprox(SpatialVector(0., 0., 0., 0., 0., -mass * g), 1.e-10));
282 }
283 
284 TEST_F(RdlUtilsTests, TestCOMWrenchPitchRotatedCOM)
285 {
286  Model model;
287  double g = -9.81;
288  double mass = 2.;
289  model.gravity = MotionVector(0., 0., 0., 0., 0., g);
290  Matrix3d inertia = Matrix3d::Identity(3, 3);
291  Body b1(mass, Vector3d(0., 0., 0.), inertia);
292  Joint tx(JointTypePrismatic, Math::Vector3d(1., 0., 0.));
293 
294  model.appendBody(Xtrans(Vector3d(1., 0., 0.)), tx, b1);
295 
296  VectorNd q = VectorNd::Zero(model.q_size);
297  VectorNd qdot = VectorNd::Zero(model.qdot_size);
298 
299  double m;
300  Vector3d com;
301  Utils::calcCenterOfMass(model, q, qdot, mass, com, nullptr);
302  Utils::updateCenterOfMassFrame(model, com, Vector3d(0., M_PI, 0.));
303  SpatialForce gravity_wrench = Utils::calcGravityWrenchOnCenterOfMass(model, q);
304  EXPECT_TRUE(gravity_wrench.isApprox(SpatialVector(0., 0., 0., 0., 0., -mass * g), 1.e-10));
305 }
306 
307 TEST_F(RdlUtilsTests, TestCOMWrenchPitchRotatedCOMQuaternion)
308 {
309  Model model;
310  double g = -9.81;
311  double mass = 2.;
312  model.gravity = MotionVector(0., 0., 0., 0., 0., g);
313  Matrix3d inertia = Matrix3d::Identity(3, 3);
314  Body b1(mass, Vector3d(0., 0., 0.), inertia);
315  Joint tx(JointTypePrismatic, Math::Vector3d(1., 0., 0.));
316 
317  model.appendBody(Xtrans(Vector3d(1., 0., 0.)), tx, b1);
318 
319  VectorNd q = VectorNd::Zero(model.q_size);
320  VectorNd qdot = VectorNd::Zero(model.qdot_size);
321 
322  double m;
323  Vector3d com;
324  Utils::calcCenterOfMass(model, q, qdot, mass, com, nullptr);
325  Utils::updateCenterOfMassFrame(model, com, Math::intrinsicZYXAnglesToQuaternion(Vector3d(0., M_PI, 0.)));
326  SpatialForce gravity_wrench = Utils::calcGravityWrenchOnCenterOfMass(model, q);
327  EXPECT_TRUE(gravity_wrench.isApprox(SpatialVector(0., 0., 0., 0., 0., -mass * g), 1.e-10));
328 }
329 
330 TEST_F(Human36, TestCOM)
331 {
332  randomizeStates();
333 
334  double mass;
335  Vector3d com;
336  Vector3d com_velocity;
337  Utils::calcCenterOfMass(*model, q, qdot, mass, com, &com_velocity);
338 
339  FrameVector com_v2;
340 
341  FramePoint p_com, pcom_2;
342  FrameVector v_com;
343  Utils::calcCenterOfMass(*model, q, qdot, p_com, &v_com);
344  Utils::calcCenterOfMassVelocity(*model, q, qdot, com_v2);
345  Utils::updateCenterOfMassFrame(*model, p_com.vec());
346  EXPECT_TRUE(model->comFrame->getTransformFromParent().r.isApprox(p_com.vec(), 1.e-10));
347  EXPECT_TRUE(model->comFrame->getTransformFromParent().E.isApprox(RobotDynamics::Math::Matrix3d::Identity(), 1.e-10));
348  Utils::calcCenterOfMass(*model, q, pcom_2);
349  Utils::updateCenterOfMassFrame(*model, pcom_2.vec());
350  EXPECT_TRUE(model->comFrame->getTransformFromParent().r.isApprox(pcom_2.vec(), 1.e-10));
351  EXPECT_TRUE(model->comFrame->getTransformFromParent().E.isApprox(RobotDynamics::Math::Matrix3d::Identity(), 1.e-10));
352 
353  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(com, p_com.vec(), unit_test_utils::TEST_PREC));
354  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(pcom_2.vec(), p_com.vec(), unit_test_utils::TEST_PREC));
355  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(com_velocity, v_com, unit_test_utils::TEST_PREC));
356  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(com_velocity, com_v2, unit_test_utils::TEST_PREC));
357 
358  randomizeStates();
359 
360  Utils::calcCenterOfMass(*model_3dof, q, qdot, mass, com, &com_velocity);
361  Utils::updateCenterOfMassFrame(*model_3dof, com);
362  Utils::calcCenterOfMassVelocity(*model, q, qdot, com_v2);
363  EXPECT_TRUE(model_3dof->comFrame->getTransformFromParent().r.isApprox(com, 1.e-10));
364  EXPECT_TRUE(model_3dof->comFrame->getTransformFromParent().E.isApprox(RobotDynamics::Math::Matrix3d::Identity(), 1.e-10));
365  Utils::calcCenterOfMass(*model_3dof, q, qdot, mass, p_com, &v_com, nullptr);
366  Utils::updateCenterOfMassFrame(*model_3dof, p_com.vec());
367  EXPECT_TRUE(model_3dof->comFrame->getTransformFromParent().r.isApprox(p_com.vec(), 1.e-10));
368  EXPECT_TRUE(model_3dof->comFrame->getTransformFromParent().E.isApprox(RobotDynamics::Math::Matrix3d::Identity(), 1.e-10));
369  Utils::calcCenterOfMass(*model_3dof, q, pcom_2);
370  Utils::updateCenterOfMassFrame(*model_3dof, pcom_2.vec());
371  EXPECT_TRUE(model_3dof->comFrame->getTransformFromParent().r.isApprox(pcom_2.vec(), 1.e-10));
372  EXPECT_TRUE(model_3dof->comFrame->getTransformFromParent().E.isApprox(RobotDynamics::Math::Matrix3d::Identity(), 1.e-10));
373  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(com, p_com.vec(), unit_test_utils::TEST_PREC));
374  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(pcom_2.vec(), p_com.vec(), unit_test_utils::TEST_PREC));
375  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(com_velocity, v_com, unit_test_utils::TEST_PREC));
376  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(com_velocity, com_v2, unit_test_utils::TEST_PREC));
377 
378  randomizeStates();
379 
380  Utils::calcCenterOfMass(*model_emulated, q, qdot, mass, com, &com_velocity);
381  Utils::calcCenterOfMass(*model_emulated, q, qdot, p_com, &v_com);
382  Utils::calcCenterOfMass(*model_emulated, q, pcom_2);
383  Utils::calcCenterOfMassVelocity(*model, q, qdot, com_v2);
384  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(com, p_com.vec(), unit_test_utils::TEST_PREC));
385  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(pcom_2.vec(), p_com.vec(), unit_test_utils::TEST_PREC));
386  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(com_velocity, v_com, unit_test_utils::TEST_PREC));
387  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(com_v2, v_com, unit_test_utils::TEST_PREC));
388 }
389 
390 TEST_F(Human36, TestCOMCalcSubtreeMass)
391 {
392  double subtreeMass, wholeRobotMass;
393  Vector3d com, com_velocity;
394 
395  Utils::calcCenterOfMass(*model_emulated, q, qdot, wholeRobotMass, com, nullptr);
396  subtreeMass = Utils::calcSubtreeMass(*model_emulated, 0);
397 
398  EXPECT_EQ(subtreeMass, wholeRobotMass);
399 
400  subtreeMass = Utils::calcSubtreeMass(*model_emulated, body_id_emulated[BodyPelvis] + 1);
401  EXPECT_EQ(subtreeMass, 10.3368 + 3.1609 + 1.001);
402 }
403 
404 TEST_F(RdlUtilsTests, TestCOMJacobian)
405 {
406  Model model;
407 
408  unsigned int id1 = model.addBody(0, Xtrans(Vector3d(0, 1, 0)), Joint(SpatialVector(1, 0, 0, 0, 0, 0)), Body(2, Vector3d(0, 1, 0), Vector3d(1, 1, 1)));
409  model.addBody(id1, Xtrans(Vector3d(0, 1, 0)), Joint(SpatialVector(1, 0, 0, 0, 0, 0)), Body(3, Vector3d(0, 1, 0), Vector3d(1, 1, 1)));
410 
411  double wholeRobotMass;
412  Vector3d com, com_velocity;
413 
414  MatrixNd J_com(3, model.qdot_size);
415  J_com.setZero();
416 
417  VectorNd q(2);
418  VectorNd qdot(2);
419 
420  q << 0.1, -0.4;
421  qdot << 0.9, -0.125;
422 
423  Utils::calcCenterOfMassJacobian(model, q, J_com);
424 
425  Utils::calcCenterOfMass(model, q, qdot, wholeRobotMass, com, &com_velocity);
426  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(J_com * qdot, com_velocity, unit_test_utils::TEST_PREC));
427 }
428 
429 TEST_F(Human36, TestCOMJacobianHuman36)
430 {
431  randomizeStates();
432 
433  double wholeRobotMass;
434  Vector3d com, com_velocity;
435 
436  MatrixNd J_com(3, model_emulated->qdot_size);
437  J_com.setZero();
438 
439  Utils::calcCenterOfMassJacobian(*model, q, J_com);
440  Utils::calcCenterOfMass(*model, q, qdot, wholeRobotMass, com, &com_velocity);
441 
442  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(J_com * qdot, com_velocity, unit_test_utils::TEST_PREC));
443 
444  J_com.setZero();
445 
446  Utils::calcCenterOfMassJacobian(*model_3dof, q, J_com);
447 
448  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(J_com * qdot, com_velocity, unit_test_utils::TEST_PREC));
449 }
450 
451 TEST_F(RdlUtilsTests, TestAngularMomentumSimple)
452 {
453  Model model;
454  Matrix3d inertia = Matrix3d::Zero(3, 3);
455  inertia(0, 0) = 1.1;
456  inertia(1, 1) = 2.2;
457  inertia(2, 2) = 3.3;
458 
459  Body body(0.5, Vector3d(1., 0., 0.), inertia);
460  Joint joint(SpatialVector(1., 0., 0., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(0., 0., 1., 0., 0., 0.));
461 
462  model.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint, body);
463 
464  VectorNd q = VectorNd::Zero(model.q_size);
465  VectorNd qdot = VectorNd::Zero(model.qdot_size);
466 
467  double mass;
468  Vector3d com;
469  Vector3d angular_momentum;
470 
471  FramePoint p_com;
472  FrameVector v_com, ang_momentum;
473 
474  qdot << 1., 0., 0.;
475  Utils::calcCenterOfMass(model, q, qdot, mass, com, NULL, &angular_momentum);
476  Utils::calcCenterOfMass(model, q, qdot, mass, p_com, nullptr, &ang_momentum);
477  EXPECT_EQ(Vector3d(1.1, 0., 0.), angular_momentum);
478  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(1.1, 0., 0.), ang_momentum, unit_test_utils::TEST_PREC));
479 
480  qdot << 0., 1., 0.;
481  Utils::calcCenterOfMass(model, q, qdot, mass, com, NULL, &angular_momentum);
482  Utils::calcCenterOfMass(model, q, qdot, mass, p_com, nullptr, &ang_momentum);
483  EXPECT_EQ(Vector3d(0., 2.2, 0.), angular_momentum);
484  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(0., 2.2, 0.), ang_momentum, unit_test_utils::TEST_PREC));
485 
486  qdot << 0., 0., 1.;
487  Utils::calcCenterOfMass(model, q, qdot, mass, com, NULL, &angular_momentum);
488  Utils::calcCenterOfMass(model, q, qdot, mass, p_com, nullptr, &ang_momentum);
489  EXPECT_EQ(Vector3d(0., 0., 3.3), angular_momentum);
490  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(0., 0., 3.3), ang_momentum, unit_test_utils::TEST_PREC));
491 }
492 
493 TEST_F(TwoArms12DoF, TestAngularMomentumSimple)
494 {
495  double mass;
496  Vector3d com;
497  Vector3d angular_momentum;
498 
499  Utils::calcCenterOfMass(*model, q, qdot, mass, com, NULL, &angular_momentum);
500 
501  EXPECT_EQ(Vector3d(0., 0., 0.), angular_momentum);
502 
503  qdot[0] = 1.;
504  qdot[1] = 2.;
505  qdot[2] = 3.;
506 
507  Utils::calcCenterOfMass(*model, q, qdot, mass, com, NULL, &angular_momentum);
508 
509  // only a rough guess from test calculation
510  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(3.3, 2.54, 1.5), angular_momentum, 1.0e-1));
511 
512  qdot[3] = -qdot[0];
513  qdot[4] = -qdot[1];
514  qdot[5] = -qdot[2];
515 
516  Utils::calcCenterOfMass(*model, q, qdot, mass, com, NULL, &angular_momentum);
517 
518  EXPECT_TRUE(angular_momentum[0] == 0);
519  EXPECT_TRUE(angular_momentum[1] < 0);
520  EXPECT_TRUE(angular_momentum[2] == 0.);
521 }
522 
523 TEST_F(TwoArms12DoF, calcCentroidalMomentumMatrix)
524 {
525  double mass;
526  Vector3d com, com_velocity;
527  Vector3d angular_momentum;
528  MatrixNd A(6, model->qdot_size);
529  A.setZero();
530 
531  randomizeStates();
532 
533  Utils::calcCenterOfMass(*model, q, qdot, mass, com, &com_velocity, &angular_momentum);
534  Utils::calcCentroidalMomentumMatrix(*model, q, A);
535 
536  SpatialVector m_exp;
537  m_exp.setLinearPart(com_velocity * mass);
538  m_exp.setAngularPart(angular_momentum);
539 
540  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(SpatialVector(A * qdot), m_exp, unit_test_utils::TEST_PREC * 10.));
541 }
542 
543 TEST_F(RdlUtilsTests, calcCentroidalMomentumMatrix)
544 {
545  Model model;
546 
547  Body b1(1., Vector3d(0., 0., -0.1), Vector3d(1., 1., 1.));
548  Joint j1(JointTypeRevoluteX);
549 
550  unsigned int id = model.addBody(0, SpatialTransform(), j1, b1, "b1");
551 
552  model.addBody(id, Xtrans(Vector3d(0., 0., -1.)), j1, b1, "b2");
553 
554  VectorNd Q(model.q_size);
555  VectorNd QDot(model.qdot_size);
556  Q.setZero();
557  QDot.setZero();
558  QDot[0] = 0.1;
559  QDot[1] = 0.1;
560  MatrixNd A(6, model.qdot_size);
561  A.setZero();
562 
563  double mass;
564  Vector3d com, com_velocity, ang_momentum;
565 
566  Utils::calcCenterOfMass(model, Q, QDot, mass, com, &com_velocity, &ang_momentum);
567 
568  SpatialVector m_exp;
569  m_exp.setAngularPart(ang_momentum);
570  m_exp.setLinearPart(com_velocity * mass);
571 
572  Utils::calcCentroidalMomentumMatrix(model, Q, A, true);
573 
574  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(SpatialVector(A * QDot), m_exp, unit_test_utils::TEST_PREC * 10.));
575 }
576 
577 TEST_F(FixedBase3DoFPlanar, calcCentroidalMomentumMatrix)
578 {
579  double mass;
580  Vector3d com, com_velocity, ang_momentum;
581 
582  randomizeStates();
583 
584  MatrixNd A(6, model->qdot_size), G(3, model->qdot_size);
585  A.setZero();
586 
587  Utils::calcCenterOfMass(*model, Q, QDot, mass, com, &com_velocity, &ang_momentum);
588  Utils::calcCentroidalMomentumMatrix(*model, Q, A);
589 
590  SpatialVector m_exp;
591  m_exp.setLinearPart(com_velocity * mass);
592  m_exp.setAngularPart(ang_momentum);
593 
594  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(SpatialVector(A * QDot), m_exp, unit_test_utils::TEST_PREC * 10.));
595 }
596 
597 TEST_F(Human36, calcCentroidalMomentumMatrix)
598 {
599  double mass;
600  Vector3d com, com_velocity, ang_momentum;
601 
602  randomizeStates();
603 
604  MatrixNd A(6, model->qdot_size), G(3, model->qdot_size);
605  A.setZero();
606 
607  Utils::calcCenterOfMass(*model, q, qdot, mass, com, &com_velocity, &ang_momentum);
608  Utils::calcCentroidalMomentumMatrix(*model, q, A);
609 
610  SpatialVector m_exp;
611  m_exp.setLinearPart(com_velocity * mass);
612  m_exp.setAngularPart(ang_momentum);
613 
614  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(SpatialVector(A * qdot), m_exp, unit_test_utils::TEST_PREC * 10.));
615 
616  A.setZero();
617 
618  Utils::calcCenterOfMass(*model_3dof, q, qdot, mass, com, &com_velocity, &ang_momentum);
619  Utils::calcCentroidalMomentumMatrix(*model_3dof, q, A);
620 
621  m_exp.setLinearPart(com_velocity * mass);
622  m_exp.setAngularPart(ang_momentum);
623 
624  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(SpatialVector(A * qdot), m_exp, unit_test_utils::TEST_PREC * 10.));
625 
626  A.setZero();
627 
628  Utils::calcCenterOfMass(*model_emulated, q, qdot, mass, com, &com_velocity, &ang_momentum);
629  Utils::calcCentroidalMomentumMatrix(*model_emulated, q, A);
630 
631  m_exp.setLinearPart(com_velocity * mass);
632  m_exp.setAngularPart(ang_momentum);
633 
634  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(SpatialVector(A * qdot), m_exp, unit_test_utils::TEST_PREC * 10.));
635 }
636 
637 TEST_F(RdlUtilsTests, calcCentroidalMomentumMatrixDot)
638 {
639  Model model;
640  model.gravity.setZero();
641 
642  Body b1(1., Vector3d(0., 0., -0.1), Vector3d(1., 1., 1.));
643  Joint jx(JointTypeRevoluteX);
644 
645  model.addBody(0, SpatialTransform(), jx, b1, "b1");
646  model.appendBody(Xtrans(Vector3d(0., 0., -1.)), jx, b1, "b2");
647 
648  VectorNd Q(model.q_size);
649  VectorNd QDot(model.qdot_size);
650  VectorNd QDDot(model.qdot_size);
651  VectorNd Tau(model.qdot_size);
652  Q.setZero();
653  QDot.setZero();
654  QDDot.setZero();
655  QDot[0] = 0.1;
656  QDot[1] = 0.1;
657  Tau[0] = 4;
658  Tau[1] = 5;
659  Tau.setZero();
660  MatrixNd A(6, model.qdot_size), ADot(6, model.qdot_size), ADot_num(6, model.qdot_size);
661  A.setZero();
662  ADot.setZero();
663 
664  double mass;
665  Vector3d com, com_velocity, ang_momentum;
666 
667  Utils::calcCenterOfMass(model, Q, QDot, mass, com, &com_velocity, &ang_momentum);
668 
669  SpatialVector m_exp;
670  m_exp.setAngularPart(ang_momentum);
671  m_exp.setLinearPart(com_velocity * mass);
672 
673  Utils::calcCentroidalMomentumMatrix(model, Q, A, true);
674 
675  double h = 0.00000005;
676  Math::VectorNd x_euler = Math::VectorNd::Zero(model.q_size + model.qdot_size);
677  x_euler.setZero();
678  Math::VectorNd x_rk4 = x_euler;
679 
680  unit_test_utils::integrateRk4(model, Q, QDot, x_rk4, Tau, h);
681  Q = x_rk4.head(model.q_size);
682  QDot = x_rk4.tail(model.qdot_size);
683 
684  MatrixNd A2(6, model.qdot_size);
685  A2.setZero();
686  Utils::calcCentroidalMomentumMatrix(model, Q, A2, true);
687  Utils::calcCentroidalMomentumMatrixDot(model, Q, QDot, ADot, true);
688 
689  ADot_num = ((1.0 / h) * (A2 - A));
690 
691  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(ADot_num * QDot, ADot * QDot, 1e-6));
692 }
693 
694 TEST_F(FixedBase3DoFPlanar, calcCentroidalMomentumMatrixDot)
695 {
696  randomizeStates();
697  double h = 0.00000005;
698 
699  RobotDynamics::Math::MatrixNd A1(6, model->qdot_size), A2(6, model->qdot_size), ADot(6, model->qdot_size), ADot_num(6, model->qdot_size);
700  A1.setZero();
701  A2.setZero();
702  ADot.setZero();
703  ADot_num.setZero();
704  RobotDynamics::Utils::calcCentroidalMomentumMatrix(*model, Q, A1);
705 
706  Math::VectorNd x_rk4 = Math::VectorNd::Zero(model->q_size + model->qdot_size);
707  unit_test_utils::integrateRk4(*model, Q, QDot, x_rk4, Tau, h);
708 
709  // integrate
710  Q = x_rk4.head(model->q_size);
711  QDot = x_rk4.tail(model->qdot_size);
712 
713  RobotDynamics::Utils::calcCentroidalMomentumMatrixDot(*model, Q, QDot, ADot);
714 
715  RobotDynamics::Utils::calcCentroidalMomentumMatrix(*model, Q, A2);
716 
717  ADot_num = (1.0 / h) * (A2 - A1);
718 
719  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(ADot_num * QDot, ADot * QDot, 1e-4));
720 }
721 
722 TEST_F(Human36, calcCentroidalMomentumMatrixDot)
723 {
724  randomizeStates();
725  double h = 0.00000005;
726 
727  RobotDynamics::Math::MatrixNd A1(6, model_emulated->qdot_size), A2(6, model_emulated->qdot_size), ADot(6, model_emulated->qdot_size),
728  ADot_num(6, model_emulated->qdot_size);
729  A1.setZero();
730  A2.setZero();
731  ADot.setZero();
732  ADot_num.setZero();
733  RobotDynamics::Utils::calcCentroidalMomentumMatrix(*model_emulated, q, A1);
734 
735  Math::VectorNd x_rk4 = Math::VectorNd::Zero(model_emulated->q_size + model_emulated->qdot_size);
736  unit_test_utils::integrateRk4(*model_emulated, q, qdot, x_rk4, tau, h);
737  // integra
738  q = x_rk4.head(model_emulated->q_size);
739  qdot = x_rk4.tail(model_emulated->qdot_size);
740 
741  RobotDynamics::Utils::calcCentroidalMomentumMatrixDot(*model_emulated, q, qdot, ADot);
742 
743  RobotDynamics::Utils::calcCentroidalMomentumMatrix(*model_emulated, q, A2);
744 
745  ADot_num = (1.0 / h) * (A2 - A1);
746 
747  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(ADot_num * qdot, ADot * qdot, 1e-3));
748 }
749 
750 TEST_F(RdlUtilsTests, calcCentroidalMomentumMatrixDotSphericalJoint)
751 {
752  Model model;
753  Body b(1., Vector3d(1., 1., 1.), Vector3d(1., 1., 1.));
754  model.appendBody(SpatialTransform(), Joint(SpatialVector(0., 0., 0., 1., 0., 0.)), b);
755  model.appendBody(SpatialTransform(), Joint(SpatialVector(0., 0., 0., 0., 1., 0.)), b);
756  model.appendBody(SpatialTransform(), Joint(SpatialVector(0., 0., 0., 0., 0., 1.)), b);
757  unsigned int id = model.appendBody(SpatialTransform(), Joint(JointTypeSpherical), b);
758  RobotDynamics::Math::Quaternion quat(1., 2., 3., 4.);
759  quat.normalize();
760  double h = 0.00000005;
761  VectorNd q(model.q_size), qdot(model.qdot_size), tau(model.qdot_size);
762  q.setZero();
763  qdot.setZero();
764  tau.setZero();
765  RobotDynamics::Math::MatrixNd A1(6, model.qdot_size), A2(6, model.qdot_size), ADot(6, model.qdot_size), ADot_num(6, model.qdot_size);
766  model.SetQuaternion(id, quat, q);
767  qdot[0] = 1.;
768  qdot[1] = -0.3;
769  qdot[2] = 0.115;
770  qdot[3] = 0.36;
771  qdot[4] = -1.1;
772  qdot[5] = 2.2;
773  A1.setZero();
774  A2.setZero();
775  ADot.setZero();
776  ADot_num.setZero();
777  RobotDynamics::Utils::calcCentroidalMomentumMatrix(model, q, A1);
778 
779  Math::VectorNd x_rk4 = Math::VectorNd::Zero(model.q_size + model.qdot_size);
780  unit_test_utils::integrateEuler(model, q, qdot, x_rk4, tau, h);
781  q = x_rk4.head(model.q_size);
782  qdot = x_rk4.tail(model.qdot_size);
783 
784  RobotDynamics::Utils::calcCentroidalMomentumMatrixDot(model, q, qdot, ADot);
785 
786  RobotDynamics::Utils::calcCentroidalMomentumMatrix(model, q, A2);
787 
788  ADot_num = (1.0 / h) * (A2 - A1);
789 
790  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(ADot_num * qdot, ADot * qdot, 1e-4));
791 }
792 
793 int main(int argc, char** argv)
794 {
795  ::testing::InitGoogleTest(&argc, argv);
796  return RUN_ALL_TESTS();
797 }
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::Utils::calcCenterOfMassVelocity
void calcCenterOfMassVelocity(Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, Math::Vector3d &com_vel, bool update_kinematics=true)
Computes the Center of Mass (COM) velocity in world frame.
Definition: rdl_utils.cc:420
RobotDynamics::Utils::calcCenterOfMassJacobian
void calcCenterOfMassJacobian(Model &model, const Math::VectorNd &q, Math::MatrixNd &jCom, bool update_kinematics=true)
Computes the matrix such that .
Definition: rdl_utils.cc:464
RobotDynamics::Math::FramePoint
A FramePoint is a 3D point that is expressed in a ReferenceFrame. To change the ReferenceFrame a Fram...
Definition: FramePoint.hpp:40
RobotDynamics::Body
Describes all properties of a single body.
Definition: Body.h:30
Tau
VectorNd Tau
Definition: RdlTwoLegModelTests.cpp:32
QDDot
VectorNd QDDot
Definition: RdlTwoLegModelTests.cpp:31
main
int main(int argc, char **argv)
Definition: RdlUtilsTests.cpp:793
rdl_utils.h
RobotDynamics::Model::SetQuaternion
void SetQuaternion(unsigned int i, const Math::Quaternion &quat, Math::VectorNd &Q) const
Sets the quaternion for body i (only valid if body i is connected by a JointTypeSpherical joint) ...
Definition: Model.h:609
RobotDynamics::Math::SpatialVector::setAngularPart
void setAngularPart(const Vector3d &v)
Definition: rdl_eigenmath.h:217
QDot
VectorNd QDot
Definition: RdlTwoLegModelTests.cpp:30
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
Q
VectorNd Q
Definition: RdlTwoLegModelTests.cpp:29
RobotDynamics::Math::Xtrans
SpatialTransform Xtrans(const Vector3d &r)
Get pure translation transform.
Definition: SpatialAlgebraOperators.h:369
RobotDynamics::Utils::updateCenterOfMassFrame
RobotDynamics::Math::FramePoint updateCenterOfMassFrame(Model &model, const Math::VectorNd &q, bool update_kinematics=true)
Computes the center of mass and updates the center of mass reference frame of the model...
Definition: rdl_utils.cc:348
TEST_F
TEST_F(RdlUtilsTests, testGetDofName)
Definition: RdlUtilsTests.cpp:16
RobotDynamics::Model::gravity
Math::MotionVector gravity
the cartesian vector of the gravity
Definition: Model.h:196
RobotDynamics::Model::appendBody
unsigned int appendBody(const Math::SpatialTransform &joint_frame, const Joint &joint, const Body &body, std::string body_name="")
Adds a Body to the model such that the previously added Body is the Parent.
Definition: Model.cc:517
RobotDynamics::Utils::getBodyName
std::string getBodyName(const RobotDynamics::Model &model, unsigned int body_id)
get body name, returns empty string if bodyid is virtual and has multiple child bodies ...
Definition: rdl_utils.cc:59
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::Math::Point3d::vec
EIGEN_STRONG_INLINE Math::Vector3d vec() const
Definition: Point3.hpp:269
unit_test_utils::checkVector3dEq
static bool checkVector3dEq(const RobotDynamics::Math::Vector3d &v1, const RobotDynamics::Math::Vector3d &v2)
Definition: UnitTestUtils.hpp:212
RobotDynamics::Model::q_size
unsigned int q_size
The size of the -vector. For models without spherical joints the value is the same as Model::dof_coun...
Definition: Model.h:178
RobotDynamics::Math::SpatialTransform
Compact representation of spatial transformations.
Definition: rdl_eigenmath.h:337
RobotDynamics::Utils::calcSubtreeMass
double calcSubtreeMass(Model &model, const unsigned int bodyId)
Calculates the total mass of the subtree beginning with body bodyId and traversing outwards from ther...
Definition: rdl_utils.cc:519
RobotDynamics::Utils::calcPotentialEnergy
double calcPotentialEnergy(Model &model, const Math::VectorNd &q, bool update_kinematics=true)
Computes the potential energy of the full model.
Definition: rdl_utils.cc:533
RobotDynamics::Utils::calcGravityWrenchOnCenterOfMass
Math::SpatialForce calcGravityWrenchOnCenterOfMass(Model &model, const Math::VectorNd &q, bool update_kinematics=true)
Computes the gravitational wrench experienced on the robots center of mass.
Definition: rdl_utils.cc:440
RobotDynamics::Math::FrameVector
A FrameVector is a 3D vector with a ReferenceFrame, and all operations between FrameVectors and other...
Definition: FrameVector.hpp:33
RobotDynamics::JointType6DoF
Emulated 6 DoF joint.
Definition: Joint.h:212
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::Math::Quaternion
Quaternion that are used for singularity free joints.
Definition: Quaternion.h:25
RobotDynamics::Math::VectorNd
Eigen::VectorXd VectorNd
Definition: rdl_eigenmath.h:25
unit_test_utils::integrateRk4
void integrateRk4(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::VectorNd &x, const Math::VectorNd &tau, double h, Math::SpatialForceV *f_ext=nullptr)
Using a Runge-Kutta 4th order algorithm, integrate the system dynamics one step.
Definition: UnitTestUtils.hpp:471
RobotDynamics::Utils::calcCentroidalMomentumMatrix
void calcCentroidalMomentumMatrix(Model &model, const Math::VectorNd &q, Math::MatrixNd &A, bool update_kinematics=true)
Calculates the centroidal momentum matrix, for a model. The centroidal momentum matrix is a matrix ...
Definition: rdl_utils.cc:560
RobotDynamics::Joint::mJointAxes
Math::SpatialVector * mJointAxes
The spatial axes of the joint.
Definition: Joint.h:625
RobotDynamics::Utils::getDofName
std::string getDofName(const Math::SpatialVector &joint_dof)
get string abbreviation for dof name from spatial vector.
Definition: rdl_utils.cc:27
RobotDynamics::Model
Contains all information about the rigid body model.
Definition: Model.h:121
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::integrateEuler
void integrateEuler(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::VectorNd &x, const Math::VectorNd &tau, double h, Math::SpatialForceV *f_ext=nullptr)
Using simple Euler integration, integrate the system dynamics one step.
Definition: UnitTestUtils.hpp:395
RobotDynamics::Math::SpatialForce
A SpatialForce is a spatial vector with the angular part being three moments and the linear part bein...
Definition: SpatialForce.hpp:32
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::JointTypeSpherical
3 DoF joint using Quaternions for joint positional variables and
Definition: Joint.h:195
RobotDynamics::Utils::calcKineticEnergy
double calcKineticEnergy(Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, bool update_kinematics=true)
Computes the kinetic energy of the full model.
Definition: rdl_utils.cc:544
RobotDynamics::Math::intrinsicZYXAnglesToQuaternion
static Quaternion intrinsicZYXAnglesToQuaternion(double yaw, double pitch, double roll)
Convert YPR angles to quaternion.
Definition: SpatialAlgebraOperators.h:82
RobotDynamics
Namespace for all structures of the RobotDynamics library.
Definition: Body.h:21
RobotDynamics::Model::comFrame
ReferenceFramePtr comFrame
Definition: Model.h:331
RobotDynamics::Model::qdot_size
unsigned int qdot_size
The size of the.
Definition: Model.h:187

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