Debian Codegraph @

unittests/integration/test_Dynamics.cpp

raw
1 /*
2 * Copyright (c) 2011-2022, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include <iostream>
34
35 #include <Eigen/Dense>
36 #include <gtest/gtest.h>
37
38 #include "dart/common/Console.hpp"
39 #include "dart/dynamics/BodyNode.hpp"
40 #include "dart/dynamics/SimpleFrame.hpp"
41 #include "dart/dynamics/Skeleton.hpp"
42 #include "dart/math/Geometry.hpp"
43 #include "dart/math/Helpers.hpp"
44 #include "dart/math/Random.hpp"
45 #include "dart/simulation/World.hpp"
46 #include "dart/utils/SkelParser.hpp"
47
48 #include "TestHelpers.hpp"
49
50 using namespace Eigen;
51 using namespace dart;
52
53 //==============================================================================
54 class DynamicsTest : public ::testing::Test
55 {
56 public:
57 // Get Skel file URI to test.
58 const std::vector<common::Uri>& getList() const;
59
60 // Get reference frames
61 const std::vector<SimpleFrame*>& getRefFrames() const;
62
63 // Randomize the properties of all the reference frames
64 void randomizeRefFrames();
65
66 // Get mass matrix of _skel using Jacobians and inertias of each body
67 // in _skel.
68 MatrixXd getMassMatrix(dynamics::SkeletonPtr _skel);
69
70 // Get augmented mass matrix of _skel using Jacobians and inertias of
71 // each body in _skel.
72 MatrixXd getAugMassMatrix(dynamics::SkeletonPtr _skel);
73
74 // Compare velocities computed by recursive method, Jacobian, and finite
75 // difference.
76 void testJacobians(const common::Uri& uri);
77
78 // Compare velocities and accelerations with actual vaules and approximates
79 // using finite differece method.
80 void testFiniteDifferenceGeneralizedCoordinates(const common::Uri& uri);
81
82 // Compare spatial velocities computed by forward kinematics and finite
83 // difference.
84 void testFiniteDifferenceBodyNodeVelocity(const common::Uri& uri);
85
86 // Compare accelerations computed by recursive method, Jacobian, and finite
87 // difference.
88 void testFiniteDifferenceBodyNodeAcceleration(const common::Uri& uri);
89
90 // Test if the recursive forward kinematics algorithm computes
91 // transformations, spatial velocities, and spatial accelerations correctly.
92 void testForwardKinematics(const common::Uri& uri);
93
94 // Test inverse dynamics with various joint forces.
95 void testInverseDynamics(const common::Uri& uri);
96
97 // Compare dynamics terms in equations of motion such as mass matrix, mass
98 // inverse matrix, Coriolis force vector, gravity force vector, and external
99 // force vector.
100 void compareEquationsOfMotion(const common::Uri& uri);
101
102 // Test skeleton's COM and its related quantities.
103 void testCenterOfMass(const common::Uri& uri);
104
105 // Test if the com acceleration is equal to the gravity
106 void testCenterOfMassFreeFall(const common::Uri& uri);
107
108 //
109 void testConstraintImpulse(const common::Uri& uri);
110
111 // Test impulse based dynamics
112 void testImpulseBasedDynamics(const common::Uri& uri);
113
114 protected:
115 // Sets up the test fixture.
116 void SetUp() override;
117
118 // Skel file list.
119 std::vector<common::Uri> fileList;
120
121 std::vector<SimpleFrame*> refFrames;
122 };
123
124 //==============================================================================
125 void DynamicsTest::SetUp()
126 {
127 // Create a list of skel files to test with
128 fileList.push_back("dart://sample/skel/test/chainwhipa.skel");
129 fileList.push_back("dart://sample/skel/test/single_pendulum.skel");
130 fileList.push_back(
131 "dart://sample/skel/test/single_pendulum_euler_joint.skel");
132 fileList.push_back("dart://sample/skel/test/single_pendulum_ball_joint.skel");
133 fileList.push_back("dart://sample/skel/test/double_pendulum.skel");
134 fileList.push_back(
135 "dart://sample/skel/test/double_pendulum_euler_joint.skel");
136 fileList.push_back("dart://sample/skel/test/double_pendulum_ball_joint.skel");
137 fileList.push_back(
138 "dart://sample/skel/test/serial_chain_revolute_joint.skel");
139 fileList.push_back(
140 "dart://sample/skel/test/serial_chain_eulerxyz_joint.skel");
141 fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint.skel");
142 fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint_20.skel");
143 fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint_40.skel");
144 fileList.push_back("dart://sample/skel/test/simple_tree_structure.skel");
145 fileList.push_back(
146 "dart://sample/skel/test/simple_tree_structure_euler_joint.skel");
147 fileList.push_back(
148 "dart://sample/skel/test/simple_tree_structure_ball_joint.skel");
149 fileList.push_back("dart://sample/skel/test/tree_structure.skel");
150 fileList.push_back("dart://sample/skel/test/tree_structure_euler_joint.skel");
151 fileList.push_back("dart://sample/skel/test/tree_structure_ball_joint.skel");
152 fileList.push_back("dart://sample/skel/fullbody1.skel");
153
154 // Create a list of reference frames to use during tests
155 refFrames.push_back(new SimpleFrame(Frame::World(), "refFrame1"));
156 refFrames.push_back(new SimpleFrame(refFrames.back(), "refFrame2"));
157 refFrames.push_back(new SimpleFrame(refFrames.back(), "refFrame3"));
158 refFrames.push_back(new SimpleFrame(refFrames.back(), "refFrame4"));
159 refFrames.push_back(new SimpleFrame(Frame::World(), "refFrame5"));
160 refFrames.push_back(new SimpleFrame(refFrames.back(), "refFrame6"));
161 }
162
163 //==============================================================================
164 const std::vector<common::Uri>& DynamicsTest::getList() const
165 {
166 return fileList;
167 }
168
169 //==============================================================================
170 const std::vector<SimpleFrame*>& DynamicsTest::getRefFrames() const
171 {
172 return refFrames;
173 }
174
175 //==============================================================================
176 void DynamicsTest::randomizeRefFrames()
177 {
178 for (std::size_t i = 0; i < refFrames.size(); ++i)
179 {
180 SimpleFrame* F = refFrames[i];
181
182 Eigen::Vector3d p = Random::uniform<Eigen::Vector3d>(-100, 100);
183 Eigen::Vector3d theta = Random::uniform<Eigen::Vector3d>(
184 -2 * dart::math::constantsd::pi(), 2 * dart::math::constantsd::pi());
185
186 Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
187 tf.translate(p);
188 tf.linear() = math::eulerXYZToMatrix(theta);
189
190 F->setRelativeTransform(tf);
191 F->setRelativeSpatialVelocity(Random::uniform<Eigen::Vector6d>(-100, 100));
192 F->setRelativeSpatialAcceleration(
193 Random::uniform<Eigen::Vector6d>(-100, 100));
194 }
195 }
196
197 //==============================================================================
198 MatrixXd DynamicsTest::getMassMatrix(dynamics::SkeletonPtr _skel)
199 {
200 int skelDof = _skel->getNumDofs();
201
202 MatrixXd skelM = MatrixXd::Zero(skelDof, skelDof); // Mass matrix of skeleton
203 MatrixXd M; // Body mass
204 MatrixXd I; // Body inertia
205 MatrixXd J; // Body Jacobian
206
207 for (std::size_t i = 0; i < _skel->getNumBodyNodes(); ++i)
208 {
209 dynamics::BodyNode* body = _skel->getBodyNode(i);
210
211 int dof = body->getNumDependentGenCoords();
212 I = body->getSpatialInertia();
213 J = body->getJacobian();
214
215 EXPECT_EQ(I.rows(), 6);
216 EXPECT_EQ(I.cols(), 6);
217 EXPECT_EQ(J.rows(), 6);
218 EXPECT_EQ(J.cols(), dof);
219
220 M = J.transpose() * I * J; // (dof x dof) matrix
221
222 for (int j = 0; j < dof; ++j)
223 {
224 int jIdx = body->getDependentGenCoordIndex(j);
225
226 for (int k = 0; k < dof; ++k)
227 {
228 int kIdx = body->getDependentGenCoordIndex(k);
229
230 skelM(jIdx, kIdx) += M(j, k);
231 }
232 }
233 }
234
235 return skelM;
236 }
237
238 //==============================================================================
239 MatrixXd DynamicsTest::getAugMassMatrix(dynamics::SkeletonPtr _skel)
240 {
241 int dof = _skel->getNumDofs();
242 double dt = _skel->getTimeStep();
243
244 MatrixXd M = getMassMatrix(_skel);
245 MatrixXd D = MatrixXd::Zero(dof, dof);
246 MatrixXd K = MatrixXd::Zero(dof, dof);
247 MatrixXd AugM;
248
249 // Compute diagonal matrices of joint damping and joint stiffness
250 for (std::size_t i = 0; i < _skel->getNumBodyNodes(); ++i)
251 {
252 dynamics::BodyNode* body = _skel->getBodyNode(i);
253 dynamics::Joint* joint = body->getParentJoint();
254
255 EXPECT_TRUE(body != nullptr);
256 EXPECT_TRUE(joint != nullptr);
257
258 int dof = joint->getNumDofs();
259
260 for (int j = 0; j < dof; ++j)
261 {
262 int idx = joint->getIndexInSkeleton(j);
263
264 D(idx, idx) = joint->getDampingCoefficient(j);
265 K(idx, idx) = joint->getSpringStiffness(j);
266 }
267 }
268
269 AugM = M + (dt * D) + (dt * dt * K);
270
271 return AugM;
272 }
273
274 template <typename T>
275 void printComparisonError(
276 const std::string& _comparison,
277 const std::string& _name,
278 const std::string& _frame,
279 const T& fk,
280 const T& jac)
281 {
282 std::cout << "Disagreement between FK and Jacobian results for "
283 << _comparison << " of '" << _name
284 << "' with a reference Frame of '" << _frame << "'\n"
285 << "FK: " << fk.transpose() << "\n"
286 << "Jac: " << jac.transpose() << "\n";
287 }
288
289 //==============================================================================
290 void compareBodyNodeFkToJacobian(
291 const BodyNode* bn, const Frame* refFrame, double tolerance)
292 {
293 using math::AngularJacobian;
294 using math::Jacobian;
295 using math::LinearJacobian;
296
297 ConstSkeletonPtr skel = bn->getSkeleton();
298
299 VectorXd dq = skel->getVelocities();
300 VectorXd ddq = skel->getAccelerations();
301
302 const std::vector<std::size_t>& coords = bn->getDependentGenCoordIndices();
303 VectorXd dqSeg = skel->getVelocities(coords);
304 VectorXd ddqSeg = skel->getAccelerations(coords);
305
306 //-- Spatial Jacobian tests --------------------------------------------------
307
308 Vector6d SpatialVelFk = bn->getSpatialVelocity(Frame::World(), refFrame);
309 Vector6d SpatialAccFk = bn->getSpatialAcceleration(Frame::World(), refFrame);
310
311 Jacobian SpatialJacSeg = bn->getJacobian(refFrame);
312 Jacobian SpatialJacDerivSeg = bn->getJacobianSpatialDeriv(refFrame);
313
314 Vector6d SpatialVelJacSeg = SpatialJacSeg * dqSeg;
315 Vector6d SpatialAccJacSeg
316 = SpatialJacSeg * ddqSeg + SpatialJacDerivSeg * dqSeg;
317
318 Jacobian SpatialJac = skel->getJacobian(bn, refFrame);
319 Jacobian SpatialJacDeriv = skel->getJacobianSpatialDeriv(bn, refFrame);
320
321 Vector6d SpatialVelJac = SpatialJac * dq;
322 Vector6d SpatialAccJac = SpatialJac * ddq + SpatialJacDeriv * dq;
323
324 bool spatialVelSegEqual = equals(SpatialVelFk, SpatialVelJacSeg, tolerance);
325 bool spatialVelEqual = equals(SpatialVelFk, SpatialVelJac, tolerance);
326 EXPECT_TRUE(spatialVelSegEqual);
327 EXPECT_TRUE(spatialVelEqual);
328 if (!spatialVelSegEqual)
329 printComparisonError(
330 "spatial velocity (seg)",
331 bn->getName(),
332 refFrame->getName(),
333 SpatialVelFk,
334 SpatialVelJacSeg);
335 if (!spatialVelEqual)
336 printComparisonError(
337 "spatial velocity",
338 bn->getName(),
339 refFrame->getName(),
340 SpatialVelFk,
341 SpatialVelJac);
342
343 bool spatialAccSegEqual = equals(SpatialAccFk, SpatialAccJacSeg, tolerance);
344 bool spatialAccEqual = equals(SpatialAccFk, SpatialAccJac, tolerance);
345 EXPECT_TRUE(spatialAccSegEqual);
346 EXPECT_TRUE(spatialAccEqual);
347 if (!spatialAccSegEqual)
348 printComparisonError(
349 "spatial acceleration (seg)",
350 bn->getName(),
351 refFrame->getName(),
352 SpatialAccFk,
353 SpatialAccJacSeg);
354 if (!spatialAccEqual)
355 printComparisonError(
356 "spatial acceleration",
357 bn->getName(),
358 refFrame->getName(),
359 SpatialAccFk,
360 SpatialAccJac);
361
362 //-- Linear Jacobian tests ---------------------------------------------------
363
364 Vector3d LinearVelFk = bn->getLinearVelocity(Frame::World(), refFrame);
365 Vector3d LinearAccFk = bn->getLinearAcceleration(Frame::World(), refFrame);
366
367 LinearJacobian LinearJacSeg = bn->getLinearJacobian(refFrame);
368 LinearJacobian LinearJacDerivSeg = bn->getLinearJacobianDeriv(refFrame);
369
370 Vector3d LinearVelJacSeg = LinearJacSeg * dqSeg;
371 Vector3d LinearAccJacSeg = LinearJacSeg * ddqSeg + LinearJacDerivSeg * dqSeg;
372
373 LinearJacobian LinearJac = skel->getLinearJacobian(bn, refFrame);
374 LinearJacobian LinearJacDeriv = skel->getLinearJacobianDeriv(bn, refFrame);
375
376 Vector3d LinearVelJac = LinearJac * dq;
377 Vector3d LinearAccJac = LinearJac * ddq + LinearJacDeriv * dq;
378
379 bool linearVelSegEqual = equals(LinearVelFk, LinearVelJacSeg, tolerance);
380 bool linearVelEqual = equals(LinearVelFk, LinearVelJac, tolerance);
381 EXPECT_TRUE(linearVelSegEqual);
382 EXPECT_TRUE(linearVelEqual);
383 if (!linearVelSegEqual)
384 printComparisonError(
385 "linear velocity (seg)",
386 bn->getName(),
387 refFrame->getName(),
388 LinearVelFk,
389 LinearVelJacSeg);
390 if (!linearVelEqual)
391 printComparisonError(
392 "linear velocity",
393 bn->getName(),
394 refFrame->getName(),
395 LinearVelFk,
396 LinearVelJac);
397
398 bool linearAccSegEqual = equals(LinearAccFk, LinearAccJacSeg, tolerance);
399 bool linearAccEqual = equals(LinearAccFk, LinearAccJac, tolerance);
400 EXPECT_TRUE(linearAccSegEqual);
401 EXPECT_TRUE(linearAccEqual);
402 if (!linearAccSegEqual)
403 printComparisonError(
404 "linear acceleration (seg)",
405 bn->getName(),
406 refFrame->getName(),
407 LinearAccFk,
408 LinearAccJacSeg);
409 if (!linearAccEqual)
410 printComparisonError(
411 "linear acceleration",
412 bn->getName(),
413 refFrame->getName(),
414 LinearAccFk,
415 LinearAccJac);
416
417 //-- Angular Jacobian tests
418
419 Vector3d AngularVelFk = bn->getAngularVelocity(Frame::World(), refFrame);
420 Vector3d AngularAccFk = bn->getAngularAcceleration(Frame::World(), refFrame);
421
422 AngularJacobian AngularJacSeg = bn->getAngularJacobian(refFrame);
423 AngularJacobian AngularJacDerivSeg = bn->getAngularJacobianDeriv(refFrame);
424
425 Vector3d AngularVelJacSeg = AngularJacSeg * dqSeg;
426 Vector3d AngularAccJacSeg
427 = AngularJacSeg * ddqSeg + AngularJacDerivSeg * dqSeg;
428
429 AngularJacobian AngularJac = skel->getAngularJacobian(bn, refFrame);
430 AngularJacobian AngularJacDeriv = skel->getAngularJacobianDeriv(bn, refFrame);
431
432 Vector3d AngularVelJac = AngularJac * dq;
433 Vector3d AngularAccJac = AngularJac * ddq + AngularJacDeriv * dq;
434
435 bool angularVelSegEqual = equals(AngularVelFk, AngularVelJacSeg, tolerance);
436 bool angularVelEqual = equals(AngularVelFk, AngularVelJac, tolerance);
437 EXPECT_TRUE(angularVelSegEqual);
438 EXPECT_TRUE(angularVelEqual);
439 if (!angularVelSegEqual)
440 printComparisonError(
441 "angular velocity (seg)",
442 bn->getName(),
443 refFrame->getName(),
444 AngularVelFk,
445 AngularVelJacSeg);
446 if (!angularVelEqual)
447 printComparisonError(
448 "angular velocity",
449 bn->getName(),
450 refFrame->getName(),
451 AngularVelFk,
452 AngularVelJac);
453
454 bool angularAccSegEqual = equals(AngularAccFk, AngularAccJacSeg, tolerance);
455 bool angularAccEqual = equals(AngularAccFk, AngularAccJac, tolerance);
456 EXPECT_TRUE(angularAccSegEqual);
457 EXPECT_TRUE(angularAccEqual);
458 if (!angularAccSegEqual)
459 printComparisonError(
460 "angular acceleration (seg)",
461 bn->getName(),
462 refFrame->getName(),
463 AngularAccFk,
464 AngularAccJacSeg);
465 if (!angularAccEqual)
466 printComparisonError(
467 "angular acceleration",
468 bn->getName(),
469 refFrame->getName(),
470 AngularAccFk,
471 AngularAccJac);
472 }
473
474 //==============================================================================
475 void compareBodyNodeFkToJacobian(
476 const BodyNode* bn,
477 const Frame* refFrame,
478 const Eigen::Vector3d& offset,
479 double tolerance)
480 {
481 using math::AngularJacobian;
482 using math::Jacobian;
483 using math::LinearJacobian;
484
485 ConstSkeletonPtr skel = bn->getSkeleton();
486
487 VectorXd dq = skel->getVelocities();
488 VectorXd ddq = skel->getAccelerations();
489
490 const std::vector<std::size_t>& coords = bn->getDependentGenCoordIndices();
491 VectorXd dqSeg = skel->getVelocities(coords);
492 VectorXd ddqSeg = skel->getAccelerations(coords);
493
494 //-- Spatial Jacobian tests --------------------------------------------------
495
496 Vector6d SpatialVelFk
497 = bn->getSpatialVelocity(offset, Frame::World(), refFrame);
498 Vector6d SpatialAccFk
499 = bn->getSpatialAcceleration(offset, Frame::World(), refFrame);
500
501 Jacobian SpatialJacSeg = bn->getJacobian(offset, refFrame);
502 Jacobian SpatialJacDerivSeg = bn->getJacobianSpatialDeriv(offset, refFrame);
503
504 Vector6d SpatialVelJacSeg = SpatialJacSeg * dqSeg;
505 Vector6d SpatialAccJacSeg
506 = SpatialJacSeg * ddqSeg + SpatialJacDerivSeg * dqSeg;
507
508 Jacobian SpatialJac = skel->getJacobian(bn, offset, refFrame);
509 Jacobian SpatialJacDeriv
510 = skel->getJacobianSpatialDeriv(bn, offset, refFrame);
511
512 Vector6d SpatialVelJac = SpatialJac * dq;
513 Vector6d SpatialAccJac = SpatialJac * ddq + SpatialJacDeriv * dq;
514
515 bool spatialVelSegEqual = equals(SpatialVelFk, SpatialVelJacSeg, tolerance);
516 bool spatialVelEqual = equals(SpatialVelFk, SpatialVelJac, tolerance);
517 EXPECT_TRUE(spatialVelSegEqual);
518 EXPECT_TRUE(spatialVelEqual);
519 if (!spatialVelSegEqual)
520 printComparisonError(
521 "spatial velocity w/ offset (seg)",
522 bn->getName(),
523 refFrame->getName(),
524 SpatialVelFk,
525 SpatialVelJacSeg);
526 if (!spatialVelEqual)
527 printComparisonError(
528 "spatial velocity w/ offset",
529 bn->getName(),
530 refFrame->getName(),
531 SpatialVelFk,
532 SpatialVelJac);
533
534 bool spatialAccSegEqual = equals(SpatialAccFk, SpatialAccJacSeg, tolerance);
535 bool spatialAccEqual = equals(SpatialAccFk, SpatialAccJac, tolerance);
536 EXPECT_TRUE(spatialAccSegEqual);
537 EXPECT_TRUE(spatialAccEqual);
538 if (!spatialAccSegEqual)
539 printComparisonError(
540 "spatial acceleration w/ offset (seg)",
541 bn->getName(),
542 refFrame->getName(),
543 SpatialAccFk,
544 SpatialAccJacSeg);
545 if (!spatialAccEqual)
546 printComparisonError(
547 "spatial acceleration w/ offset",
548 bn->getName(),
549 refFrame->getName(),
550 SpatialAccFk,
551 SpatialAccJac);
552
553 //-- Linear Jacobian tests ---------------------------------------------------
554
555 Vector3d LinearVelFk
556 = bn->getLinearVelocity(offset, Frame::World(), refFrame);
557 Vector3d LinearAccFk
558 = bn->getLinearAcceleration(offset, Frame::World(), refFrame);
559
560 LinearJacobian LinearJacSeg = bn->getLinearJacobian(offset, refFrame);
561 LinearJacobian LinearJacDerivSeg
562 = bn->getLinearJacobianDeriv(offset, refFrame);
563
564 Vector3d LinearVelJacSeg = LinearJacSeg * dqSeg;
565 Vector3d LinearAccJacSeg = LinearJacSeg * ddqSeg + LinearJacDerivSeg * dqSeg;
566
567 LinearJacobian LinearJac = skel->getLinearJacobian(bn, offset, refFrame);
568 LinearJacobian LinearJacDeriv
569 = skel->getLinearJacobianDeriv(bn, offset, refFrame);
570
571 Vector3d LinearVelJac = LinearJac * dq;
572 Vector3d LinearAccJac = LinearJac * ddq + LinearJacDeriv * dq;
573
574 bool linearVelSegEqual = equals(LinearVelFk, LinearVelJacSeg, tolerance);
575 bool linearVelEqual = equals(LinearVelFk, LinearVelJac, tolerance);
576 EXPECT_TRUE(linearVelSegEqual);
577 EXPECT_TRUE(linearVelEqual);
578 if (!linearVelSegEqual)
579 printComparisonError(
580 "linear velocity w/ offset (seg)",
581 bn->getName(),
582 refFrame->getName(),
583 LinearVelFk,
584 LinearVelJacSeg);
585 if (!linearVelEqual)
586 printComparisonError(
587 "linear velocity w/ offset",
588 bn->getName(),
589 refFrame->getName(),
590 LinearVelFk,
591 LinearVelJac);
592
593 bool linearAccSegEqual = equals(LinearAccFk, LinearAccJacSeg, tolerance);
594 bool linearAccEqual = equals(LinearAccFk, LinearAccJac, tolerance);
595 EXPECT_TRUE(linearAccSegEqual);
596 EXPECT_TRUE(linearAccEqual);
597 if (!linearAccSegEqual)
598 printComparisonError(
599 "linear acceleration w/ offset (seg)",
600 bn->getName(),
601 refFrame->getName(),
602 LinearAccFk,
603 LinearAccJacSeg);
604 if (!linearAccEqual)
605 printComparisonError(
606 "linear acceleration w/ offset",
607 bn->getName(),
608 refFrame->getName(),
609 LinearAccFk,
610 LinearAccJac);
611
612 //-- Angular Jacobian tests --------------------------------------------------
613
614 Vector3d AngularVelFk = bn->getAngularVelocity(Frame::World(), refFrame);
615 Vector3d AngularAccFk = bn->getAngularAcceleration(Frame::World(), refFrame);
616
617 AngularJacobian AngularJacSeg = bn->getAngularJacobian(refFrame);
618 AngularJacobian AngularJacDerivSeg = bn->getAngularJacobianDeriv(refFrame);
619
620 Vector3d AngularVelJacSeg = AngularJacSeg * dqSeg;
621 Vector3d AngularAccJacSeg
622 = AngularJacSeg * ddqSeg + AngularJacDerivSeg * dqSeg;
623
624 AngularJacobian AngularJac = skel->getAngularJacobian(bn, refFrame);
625 AngularJacobian AngularJacDeriv = skel->getAngularJacobianDeriv(bn, refFrame);
626
627 Vector3d AngularVelJac = AngularJac * dq;
628 Vector3d AngularAccJac = AngularJac * ddq + AngularJacDeriv * dq;
629
630 bool angularVelSegEqual = equals(AngularVelFk, AngularVelJacSeg, tolerance);
631 bool angularVelEqual = equals(AngularVelFk, AngularVelJac, tolerance);
632 EXPECT_TRUE(angularVelSegEqual);
633 EXPECT_TRUE(angularVelEqual);
634 if (!angularVelSegEqual)
635 printComparisonError(
636 "angular velocity w/ offset (seg)",
637 bn->getName(),
638 refFrame->getName(),
639 AngularVelFk,
640 AngularVelJacSeg);
641 if (!angularVelEqual)
642 printComparisonError(
643 "angular velocity w/ offset",
644 bn->getName(),
645 refFrame->getName(),
646 AngularVelFk,
647 AngularVelJac);
648
649 bool angularAccSegEqual = equals(AngularAccFk, AngularAccJacSeg, tolerance);
650 bool angularAccEqual = equals(AngularAccFk, AngularAccJac, tolerance);
651 EXPECT_TRUE(angularAccSegEqual);
652 if (!angularAccSegEqual)
653 printComparisonError(
654 "angular acceleration w/ offset (seg)",
655 bn->getName(),
656 refFrame->getName(),
657 AngularAccFk,
658 AngularAccJacSeg);
659 EXPECT_TRUE(angularAccEqual);
660 if (!angularAccEqual)
661 printComparisonError(
662 "angular acceleration w/ offset",
663 bn->getName(),
664 refFrame->getName(),
665 AngularAccFk,
666 AngularAccJac);
667 }
668
669 //==============================================================================
670 template <typename T>
671 void printComparisonError(
672 const std::string& _comparison,
673 const std::string& _nameBN,
674 const std::string& _nameBNRelativeTo,
675 const std::string& _frame,
676 const T& fk,
677 const T& jac)
678 {
679 std::cout << "Disagreement between FK and relative Jacobian results for "
680 << _comparison << " of '" << _nameBN << "' relative to '"
681 << _nameBNRelativeTo << "' with a reference Frame of '" << _frame
682 << "'\n"
683 << "FK: " << fk.transpose() << "\n"
684 << "Jac: " << jac.transpose() << "\n";
685 }
686
687 //==============================================================================
688 void compareBodyNodeFkToJacobianRelative(
689 const JacobianNode* bn,
690 const JacobianNode* relativeTo,
691 const Frame* refFrame,
692 double tolerance)
693 {
694 using math::AngularJacobian;
695 using math::Jacobian;
696 using math::LinearJacobian;
697
698 assert(bn->getSkeleton() == relativeTo->getSkeleton());
699 auto skel = bn->getSkeleton();
700
701 VectorXd dq = skel->getVelocities();
702 VectorXd ddq = skel->getAccelerations();
703
704 //-- Spatial Jacobian tests --------------------------------------------------
705
706 Vector6d SpatialVelFk = bn->getSpatialVelocity(relativeTo, refFrame);
707 Vector6d SpatialAccFk = bn->getSpatialAcceleration(relativeTo, refFrame);
708
709 Jacobian SpatialJac = skel->getJacobian(bn, relativeTo, refFrame);
710 Jacobian SpatialJacDeriv
711 = skel->getJacobianSpatialDeriv(bn, relativeTo, refFrame);
712
713 Vector6d SpatialVelJac = SpatialJac * dq;
714 Vector6d SpatialAccJac = SpatialJac * ddq + SpatialJacDeriv * dq;
715
716 bool spatialVelEqual = equals(SpatialVelFk, SpatialVelJac, tolerance);
717 EXPECT_TRUE(spatialVelEqual);
718 if (!spatialVelEqual)
719 {
720 printComparisonError(
721 "spatial velocity",
722 bn->getName(),
723 relativeTo->getName(),
724 refFrame->getName(),
725 SpatialVelFk,
726 SpatialVelJac);
727 }
728
729 bool spatialAccEqual = equals(SpatialAccFk, SpatialAccJac, tolerance);
730 EXPECT_TRUE(spatialAccEqual);
731 if (!spatialAccEqual)
732 {
733 printComparisonError(
734 "spatial acceleration",
735 bn->getName(),
736 relativeTo->getName(),
737 refFrame->getName(),
738 SpatialAccFk,
739 SpatialAccJac);
740 }
741
742 //-- Linear Jacobian tests --------------------------------------------------
743
744 Vector3d LinearVelFk = bn->getLinearVelocity(relativeTo, refFrame);
745
746 LinearJacobian LinearJac = skel->getLinearJacobian(bn, relativeTo, refFrame);
747
748 Vector3d LinearVelJac = LinearJac * dq;
749
750 bool linearVelEqual = equals(LinearVelFk, LinearVelJac, tolerance);
751 EXPECT_TRUE(linearVelEqual);
752 if (!linearVelEqual)
753 {
754 printComparisonError(
755 "linear velocity",
756 bn->getName(),
757 relativeTo->getName(),
758 refFrame->getName(),
759 LinearVelFk,
760 LinearVelJac);
761 }
762
763 //-- Angular Jacobian tests --------------------------------------------------
764
765 Vector3d AngularVelFk = bn->getAngularVelocity(relativeTo, refFrame);
766
767 AngularJacobian AngularJac
768 = skel->getAngularJacobian(bn, relativeTo, refFrame);
769
770 Vector3d AngularVelJac = AngularJac * dq;
771
772 bool angularVelEqual = equals(AngularVelFk, AngularVelJac, tolerance);
773 EXPECT_TRUE(angularVelEqual);
774 if (!angularVelEqual)
775 {
776 printComparisonError(
777 "angular velocity",
778 bn->getName(),
779 relativeTo->getName(),
780 refFrame->getName(),
781 AngularVelFk,
782 AngularVelJac);
783 }
784 }
785
786 //==============================================================================
787 void compareBodyNodeFkToJacobianRelative(
788 const JacobianNode* bn,
789 const Eigen::Vector3d& _offset,
790 const JacobianNode* relativeTo,
791 const Frame* refFrame,
792 double tolerance)
793 {
794 using math::AngularJacobian;
795 using math::Jacobian;
796 using math::LinearJacobian;
797
798 assert(bn->getSkeleton() == relativeTo->getSkeleton());
799 auto skel = bn->getSkeleton();
800
801 VectorXd dq = skel->getVelocities();
802 VectorXd ddq = skel->getAccelerations();
803
804 //-- Spatial Jacobian tests --------------------------------------------------
805
806 Vector6d SpatialVelFk = bn->getSpatialVelocity(_offset, relativeTo, refFrame);
807 Vector6d SpatialAccFk
808 = bn->getSpatialAcceleration(_offset, relativeTo, refFrame);
809
810 Jacobian SpatialJac = skel->getJacobian(bn, _offset, relativeTo, refFrame);
811 Jacobian SpatialJacDeriv
812 = skel->getJacobianSpatialDeriv(bn, _offset, relativeTo, refFrame);
813
814 Vector6d SpatialVelJac = SpatialJac * dq;
815 Vector6d SpatialAccJac = SpatialJac * ddq + SpatialJacDeriv * dq;
816
817 bool spatialVelEqual = equals(SpatialVelFk, SpatialVelJac, tolerance);
818 EXPECT_TRUE(spatialVelEqual);
819 if (!spatialVelEqual)
820 {
821 printComparisonError(
822 "spatial velocity w/ offset",
823 bn->getName(),
824 relativeTo->getName(),
825 refFrame->getName(),
826 SpatialVelFk,
827 SpatialVelJac);
828 }
829
830 bool spatialAccEqual = equals(SpatialAccFk, SpatialAccJac, tolerance);
831 EXPECT_TRUE(spatialAccEqual);
832 if (!spatialAccEqual)
833 {
834 printComparisonError(
835 "spatial acceleration w/ offset",
836 bn->getName(),
837 relativeTo->getName(),
838 refFrame->getName(),
839 SpatialAccFk,
840 SpatialAccJac);
841 }
842
843 //-- Linear Jacobian tests --------------------------------------------------
844
845 Vector3d LinearVelFk = bn->getLinearVelocity(_offset, relativeTo, refFrame);
846
847 LinearJacobian LinearJac
848 = skel->getLinearJacobian(bn, _offset, relativeTo, refFrame);
849
850 Vector3d LinearVelJac = LinearJac * dq;
851
852 bool linearVelEqual = equals(LinearVelFk, LinearVelJac, tolerance);
853 EXPECT_TRUE(linearVelEqual);
854 if (!linearVelEqual)
855 {
856 printComparisonError(
857 "linear velocity w/ offset",
858 bn->getName(),
859 relativeTo->getName(),
860 refFrame->getName(),
861 LinearVelFk,
862 LinearVelJac);
863 }
864 }
865
866 //==============================================================================
867 void DynamicsTest::testJacobians(const common::Uri& uri)
868 {
869 using namespace std;
870 using namespace Eigen;
871 using namespace dart;
872 using namespace math;
873 using namespace dynamics;
874 using namespace simulation;
875 using namespace utils;
876
877 //----------------------------- Settings -------------------------------------
878 const double TOLERANCE = 1.0e-6;
879 #ifndef NDEBUG // Debug mode
880 int nTestItr = 2;
881 #else
882 int nTestItr = 5;
883 #endif
884 double qLB = -0.5 * constantsd::pi();
885 double qUB = 0.5 * constantsd::pi();
886 double dqLB = -0.5 * constantsd::pi();
887 double dqUB = 0.5 * constantsd::pi();
888 double ddqLB = -0.5 * constantsd::pi();
889 double ddqUB = 0.5 * constantsd::pi();
890 Vector3d gravity(0.0, -9.81, 0.0);
891
892 // load skeleton
893 WorldPtr world = SkelParser::readWorld(uri);
894 assert(world != nullptr);
895 world->setGravity(gravity);
896
897 //------------------------------ Tests ---------------------------------------
898 for (std::size_t i = 0; i < world->getNumSkeletons(); ++i)
899 {
900 SkeletonPtr skeleton = world->getSkeleton(i);
901 assert(skeleton != nullptr);
902 int dof = skeleton->getNumDofs();
903
904 for (int j = 0; j < nTestItr; ++j)
905 {
906 // For the second half of the tests, scramble up the Skeleton
907 if (j > std::ceil(nTestItr / 2))
908 {
909 SkeletonPtr copy = skeleton->cloneSkeleton();
910 std::size_t maxNode = skeleton->getNumBodyNodes() - 1;
911 BodyNode* bn1 = skeleton->getBodyNode(
912 math::Random::uniform<std::size_t>(0, maxNode));
913 BodyNode* bn2 = skeleton->getBodyNode(
914 math::Random::uniform<std::size_t>(0, maxNode));
915
916 if (bn1 != bn2)
917 {
918 BodyNode* child = bn1->descendsFrom(bn2) ? bn1 : bn2;
919 BodyNode* parent = child == bn1 ? bn2 : bn1;
920
921 child->moveTo(parent);
922 }
923
924 EXPECT_TRUE(skeleton->getNumBodyNodes() == copy->getNumBodyNodes());
925 EXPECT_TRUE(skeleton->getNumDofs() == copy->getNumDofs());
926 }
927
928 // Generate a random state
929 VectorXd q = VectorXd(dof);
930 VectorXd dq = VectorXd(dof);
931 VectorXd ddq = VectorXd(dof);
932 for (int k = 0; k < dof; ++k)
933 {
934 q[k] = math::Random::uniform(qLB, qUB);
935 dq[k] = math::Random::uniform(dqLB, dqUB);
936 ddq[k] = math::Random::uniform(ddqLB, ddqUB);
937 }
938 skeleton->setPositions(q);
939 skeleton->setVelocities(dq);
940 skeleton->setAccelerations(ddq);
941
942 randomizeRefFrames();
943
944 // For each body node
945 for (std::size_t k = 0; k < skeleton->getNumBodyNodes(); ++k)
946 {
947 const BodyNode* bn = skeleton->getBodyNode(k);
948
949 // Compare results using the World reference Frame
950 compareBodyNodeFkToJacobian(bn, Frame::World(), TOLERANCE);
951 compareBodyNodeFkToJacobian(
952 bn, Frame::World(), bn->getLocalCOM(), TOLERANCE);
953 compareBodyNodeFkToJacobian(
954 bn,
955 Frame::World(),
956 Random::uniform<Eigen::Vector3d>(-10, 10),
957 TOLERANCE);
958
959 // Compare results using this BodyNode's own reference Frame
960 compareBodyNodeFkToJacobian(bn, bn, TOLERANCE);
961 compareBodyNodeFkToJacobian(bn, bn, bn->getLocalCOM(), TOLERANCE);
962 compareBodyNodeFkToJacobian(
963 bn, bn, Random::uniform<Eigen::Vector3d>(-10, 10), TOLERANCE);
964
965 // Compare results using the randomized reference Frames
966 for (std::size_t r = 0; r < refFrames.size(); ++r)
967 {
968 compareBodyNodeFkToJacobian(bn, refFrames[r], TOLERANCE);
969 compareBodyNodeFkToJacobian(
970 bn, refFrames[r], bn->getLocalCOM(), TOLERANCE);
971 compareBodyNodeFkToJacobian(
972 bn,
973 refFrames[r],
974 Random::uniform<Eigen::Vector3d>(-10, 10),
975 TOLERANCE);
976 }
977
978 // -- Relative Jacobian tests
979
980 compareBodyNodeFkToJacobianRelative(bn, bn, Frame::World(), TOLERANCE);
981
982 #ifndef NDEBUG // Debug mode
983 if (skeleton->getNumBodyNodes() == 0u)
984 continue;
985
986 for (std::size_t l = skeleton->getNumBodyNodes() - 1;
987 l < skeleton->getNumBodyNodes();
988 ++l)
989 #else
990 for (std::size_t l = 0; l < skeleton->getNumBodyNodes(); ++l)
991 #endif
992 {
993 const BodyNode* relativeTo = skeleton->getBodyNode(l);
994
995 compareBodyNodeFkToJacobianRelative(
996 bn, relativeTo, Frame::World(), TOLERANCE);
997 compareBodyNodeFkToJacobianRelative(
998 bn, bn->getLocalCOM(), relativeTo, Frame::World(), TOLERANCE);
999 compareBodyNodeFkToJacobianRelative(
1000 bn,
1001 Random::uniform<Eigen::Vector3d>(-10, 10),
1002 relativeTo,
1003 Frame::World(),
1004 TOLERANCE);
1005
1006 compareBodyNodeFkToJacobianRelative(bn, relativeTo, bn, TOLERANCE);
1007 compareBodyNodeFkToJacobianRelative(
1008 bn, bn->getLocalCOM(), relativeTo, bn, TOLERANCE);
1009 compareBodyNodeFkToJacobianRelative(
1010 bn,
1011 Random::uniform<Eigen::Vector3d>(-10, 10),
1012 relativeTo,
1013 bn,
1014 TOLERANCE);
1015
1016 compareBodyNodeFkToJacobianRelative(
1017 bn, relativeTo, relativeTo, TOLERANCE);
1018 compareBodyNodeFkToJacobianRelative(
1019 bn, bn->getLocalCOM(), relativeTo, relativeTo, TOLERANCE);
1020 compareBodyNodeFkToJacobianRelative(
1021 bn,
1022 Random::uniform<Eigen::Vector3d>(-10, 10),
1023 relativeTo,
1024 relativeTo,
1025 TOLERANCE);
1026
1027 for (std::size_t r = 0; r < refFrames.size(); ++r)
1028 {
1029 compareBodyNodeFkToJacobianRelative(
1030 bn, relativeTo, refFrames[r], TOLERANCE);
1031 compareBodyNodeFkToJacobianRelative(
1032 bn, bn->getLocalCOM(), relativeTo, refFrames[r], TOLERANCE);
1033 compareBodyNodeFkToJacobianRelative(
1034 bn,
1035 Random::uniform<Eigen::Vector3d>(-10, 10),
1036 relativeTo,
1037 refFrames[r],
1038 TOLERANCE);
1039 }
1040 }
1041 }
1042 }
1043 }
1044 }
1045
1046 //==============================================================================
1047 void DynamicsTest::testFiniteDifferenceGeneralizedCoordinates(
1048 const common::Uri& uri)
1049 {
1050 using namespace std;
1051 using namespace Eigen;
1052 using namespace dart;
1053 using namespace math;
1054 using namespace dynamics;
1055 using namespace simulation;
1056 using namespace utils;
1057
1058 //----------------------------- Settings -------------------------------------
1059 #ifndef NDEBUG // Debug mode
1060 int nRandomItr = 2;
1061 #else
1062 int nRandomItr = 10;
1063 #endif
1064 double qLB = -0.5 * constantsd::pi();
1065 double qUB = 0.5 * constantsd::pi();
1066 double dqLB = -0.3 * constantsd::pi();
1067 double dqUB = 0.3 * constantsd::pi();
1068 double ddqLB = -0.1 * constantsd::pi();
1069 double ddqUB = 0.1 * constantsd::pi();
1070 Vector3d gravity(0.0, -9.81, 0.0);
1071 double timeStep = 1e-3;
1072 double TOLERANCE = 5e-4;
1073
1074 // load skeleton
1075 WorldPtr world = SkelParser::readWorld(uri);
1076 assert(world != nullptr);
1077 world->setGravity(gravity);
1078 world->setTimeStep(timeStep);
1079
1080 //------------------------------ Tests ---------------------------------------
1081 for (std::size_t i = 0; i < world->getNumSkeletons(); ++i)
1082 {
1083 SkeletonPtr skeleton = world->getSkeleton(i);
1084 assert(skeleton != nullptr);
1085 int dof = skeleton->getNumDofs();
1086
1087 for (int j = 0; j < nRandomItr; ++j)
1088 {
1089 // Generate a random state and ddq
1090 VectorXd q0 = VectorXd(dof);
1091 VectorXd dq0 = VectorXd(dof);
1092 VectorXd ddq0 = VectorXd(dof);
1093 for (int k = 0; k < dof; ++k)
1094 {
1095 q0[k] = math::Random::uniform(qLB, qUB);
1096 dq0[k] = math::Random::uniform(dqLB, dqUB);
1097 ddq0[k] = math::Random::uniform(ddqLB, ddqUB);
1098 }
1099
1100 skeleton->setPositions(q0);
1101 skeleton->setVelocities(dq0);
1102 skeleton->setAccelerations(ddq0);
1103
1104 skeleton->integratePositions(timeStep);
1105 VectorXd q1 = skeleton->getPositions();
1106 skeleton->integrateVelocities(timeStep);
1107 VectorXd dq1 = skeleton->getVelocities();
1108
1109 skeleton->integratePositions(timeStep);
1110 VectorXd q2 = skeleton->getPositions();
1111 skeleton->integrateVelocities(timeStep);
1112 VectorXd dq2 = skeleton->getVelocities();
1113
1114 VectorXd dq0FD = skeleton->getPositionDifferences(q1, q0) / timeStep;
1115 VectorXd dq1FD = skeleton->getPositionDifferences(q2, q1) / timeStep;
1116 VectorXd ddqFD1
1117 = skeleton->getVelocityDifferences(dq1FD, dq0FD) / timeStep;
1118 VectorXd ddqFD2 = skeleton->getVelocityDifferences(dq2, dq1) / timeStep;
1119
1120 EXPECT_TRUE(equals(dq0, dq0FD, TOLERANCE))
1121 << "dq0 : " << dq0.transpose() << "\ndq0FD: " << dq0FD.transpose();
1122
1123 EXPECT_TRUE(equals(dq1, dq1FD, TOLERANCE))
1124 << "dq1 : " << dq1.transpose() << "\ndq1FD: " << dq1FD.transpose();
1125
1126 EXPECT_TRUE(equals(ddq0, ddqFD1, 10 * TOLERANCE))
1127 << "ddq0 : " << ddq0.transpose()
1128 << "\nddqFD1: " << ddqFD1.transpose();
1129
1130 EXPECT_TRUE(equals(ddq0, ddqFD2, 10 * TOLERANCE))
1131 << "ddq0 : " << ddq0.transpose()
1132 << "\nddqFD2: " << ddqFD2.transpose() << std::endl;
1133 }
1134 }
1135 }
1136
1137 //==============================================================================
1138 void DynamicsTest::testFiniteDifferenceBodyNodeVelocity(const common::Uri& uri)
1139 {
1140 using namespace std;
1141 using namespace Eigen;
1142 using namespace dart;
1143 using namespace math;
1144 using namespace dynamics;
1145 using namespace simulation;
1146 using namespace utils;
1147
1148 //----------------------------- Settings -------------------------------------
1149 #ifndef NDEBUG // Debug mode
1150 int nRandomItr = 2;
1151 std::size_t numSteps = 1e+1;
1152 #else
1153 int nRandomItr = 10;
1154 std::size_t numSteps = 1e+3;
1155 #endif
1156 double qLB = -0.5 * constantsd::pi();
1157 double qUB = 0.5 * constantsd::pi();
1158 double dqLB = -0.5 * constantsd::pi();
1159 double dqUB = 0.5 * constantsd::pi();
1160 double ddqLB = -0.5 * constantsd::pi();
1161 double ddqUB = 0.5 * constantsd::pi();
1162 Vector3d gravity(0.0, -9.81, 0.0);
1163 double timeStep = 1.0e-6;
1164 const double tol = timeStep * 1e+2;
1165
1166 // load skeleton
1167 WorldPtr world = SkelParser::readWorld(uri);
1168 assert(world != nullptr);
1169 world->setGravity(gravity);
1170 world->setTimeStep(timeStep);
1171
1172 //------------------------------ Tests ---------------------------------------
1173 for (int i = 0; i < nRandomItr; ++i)
1174 {
1175 for (std::size_t j = 0; j < world->getNumSkeletons(); ++j)
1176 {
1177 SkeletonPtr skeleton = world->getSkeleton(j);
1178 EXPECT_NE(skeleton, nullptr);
1179
1180 std::size_t dof = skeleton->getNumDofs();
1181 std::size_t numBodies = skeleton->getNumBodyNodes();
1182
1183 // Generate random states
1184 VectorXd q = Random::uniform<Eigen::VectorXd>(dof, qLB, qUB);
1185 VectorXd dq = Random::uniform<Eigen::VectorXd>(dof, dqLB, dqUB);
1186 VectorXd ddq = Random::uniform<Eigen::VectorXd>(dof, ddqLB, ddqUB);
1187
1188 skeleton->setPositions(q);
1189 skeleton->setVelocities(dq);
1190 skeleton->setAccelerations(ddq);
1191
1192 common::aligned_map<dynamics::BodyNodePtr, Eigen::Isometry3d> Tmap;
1193 for (auto k = 0u; k < numBodies; ++k)
1194 {
1195 auto body = skeleton->getBodyNode(k);
1196 Tmap[body] = body->getTransform();
1197 }
1198
1199 for (std::size_t k = 0; k < numSteps; ++k)
1200 {
1201 skeleton->integrateVelocities(skeleton->getTimeStep());
1202 skeleton->integratePositions(skeleton->getTimeStep());
1203
1204 for (std::size_t l = 0; l < skeleton->getNumBodyNodes(); ++l)
1205 {
1206 BodyNodePtr body = skeleton->getBodyNode(l);
1207
1208 Isometry3d T1 = Tmap[body];
1209 Isometry3d T2 = body->getTransform();
1210
1211 Vector6d V_diff = math::logMap(T1.inverse() * T2) / timeStep;
1212 Vector6d V_actual = body->getSpatialVelocity();
1213
1214 bool checkSpatialVelocity = equals(V_diff, V_actual, tol);
1215 EXPECT_TRUE(checkSpatialVelocity);
1216 if (!checkSpatialVelocity)
1217 {
1218 std::cout << "[" << body->getName() << "]" << std::endl;
1219 std::cout << "V_diff : " << V_diff.transpose() << std::endl;
1220 std::cout << "V_actual : " << V_actual.transpose() << std::endl;
1221 std::cout << std::endl;
1222 }
1223
1224 Tmap[body] = body->getTransform();
1225 }
1226 }
1227 }
1228 }
1229 }
1230
1231 //==============================================================================
1232 void DynamicsTest::testFiniteDifferenceBodyNodeAcceleration(
1233 const common::Uri& uri)
1234 {
1235 using namespace std;
1236 using namespace Eigen;
1237 using namespace dart;
1238 using namespace math;
1239 using namespace dynamics;
1240 using namespace simulation;
1241 using namespace utils;
1242
1243 //----------------------------- Settings -------------------------------------
1244 const double TOLERANCE = 1.0e-2;
1245 #ifndef NDEBUG // Debug mode
1246 int nRandomItr = 2;
1247 #else
1248 int nRandomItr = 10;
1249 #endif
1250 double qLB = -0.5 * constantsd::pi();
1251 double qUB = 0.5 * constantsd::pi();
1252 double dqLB = -0.5 * constantsd::pi();
1253 double dqUB = 0.5 * constantsd::pi();
1254 double ddqLB = -0.5 * constantsd::pi();
1255 double ddqUB = 0.5 * constantsd::pi();
1256 Vector3d gravity(0.0, -9.81, 0.0);
1257 double timeStep = 1.0e-6;
1258
1259 // load skeleton
1260 WorldPtr world = SkelParser::readWorld(uri);
1261 assert(world != nullptr);
1262 world->setGravity(gravity);
1263 world->setTimeStep(timeStep);
1264
1265 //------------------------------ Tests ---------------------------------------
1266 for (std::size_t i = 0; i < world->getNumSkeletons(); ++i)
1267 {
1268 SkeletonPtr skeleton = world->getSkeleton(i);
1269 assert(skeleton != nullptr);
1270 int dof = skeleton->getNumDofs();
1271
1272 for (int j = 0; j < nRandomItr; ++j)
1273 {
1274 // Generate a random state and ddq
1275 VectorXd q = VectorXd(dof);
1276 VectorXd dq = VectorXd(dof);
1277 VectorXd ddq = VectorXd(dof);
1278 for (int k = 0; k < dof; ++k)
1279 {
1280 q[k] = math::Random::uniform(qLB, qUB);
1281 dq[k] = math::Random::uniform(dqLB, dqUB);
1282 ddq[k] = math::Random::uniform(ddqLB, ddqUB);
1283 }
1284
1285 // For each body node
1286 for (std::size_t k = 0; k < skeleton->getNumBodyNodes(); ++k)
1287 {
1288 BodyNode* bn = skeleton->getBodyNode(k);
1289
1290 // Calculation of velocities and Jacobian at k-th time step
1291 skeleton->setPositions(q);
1292 skeleton->setVelocities(dq);
1293 skeleton->setAccelerations(ddq);
1294
1295 Vector3d BodyLinVel1 = bn->getLinearVelocity(Frame::World(), bn);
1296 Vector3d BodyAngVel1 = bn->getAngularVelocity(Frame::World(), bn);
1297 Vector3d WorldLinVel1 = bn->getLinearVelocity();
1298 Vector3d WorldAngVel1 = bn->getAngularVelocity();
1299 // Isometry3d T1 = bn->getTransform();
1300
1301 // Get accelerations and time derivatives of Jacobians at k-th time step
1302 Vector3d BodyLinAcc1 = bn->getSpatialAcceleration().tail<3>();
1303 Vector3d BodyAngAcc1 = bn->getSpatialAcceleration().head<3>();
1304 Vector3d WorldLinAcc1 = bn->getLinearAcceleration();
1305 Vector3d WorldAngAcc1 = bn->getAngularAcceleration();
1306
1307 // Calculation of velocities and Jacobian at (k+1)-th time step
1308 skeleton->integrateVelocities(skeleton->getTimeStep());
1309 skeleton->integratePositions(skeleton->getTimeStep());
1310
1311 Vector3d BodyLinVel2 = bn->getLinearVelocity(Frame::World(), bn);
1312 Vector3d BodyAngVel2 = bn->getAngularVelocity(Frame::World(), bn);
1313 Vector3d WorldLinVel2 = bn->getLinearVelocity();
1314 Vector3d WorldAngVel2 = bn->getAngularVelocity();
1315 // Isometry3d T2 = bn->getTransform();
1316
1317 // Get accelerations and time derivatives of Jacobians at k-th time step
1318 Vector3d BodyLinAcc2 = bn->getSpatialAcceleration().tail<3>();
1319 Vector3d BodyAngAcc2 = bn->getSpatialAcceleration().head<3>();
1320 Vector3d WorldLinAcc2 = bn->getLinearAcceleration();
1321 Vector3d WorldAngAcc2 = bn->getAngularAcceleration();
1322
1323 // Calculation of approximated accelerations
1324 Vector3d BodyLinAccApprox = (BodyLinVel2 - BodyLinVel1) / timeStep;
1325 Vector3d BodyAngAccApprox = (BodyAngVel2 - BodyAngVel1) / timeStep;
1326 Vector3d WorldLinAccApprox = (WorldLinVel2 - WorldLinVel1) / timeStep;
1327 Vector3d WorldAngAccApprox = (WorldAngVel2 - WorldAngVel1) / timeStep;
1328
1329 // Comparing two velocities
1330 EXPECT_TRUE(equals(BodyLinAcc1, BodyLinAccApprox, TOLERANCE));
1331 EXPECT_TRUE(equals(BodyAngAcc1, BodyAngAccApprox, TOLERANCE));
1332 EXPECT_TRUE(equals(BodyLinAcc2, BodyLinAccApprox, TOLERANCE));
1333 EXPECT_TRUE(equals(BodyAngAcc2, BodyAngAccApprox, TOLERANCE));
1334 EXPECT_TRUE(equals(WorldLinAcc1, WorldLinAccApprox, TOLERANCE));
1335 EXPECT_TRUE(equals(WorldAngAcc1, WorldAngAccApprox, TOLERANCE));
1336 EXPECT_TRUE(equals(WorldLinAcc2, WorldLinAccApprox, TOLERANCE));
1337 EXPECT_TRUE(equals(WorldAngAcc2, WorldAngAccApprox, TOLERANCE));
1338
1339 // Debugging code
1340 if (!equals(BodyLinAcc1, BodyLinAccApprox, TOLERANCE))
1341 {
1342 cout << "BodyLinAcc1 :" << BodyLinAcc1.transpose() << endl;
1343 cout << "BodyLinAccApprox:" << BodyLinAccApprox.transpose() << endl;
1344 }
1345 if (!equals(BodyAngAcc1, BodyAngAccApprox, TOLERANCE))
1346 {
1347 cout << "BodyAngAcc1 :" << BodyAngAcc1.transpose() << endl;
1348 cout << "BodyAngAccApprox:" << BodyAngAccApprox.transpose() << endl;
1349 }
1350 if (!equals(BodyLinAcc2, BodyLinAccApprox, TOLERANCE))
1351 {
1352 cout << "BodyLinAcc2 :" << BodyLinAcc2.transpose() << endl;
1353 cout << "BodyLinAccApprox:" << BodyLinAccApprox.transpose() << endl;
1354 }
1355 if (!equals(BodyAngAcc2, BodyAngAccApprox, TOLERANCE))
1356 {
1357 cout << "BodyAngAcc2 :" << BodyAngAcc2.transpose() << endl;
1358 cout << "BodyAngAccApprox:" << BodyAngAccApprox.transpose() << endl;
1359 }
1360 if (!equals(WorldLinAcc1, WorldLinAccApprox, TOLERANCE))
1361 {
1362 cout << "WorldLinAcc1 :" << WorldLinAcc1.transpose() << endl;
1363 cout << "WorldLinAccApprox:" << WorldLinAccApprox.transpose() << endl;
1364 }
1365 if (!equals(WorldAngAcc1, WorldAngAccApprox, TOLERANCE))
1366 {
1367 cout << "WorldAngAcc1 :" << WorldAngAcc1.transpose() << endl;
1368 cout << "WorldAngAccApprox:" << WorldAngAccApprox.transpose() << endl;
1369 }
1370 if (!equals(WorldLinAcc2, WorldLinAccApprox, TOLERANCE))
1371 {
1372 cout << "WorldLinAcc2 :" << WorldLinAcc2.transpose() << endl;
1373 cout << "WorldLinAccApprox:" << WorldLinAccApprox.transpose() << endl;
1374 }
1375 if (!equals(WorldAngAcc2, WorldAngAccApprox, TOLERANCE))
1376 {
1377 cout << "WorldAngAcc2 :" << WorldAngAcc2.transpose() << endl;
1378 cout << "WorldAngAccApprox:" << WorldAngAccApprox.transpose() << endl;
1379 }
1380 }
1381 }
1382 }
1383 }
1384
1385 //==============================================================================
1386 void testForwardKinematicsSkeleton(const dynamics::SkeletonPtr& skel)
1387 {
1388 #ifndef NDEBUG // Debug mode
1389 std::size_t nRandomItr = 1e+1;
1390 std::size_t numSteps = 1e+1;
1391 #else
1392 std::size_t nRandomItr = 1e+2;
1393 std::size_t numSteps = 1e+2;
1394 #endif
1395 double qLB = -0.5 * constantsd::pi();
1396 double qUB = 0.5 * constantsd::pi();
1397 double dqLB = -0.3 * constantsd::pi();
1398 double dqUB = 0.3 * constantsd::pi();
1399 double ddqLB = -0.1 * constantsd::pi();
1400 double ddqUB = 0.1 * constantsd::pi();
1401 double timeStep = 1e-6;
1402
1403 EXPECT_NE(skel, nullptr);
1404
1405 skel->setTimeStep(timeStep);
1406
1407 auto dof = skel->getNumDofs();
1408 auto numBodies = skel->getNumBodyNodes();
1409
1410 Eigen::VectorXd q;
1411 Eigen::VectorXd dq;
1412 Eigen::VectorXd ddq;
1413
1414 common::aligned_map<dynamics::BodyNodePtr, Eigen::Isometry3d> Tmap;
1415 common::aligned_map<dynamics::BodyNodePtr, Eigen::Vector6d> Vmap;
1416 common::aligned_map<dynamics::BodyNodePtr, Eigen::Vector6d> dVmap;
1417
1418 for (auto j = 0u; j < numBodies; ++j)
1419 {
1420 auto body = skel->getBodyNode(j);
1421
1422 Tmap[body] = Eigen::Isometry3d::Identity();
1423 Vmap[body] = Eigen::Vector6d::Zero();
1424 dVmap[body] = Eigen::Vector6d::Zero();
1425 }
1426
1427 for (auto i = 0u; i < nRandomItr; ++i)
1428 {
1429 q = Random::uniform<Eigen::VectorXd>(dof, qLB, qUB);
1430 dq = Random::uniform<Eigen::VectorXd>(dof, dqLB, dqUB);
1431 ddq = Random::uniform<Eigen::VectorXd>(dof, ddqLB, ddqUB);
1432
1433 skel->setPositions(q);
1434 skel->setVelocities(dq);
1435 skel->setAccelerations(ddq);
1436
1437 for (auto j = 0u; j < numSteps; ++j)
1438 {
1439 for (auto k = 0u; k < numBodies; ++k)
1440 {
1441 auto body = skel->getBodyNode(k);
1442 auto joint = skel->getJoint(k);
1443 auto parentBody = body->getParentBodyNode();
1444 Eigen::MatrixXd S = joint->getRelativeJacobian();
1445 Eigen::MatrixXd dS = joint->getRelativeJacobianTimeDeriv();
1446 Eigen::VectorXd jointQ = joint->getPositions();
1447 Eigen::VectorXd jointDQ = joint->getVelocities();
1448 Eigen::VectorXd jointDDQ = joint->getAccelerations();
1449
1450 Eigen::Isometry3d relT = body->getRelativeTransform();
1451 Eigen::Vector6d relV = S * jointDQ;
1452 Eigen::Vector6d relDV = dS * jointDQ + S * jointDDQ;
1453
1454 if (parentBody)
1455 {
1456 Tmap[body] = Tmap[parentBody] * relT;
1457 Vmap[body] = math::AdInvT(relT, Vmap[parentBody]) + relV;
1458 dVmap[body] = math::AdInvT(relT, dVmap[parentBody])
1459 + math::ad(Vmap[body], S * jointDQ) + relDV;
1460 }
1461 else
1462 {
1463 Tmap[body] = relT;
1464 Vmap[body] = relV;
1465 dVmap[body] = relDV;
1466 }
1467
1468 bool checkT
1469 = equals(body->getTransform().matrix(), Tmap[body].matrix());
1470 bool checkV = equals(body->getSpatialVelocity(), Vmap[body]);
1471 bool checkDV = equals(body->getSpatialAcceleration(), dVmap[body]);
1472
1473 EXPECT_TRUE(checkT);
1474 EXPECT_TRUE(checkV);
1475 EXPECT_TRUE(checkDV);
1476
1477 if (!checkT)
1478 {
1479 std::cout << "[" << body->getName() << "]" << std::endl;
1480 std::cout << "actual T : " << std::endl
1481 << body->getTransform().matrix() << std::endl;
1482 std::cout << "expected T: " << std::endl
1483 << Tmap[body].matrix() << std::endl;
1484 std::cout << std::endl;
1485 }
1486
1487 if (!checkV)
1488 {
1489 std::cout << "[" << body->getName() << "]" << std::endl;
1490 std::cout << "actual V : " << body->getSpatialVelocity().transpose()
1491 << std::endl;
1492 std::cout << "expected V: " << Vmap[body].transpose() << std::endl;
1493 std::cout << std::endl;
1494 }
1495
1496 if (!checkDV)
1497 {
1498 std::cout << "[" << body->getName() << "]" << std::endl;
1499 std::cout << "actual DV : "
1500 << body->getSpatialAcceleration().transpose() << std::endl;
1501 std::cout << "expected DV: " << dVmap[body].transpose() << std::endl;
1502 std::cout << std::endl;
1503 }
1504 }
1505 }
1506 }
1507 }
1508
1509 //==============================================================================
1510 void DynamicsTest::testForwardKinematics(const common::Uri& uri)
1511 {
1512 auto world = utils::SkelParser::readWorld(uri);
1513 EXPECT_TRUE(world != nullptr);
1514
1515 auto numSkeletons = world->getNumSkeletons();
1516 for (auto i = 0u; i < numSkeletons; ++i)
1517 {
1518 auto skeleton = world->getSkeleton(i);
1519 testForwardKinematicsSkeleton(skeleton);
1520 }
1521 }
1522
1523 //==============================================================================
1524 void DynamicsTest::testInverseDynamics(const common::Uri& uri)
1525 {
1526 //---------------------------- Settings --------------------------------------
1527 // Number of random state tests for each skeleton
1528 #ifndef NDEBUG // Debug mode
1529 const std::size_t nRandomItr = 2;
1530 #else
1531 const std::size_t nRandomItr = 100;
1532 #endif
1533
1534 // Lower and upper bound of configuration for system
1535 const double lb = -1.0 * math::constantsd::pi();
1536 const double ub = 1.0 * math::constantsd::pi();
1537
1538 // Lower and upper bound of joint damping and stiffness
1539 const double lbD = 0.0;
1540 const double ubD = 10.0;
1541 const double lbK = 0.0;
1542 const double ubK = 10.0;
1543
1544 // Lower and upper bound of joint forces
1545 const double lbF = -10.0;
1546 const double ubF = 10.0;
1547
1548 simulation::WorldPtr world = utils::SkelParser::readWorld(uri);
1549 ASSERT_TRUE(world != nullptr);
1550
1551 for (std::size_t i = 0; i < world->getNumSkeletons(); ++i)
1552 {
1553 dynamics::SkeletonPtr skel = world->getSkeleton(i);
1554 const std::size_t dofs = skel->getNumDofs();
1555
1556 for (std::size_t j = 0; j < nRandomItr; ++j)
1557 {
1558 // Random joint stiffness and damping coefficients
1559 for (std::size_t l = 0; l < dofs; ++l)
1560 {
1561 dynamics::DegreeOfFreedom* dof = skel->getDof(l);
1562 dof->setDampingCoefficient(math::Random::uniform(lbD, ubD));
1563 dof->setSpringStiffness(math::Random::uniform(lbK, ubK));
1564
1565 double lbRP = dof->getPositionLowerLimit();
1566 double ubRP = dof->getPositionUpperLimit();
1567 lbRP = std::max(lbRP, -math::constantsd::pi());
1568 ubRP = std::min(ubRP, +math::constantsd::pi());
1569 dof->setRestPosition(math::Random::uniform(lbRP, ubRP));
1570 }
1571
1572 // Set random states and forces
1573 const VectorXd q0 = math::Random::uniform<VectorXd>(dofs, lb, ub);
1574 const VectorXd dq0 = math::Random::uniform<VectorXd>(dofs, lb, ub);
1575 const VectorXd f0 = math::Random::uniform<VectorXd>(dofs, lbF, ubF);
1576
1577 // Forward dynamics to compute accelerations
1578 skel->setPositions(q0);
1579 skel->setVelocities(dq0);
1580 skel->setForces(f0);
1581 skel->computeForwardDynamics();
1582 const VectorXd ddq = skel->getAccelerations();
1583
1584 // Test inverse dynamics to ensure the result joint forces are identical
1585 // to the forces used to compute the input accelerations
1586 skel->setAccelerations(ddq);
1587 skel->computeInverseDynamics(false, true, true);
1588 const VectorXd f = skel->getForces();
1589 EXPECT_TRUE(equals(f, f0));
1590
1591 // Test forward dynamics to ensure the result joint accelerations are
1592 // identical to the accelerations used to compute the input forces
1593 skel->setForces(f);
1594 skel->computeForwardDynamics();
1595 const VectorXd ddq2 = skel->getAccelerations();
1596 EXPECT_TRUE(equals(ddq, ddq2));
1597 }
1598 }
1599 }
1600
1601 //==============================================================================
1602 void DynamicsTest::compareEquationsOfMotion(const common::Uri& uri)
1603 {
1604 using namespace std;
1605 using namespace Eigen;
1606 using namespace dart;
1607 using namespace math;
1608 using namespace dynamics;
1609 using namespace simulation;
1610 using namespace utils;
1611
1612 //---------------------------- Settings --------------------------------------
1613 // Number of random state tests for each skeletons
1614 #ifndef NDEBUG // Debug mode
1615 std::size_t nRandomItr = 2;
1616 #else
1617 std::size_t nRandomItr = 100;
1618 #endif
1619
1620 // Lower and upper bound of configuration for system
1621 double lb = -1.0 * constantsd::pi();
1622 double ub = 1.0 * constantsd::pi();
1623
1624 // Lower and upper bound of joint damping and stiffness
1625 double lbD = 0.0;
1626 double ubD = 10.0;
1627 double lbK = 0.0;
1628 double ubK = 10.0;
1629
1630 simulation::WorldPtr myWorld;
1631
1632 //----------------------------- Tests ----------------------------------------
1633 // Check whether multiplication of mass matrix and its inverse is identity
1634 // matrix.
1635 myWorld = utils::SkelParser::readWorld(uri);
1636 EXPECT_TRUE(myWorld != nullptr);
1637
1638 for (std::size_t i = 0; i < myWorld->getNumSkeletons(); ++i)
1639 {
1640 dynamics::SkeletonPtr skel = myWorld->getSkeleton(i);
1641
1642 std::size_t dof = skel->getNumDofs();
1643 // int nBodyNodes = skel->getNumBodyNodes();
1644
1645 if (dof == 0)
1646 {
1647 dtmsg << "Skeleton [" << skel->getName() << "] is skipped since it has "
1648 << "0 DOF." << endl;
1649 continue;
1650 }
1651
1652 for (std::size_t j = 0; j < nRandomItr; ++j)
1653 {
1654 // Random joint stiffness and damping coefficient
1655 for (std::size_t k = 0; k < skel->getNumBodyNodes(); ++k)
1656 {
1657 BodyNode* body = skel->getBodyNode(k);
1658 Joint* joint = body->getParentJoint();
1659 std::size_t localDof = joint->getNumDofs();
1660
1661 for (std::size_t l = 0; l < localDof; ++l)
1662 {
1663 joint->setDampingCoefficient(l, Random::uniform(lbD, ubD));
1664 joint->setSpringStiffness(l, Random::uniform(lbK, ubK));
1665
1666 double lbRP = joint->getPositionLowerLimit(l);
1667 double ubRP = joint->getPositionUpperLimit(l);
1668 if (lbRP < -constantsd::pi())
1669 lbRP = -constantsd::pi();
1670 if (ubRP > constantsd::pi())
1671 ubRP = constantsd::pi();
1672 joint->setRestPosition(l, Random::uniform(lbRP, ubRP));
1673 }
1674 }
1675
1676 // Set random states
1677 Skeleton::Configuration x = skel->getConfiguration(
1678 Skeleton::CONFIG_POSITIONS | Skeleton::CONFIG_VELOCITIES);
1679 for (auto k = 0u; k < skel->getNumDofs(); ++k)
1680 {
1681 x.mPositions[k] = Random::uniform(lb, ub);
1682 x.mVelocities[k] = Random::uniform(lb, ub);
1683 }
1684 skel->setConfiguration(x);
1685
1686 //------------------------ Mass Matrix Test ----------------------------
1687 // Get matrices
1688 MatrixXd M = skel->getMassMatrix();
1689 MatrixXd M2 = getMassMatrix(skel);
1690 MatrixXd InvM = skel->getInvMassMatrix();
1691 MatrixXd M_InvM = M * InvM;
1692 MatrixXd InvM_M = InvM * M;
1693
1694 MatrixXd AugM = skel->getAugMassMatrix();
1695 MatrixXd AugM2 = getAugMassMatrix(skel);
1696 MatrixXd InvAugM = skel->getInvAugMassMatrix();
1697 MatrixXd AugM_InvAugM = AugM * InvAugM;
1698 MatrixXd InvAugM_AugM = InvAugM * AugM;
1699
1700 MatrixXd I = MatrixXd::Identity(dof, dof);
1701
1702 bool failure = false;
1703
1704 // Check if the number of generalized coordinates and dimension of mass
1705 // matrix are same.
1706 EXPECT_EQ(M.rows(), (int)dof);
1707 EXPECT_EQ(M.cols(), (int)dof);
1708
1709 // Check mass matrix
1710 EXPECT_TRUE(equals(M, M2, 1e-6));
1711 if (!equals(M, M2, 1e-6))
1712 {
1713 cout << "M :" << endl << M << endl << endl;
1714 cout << "M2:" << endl << M2 << endl << endl;
1715 failure = true;
1716 }
1717
1718 // Check augmented mass matrix
1719 EXPECT_TRUE(equals(AugM, AugM2, 1e-6));
1720 if (!equals(AugM, AugM2, 1e-6))
1721 {
1722 cout << "AugM :" << endl << AugM << endl << endl;
1723 cout << "AugM2:" << endl << AugM2 << endl << endl;
1724 failure = true;
1725 }
1726
1727 // Check if both of (M * InvM) and (InvM * M) are identity.
1728 EXPECT_TRUE(equals(M_InvM, I, 1e-6));
1729 if (!equals(M_InvM, I, 1e-6))
1730 {
1731 cout << "InvM :" << endl << InvM << endl << endl;
1732 failure = true;
1733 }
1734 EXPECT_TRUE(equals(InvM_M, I, 1e-6));
1735 if (!equals(InvM_M, I, 1e-6))
1736 {
1737 cout << "InvM_M:" << endl << InvM_M << endl << endl;
1738 failure = true;
1739 }
1740
1741 // Check if both of (M * InvM) and (InvM * M) are identity.
1742 EXPECT_TRUE(equals(AugM_InvAugM, I, 1e-6));
1743 if (!equals(AugM_InvAugM, I, 1e-6))
1744 {
1745 cout << "AugM_InvAugM :" << endl << AugM_InvAugM << endl << endl;
1746 failure = true;
1747 }
1748 EXPECT_TRUE(equals(InvAugM_AugM, I, 1e-6));
1749 if (!equals(InvAugM_AugM, I, 1e-6))
1750 {
1751 cout << "InvAugM_AugM:" << endl << InvAugM_AugM << endl << endl;
1752 }
1753
1754 //------- Coriolis Force Vector and Combined Force Vector Tests --------
1755 // Get C1, Coriolis force vector using recursive method
1756 VectorXd C = skel->getCoriolisForces();
1757 VectorXd Cg = skel->getCoriolisAndGravityForces();
1758
1759 // Get C2, Coriolis force vector using inverse dynamics algorithm
1760 Vector3d oldGravity = skel->getGravity();
1761 VectorXd oldTau = skel->getForces();
1762 VectorXd oldDdq = skel->getAccelerations();
1763 // TODO(JS): Save external forces of body nodes
1764
1765 skel->clearInternalForces();
1766 skel->clearExternalForces();
1767 skel->setAccelerations(VectorXd::Zero(dof));
1768
1769 EXPECT_TRUE(skel->getForces() == VectorXd::Zero(dof));
1770 EXPECT_TRUE(skel->getExternalForces() == VectorXd::Zero(dof));
1771 EXPECT_TRUE(skel->getAccelerations() == VectorXd::Zero(dof));
1772
1773 skel->setGravity(Vector3d::Zero());
1774 EXPECT_TRUE(skel->getGravity() == Vector3d::Zero());
1775 skel->computeInverseDynamics(false, false);
1776 VectorXd C2 = skel->getForces();
1777
1778 skel->setGravity(oldGravity);
1779 EXPECT_TRUE(skel->getGravity() == oldGravity);
1780 skel->computeInverseDynamics(false, false);
1781 VectorXd Cg2 = skel->getForces();
1782
1783 EXPECT_TRUE(equals(C, C2, 1e-6));
1784 if (!equals(C, C2, 1e-6))
1785 {
1786 cout << "C :" << C.transpose() << endl;
1787 cout << "C2:" << C2.transpose() << endl;
1788 failure = true;
1789 }
1790
1791 EXPECT_TRUE(equals(Cg, Cg2, 1e-6));
1792 if (!equals(Cg, Cg2, 1e-6))
1793 {
1794 cout << "Cg :" << Cg.transpose() << endl;
1795 cout << "Cg2:" << Cg2.transpose() << endl;
1796 failure = true;
1797 }
1798
1799 skel->setForces(oldTau);
1800 skel->setAccelerations(oldDdq);
1801 // TODO(JS): Restore external forces of body nodes
1802
1803 //------------------- Combined Force Vector Test -----------------------
1804 // TODO(JS): Not implemented yet.
1805
1806 //---------------------- Damping Force Test ----------------------------
1807 // TODO(JS): Not implemented yet.
1808
1809 //--------------------- External Force Test ----------------------------
1810 // TODO(JS): Not implemented yet.
1811
1812 if (failure)
1813 {
1814 std::cout << "Failure occurred in the World of file: " << uri.toString()
1815 << "\nWith Skeleton named: " << skel->getName() << "\n\n";
1816 }
1817 }
1818 }
1819 }
1820
1821 //==============================================================================
1822 void compareCOMJacobianToFk(
1823 const SkeletonPtr& skel, const Frame* refFrame, double tolerance)
1824 {
1825 VectorXd dq = skel->getVelocities();
1826 VectorXd ddq = skel->getAccelerations();
1827
1828 Vector6d comSpatialVelFk
1829 = skel->getCOMSpatialVelocity(Frame::World(), refFrame);
1830 Vector6d comSpatialAccFk
1831 = skel->getCOMSpatialAcceleration(Frame::World(), refFrame);
1832
1833 math::Jacobian comSpatialJac = skel->getCOMJacobian(refFrame);
1834 math::Jacobian comSpatialJacDeriv
1835 = skel->getCOMJacobianSpatialDeriv(refFrame);
1836
1837 Vector6d comSpatialVelJac = comSpatialJac * dq;
1838 Vector6d comSpatialAccJac = comSpatialJac * ddq + comSpatialJacDeriv * dq;
1839
1840 bool spatialVelEqual = equals(comSpatialVelFk, comSpatialVelJac, tolerance);
1841 EXPECT_TRUE(spatialVelEqual);
1842 if (!spatialVelEqual)
1843 printComparisonError(
1844 "COM spatial velocity",
1845 skel->getName(),
1846 refFrame->getName(),
1847 comSpatialVelFk,
1848 comSpatialVelJac);
1849
1850 bool spatialAccEqual = equals(comSpatialAccFk, comSpatialAccJac, tolerance);
1851 EXPECT_TRUE(spatialAccEqual);
1852 if (!spatialAccEqual)
1853 printComparisonError(
1854 "COM spatial acceleration",
1855 skel->getName(),
1856 refFrame->getName(),
1857 comSpatialAccFk,
1858 comSpatialAccJac);
1859
1860 Vector3d comLinearVelFk
1861 = skel->getCOMLinearVelocity(Frame::World(), refFrame);
1862 Vector3d comLinearAccFk
1863 = skel->getCOMLinearAcceleration(Frame::World(), refFrame);
1864
1865 math::LinearJacobian comLinearJac = skel->getCOMLinearJacobian(refFrame);
1866 math::LinearJacobian comLinearJacDeriv
1867 = skel->getCOMLinearJacobianDeriv(refFrame);
1868
1869 Vector3d comLinearVelJac = comLinearJac * dq;
1870 Vector3d comLinearAccJac = comLinearJac * ddq + comLinearJacDeriv * dq;
1871
1872 bool linearVelEqual = equals(comLinearVelFk, comLinearVelJac);
1873 EXPECT_TRUE(linearVelEqual);
1874 if (!linearVelEqual)
1875 printComparisonError(
1876 "COM linear velocity",
1877 skel->getName(),
1878 refFrame->getName(),
1879 comLinearVelFk,
1880 comLinearVelJac);
1881
1882 bool linearAccEqual = equals(comLinearAccFk, comLinearAccJac);
1883 EXPECT_TRUE(linearAccEqual);
1884 if (!linearAccEqual)
1885 printComparisonError(
1886 "COM linear acceleration",
1887 skel->getName(),
1888 refFrame->getName(),
1889 comLinearAccFk,
1890 comLinearAccJac);
1891 }
1892
1893 //==============================================================================
1894 void DynamicsTest::testCenterOfMass(const common::Uri& uri)
1895 {
1896 using namespace std;
1897 using namespace Eigen;
1898 using namespace dart;
1899 using namespace math;
1900 using namespace dynamics;
1901 using namespace simulation;
1902 using namespace utils;
1903
1904 //---------------------------- Settings --------------------------------------
1905 // Number of random state tests for each skeletons
1906 #ifndef NDEBUG // Debug mode
1907 std::size_t nRandomItr = 2;
1908 #else
1909 std::size_t nRandomItr = 100;
1910 #endif
1911
1912 // Lower and upper bound of configuration for system
1913 double lb = -1.5 * constantsd::pi();
1914 double ub = 1.5 * constantsd::pi();
1915
1916 // Lower and upper bound of joint damping and stiffness
1917 double lbD = 0.0;
1918 double ubD = 10.0;
1919 double lbK = 0.0;
1920 double ubK = 10.0;
1921
1922 simulation::WorldPtr myWorld;
1923
1924 //----------------------------- Tests ----------------------------------------
1925 // Check whether multiplication of mass matrix and its inverse is identity
1926 // matrix.
1927 myWorld = utils::SkelParser::readWorld(uri);
1928 EXPECT_TRUE(myWorld != nullptr);
1929
1930 for (std::size_t i = 0; i < myWorld->getNumSkeletons(); ++i)
1931 {
1932 dynamics::SkeletonPtr skeleton = myWorld->getSkeleton(i);
1933
1934 std::size_t dof = skeleton->getNumDofs();
1935 if (dof == 0)
1936 {
1937 dtmsg << "Skeleton [" << skeleton->getName() << "] is skipped since it "
1938 << "has 0 DOF." << endl;
1939 continue;
1940 }
1941
1942 for (std::size_t j = 0; j < nRandomItr; ++j)
1943 {
1944 // For the second half of the tests, scramble up the Skeleton
1945 if (j > ceil(nRandomItr / 2))
1946 {
1947 SkeletonPtr copy = skeleton->cloneSkeleton();
1948 std::size_t maxNode = skeleton->getNumBodyNodes() - 1;
1949 BodyNode* bn1 = skeleton->getBodyNode(
1950 math::Random::uniform<std::size_t>(0, maxNode));
1951 BodyNode* bn2 = skeleton->getBodyNode(
1952 math::Random::uniform<std::size_t>(0, maxNode));
1953
1954 if (bn1 != bn2)
1955 {
1956 BodyNode* child = bn1->descendsFrom(bn2) ? bn1 : bn2;
1957 BodyNode* parent = child == bn1 ? bn2 : bn1;
1958
1959 child->moveTo(parent);
1960 }
1961
1962 EXPECT_TRUE(skeleton->getNumBodyNodes() == copy->getNumBodyNodes());
1963 EXPECT_TRUE(skeleton->getNumDofs() == copy->getNumDofs());
1964 }
1965
1966 // Random joint stiffness and damping coefficient
1967 for (std::size_t k = 0; k < skeleton->getNumBodyNodes(); ++k)
1968 {
1969 BodyNode* body = skeleton->getBodyNode(k);
1970 Joint* joint = body->getParentJoint();
1971 int localDof = joint->getNumDofs();
1972
1973 for (int l = 0; l < localDof; ++l)
1974 {
1975 joint->setDampingCoefficient(l, Random::uniform(lbD, ubD));
1976 joint->setSpringStiffness(l, Random::uniform(lbK, ubK));
1977
1978 double lbRP = joint->getPositionLowerLimit(l);
1979 double ubRP = joint->getPositionUpperLimit(l);
1980 if (lbRP < -constantsd::pi())
1981 lbRP = -constantsd::pi();
1982 if (ubRP > constantsd::pi())
1983 ubRP = constantsd::pi();
1984 joint->setRestPosition(l, Random::uniform(lbRP, ubRP));
1985 }
1986 }
1987
1988 // Set random states
1989 VectorXd q = VectorXd(dof);
1990 VectorXd dq = VectorXd(dof);
1991 VectorXd ddq = VectorXd(dof);
1992 for (std::size_t k = 0; k < dof; ++k)
1993 {
1994 q[k] = math::Random::uniform(lb, ub);
1995 dq[k] = math::Random::uniform(lb, ub);
1996 ddq[k] = math::Random::uniform(lb, ub);
1997 }
1998 skeleton->setPositions(q);
1999 skeleton->setVelocities(dq);
2000 skeleton->setAccelerations(ddq);
2001
2002 randomizeRefFrames();
2003
2004 compareCOMJacobianToFk(skeleton, Frame::World(), 1e-6);
2005
2006 for (std::size_t r = 0; r < refFrames.size(); ++r)
2007 compareCOMJacobianToFk(skeleton, refFrames[r], 1e-6);
2008 }
2009 }
2010 }
2011
2012 //==============================================================================
2013 void compareCOMAccelerationToGravity(
2014 SkeletonPtr skel, const Eigen::Vector3d& gravity, double tolerance)
2015 {
2016 const std::size_t numFrames = 1e+2;
2017 skel->setGravity(gravity);
2018
2019 for (std::size_t i = 0; i < numFrames; ++i)
2020 {
2021 skel->computeForwardDynamics();
2022
2023 Vector3d comLinearAccFk = skel->getCOMLinearAcceleration();
2024
2025 bool comLinearAccFkEqual = equals(gravity, comLinearAccFk, tolerance);
2026 EXPECT_TRUE(comLinearAccFkEqual);
2027 if (!comLinearAccFkEqual)
2028 {
2029 printComparisonError(
2030 "COM linear acceleration",
2031 skel->getName(),
2032 Frame::World()->getName(),
2033 gravity,
2034 comLinearAccFk);
2035 }
2036
2037 VectorXd dq = skel->getVelocities();
2038 VectorXd ddq = skel->getAccelerations();
2039 math::LinearJacobian comLinearJac = skel->getCOMLinearJacobian();
2040 math::LinearJacobian comLinearJacDeriv = skel->getCOMLinearJacobianDeriv();
2041 Vector3d comLinearAccJac = comLinearJac * ddq + comLinearJacDeriv * dq;
2042
2043 bool comLinearAccJacEqual = equals(gravity, comLinearAccJac, tolerance);
2044 EXPECT_TRUE(comLinearAccJacEqual);
2045 if (!comLinearAccJacEqual)
2046 {
2047 printComparisonError(
2048 "COM linear acceleration",
2049 skel->getName(),
2050 Frame::World()->getName(),
2051 gravity,
2052 comLinearAccJac);
2053 }
2054 }
2055 }
2056
2057 //==============================================================================
2058 void DynamicsTest::testCenterOfMassFreeFall(const common::Uri& uri)
2059 {
2060 using namespace std;
2061 using namespace Eigen;
2062 using namespace dart;
2063 using namespace math;
2064 using namespace dynamics;
2065 using namespace simulation;
2066 using namespace utils;
2067
2068 //---------------------------- Settings --------------------------------------
2069 // Number of random state tests for each skeletons
2070 #ifndef NDEBUG // Debug mode
2071 std::size_t nRandomItr = 2;
2072 #else
2073 std::size_t nRandomItr = 10;
2074 #endif // ------- Debug mode
2075
2076 // Lower and upper bound of configuration for system
2077 double lb = -1.5 * constantsd::pi();
2078 double ub = 1.5 * constantsd::pi();
2079
2080 // Lower and upper bound of joint damping and stiffness
2081 double lbD = 0.0;
2082 double ubD = 10.0;
2083 double lbK = 0.0;
2084 double ubK = 10.0;
2085
2086 simulation::WorldPtr myWorld;
2087 std::vector<Vector3d> gravities(4);
2088 gravities[0] = Vector3d::Zero();
2089 gravities[1] = Vector3d(-9.81, 0, 0);
2090 gravities[2] = Vector3d(0, -9.81, 0);
2091 gravities[3] = Vector3d(0, 0, -9.81);
2092
2093 //----------------------------- Tests ----------------------------------------
2094 // Check whether multiplication of mass matrix and its inverse is identity
2095 // matrix.
2096 myWorld = utils::SkelParser::readWorld(uri);
2097 EXPECT_TRUE(myWorld != nullptr);
2098
2099 for (std::size_t i = 0; i < myWorld->getNumSkeletons(); ++i)
2100 {
2101 auto skel = myWorld->getSkeleton(i);
2102 auto rootJoint = skel->getJoint(0);
2103 auto rootFreeJoint = dynamic_cast<dynamics::FreeJoint*>(rootJoint);
2104
2105 auto dof = skel->getNumDofs();
2106
2107 if (nullptr == rootFreeJoint || !skel->isMobile() || 0 == dof)
2108 {
2109 #if BUILD_TYPE_DEBUG
2110 dtmsg << "Skipping COM free fall test for Skeleton [" << skel->getName()
2111 << "] since the Skeleton doesn't have FreeJoint at the root body "
2112 << " or immobile." << endl;
2113 #endif
2114 continue;
2115 }
2116 else
2117 {
2118 rootFreeJoint->setActuatorType(dynamics::Joint::PASSIVE);
2119 }
2120
2121 // Make sure the damping and spring forces are zero for the root FreeJoint.
2122 for (std::size_t l = 0; l < rootJoint->getNumDofs(); ++l)
2123 {
2124 rootJoint->setDampingCoefficient(l, 0.0);
2125 rootJoint->setSpringStiffness(l, 0.0);
2126 rootJoint->setRestPosition(l, 0.0);
2127 }
2128
2129 for (std::size_t j = 0; j < nRandomItr; ++j)
2130 {
2131 // Random joint stiffness and damping coefficient
2132 for (std::size_t k = 1; k < skel->getNumBodyNodes(); ++k)
2133 {
2134 auto body = skel->getBodyNode(k);
2135 auto joint = body->getParentJoint();
2136 auto localDof = joint->getNumDofs();
2137
2138 for (std::size_t l = 0; l < localDof; ++l)
2139 {
2140 joint->setDampingCoefficient(l, Random::uniform(lbD, ubD));
2141 joint->setSpringStiffness(l, Random::uniform(lbK, ubK));
2142
2143 double lbRP = joint->getPositionLowerLimit(l);
2144 double ubRP = joint->getPositionUpperLimit(l);
2145 if (lbRP < -constantsd::pi())
2146 lbRP = -constantsd::pi();
2147 if (ubRP > constantsd::pi())
2148 ubRP = constantsd::pi();
2149 joint->setRestPosition(l, Random::uniform(lbRP, ubRP));
2150 }
2151 }
2152
2153 // Set random states
2154 VectorXd q = VectorXd(dof);
2155 VectorXd dq = VectorXd(dof);
2156 for (std::size_t k = 0; k < dof; ++k)
2157 {
2158 q[k] = math::Random::uniform(lb, ub);
2159 dq[k] = math::Random::uniform(lb, ub);
2160 }
2161 VectorXd ddq = VectorXd::Zero(dof);
2162 skel->setPositions(q);
2163 skel->setVelocities(dq);
2164 skel->setAccelerations(ddq);
2165
2166 for (const auto& gravity : gravities)
2167 compareCOMAccelerationToGravity(skel, gravity, 1e-6);
2168 }
2169 }
2170 }
2171
2172 //==============================================================================
2173 void DynamicsTest::testConstraintImpulse(const common::Uri& uri)
2174 {
2175 using namespace std;
2176 using namespace Eigen;
2177 using namespace dart;
2178 using namespace math;
2179 using namespace dynamics;
2180 using namespace simulation;
2181 using namespace utils;
2182
2183 //---------------------------- Settings --------------------------------------
2184 // Number of random state tests for each skeletons
2185 #ifndef NDEBUG // Debug mode
2186 std::size_t nRandomItr = 1;
2187 #else
2188 std::size_t nRandomItr = 1;
2189 #endif
2190
2191 // Lower and upper bound of configuration for system
2192 // double lb = -1.5 * constantsd::pi();
2193 // double ub = 1.5 * constantsd::pi();
2194
2195 simulation::WorldPtr myWorld;
2196
2197 //----------------------------- Tests ----------------------------------------
2198 // Check whether multiplication of mass matrix and its inverse is identity
2199 // matrix.
2200 myWorld = utils::SkelParser::readWorld(uri);
2201 EXPECT_TRUE(myWorld != nullptr);
2202
2203 for (std::size_t i = 0; i < myWorld->getNumSkeletons(); ++i)
2204 {
2205 dynamics::SkeletonPtr skel = myWorld->getSkeleton(i);
2206
2207 std::size_t dof = skel->getNumDofs();
2208 // int nBodyNodes = skel->getNumBodyNodes();
2209
2210 if (dof == 0 || !skel->isMobile())
2211 {
2212 dtdbg << "Skeleton [" << skel->getName() << "] is skipped since it has "
2213 << "0 DOF or is immobile." << endl;
2214 continue;
2215 }
2216
2217 for (std::size_t j = 0; j < nRandomItr; ++j)
2218 {
2219 // Set random configurations
2220 for (std::size_t k = 0; k < skel->getNumBodyNodes(); ++k)
2221 {
2222 BodyNode* body = skel->getBodyNode(k);
2223 Joint* joint = body->getParentJoint();
2224 int localDof = joint->getNumDofs();
2225
2226 for (int l = 0; l < localDof; ++l)
2227 {
2228 double lbRP = joint->getPositionLowerLimit(l);
2229 double ubRP = joint->getPositionUpperLimit(l);
2230 if (lbRP < -constantsd::pi())
2231 lbRP = -constantsd::pi();
2232 if (ubRP > constantsd::pi())
2233 ubRP = constantsd::pi();
2234 joint->setPosition(l, Random::uniform(lbRP, ubRP));
2235 }
2236
2237 // Set constraint impulse on each body
2238 skel->clearConstraintImpulses();
2239 Eigen::Vector6d impulseOnBody = Eigen::Vector6d::Random();
2240 body->setConstraintImpulse(impulseOnBody);
2241
2242 // Get constraint force vector
2243 Eigen::VectorXd constraintVector1 = skel->getConstraintForces();
2244
2245 // Get constraint force vector by using Jacobian of skeleon
2246 Eigen::MatrixXd bodyJacobian = body->getJacobian();
2247 Eigen::VectorXd constraintVector2
2248 = bodyJacobian.transpose() * impulseOnBody / skel->getTimeStep();
2249
2250 std::size_t index = 0;
2251 for (std::size_t l = 0; l < dof; ++l)
2252 {
2253 if (constraintVector1(l) == 0.0)
2254 continue;
2255
2256 EXPECT_NEAR(constraintVector1(l), constraintVector2(index), 1e-6);
2257 index++;
2258 }
2259 assert(static_cast<std::size_t>(bodyJacobian.cols()) == index);
2260 }
2261 }
2262 }
2263 }
2264
2265 //==============================================================================
2266 void DynamicsTest::testImpulseBasedDynamics(const common::Uri& uri)
2267 {
2268 using namespace std;
2269 using namespace Eigen;
2270 using namespace dart;
2271 using namespace math;
2272 using namespace dynamics;
2273 using namespace simulation;
2274 using namespace utils;
2275
2276 //---------------------------- Settings --------------------------------------
2277 // Number of random state tests for each skeletons
2278 #ifndef NDEBUG // Debug mode
2279 std::size_t nRandomItr = 1;
2280 #else
2281 std::size_t nRandomItr = 100;
2282 #endif
2283
2284 double TOLERANCE = 1e-1;
2285
2286 // Lower and upper bound of configuration for system
2287 double lb = -1.5 * constantsd::pi();
2288 double ub = 1.5 * constantsd::pi();
2289
2290 simulation::WorldPtr myWorld;
2291
2292 //----------------------------- Tests ----------------------------------------
2293 // Check whether multiplication of mass matrix and its inverse is identity
2294 // matrix.
2295 myWorld = utils::SkelParser::readWorld(uri);
2296 EXPECT_TRUE(myWorld != nullptr);
2297
2298 for (std::size_t i = 0; i < myWorld->getNumSkeletons(); ++i)
2299 {
2300 dynamics::SkeletonPtr skel = myWorld->getSkeleton(i);
2301
2302 int dof = skel->getNumDofs();
2303 // int nBodyNodes = skel->getNumBodyNodes();
2304
2305 if (dof == 0 || !skel->isMobile())
2306 {
2307 dtdbg << "Skeleton [" << skel->getName() << "] is skipped since it has "
2308 << "0 DOF or is immobile." << endl;
2309 continue;
2310 }
2311
2312 for (std::size_t j = 0; j < nRandomItr; ++j)
2313 {
2314 // Set random configurations
2315 for (std::size_t k = 0; k < skel->getNumBodyNodes(); ++k)
2316 {
2317 BodyNode* body = skel->getBodyNode(k);
2318 Joint* joint = body->getParentJoint();
2319 int localDof = joint->getNumDofs();
2320
2321 for (int l = 0; l < localDof; ++l)
2322 {
2323 double lbRP = joint->getPositionLowerLimit(l);
2324 double ubRP = joint->getPositionUpperLimit(l);
2325 if (lbRP < -constantsd::pi())
2326 lbRP = -constantsd::pi();
2327 if (ubRP > constantsd::pi())
2328 ubRP = constantsd::pi();
2329 joint->setPosition(l, Random::uniform(lbRP, ubRP));
2330 }
2331 }
2332 // skel->setPositions(VectorXd::Zero(dof));
2333
2334 // TODO(JS): Just clear what should be
2335 skel->clearExternalForces();
2336 skel->clearConstraintImpulses();
2337
2338 // Set random impulses
2339 VectorXd impulses = VectorXd::Zero(dof);
2340 for (int k = 0; k < impulses.size(); ++k)
2341 impulses[k] = Random::uniform(lb, ub);
2342 skel->setJointConstraintImpulses(impulses);
2343
2344 // Compute impulse-based forward dynamics
2345 skel->computeImpulseForwardDynamics();
2346
2347 // Compare resultant velocity change and invM * impulses
2348 VectorXd deltaVel1 = skel->getVelocityChanges();
2349 MatrixXd invM = skel->getInvMassMatrix();
2350 VectorXd deltaVel2 = invM * impulses;
2351
2352 EXPECT_TRUE(equals(deltaVel1, deltaVel2, TOLERANCE));
2353 if (!equals(deltaVel1, deltaVel2, TOLERANCE))
2354 {
2355 cout << "deltaVel1: " << deltaVel1.transpose() << endl;
2356 cout << "deltaVel2: " << deltaVel2.transpose() << endl;
2357 cout << "error: " << (deltaVel1 - deltaVel2).norm() << endl;
2358 }
2359 }
2360 }
2361 }
2362
2363 //==============================================================================
2364 TEST_F(DynamicsTest, testJacobians)
2365 {
2366 for (std::size_t i = 0; i < getList().size(); ++i)
2367 {
2368 #ifndef NDEBUG
2369 dtdbg << getList()[i].toString() << std::endl;
2370 #endif
2371 testJacobians(getList()[i]);
2372 }
2373 }
2374
2375 //==============================================================================
2376 TEST_F(DynamicsTest, testFiniteDifference)
2377 {
2378 for (std::size_t i = 0; i < getList().size(); ++i)
2379 {
2380 #if BUILD_TYPE_DEBUG
2381 dtdbg << getList()[i].toString() << std::endl;
2382 #endif
2383 testFiniteDifferenceGeneralizedCoordinates(getList()[i]);
2384 testFiniteDifferenceBodyNodeVelocity(getList()[i]);
2385 testFiniteDifferenceBodyNodeAcceleration(getList()[i]);
2386 }
2387 }
2388
2389 //==============================================================================
2390 TEST_F(DynamicsTest, testForwardKinematics)
2391 {
2392 for (std::size_t i = 0; i < getList().size(); ++i)
2393 {
2394 #ifndef NDEBUG
2395 dtdbg << getList()[i].toString() << std::endl;
2396 #endif
2397 testForwardKinematics(getList()[i]);
2398 }
2399 }
2400
2401 //==============================================================================
2402 TEST_F(DynamicsTest, testInverseDynamics)
2403 {
2404 for (std::size_t i = 0; i < getList().size(); ++i)
2405 {
2406 #ifndef NDEBUG
2407 dtdbg << getList()[i].toString() << std::endl;
2408 #endif
2409 testInverseDynamics(getList()[i]);
2410 }
2411 }
2412
2413 //==============================================================================
2414 TEST_F(DynamicsTest, compareEquationsOfMotion)
2415 {
2416 for (std::size_t i = 0; i < getList().size(); ++i)
2417 {
2418 ////////////////////////////////////////////////////////////////////////////
2419 // TODO(JS): Following skel files, which contain euler joints couldn't
2420 // pass EQUATIONS_OF_MOTION, are disabled.
2421 const auto uri = getList()[i];
2422 if (uri.toString() == "dart://sample/skel/test/double_pendulum_euler_joint.skel"
2423 || uri.toString() == "dart://sample/skel/test/chainwhipa.skel"
2424 || uri.toString() == "dart://sample/skel/test/serial_chain_eulerxyz_joint.skel"
2425 || uri.toString() == "dart://sample/skel/test/simple_tree_structure_euler_joint.skel"
2426 || uri.toString() == "dart://sample/skel/test/tree_structure_euler_joint.skel"
2427 || uri.toString() == "dart://sample/skel/fullbody1.skel")
2428 {
2429 continue;
2430 }
2431 ////////////////////////////////////////////////////////////////////////////
2432
2433 #ifndef NDEBUG
2434 dtdbg << getList()[i].toString() << std::endl;
2435 #endif
2436 compareEquationsOfMotion(getList()[i]);
2437 }
2438 }
2439
2440 //==============================================================================
2441 TEST_F(DynamicsTest, testCenterOfMass)
2442 {
2443 for (std::size_t i = 0; i < getList().size(); ++i)
2444 {
2445 #ifndef NDEBUG
2446 dtdbg << getList()[i].toString() << std::endl;
2447 #endif
2448 testCenterOfMass(getList()[i]);
2449 }
2450 }
2451
2452 //==============================================================================
2453 TEST_F(DynamicsTest, testCenterOfMassFreeFall)
2454 {
2455 for (std::size_t i = 0; i < getList().size(); ++i)
2456 {
2457 #ifndef NDEBUG
2458 dtdbg << getList()[i].toString() << std::endl;
2459 #endif
2460 testCenterOfMassFreeFall(getList()[i]);
2461 }
2462 }
2463
2464 //==============================================================================
2465 TEST_F(DynamicsTest, testConstraintImpulse)
2466 {
2467 for (std::size_t i = 0; i < getList().size(); ++i)
2468 {
2469 #ifndef NDEBUG
2470 dtdbg << getList()[i].toString() << std::endl;
2471 #endif
2472 testConstraintImpulse(getList()[i]);
2473 }
2474 }
2475
2476 //==============================================================================
2477 TEST_F(DynamicsTest, testImpulseBasedDynamics)
2478 {
2479 for (std::size_t i = 0; i < getList().size(); ++i)
2480 {
2481 #ifndef NDEBUG
2482 dtdbg << getList()[i].toString() << std::endl;
2483 #endif
2484 testImpulseBasedDynamics(getList()[i]);
2485 }
2486 }
2487
2488 //==============================================================================
2489 TEST_F(DynamicsTest, HybridDynamics)
2490 {
2491 const double tol = 1e-8;
2492 const double timeStep = 1e-3;
2493 #ifndef NDEBUG // Debug mode
2494 const std::size_t numFrames = 50; // 0.05 secs
2495 #else
2496 const std::size_t numFrames = 5e+3; // 5 secs
2497 #endif // ------- Debug mode
2498
2499 // Load world and skeleton
2500 WorldPtr world = utils::SkelParser::readWorld(
2501 "dart://sample/skel/test/hybrid_dynamics_test.skel");
2502 world->setTimeStep(timeStep);
2503 EXPECT_TRUE(world != nullptr);
2504 EXPECT_NEAR(world->getTimeStep(), timeStep, tol);
2505
2506 SkeletonPtr skel = world->getSkeleton("skeleton 1");
2507 EXPECT_TRUE(skel != nullptr);
2508 EXPECT_NEAR(skel->getTimeStep(), timeStep, tol);
2509
2510 const std::size_t numDofs = skel->getNumDofs();
2511
2512 // Zero initial states
2513 Eigen::VectorXd q0 = Eigen::VectorXd::Zero(numDofs);
2514 Eigen::VectorXd dq0 = Eigen::VectorXd::Zero(numDofs);
2515
2516 // Initialize the skeleton with the zero initial states
2517 skel->setPositions(q0);
2518 skel->setVelocities(dq0);
2519 EXPECT_TRUE(equals(skel->getPositions(), q0));
2520 EXPECT_TRUE(equals(skel->getVelocities(), dq0));
2521
2522 // Make sure all the joint actuator types
2523 EXPECT_EQ(skel->getJoint(0)->getActuatorType(), Joint::FORCE);
2524 EXPECT_EQ(skel->getJoint(1)->getActuatorType(), Joint::ACCELERATION);
2525 EXPECT_EQ(skel->getJoint(2)->getActuatorType(), Joint::VELOCITY);
2526 EXPECT_EQ(skel->getJoint(3)->getActuatorType(), Joint::ACCELERATION);
2527 EXPECT_EQ(skel->getJoint(4)->getActuatorType(), Joint::VELOCITY);
2528
2529 // Prepare command for each joint types per simulation steps
2530 Eigen::MatrixXd command = Eigen::MatrixXd::Zero(numFrames, numDofs);
2531 Eigen::VectorXd amp = Eigen::VectorXd::Zero(numDofs);
2532 for (std::size_t i = 0; i < numDofs; ++i)
2533 amp[i] = math::Random::uniform(-1.5, 1.5);
2534 for (std::size_t i = 0; i < numFrames; ++i)
2535 {
2536 for (std::size_t j = 0; j < numDofs; ++j)
2537 command(i, j) = amp[j] * std::sin(i * timeStep);
2538 }
2539
2540 // Record joint forces for joint[1~4]
2541 Eigen::MatrixXd forces = Eigen::MatrixXd::Zero(numFrames, numDofs);
2542 for (std::size_t i = 0; i < numFrames; ++i)
2543 {
2544 skel->setCommands(command.row(i));
2545
2546 world->step(false);
2547
2548 forces.row(i) = skel->getForces();
2549
2550 EXPECT_NEAR(command(i, 0), skel->getForce(0), tol);
2551 EXPECT_NEAR(command(i, 1), skel->getAcceleration(1), tol);
2552 EXPECT_NEAR(command(i, 2), skel->getVelocity(2), tol);
2553 EXPECT_NEAR(command(i, 3), skel->getAcceleration(3), tol);
2554 EXPECT_NEAR(command(i, 4), skel->getVelocity(4), tol);
2555 }
2556
2557 // Restore the skeleton to the initial state
2558 skel->setPositions(q0);
2559 skel->setVelocities(dq0);
2560 EXPECT_TRUE(equals(skel->getPositions(), q0));
2561 EXPECT_TRUE(equals(skel->getVelocities(), dq0));
2562
2563 // Change all the actuator types to force
2564 skel->getJoint(0)->setActuatorType(Joint::FORCE);
2565 skel->getJoint(1)->setActuatorType(Joint::FORCE);
2566 skel->getJoint(2)->setActuatorType(Joint::FORCE);
2567 skel->getJoint(3)->setActuatorType(Joint::FORCE);
2568 skel->getJoint(4)->setActuatorType(Joint::FORCE);
2569 EXPECT_EQ(skel->getJoint(0)->getActuatorType(), Joint::FORCE);
2570 EXPECT_EQ(skel->getJoint(1)->getActuatorType(), Joint::FORCE);
2571 EXPECT_EQ(skel->getJoint(2)->getActuatorType(), Joint::FORCE);
2572 EXPECT_EQ(skel->getJoint(3)->getActuatorType(), Joint::FORCE);
2573 EXPECT_EQ(skel->getJoint(4)->getActuatorType(), Joint::FORCE);
2574
2575 // Test if the skeleton moves as the command with the joint forces
2576 Eigen::MatrixXd output = Eigen::MatrixXd::Zero(numFrames, numDofs);
2577 for (std::size_t i = 0; i < numFrames; ++i)
2578 {
2579 skel->setCommands(forces.row(i));
2580
2581 world->step(false);
2582
2583 output(i, 0) = skel->getJoint(0)->getForce(0);
2584 output(i, 1) = skel->getJoint(1)->getAcceleration(0);
2585 output(i, 2) = skel->getJoint(2)->getVelocity(0);
2586 output(i, 3) = skel->getJoint(3)->getAcceleration(0);
2587 output(i, 4) = skel->getJoint(4)->getVelocity(0);
2588
2589 EXPECT_NEAR(command(i, 0), output(i, 0), tol);
2590 EXPECT_NEAR(command(i, 1), output(i, 1), tol);
2591 EXPECT_NEAR(command(i, 2), output(i, 2), tol);
2592 EXPECT_NEAR(command(i, 3), output(i, 3), tol);
2593 EXPECT_NEAR(command(i, 4), output(i, 4), tol);
2594 }
2595 }
2596
2597 //==============================================================================
2598 TEST_F(DynamicsTest, OffsetCOM)
2599 {
2600 WorldPtr world = World::create();
2601 ASSERT_TRUE(world != nullptr);
2602 world->setGravity(Eigen::Vector3d(0.0, 0.0, 0.0));
2603 const double dt = 0.001;
2604 world->setTimeStep(dt);
2605
2606 SkeletonPtr boxSkel = createBox(Vector3d(1.0, 1.0, 1.0));
2607 world->addSkeleton(boxSkel);
2608 BodyNode* box = boxSkel->getBodyNode(0);
2609 dynamics::Inertia inertia;
2610 inertia.setMass(60);
2611 inertia.setMoment(
2612 box->getShapeNode(0)->getShape()->computeInertia(inertia.getMass()));
2613 inertia.setLocalCOM({0, 500, 0});
2614 box->setInertia(inertia);
2615 Eigen::Isometry3d boxInitialPose = box->getWorldTransform();
2616 Eigen::Vector3d torque(0, 0, 100);
2617 box->addExtTorque(torque, false);
2618
2619 {
2620 Vector3d comVel = box->getCOMLinearVelocity();
2621 EXPECT_TRUE(equals(Vector3d(0, 0, 0), comVel))
2622 << "comVel: " << comVel.transpose();
2623 }
2624 {
2625 Vector3d linVel = box->getLinearVelocity();
2626 EXPECT_TRUE(equals(Vector3d(0, 0, 0), linVel))
2627 << "linVel: " << linVel.transpose();
2628 }
2629
2630 world->step();
2631 // Velocity at COM should be zero since the object is just rotating.
2632 {
2633 Vector3d comVel = box->getCOMLinearVelocity();
2634 EXPECT_TRUE(equals(Vector3d(0, 0, 0), comVel, 1e-4))
2635 << "comVel: " << comVel.transpose();
2636 }
2637 {
2638 Vector3d angVel = box->getAngularVelocity();
2639 // We can compute expectedAngAccel like so because the initial angular
2640 // velocity is zero.
2641 Vector3d expectedAngAccel = inertia.getMoment().inverse() * torque;
2642 Vector3d expectedAngVel = expectedAngAccel * dt;
2643 EXPECT_TRUE(equals(expectedAngVel, angVel))
2644 << "angVel: " << angVel.transpose();
2645 Vector3d linVel = box->getLinearVelocity();
2646 Vector3d expLinVel
2647 = angVel.cross(boxInitialPose.linear() * -box->getLocalCOM());
2648 EXPECT_TRUE(equals(expLinVel, linVel))
2649 << "Expected: " << expLinVel.transpose()
2650 << "\nActual: " << linVel.transpose();
2651
2652 Vector3d angAccel = box->getAngularAcceleration();
2653 EXPECT_TRUE(equals(expectedAngAccel, angAccel))
2654 << "angAccel: " << angAccel.transpose();
2655 Vector3d linAccel = box->getLinearAcceleration();
2656 Vector3d expLinAccel
2657 = expectedAngAccel.cross(boxInitialPose.linear() * -box->getLocalCOM());
2658 EXPECT_TRUE(equals(expLinAccel, linAccel))
2659 << "Expected: " << expLinAccel.transpose()
2660 << "\nActual: " << linAccel.transpose();
2661 }
2662 }