/* * Copyright (c) 2011-2022, The DART development contributors * All rights reserved. * * The list of contributors can be found at: * https://github.com/dartsim/dart/blob/master/LICENSE * * This file is provided under the following "BSD-style" License: * Redistribution and use in source and binary forms, with or * without modification, are permitted provided that the following * conditions are met: * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include "dart/common/Console.hpp" #include "dart/dynamics/BodyNode.hpp" #include "dart/dynamics/SimpleFrame.hpp" #include "dart/dynamics/Skeleton.hpp" #include "dart/math/Geometry.hpp" #include "dart/math/Helpers.hpp" #include "dart/math/Random.hpp" #include "dart/simulation/World.hpp" #include "dart/utils/SkelParser.hpp" #include "TestHelpers.hpp" using namespace Eigen; using namespace dart; //============================================================================== class DynamicsTest : public ::testing::Test { public: // Get Skel file URI to test. const std::vector& getList() const; // Get reference frames const std::vector& getRefFrames() const; // Randomize the properties of all the reference frames void randomizeRefFrames(); // Get mass matrix of _skel using Jacobians and inertias of each body // in _skel. MatrixXd getMassMatrix(dynamics::SkeletonPtr _skel); // Get augmented mass matrix of _skel using Jacobians and inertias of // each body in _skel. MatrixXd getAugMassMatrix(dynamics::SkeletonPtr _skel); // Compare velocities computed by recursive method, Jacobian, and finite // difference. void testJacobians(const common::Uri& uri); // Compare velocities and accelerations with actual vaules and approximates // using finite differece method. void testFiniteDifferenceGeneralizedCoordinates(const common::Uri& uri); // Compare spatial velocities computed by forward kinematics and finite // difference. void testFiniteDifferenceBodyNodeVelocity(const common::Uri& uri); // Compare accelerations computed by recursive method, Jacobian, and finite // difference. void testFiniteDifferenceBodyNodeAcceleration(const common::Uri& uri); // Test if the recursive forward kinematics algorithm computes // transformations, spatial velocities, and spatial accelerations correctly. void testForwardKinematics(const common::Uri& uri); // Test inverse dynamics with various joint forces. void testInverseDynamics(const common::Uri& uri); // Compare dynamics terms in equations of motion such as mass matrix, mass // inverse matrix, Coriolis force vector, gravity force vector, and external // force vector. void compareEquationsOfMotion(const common::Uri& uri); // Test skeleton's COM and its related quantities. void testCenterOfMass(const common::Uri& uri); // Test if the com acceleration is equal to the gravity void testCenterOfMassFreeFall(const common::Uri& uri); // void testConstraintImpulse(const common::Uri& uri); // Test impulse based dynamics void testImpulseBasedDynamics(const common::Uri& uri); protected: // Sets up the test fixture. void SetUp() override; // Skel file list. std::vector fileList; std::vector refFrames; }; //============================================================================== void DynamicsTest::SetUp() { // Create a list of skel files to test with fileList.push_back("dart://sample/skel/test/chainwhipa.skel"); fileList.push_back("dart://sample/skel/test/single_pendulum.skel"); fileList.push_back( "dart://sample/skel/test/single_pendulum_euler_joint.skel"); fileList.push_back("dart://sample/skel/test/single_pendulum_ball_joint.skel"); fileList.push_back("dart://sample/skel/test/double_pendulum.skel"); fileList.push_back( "dart://sample/skel/test/double_pendulum_euler_joint.skel"); fileList.push_back("dart://sample/skel/test/double_pendulum_ball_joint.skel"); fileList.push_back( "dart://sample/skel/test/serial_chain_revolute_joint.skel"); fileList.push_back( "dart://sample/skel/test/serial_chain_eulerxyz_joint.skel"); fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint.skel"); fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint_20.skel"); fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint_40.skel"); fileList.push_back("dart://sample/skel/test/simple_tree_structure.skel"); fileList.push_back( "dart://sample/skel/test/simple_tree_structure_euler_joint.skel"); fileList.push_back( "dart://sample/skel/test/simple_tree_structure_ball_joint.skel"); fileList.push_back("dart://sample/skel/test/tree_structure.skel"); fileList.push_back("dart://sample/skel/test/tree_structure_euler_joint.skel"); fileList.push_back("dart://sample/skel/test/tree_structure_ball_joint.skel"); fileList.push_back("dart://sample/skel/fullbody1.skel"); // Create a list of reference frames to use during tests refFrames.push_back(new SimpleFrame(Frame::World(), "refFrame1")); refFrames.push_back(new SimpleFrame(refFrames.back(), "refFrame2")); refFrames.push_back(new SimpleFrame(refFrames.back(), "refFrame3")); refFrames.push_back(new SimpleFrame(refFrames.back(), "refFrame4")); refFrames.push_back(new SimpleFrame(Frame::World(), "refFrame5")); refFrames.push_back(new SimpleFrame(refFrames.back(), "refFrame6")); } //============================================================================== const std::vector& DynamicsTest::getList() const { return fileList; } //============================================================================== const std::vector& DynamicsTest::getRefFrames() const { return refFrames; } //============================================================================== void DynamicsTest::randomizeRefFrames() { for (std::size_t i = 0; i < refFrames.size(); ++i) { SimpleFrame* F = refFrames[i]; Eigen::Vector3d p = Random::uniform(-100, 100); Eigen::Vector3d theta = Random::uniform( -2 * dart::math::constantsd::pi(), 2 * dart::math::constantsd::pi()); Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); tf.translate(p); tf.linear() = math::eulerXYZToMatrix(theta); F->setRelativeTransform(tf); F->setRelativeSpatialVelocity(Random::uniform(-100, 100)); F->setRelativeSpatialAcceleration( Random::uniform(-100, 100)); } } //============================================================================== MatrixXd DynamicsTest::getMassMatrix(dynamics::SkeletonPtr _skel) { int skelDof = _skel->getNumDofs(); MatrixXd skelM = MatrixXd::Zero(skelDof, skelDof); // Mass matrix of skeleton MatrixXd M; // Body mass MatrixXd I; // Body inertia MatrixXd J; // Body Jacobian for (std::size_t i = 0; i < _skel->getNumBodyNodes(); ++i) { dynamics::BodyNode* body = _skel->getBodyNode(i); int dof = body->getNumDependentGenCoords(); I = body->getSpatialInertia(); J = body->getJacobian(); EXPECT_EQ(I.rows(), 6); EXPECT_EQ(I.cols(), 6); EXPECT_EQ(J.rows(), 6); EXPECT_EQ(J.cols(), dof); M = J.transpose() * I * J; // (dof x dof) matrix for (int j = 0; j < dof; ++j) { int jIdx = body->getDependentGenCoordIndex(j); for (int k = 0; k < dof; ++k) { int kIdx = body->getDependentGenCoordIndex(k); skelM(jIdx, kIdx) += M(j, k); } } } return skelM; } //============================================================================== MatrixXd DynamicsTest::getAugMassMatrix(dynamics::SkeletonPtr _skel) { int dof = _skel->getNumDofs(); double dt = _skel->getTimeStep(); MatrixXd M = getMassMatrix(_skel); MatrixXd D = MatrixXd::Zero(dof, dof); MatrixXd K = MatrixXd::Zero(dof, dof); MatrixXd AugM; // Compute diagonal matrices of joint damping and joint stiffness for (std::size_t i = 0; i < _skel->getNumBodyNodes(); ++i) { dynamics::BodyNode* body = _skel->getBodyNode(i); dynamics::Joint* joint = body->getParentJoint(); EXPECT_TRUE(body != nullptr); EXPECT_TRUE(joint != nullptr); int dof = joint->getNumDofs(); for (int j = 0; j < dof; ++j) { int idx = joint->getIndexInSkeleton(j); D(idx, idx) = joint->getDampingCoefficient(j); K(idx, idx) = joint->getSpringStiffness(j); } } AugM = M + (dt * D) + (dt * dt * K); return AugM; } template void printComparisonError( const std::string& _comparison, const std::string& _name, const std::string& _frame, const T& fk, const T& jac) { std::cout << "Disagreement between FK and Jacobian results for " << _comparison << " of '" << _name << "' with a reference Frame of '" << _frame << "'\n" << "FK: " << fk.transpose() << "\n" << "Jac: " << jac.transpose() << "\n"; } //============================================================================== void compareBodyNodeFkToJacobian( const BodyNode* bn, const Frame* refFrame, double tolerance) { using math::AngularJacobian; using math::Jacobian; using math::LinearJacobian; ConstSkeletonPtr skel = bn->getSkeleton(); VectorXd dq = skel->getVelocities(); VectorXd ddq = skel->getAccelerations(); const std::vector& coords = bn->getDependentGenCoordIndices(); VectorXd dqSeg = skel->getVelocities(coords); VectorXd ddqSeg = skel->getAccelerations(coords); //-- Spatial Jacobian tests -------------------------------------------------- Vector6d SpatialVelFk = bn->getSpatialVelocity(Frame::World(), refFrame); Vector6d SpatialAccFk = bn->getSpatialAcceleration(Frame::World(), refFrame); Jacobian SpatialJacSeg = bn->getJacobian(refFrame); Jacobian SpatialJacDerivSeg = bn->getJacobianSpatialDeriv(refFrame); Vector6d SpatialVelJacSeg = SpatialJacSeg * dqSeg; Vector6d SpatialAccJacSeg = SpatialJacSeg * ddqSeg + SpatialJacDerivSeg * dqSeg; Jacobian SpatialJac = skel->getJacobian(bn, refFrame); Jacobian SpatialJacDeriv = skel->getJacobianSpatialDeriv(bn, refFrame); Vector6d SpatialVelJac = SpatialJac * dq; Vector6d SpatialAccJac = SpatialJac * ddq + SpatialJacDeriv * dq; bool spatialVelSegEqual = equals(SpatialVelFk, SpatialVelJacSeg, tolerance); bool spatialVelEqual = equals(SpatialVelFk, SpatialVelJac, tolerance); EXPECT_TRUE(spatialVelSegEqual); EXPECT_TRUE(spatialVelEqual); if (!spatialVelSegEqual) printComparisonError( "spatial velocity (seg)", bn->getName(), refFrame->getName(), SpatialVelFk, SpatialVelJacSeg); if (!spatialVelEqual) printComparisonError( "spatial velocity", bn->getName(), refFrame->getName(), SpatialVelFk, SpatialVelJac); bool spatialAccSegEqual = equals(SpatialAccFk, SpatialAccJacSeg, tolerance); bool spatialAccEqual = equals(SpatialAccFk, SpatialAccJac, tolerance); EXPECT_TRUE(spatialAccSegEqual); EXPECT_TRUE(spatialAccEqual); if (!spatialAccSegEqual) printComparisonError( "spatial acceleration (seg)", bn->getName(), refFrame->getName(), SpatialAccFk, SpatialAccJacSeg); if (!spatialAccEqual) printComparisonError( "spatial acceleration", bn->getName(), refFrame->getName(), SpatialAccFk, SpatialAccJac); //-- Linear Jacobian tests --------------------------------------------------- Vector3d LinearVelFk = bn->getLinearVelocity(Frame::World(), refFrame); Vector3d LinearAccFk = bn->getLinearAcceleration(Frame::World(), refFrame); LinearJacobian LinearJacSeg = bn->getLinearJacobian(refFrame); LinearJacobian LinearJacDerivSeg = bn->getLinearJacobianDeriv(refFrame); Vector3d LinearVelJacSeg = LinearJacSeg * dqSeg; Vector3d LinearAccJacSeg = LinearJacSeg * ddqSeg + LinearJacDerivSeg * dqSeg; LinearJacobian LinearJac = skel->getLinearJacobian(bn, refFrame); LinearJacobian LinearJacDeriv = skel->getLinearJacobianDeriv(bn, refFrame); Vector3d LinearVelJac = LinearJac * dq; Vector3d LinearAccJac = LinearJac * ddq + LinearJacDeriv * dq; bool linearVelSegEqual = equals(LinearVelFk, LinearVelJacSeg, tolerance); bool linearVelEqual = equals(LinearVelFk, LinearVelJac, tolerance); EXPECT_TRUE(linearVelSegEqual); EXPECT_TRUE(linearVelEqual); if (!linearVelSegEqual) printComparisonError( "linear velocity (seg)", bn->getName(), refFrame->getName(), LinearVelFk, LinearVelJacSeg); if (!linearVelEqual) printComparisonError( "linear velocity", bn->getName(), refFrame->getName(), LinearVelFk, LinearVelJac); bool linearAccSegEqual = equals(LinearAccFk, LinearAccJacSeg, tolerance); bool linearAccEqual = equals(LinearAccFk, LinearAccJac, tolerance); EXPECT_TRUE(linearAccSegEqual); EXPECT_TRUE(linearAccEqual); if (!linearAccSegEqual) printComparisonError( "linear acceleration (seg)", bn->getName(), refFrame->getName(), LinearAccFk, LinearAccJacSeg); if (!linearAccEqual) printComparisonError( "linear acceleration", bn->getName(), refFrame->getName(), LinearAccFk, LinearAccJac); //-- Angular Jacobian tests Vector3d AngularVelFk = bn->getAngularVelocity(Frame::World(), refFrame); Vector3d AngularAccFk = bn->getAngularAcceleration(Frame::World(), refFrame); AngularJacobian AngularJacSeg = bn->getAngularJacobian(refFrame); AngularJacobian AngularJacDerivSeg = bn->getAngularJacobianDeriv(refFrame); Vector3d AngularVelJacSeg = AngularJacSeg * dqSeg; Vector3d AngularAccJacSeg = AngularJacSeg * ddqSeg + AngularJacDerivSeg * dqSeg; AngularJacobian AngularJac = skel->getAngularJacobian(bn, refFrame); AngularJacobian AngularJacDeriv = skel->getAngularJacobianDeriv(bn, refFrame); Vector3d AngularVelJac = AngularJac * dq; Vector3d AngularAccJac = AngularJac * ddq + AngularJacDeriv * dq; bool angularVelSegEqual = equals(AngularVelFk, AngularVelJacSeg, tolerance); bool angularVelEqual = equals(AngularVelFk, AngularVelJac, tolerance); EXPECT_TRUE(angularVelSegEqual); EXPECT_TRUE(angularVelEqual); if (!angularVelSegEqual) printComparisonError( "angular velocity (seg)", bn->getName(), refFrame->getName(), AngularVelFk, AngularVelJacSeg); if (!angularVelEqual) printComparisonError( "angular velocity", bn->getName(), refFrame->getName(), AngularVelFk, AngularVelJac); bool angularAccSegEqual = equals(AngularAccFk, AngularAccJacSeg, tolerance); bool angularAccEqual = equals(AngularAccFk, AngularAccJac, tolerance); EXPECT_TRUE(angularAccSegEqual); EXPECT_TRUE(angularAccEqual); if (!angularAccSegEqual) printComparisonError( "angular acceleration (seg)", bn->getName(), refFrame->getName(), AngularAccFk, AngularAccJacSeg); if (!angularAccEqual) printComparisonError( "angular acceleration", bn->getName(), refFrame->getName(), AngularAccFk, AngularAccJac); } //============================================================================== void compareBodyNodeFkToJacobian( const BodyNode* bn, const Frame* refFrame, const Eigen::Vector3d& offset, double tolerance) { using math::AngularJacobian; using math::Jacobian; using math::LinearJacobian; ConstSkeletonPtr skel = bn->getSkeleton(); VectorXd dq = skel->getVelocities(); VectorXd ddq = skel->getAccelerations(); const std::vector& coords = bn->getDependentGenCoordIndices(); VectorXd dqSeg = skel->getVelocities(coords); VectorXd ddqSeg = skel->getAccelerations(coords); //-- Spatial Jacobian tests -------------------------------------------------- Vector6d SpatialVelFk = bn->getSpatialVelocity(offset, Frame::World(), refFrame); Vector6d SpatialAccFk = bn->getSpatialAcceleration(offset, Frame::World(), refFrame); Jacobian SpatialJacSeg = bn->getJacobian(offset, refFrame); Jacobian SpatialJacDerivSeg = bn->getJacobianSpatialDeriv(offset, refFrame); Vector6d SpatialVelJacSeg = SpatialJacSeg * dqSeg; Vector6d SpatialAccJacSeg = SpatialJacSeg * ddqSeg + SpatialJacDerivSeg * dqSeg; Jacobian SpatialJac = skel->getJacobian(bn, offset, refFrame); Jacobian SpatialJacDeriv = skel->getJacobianSpatialDeriv(bn, offset, refFrame); Vector6d SpatialVelJac = SpatialJac * dq; Vector6d SpatialAccJac = SpatialJac * ddq + SpatialJacDeriv * dq; bool spatialVelSegEqual = equals(SpatialVelFk, SpatialVelJacSeg, tolerance); bool spatialVelEqual = equals(SpatialVelFk, SpatialVelJac, tolerance); EXPECT_TRUE(spatialVelSegEqual); EXPECT_TRUE(spatialVelEqual); if (!spatialVelSegEqual) printComparisonError( "spatial velocity w/ offset (seg)", bn->getName(), refFrame->getName(), SpatialVelFk, SpatialVelJacSeg); if (!spatialVelEqual) printComparisonError( "spatial velocity w/ offset", bn->getName(), refFrame->getName(), SpatialVelFk, SpatialVelJac); bool spatialAccSegEqual = equals(SpatialAccFk, SpatialAccJacSeg, tolerance); bool spatialAccEqual = equals(SpatialAccFk, SpatialAccJac, tolerance); EXPECT_TRUE(spatialAccSegEqual); EXPECT_TRUE(spatialAccEqual); if (!spatialAccSegEqual) printComparisonError( "spatial acceleration w/ offset (seg)", bn->getName(), refFrame->getName(), SpatialAccFk, SpatialAccJacSeg); if (!spatialAccEqual) printComparisonError( "spatial acceleration w/ offset", bn->getName(), refFrame->getName(), SpatialAccFk, SpatialAccJac); //-- Linear Jacobian tests --------------------------------------------------- Vector3d LinearVelFk = bn->getLinearVelocity(offset, Frame::World(), refFrame); Vector3d LinearAccFk = bn->getLinearAcceleration(offset, Frame::World(), refFrame); LinearJacobian LinearJacSeg = bn->getLinearJacobian(offset, refFrame); LinearJacobian LinearJacDerivSeg = bn->getLinearJacobianDeriv(offset, refFrame); Vector3d LinearVelJacSeg = LinearJacSeg * dqSeg; Vector3d LinearAccJacSeg = LinearJacSeg * ddqSeg + LinearJacDerivSeg * dqSeg; LinearJacobian LinearJac = skel->getLinearJacobian(bn, offset, refFrame); LinearJacobian LinearJacDeriv = skel->getLinearJacobianDeriv(bn, offset, refFrame); Vector3d LinearVelJac = LinearJac * dq; Vector3d LinearAccJac = LinearJac * ddq + LinearJacDeriv * dq; bool linearVelSegEqual = equals(LinearVelFk, LinearVelJacSeg, tolerance); bool linearVelEqual = equals(LinearVelFk, LinearVelJac, tolerance); EXPECT_TRUE(linearVelSegEqual); EXPECT_TRUE(linearVelEqual); if (!linearVelSegEqual) printComparisonError( "linear velocity w/ offset (seg)", bn->getName(), refFrame->getName(), LinearVelFk, LinearVelJacSeg); if (!linearVelEqual) printComparisonError( "linear velocity w/ offset", bn->getName(), refFrame->getName(), LinearVelFk, LinearVelJac); bool linearAccSegEqual = equals(LinearAccFk, LinearAccJacSeg, tolerance); bool linearAccEqual = equals(LinearAccFk, LinearAccJac, tolerance); EXPECT_TRUE(linearAccSegEqual); EXPECT_TRUE(linearAccEqual); if (!linearAccSegEqual) printComparisonError( "linear acceleration w/ offset (seg)", bn->getName(), refFrame->getName(), LinearAccFk, LinearAccJacSeg); if (!linearAccEqual) printComparisonError( "linear acceleration w/ offset", bn->getName(), refFrame->getName(), LinearAccFk, LinearAccJac); //-- Angular Jacobian tests -------------------------------------------------- Vector3d AngularVelFk = bn->getAngularVelocity(Frame::World(), refFrame); Vector3d AngularAccFk = bn->getAngularAcceleration(Frame::World(), refFrame); AngularJacobian AngularJacSeg = bn->getAngularJacobian(refFrame); AngularJacobian AngularJacDerivSeg = bn->getAngularJacobianDeriv(refFrame); Vector3d AngularVelJacSeg = AngularJacSeg * dqSeg; Vector3d AngularAccJacSeg = AngularJacSeg * ddqSeg + AngularJacDerivSeg * dqSeg; AngularJacobian AngularJac = skel->getAngularJacobian(bn, refFrame); AngularJacobian AngularJacDeriv = skel->getAngularJacobianDeriv(bn, refFrame); Vector3d AngularVelJac = AngularJac * dq; Vector3d AngularAccJac = AngularJac * ddq + AngularJacDeriv * dq; bool angularVelSegEqual = equals(AngularVelFk, AngularVelJacSeg, tolerance); bool angularVelEqual = equals(AngularVelFk, AngularVelJac, tolerance); EXPECT_TRUE(angularVelSegEqual); EXPECT_TRUE(angularVelEqual); if (!angularVelSegEqual) printComparisonError( "angular velocity w/ offset (seg)", bn->getName(), refFrame->getName(), AngularVelFk, AngularVelJacSeg); if (!angularVelEqual) printComparisonError( "angular velocity w/ offset", bn->getName(), refFrame->getName(), AngularVelFk, AngularVelJac); bool angularAccSegEqual = equals(AngularAccFk, AngularAccJacSeg, tolerance); bool angularAccEqual = equals(AngularAccFk, AngularAccJac, tolerance); EXPECT_TRUE(angularAccSegEqual); if (!angularAccSegEqual) printComparisonError( "angular acceleration w/ offset (seg)", bn->getName(), refFrame->getName(), AngularAccFk, AngularAccJacSeg); EXPECT_TRUE(angularAccEqual); if (!angularAccEqual) printComparisonError( "angular acceleration w/ offset", bn->getName(), refFrame->getName(), AngularAccFk, AngularAccJac); } //============================================================================== template void printComparisonError( const std::string& _comparison, const std::string& _nameBN, const std::string& _nameBNRelativeTo, const std::string& _frame, const T& fk, const T& jac) { std::cout << "Disagreement between FK and relative Jacobian results for " << _comparison << " of '" << _nameBN << "' relative to '" << _nameBNRelativeTo << "' with a reference Frame of '" << _frame << "'\n" << "FK: " << fk.transpose() << "\n" << "Jac: " << jac.transpose() << "\n"; } //============================================================================== void compareBodyNodeFkToJacobianRelative( const JacobianNode* bn, const JacobianNode* relativeTo, const Frame* refFrame, double tolerance) { using math::AngularJacobian; using math::Jacobian; using math::LinearJacobian; assert(bn->getSkeleton() == relativeTo->getSkeleton()); auto skel = bn->getSkeleton(); VectorXd dq = skel->getVelocities(); VectorXd ddq = skel->getAccelerations(); //-- Spatial Jacobian tests -------------------------------------------------- Vector6d SpatialVelFk = bn->getSpatialVelocity(relativeTo, refFrame); Vector6d SpatialAccFk = bn->getSpatialAcceleration(relativeTo, refFrame); Jacobian SpatialJac = skel->getJacobian(bn, relativeTo, refFrame); Jacobian SpatialJacDeriv = skel->getJacobianSpatialDeriv(bn, relativeTo, refFrame); Vector6d SpatialVelJac = SpatialJac * dq; Vector6d SpatialAccJac = SpatialJac * ddq + SpatialJacDeriv * dq; bool spatialVelEqual = equals(SpatialVelFk, SpatialVelJac, tolerance); EXPECT_TRUE(spatialVelEqual); if (!spatialVelEqual) { printComparisonError( "spatial velocity", bn->getName(), relativeTo->getName(), refFrame->getName(), SpatialVelFk, SpatialVelJac); } bool spatialAccEqual = equals(SpatialAccFk, SpatialAccJac, tolerance); EXPECT_TRUE(spatialAccEqual); if (!spatialAccEqual) { printComparisonError( "spatial acceleration", bn->getName(), relativeTo->getName(), refFrame->getName(), SpatialAccFk, SpatialAccJac); } //-- Linear Jacobian tests -------------------------------------------------- Vector3d LinearVelFk = bn->getLinearVelocity(relativeTo, refFrame); LinearJacobian LinearJac = skel->getLinearJacobian(bn, relativeTo, refFrame); Vector3d LinearVelJac = LinearJac * dq; bool linearVelEqual = equals(LinearVelFk, LinearVelJac, tolerance); EXPECT_TRUE(linearVelEqual); if (!linearVelEqual) { printComparisonError( "linear velocity", bn->getName(), relativeTo->getName(), refFrame->getName(), LinearVelFk, LinearVelJac); } //-- Angular Jacobian tests -------------------------------------------------- Vector3d AngularVelFk = bn->getAngularVelocity(relativeTo, refFrame); AngularJacobian AngularJac = skel->getAngularJacobian(bn, relativeTo, refFrame); Vector3d AngularVelJac = AngularJac * dq; bool angularVelEqual = equals(AngularVelFk, AngularVelJac, tolerance); EXPECT_TRUE(angularVelEqual); if (!angularVelEqual) { printComparisonError( "angular velocity", bn->getName(), relativeTo->getName(), refFrame->getName(), AngularVelFk, AngularVelJac); } } //============================================================================== void compareBodyNodeFkToJacobianRelative( const JacobianNode* bn, const Eigen::Vector3d& _offset, const JacobianNode* relativeTo, const Frame* refFrame, double tolerance) { using math::AngularJacobian; using math::Jacobian; using math::LinearJacobian; assert(bn->getSkeleton() == relativeTo->getSkeleton()); auto skel = bn->getSkeleton(); VectorXd dq = skel->getVelocities(); VectorXd ddq = skel->getAccelerations(); //-- Spatial Jacobian tests -------------------------------------------------- Vector6d SpatialVelFk = bn->getSpatialVelocity(_offset, relativeTo, refFrame); Vector6d SpatialAccFk = bn->getSpatialAcceleration(_offset, relativeTo, refFrame); Jacobian SpatialJac = skel->getJacobian(bn, _offset, relativeTo, refFrame); Jacobian SpatialJacDeriv = skel->getJacobianSpatialDeriv(bn, _offset, relativeTo, refFrame); Vector6d SpatialVelJac = SpatialJac * dq; Vector6d SpatialAccJac = SpatialJac * ddq + SpatialJacDeriv * dq; bool spatialVelEqual = equals(SpatialVelFk, SpatialVelJac, tolerance); EXPECT_TRUE(spatialVelEqual); if (!spatialVelEqual) { printComparisonError( "spatial velocity w/ offset", bn->getName(), relativeTo->getName(), refFrame->getName(), SpatialVelFk, SpatialVelJac); } bool spatialAccEqual = equals(SpatialAccFk, SpatialAccJac, tolerance); EXPECT_TRUE(spatialAccEqual); if (!spatialAccEqual) { printComparisonError( "spatial acceleration w/ offset", bn->getName(), relativeTo->getName(), refFrame->getName(), SpatialAccFk, SpatialAccJac); } //-- Linear Jacobian tests -------------------------------------------------- Vector3d LinearVelFk = bn->getLinearVelocity(_offset, relativeTo, refFrame); LinearJacobian LinearJac = skel->getLinearJacobian(bn, _offset, relativeTo, refFrame); Vector3d LinearVelJac = LinearJac * dq; bool linearVelEqual = equals(LinearVelFk, LinearVelJac, tolerance); EXPECT_TRUE(linearVelEqual); if (!linearVelEqual) { printComparisonError( "linear velocity w/ offset", bn->getName(), relativeTo->getName(), refFrame->getName(), LinearVelFk, LinearVelJac); } } //============================================================================== void DynamicsTest::testJacobians(const common::Uri& uri) { using namespace std; using namespace Eigen; using namespace dart; using namespace math; using namespace dynamics; using namespace simulation; using namespace utils; //----------------------------- Settings ------------------------------------- const double TOLERANCE = 1.0e-6; #ifndef NDEBUG // Debug mode int nTestItr = 2; #else int nTestItr = 5; #endif double qLB = -0.5 * constantsd::pi(); double qUB = 0.5 * constantsd::pi(); double dqLB = -0.5 * constantsd::pi(); double dqUB = 0.5 * constantsd::pi(); double ddqLB = -0.5 * constantsd::pi(); double ddqUB = 0.5 * constantsd::pi(); Vector3d gravity(0.0, -9.81, 0.0); // load skeleton WorldPtr world = SkelParser::readWorld(uri); assert(world != nullptr); world->setGravity(gravity); //------------------------------ Tests --------------------------------------- for (std::size_t i = 0; i < world->getNumSkeletons(); ++i) { SkeletonPtr skeleton = world->getSkeleton(i); assert(skeleton != nullptr); int dof = skeleton->getNumDofs(); for (int j = 0; j < nTestItr; ++j) { // For the second half of the tests, scramble up the Skeleton if (j > std::ceil(nTestItr / 2)) { SkeletonPtr copy = skeleton->cloneSkeleton(); std::size_t maxNode = skeleton->getNumBodyNodes() - 1; BodyNode* bn1 = skeleton->getBodyNode( math::Random::uniform(0, maxNode)); BodyNode* bn2 = skeleton->getBodyNode( math::Random::uniform(0, maxNode)); if (bn1 != bn2) { BodyNode* child = bn1->descendsFrom(bn2) ? bn1 : bn2; BodyNode* parent = child == bn1 ? bn2 : bn1; child->moveTo(parent); } EXPECT_TRUE(skeleton->getNumBodyNodes() == copy->getNumBodyNodes()); EXPECT_TRUE(skeleton->getNumDofs() == copy->getNumDofs()); } // Generate a random state VectorXd q = VectorXd(dof); VectorXd dq = VectorXd(dof); VectorXd ddq = VectorXd(dof); for (int k = 0; k < dof; ++k) { q[k] = math::Random::uniform(qLB, qUB); dq[k] = math::Random::uniform(dqLB, dqUB); ddq[k] = math::Random::uniform(ddqLB, ddqUB); } skeleton->setPositions(q); skeleton->setVelocities(dq); skeleton->setAccelerations(ddq); randomizeRefFrames(); // For each body node for (std::size_t k = 0; k < skeleton->getNumBodyNodes(); ++k) { const BodyNode* bn = skeleton->getBodyNode(k); // Compare results using the World reference Frame compareBodyNodeFkToJacobian(bn, Frame::World(), TOLERANCE); compareBodyNodeFkToJacobian( bn, Frame::World(), bn->getLocalCOM(), TOLERANCE); compareBodyNodeFkToJacobian( bn, Frame::World(), Random::uniform(-10, 10), TOLERANCE); // Compare results using this BodyNode's own reference Frame compareBodyNodeFkToJacobian(bn, bn, TOLERANCE); compareBodyNodeFkToJacobian(bn, bn, bn->getLocalCOM(), TOLERANCE); compareBodyNodeFkToJacobian( bn, bn, Random::uniform(-10, 10), TOLERANCE); // Compare results using the randomized reference Frames for (std::size_t r = 0; r < refFrames.size(); ++r) { compareBodyNodeFkToJacobian(bn, refFrames[r], TOLERANCE); compareBodyNodeFkToJacobian( bn, refFrames[r], bn->getLocalCOM(), TOLERANCE); compareBodyNodeFkToJacobian( bn, refFrames[r], Random::uniform(-10, 10), TOLERANCE); } // -- Relative Jacobian tests compareBodyNodeFkToJacobianRelative(bn, bn, Frame::World(), TOLERANCE); #ifndef NDEBUG // Debug mode if (skeleton->getNumBodyNodes() == 0u) continue; for (std::size_t l = skeleton->getNumBodyNodes() - 1; l < skeleton->getNumBodyNodes(); ++l) #else for (std::size_t l = 0; l < skeleton->getNumBodyNodes(); ++l) #endif { const BodyNode* relativeTo = skeleton->getBodyNode(l); compareBodyNodeFkToJacobianRelative( bn, relativeTo, Frame::World(), TOLERANCE); compareBodyNodeFkToJacobianRelative( bn, bn->getLocalCOM(), relativeTo, Frame::World(), TOLERANCE); compareBodyNodeFkToJacobianRelative( bn, Random::uniform(-10, 10), relativeTo, Frame::World(), TOLERANCE); compareBodyNodeFkToJacobianRelative(bn, relativeTo, bn, TOLERANCE); compareBodyNodeFkToJacobianRelative( bn, bn->getLocalCOM(), relativeTo, bn, TOLERANCE); compareBodyNodeFkToJacobianRelative( bn, Random::uniform(-10, 10), relativeTo, bn, TOLERANCE); compareBodyNodeFkToJacobianRelative( bn, relativeTo, relativeTo, TOLERANCE); compareBodyNodeFkToJacobianRelative( bn, bn->getLocalCOM(), relativeTo, relativeTo, TOLERANCE); compareBodyNodeFkToJacobianRelative( bn, Random::uniform(-10, 10), relativeTo, relativeTo, TOLERANCE); for (std::size_t r = 0; r < refFrames.size(); ++r) { compareBodyNodeFkToJacobianRelative( bn, relativeTo, refFrames[r], TOLERANCE); compareBodyNodeFkToJacobianRelative( bn, bn->getLocalCOM(), relativeTo, refFrames[r], TOLERANCE); compareBodyNodeFkToJacobianRelative( bn, Random::uniform(-10, 10), relativeTo, refFrames[r], TOLERANCE); } } } } } } //============================================================================== void DynamicsTest::testFiniteDifferenceGeneralizedCoordinates( const common::Uri& uri) { using namespace std; using namespace Eigen; using namespace dart; using namespace math; using namespace dynamics; using namespace simulation; using namespace utils; //----------------------------- Settings ------------------------------------- #ifndef NDEBUG // Debug mode int nRandomItr = 2; #else int nRandomItr = 10; #endif double qLB = -0.5 * constantsd::pi(); double qUB = 0.5 * constantsd::pi(); double dqLB = -0.3 * constantsd::pi(); double dqUB = 0.3 * constantsd::pi(); double ddqLB = -0.1 * constantsd::pi(); double ddqUB = 0.1 * constantsd::pi(); Vector3d gravity(0.0, -9.81, 0.0); double timeStep = 1e-3; double TOLERANCE = 5e-4; // load skeleton WorldPtr world = SkelParser::readWorld(uri); assert(world != nullptr); world->setGravity(gravity); world->setTimeStep(timeStep); //------------------------------ Tests --------------------------------------- for (std::size_t i = 0; i < world->getNumSkeletons(); ++i) { SkeletonPtr skeleton = world->getSkeleton(i); assert(skeleton != nullptr); int dof = skeleton->getNumDofs(); for (int j = 0; j < nRandomItr; ++j) { // Generate a random state and ddq VectorXd q0 = VectorXd(dof); VectorXd dq0 = VectorXd(dof); VectorXd ddq0 = VectorXd(dof); for (int k = 0; k < dof; ++k) { q0[k] = math::Random::uniform(qLB, qUB); dq0[k] = math::Random::uniform(dqLB, dqUB); ddq0[k] = math::Random::uniform(ddqLB, ddqUB); } skeleton->setPositions(q0); skeleton->setVelocities(dq0); skeleton->setAccelerations(ddq0); skeleton->integratePositions(timeStep); VectorXd q1 = skeleton->getPositions(); skeleton->integrateVelocities(timeStep); VectorXd dq1 = skeleton->getVelocities(); skeleton->integratePositions(timeStep); VectorXd q2 = skeleton->getPositions(); skeleton->integrateVelocities(timeStep); VectorXd dq2 = skeleton->getVelocities(); VectorXd dq0FD = skeleton->getPositionDifferences(q1, q0) / timeStep; VectorXd dq1FD = skeleton->getPositionDifferences(q2, q1) / timeStep; VectorXd ddqFD1 = skeleton->getVelocityDifferences(dq1FD, dq0FD) / timeStep; VectorXd ddqFD2 = skeleton->getVelocityDifferences(dq2, dq1) / timeStep; EXPECT_TRUE(equals(dq0, dq0FD, TOLERANCE)) << "dq0 : " << dq0.transpose() << "\ndq0FD: " << dq0FD.transpose(); EXPECT_TRUE(equals(dq1, dq1FD, TOLERANCE)) << "dq1 : " << dq1.transpose() << "\ndq1FD: " << dq1FD.transpose(); EXPECT_TRUE(equals(ddq0, ddqFD1, 10 * TOLERANCE)) << "ddq0 : " << ddq0.transpose() << "\nddqFD1: " << ddqFD1.transpose(); EXPECT_TRUE(equals(ddq0, ddqFD2, 10 * TOLERANCE)) << "ddq0 : " << ddq0.transpose() << "\nddqFD2: " << ddqFD2.transpose() << std::endl; } } } //============================================================================== void DynamicsTest::testFiniteDifferenceBodyNodeVelocity(const common::Uri& uri) { using namespace std; using namespace Eigen; using namespace dart; using namespace math; using namespace dynamics; using namespace simulation; using namespace utils; //----------------------------- Settings ------------------------------------- #ifndef NDEBUG // Debug mode int nRandomItr = 2; std::size_t numSteps = 1e+1; #else int nRandomItr = 10; std::size_t numSteps = 1e+3; #endif double qLB = -0.5 * constantsd::pi(); double qUB = 0.5 * constantsd::pi(); double dqLB = -0.5 * constantsd::pi(); double dqUB = 0.5 * constantsd::pi(); double ddqLB = -0.5 * constantsd::pi(); double ddqUB = 0.5 * constantsd::pi(); Vector3d gravity(0.0, -9.81, 0.0); double timeStep = 1.0e-6; const double tol = timeStep * 1e+2; // load skeleton WorldPtr world = SkelParser::readWorld(uri); assert(world != nullptr); world->setGravity(gravity); world->setTimeStep(timeStep); //------------------------------ Tests --------------------------------------- for (int i = 0; i < nRandomItr; ++i) { for (std::size_t j = 0; j < world->getNumSkeletons(); ++j) { SkeletonPtr skeleton = world->getSkeleton(j); EXPECT_NE(skeleton, nullptr); std::size_t dof = skeleton->getNumDofs(); std::size_t numBodies = skeleton->getNumBodyNodes(); // Generate random states VectorXd q = Random::uniform(dof, qLB, qUB); VectorXd dq = Random::uniform(dof, dqLB, dqUB); VectorXd ddq = Random::uniform(dof, ddqLB, ddqUB); skeleton->setPositions(q); skeleton->setVelocities(dq); skeleton->setAccelerations(ddq); common::aligned_map Tmap; for (auto k = 0u; k < numBodies; ++k) { auto body = skeleton->getBodyNode(k); Tmap[body] = body->getTransform(); } for (std::size_t k = 0; k < numSteps; ++k) { skeleton->integrateVelocities(skeleton->getTimeStep()); skeleton->integratePositions(skeleton->getTimeStep()); for (std::size_t l = 0; l < skeleton->getNumBodyNodes(); ++l) { BodyNodePtr body = skeleton->getBodyNode(l); Isometry3d T1 = Tmap[body]; Isometry3d T2 = body->getTransform(); Vector6d V_diff = math::logMap(T1.inverse() * T2) / timeStep; Vector6d V_actual = body->getSpatialVelocity(); bool checkSpatialVelocity = equals(V_diff, V_actual, tol); EXPECT_TRUE(checkSpatialVelocity); if (!checkSpatialVelocity) { std::cout << "[" << body->getName() << "]" << std::endl; std::cout << "V_diff : " << V_diff.transpose() << std::endl; std::cout << "V_actual : " << V_actual.transpose() << std::endl; std::cout << std::endl; } Tmap[body] = body->getTransform(); } } } } } //============================================================================== void DynamicsTest::testFiniteDifferenceBodyNodeAcceleration( const common::Uri& uri) { using namespace std; using namespace Eigen; using namespace dart; using namespace math; using namespace dynamics; using namespace simulation; using namespace utils; //----------------------------- Settings ------------------------------------- const double TOLERANCE = 1.0e-2; #ifndef NDEBUG // Debug mode int nRandomItr = 2; #else int nRandomItr = 10; #endif double qLB = -0.5 * constantsd::pi(); double qUB = 0.5 * constantsd::pi(); double dqLB = -0.5 * constantsd::pi(); double dqUB = 0.5 * constantsd::pi(); double ddqLB = -0.5 * constantsd::pi(); double ddqUB = 0.5 * constantsd::pi(); Vector3d gravity(0.0, -9.81, 0.0); double timeStep = 1.0e-6; // load skeleton WorldPtr world = SkelParser::readWorld(uri); assert(world != nullptr); world->setGravity(gravity); world->setTimeStep(timeStep); //------------------------------ Tests --------------------------------------- for (std::size_t i = 0; i < world->getNumSkeletons(); ++i) { SkeletonPtr skeleton = world->getSkeleton(i); assert(skeleton != nullptr); int dof = skeleton->getNumDofs(); for (int j = 0; j < nRandomItr; ++j) { // Generate a random state and ddq VectorXd q = VectorXd(dof); VectorXd dq = VectorXd(dof); VectorXd ddq = VectorXd(dof); for (int k = 0; k < dof; ++k) { q[k] = math::Random::uniform(qLB, qUB); dq[k] = math::Random::uniform(dqLB, dqUB); ddq[k] = math::Random::uniform(ddqLB, ddqUB); } // For each body node for (std::size_t k = 0; k < skeleton->getNumBodyNodes(); ++k) { BodyNode* bn = skeleton->getBodyNode(k); // Calculation of velocities and Jacobian at k-th time step skeleton->setPositions(q); skeleton->setVelocities(dq); skeleton->setAccelerations(ddq); Vector3d BodyLinVel1 = bn->getLinearVelocity(Frame::World(), bn); Vector3d BodyAngVel1 = bn->getAngularVelocity(Frame::World(), bn); Vector3d WorldLinVel1 = bn->getLinearVelocity(); Vector3d WorldAngVel1 = bn->getAngularVelocity(); // Isometry3d T1 = bn->getTransform(); // Get accelerations and time derivatives of Jacobians at k-th time step Vector3d BodyLinAcc1 = bn->getSpatialAcceleration().tail<3>(); Vector3d BodyAngAcc1 = bn->getSpatialAcceleration().head<3>(); Vector3d WorldLinAcc1 = bn->getLinearAcceleration(); Vector3d WorldAngAcc1 = bn->getAngularAcceleration(); // Calculation of velocities and Jacobian at (k+1)-th time step skeleton->integrateVelocities(skeleton->getTimeStep()); skeleton->integratePositions(skeleton->getTimeStep()); Vector3d BodyLinVel2 = bn->getLinearVelocity(Frame::World(), bn); Vector3d BodyAngVel2 = bn->getAngularVelocity(Frame::World(), bn); Vector3d WorldLinVel2 = bn->getLinearVelocity(); Vector3d WorldAngVel2 = bn->getAngularVelocity(); // Isometry3d T2 = bn->getTransform(); // Get accelerations and time derivatives of Jacobians at k-th time step Vector3d BodyLinAcc2 = bn->getSpatialAcceleration().tail<3>(); Vector3d BodyAngAcc2 = bn->getSpatialAcceleration().head<3>(); Vector3d WorldLinAcc2 = bn->getLinearAcceleration(); Vector3d WorldAngAcc2 = bn->getAngularAcceleration(); // Calculation of approximated accelerations Vector3d BodyLinAccApprox = (BodyLinVel2 - BodyLinVel1) / timeStep; Vector3d BodyAngAccApprox = (BodyAngVel2 - BodyAngVel1) / timeStep; Vector3d WorldLinAccApprox = (WorldLinVel2 - WorldLinVel1) / timeStep; Vector3d WorldAngAccApprox = (WorldAngVel2 - WorldAngVel1) / timeStep; // Comparing two velocities EXPECT_TRUE(equals(BodyLinAcc1, BodyLinAccApprox, TOLERANCE)); EXPECT_TRUE(equals(BodyAngAcc1, BodyAngAccApprox, TOLERANCE)); EXPECT_TRUE(equals(BodyLinAcc2, BodyLinAccApprox, TOLERANCE)); EXPECT_TRUE(equals(BodyAngAcc2, BodyAngAccApprox, TOLERANCE)); EXPECT_TRUE(equals(WorldLinAcc1, WorldLinAccApprox, TOLERANCE)); EXPECT_TRUE(equals(WorldAngAcc1, WorldAngAccApprox, TOLERANCE)); EXPECT_TRUE(equals(WorldLinAcc2, WorldLinAccApprox, TOLERANCE)); EXPECT_TRUE(equals(WorldAngAcc2, WorldAngAccApprox, TOLERANCE)); // Debugging code if (!equals(BodyLinAcc1, BodyLinAccApprox, TOLERANCE)) { cout << "BodyLinAcc1 :" << BodyLinAcc1.transpose() << endl; cout << "BodyLinAccApprox:" << BodyLinAccApprox.transpose() << endl; } if (!equals(BodyAngAcc1, BodyAngAccApprox, TOLERANCE)) { cout << "BodyAngAcc1 :" << BodyAngAcc1.transpose() << endl; cout << "BodyAngAccApprox:" << BodyAngAccApprox.transpose() << endl; } if (!equals(BodyLinAcc2, BodyLinAccApprox, TOLERANCE)) { cout << "BodyLinAcc2 :" << BodyLinAcc2.transpose() << endl; cout << "BodyLinAccApprox:" << BodyLinAccApprox.transpose() << endl; } if (!equals(BodyAngAcc2, BodyAngAccApprox, TOLERANCE)) { cout << "BodyAngAcc2 :" << BodyAngAcc2.transpose() << endl; cout << "BodyAngAccApprox:" << BodyAngAccApprox.transpose() << endl; } if (!equals(WorldLinAcc1, WorldLinAccApprox, TOLERANCE)) { cout << "WorldLinAcc1 :" << WorldLinAcc1.transpose() << endl; cout << "WorldLinAccApprox:" << WorldLinAccApprox.transpose() << endl; } if (!equals(WorldAngAcc1, WorldAngAccApprox, TOLERANCE)) { cout << "WorldAngAcc1 :" << WorldAngAcc1.transpose() << endl; cout << "WorldAngAccApprox:" << WorldAngAccApprox.transpose() << endl; } if (!equals(WorldLinAcc2, WorldLinAccApprox, TOLERANCE)) { cout << "WorldLinAcc2 :" << WorldLinAcc2.transpose() << endl; cout << "WorldLinAccApprox:" << WorldLinAccApprox.transpose() << endl; } if (!equals(WorldAngAcc2, WorldAngAccApprox, TOLERANCE)) { cout << "WorldAngAcc2 :" << WorldAngAcc2.transpose() << endl; cout << "WorldAngAccApprox:" << WorldAngAccApprox.transpose() << endl; } } } } } //============================================================================== void testForwardKinematicsSkeleton(const dynamics::SkeletonPtr& skel) { #ifndef NDEBUG // Debug mode std::size_t nRandomItr = 1e+1; std::size_t numSteps = 1e+1; #else std::size_t nRandomItr = 1e+2; std::size_t numSteps = 1e+2; #endif double qLB = -0.5 * constantsd::pi(); double qUB = 0.5 * constantsd::pi(); double dqLB = -0.3 * constantsd::pi(); double dqUB = 0.3 * constantsd::pi(); double ddqLB = -0.1 * constantsd::pi(); double ddqUB = 0.1 * constantsd::pi(); double timeStep = 1e-6; EXPECT_NE(skel, nullptr); skel->setTimeStep(timeStep); auto dof = skel->getNumDofs(); auto numBodies = skel->getNumBodyNodes(); Eigen::VectorXd q; Eigen::VectorXd dq; Eigen::VectorXd ddq; common::aligned_map Tmap; common::aligned_map Vmap; common::aligned_map dVmap; for (auto j = 0u; j < numBodies; ++j) { auto body = skel->getBodyNode(j); Tmap[body] = Eigen::Isometry3d::Identity(); Vmap[body] = Eigen::Vector6d::Zero(); dVmap[body] = Eigen::Vector6d::Zero(); } for (auto i = 0u; i < nRandomItr; ++i) { q = Random::uniform(dof, qLB, qUB); dq = Random::uniform(dof, dqLB, dqUB); ddq = Random::uniform(dof, ddqLB, ddqUB); skel->setPositions(q); skel->setVelocities(dq); skel->setAccelerations(ddq); for (auto j = 0u; j < numSteps; ++j) { for (auto k = 0u; k < numBodies; ++k) { auto body = skel->getBodyNode(k); auto joint = skel->getJoint(k); auto parentBody = body->getParentBodyNode(); Eigen::MatrixXd S = joint->getRelativeJacobian(); Eigen::MatrixXd dS = joint->getRelativeJacobianTimeDeriv(); Eigen::VectorXd jointQ = joint->getPositions(); Eigen::VectorXd jointDQ = joint->getVelocities(); Eigen::VectorXd jointDDQ = joint->getAccelerations(); Eigen::Isometry3d relT = body->getRelativeTransform(); Eigen::Vector6d relV = S * jointDQ; Eigen::Vector6d relDV = dS * jointDQ + S * jointDDQ; if (parentBody) { Tmap[body] = Tmap[parentBody] * relT; Vmap[body] = math::AdInvT(relT, Vmap[parentBody]) + relV; dVmap[body] = math::AdInvT(relT, dVmap[parentBody]) + math::ad(Vmap[body], S * jointDQ) + relDV; } else { Tmap[body] = relT; Vmap[body] = relV; dVmap[body] = relDV; } bool checkT = equals(body->getTransform().matrix(), Tmap[body].matrix()); bool checkV = equals(body->getSpatialVelocity(), Vmap[body]); bool checkDV = equals(body->getSpatialAcceleration(), dVmap[body]); EXPECT_TRUE(checkT); EXPECT_TRUE(checkV); EXPECT_TRUE(checkDV); if (!checkT) { std::cout << "[" << body->getName() << "]" << std::endl; std::cout << "actual T : " << std::endl << body->getTransform().matrix() << std::endl; std::cout << "expected T: " << std::endl << Tmap[body].matrix() << std::endl; std::cout << std::endl; } if (!checkV) { std::cout << "[" << body->getName() << "]" << std::endl; std::cout << "actual V : " << body->getSpatialVelocity().transpose() << std::endl; std::cout << "expected V: " << Vmap[body].transpose() << std::endl; std::cout << std::endl; } if (!checkDV) { std::cout << "[" << body->getName() << "]" << std::endl; std::cout << "actual DV : " << body->getSpatialAcceleration().transpose() << std::endl; std::cout << "expected DV: " << dVmap[body].transpose() << std::endl; std::cout << std::endl; } } } } } //============================================================================== void DynamicsTest::testForwardKinematics(const common::Uri& uri) { auto world = utils::SkelParser::readWorld(uri); EXPECT_TRUE(world != nullptr); auto numSkeletons = world->getNumSkeletons(); for (auto i = 0u; i < numSkeletons; ++i) { auto skeleton = world->getSkeleton(i); testForwardKinematicsSkeleton(skeleton); } } //============================================================================== void DynamicsTest::testInverseDynamics(const common::Uri& uri) { //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeleton #ifndef NDEBUG // Debug mode const std::size_t nRandomItr = 2; #else const std::size_t nRandomItr = 100; #endif // Lower and upper bound of configuration for system const double lb = -1.0 * math::constantsd::pi(); const double ub = 1.0 * math::constantsd::pi(); // Lower and upper bound of joint damping and stiffness const double lbD = 0.0; const double ubD = 10.0; const double lbK = 0.0; const double ubK = 10.0; // Lower and upper bound of joint forces const double lbF = -10.0; const double ubF = 10.0; simulation::WorldPtr world = utils::SkelParser::readWorld(uri); ASSERT_TRUE(world != nullptr); for (std::size_t i = 0; i < world->getNumSkeletons(); ++i) { dynamics::SkeletonPtr skel = world->getSkeleton(i); const std::size_t dofs = skel->getNumDofs(); for (std::size_t j = 0; j < nRandomItr; ++j) { // Random joint stiffness and damping coefficients for (std::size_t l = 0; l < dofs; ++l) { dynamics::DegreeOfFreedom* dof = skel->getDof(l); dof->setDampingCoefficient(math::Random::uniform(lbD, ubD)); dof->setSpringStiffness(math::Random::uniform(lbK, ubK)); double lbRP = dof->getPositionLowerLimit(); double ubRP = dof->getPositionUpperLimit(); lbRP = std::max(lbRP, -math::constantsd::pi()); ubRP = std::min(ubRP, +math::constantsd::pi()); dof->setRestPosition(math::Random::uniform(lbRP, ubRP)); } // Set random states and forces const VectorXd q0 = math::Random::uniform(dofs, lb, ub); const VectorXd dq0 = math::Random::uniform(dofs, lb, ub); const VectorXd f0 = math::Random::uniform(dofs, lbF, ubF); // Forward dynamics to compute accelerations skel->setPositions(q0); skel->setVelocities(dq0); skel->setForces(f0); skel->computeForwardDynamics(); const VectorXd ddq = skel->getAccelerations(); // Test inverse dynamics to ensure the result joint forces are identical // to the forces used to compute the input accelerations skel->setAccelerations(ddq); skel->computeInverseDynamics(false, true, true); const VectorXd f = skel->getForces(); EXPECT_TRUE(equals(f, f0)); // Test forward dynamics to ensure the result joint accelerations are // identical to the accelerations used to compute the input forces skel->setForces(f); skel->computeForwardDynamics(); const VectorXd ddq2 = skel->getAccelerations(); EXPECT_TRUE(equals(ddq, ddq2)); } } } //============================================================================== void DynamicsTest::compareEquationsOfMotion(const common::Uri& uri) { using namespace std; using namespace Eigen; using namespace dart; using namespace math; using namespace dynamics; using namespace simulation; using namespace utils; //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeletons #ifndef NDEBUG // Debug mode std::size_t nRandomItr = 2; #else std::size_t nRandomItr = 100; #endif // Lower and upper bound of configuration for system double lb = -1.0 * constantsd::pi(); double ub = 1.0 * constantsd::pi(); // Lower and upper bound of joint damping and stiffness double lbD = 0.0; double ubD = 10.0; double lbK = 0.0; double ubK = 10.0; simulation::WorldPtr myWorld; //----------------------------- Tests ---------------------------------------- // Check whether multiplication of mass matrix and its inverse is identity // matrix. myWorld = utils::SkelParser::readWorld(uri); EXPECT_TRUE(myWorld != nullptr); for (std::size_t i = 0; i < myWorld->getNumSkeletons(); ++i) { dynamics::SkeletonPtr skel = myWorld->getSkeleton(i); std::size_t dof = skel->getNumDofs(); // int nBodyNodes = skel->getNumBodyNodes(); if (dof == 0) { dtmsg << "Skeleton [" << skel->getName() << "] is skipped since it has " << "0 DOF." << endl; continue; } for (std::size_t j = 0; j < nRandomItr; ++j) { // Random joint stiffness and damping coefficient for (std::size_t k = 0; k < skel->getNumBodyNodes(); ++k) { BodyNode* body = skel->getBodyNode(k); Joint* joint = body->getParentJoint(); std::size_t localDof = joint->getNumDofs(); for (std::size_t l = 0; l < localDof; ++l) { joint->setDampingCoefficient(l, Random::uniform(lbD, ubD)); joint->setSpringStiffness(l, Random::uniform(lbK, ubK)); double lbRP = joint->getPositionLowerLimit(l); double ubRP = joint->getPositionUpperLimit(l); if (lbRP < -constantsd::pi()) lbRP = -constantsd::pi(); if (ubRP > constantsd::pi()) ubRP = constantsd::pi(); joint->setRestPosition(l, Random::uniform(lbRP, ubRP)); } } // Set random states Skeleton::Configuration x = skel->getConfiguration( Skeleton::CONFIG_POSITIONS | Skeleton::CONFIG_VELOCITIES); for (auto k = 0u; k < skel->getNumDofs(); ++k) { x.mPositions[k] = Random::uniform(lb, ub); x.mVelocities[k] = Random::uniform(lb, ub); } skel->setConfiguration(x); //------------------------ Mass Matrix Test ---------------------------- // Get matrices MatrixXd M = skel->getMassMatrix(); MatrixXd M2 = getMassMatrix(skel); MatrixXd InvM = skel->getInvMassMatrix(); MatrixXd M_InvM = M * InvM; MatrixXd InvM_M = InvM * M; MatrixXd AugM = skel->getAugMassMatrix(); MatrixXd AugM2 = getAugMassMatrix(skel); MatrixXd InvAugM = skel->getInvAugMassMatrix(); MatrixXd AugM_InvAugM = AugM * InvAugM; MatrixXd InvAugM_AugM = InvAugM * AugM; MatrixXd I = MatrixXd::Identity(dof, dof); bool failure = false; // Check if the number of generalized coordinates and dimension of mass // matrix are same. EXPECT_EQ(M.rows(), (int)dof); EXPECT_EQ(M.cols(), (int)dof); // Check mass matrix EXPECT_TRUE(equals(M, M2, 1e-6)); if (!equals(M, M2, 1e-6)) { cout << "M :" << endl << M << endl << endl; cout << "M2:" << endl << M2 << endl << endl; failure = true; } // Check augmented mass matrix EXPECT_TRUE(equals(AugM, AugM2, 1e-6)); if (!equals(AugM, AugM2, 1e-6)) { cout << "AugM :" << endl << AugM << endl << endl; cout << "AugM2:" << endl << AugM2 << endl << endl; failure = true; } // Check if both of (M * InvM) and (InvM * M) are identity. EXPECT_TRUE(equals(M_InvM, I, 1e-6)); if (!equals(M_InvM, I, 1e-6)) { cout << "InvM :" << endl << InvM << endl << endl; failure = true; } EXPECT_TRUE(equals(InvM_M, I, 1e-6)); if (!equals(InvM_M, I, 1e-6)) { cout << "InvM_M:" << endl << InvM_M << endl << endl; failure = true; } // Check if both of (M * InvM) and (InvM * M) are identity. EXPECT_TRUE(equals(AugM_InvAugM, I, 1e-6)); if (!equals(AugM_InvAugM, I, 1e-6)) { cout << "AugM_InvAugM :" << endl << AugM_InvAugM << endl << endl; failure = true; } EXPECT_TRUE(equals(InvAugM_AugM, I, 1e-6)); if (!equals(InvAugM_AugM, I, 1e-6)) { cout << "InvAugM_AugM:" << endl << InvAugM_AugM << endl << endl; } //------- Coriolis Force Vector and Combined Force Vector Tests -------- // Get C1, Coriolis force vector using recursive method VectorXd C = skel->getCoriolisForces(); VectorXd Cg = skel->getCoriolisAndGravityForces(); // Get C2, Coriolis force vector using inverse dynamics algorithm Vector3d oldGravity = skel->getGravity(); VectorXd oldTau = skel->getForces(); VectorXd oldDdq = skel->getAccelerations(); // TODO(JS): Save external forces of body nodes skel->clearInternalForces(); skel->clearExternalForces(); skel->setAccelerations(VectorXd::Zero(dof)); EXPECT_TRUE(skel->getForces() == VectorXd::Zero(dof)); EXPECT_TRUE(skel->getExternalForces() == VectorXd::Zero(dof)); EXPECT_TRUE(skel->getAccelerations() == VectorXd::Zero(dof)); skel->setGravity(Vector3d::Zero()); EXPECT_TRUE(skel->getGravity() == Vector3d::Zero()); skel->computeInverseDynamics(false, false); VectorXd C2 = skel->getForces(); skel->setGravity(oldGravity); EXPECT_TRUE(skel->getGravity() == oldGravity); skel->computeInverseDynamics(false, false); VectorXd Cg2 = skel->getForces(); EXPECT_TRUE(equals(C, C2, 1e-6)); if (!equals(C, C2, 1e-6)) { cout << "C :" << C.transpose() << endl; cout << "C2:" << C2.transpose() << endl; failure = true; } EXPECT_TRUE(equals(Cg, Cg2, 1e-6)); if (!equals(Cg, Cg2, 1e-6)) { cout << "Cg :" << Cg.transpose() << endl; cout << "Cg2:" << Cg2.transpose() << endl; failure = true; } skel->setForces(oldTau); skel->setAccelerations(oldDdq); // TODO(JS): Restore external forces of body nodes //------------------- Combined Force Vector Test ----------------------- // TODO(JS): Not implemented yet. //---------------------- Damping Force Test ---------------------------- // TODO(JS): Not implemented yet. //--------------------- External Force Test ---------------------------- // TODO(JS): Not implemented yet. if (failure) { std::cout << "Failure occurred in the World of file: " << uri.toString() << "\nWith Skeleton named: " << skel->getName() << "\n\n"; } } } } //============================================================================== void compareCOMJacobianToFk( const SkeletonPtr& skel, const Frame* refFrame, double tolerance) { VectorXd dq = skel->getVelocities(); VectorXd ddq = skel->getAccelerations(); Vector6d comSpatialVelFk = skel->getCOMSpatialVelocity(Frame::World(), refFrame); Vector6d comSpatialAccFk = skel->getCOMSpatialAcceleration(Frame::World(), refFrame); math::Jacobian comSpatialJac = skel->getCOMJacobian(refFrame); math::Jacobian comSpatialJacDeriv = skel->getCOMJacobianSpatialDeriv(refFrame); Vector6d comSpatialVelJac = comSpatialJac * dq; Vector6d comSpatialAccJac = comSpatialJac * ddq + comSpatialJacDeriv * dq; bool spatialVelEqual = equals(comSpatialVelFk, comSpatialVelJac, tolerance); EXPECT_TRUE(spatialVelEqual); if (!spatialVelEqual) printComparisonError( "COM spatial velocity", skel->getName(), refFrame->getName(), comSpatialVelFk, comSpatialVelJac); bool spatialAccEqual = equals(comSpatialAccFk, comSpatialAccJac, tolerance); EXPECT_TRUE(spatialAccEqual); if (!spatialAccEqual) printComparisonError( "COM spatial acceleration", skel->getName(), refFrame->getName(), comSpatialAccFk, comSpatialAccJac); Vector3d comLinearVelFk = skel->getCOMLinearVelocity(Frame::World(), refFrame); Vector3d comLinearAccFk = skel->getCOMLinearAcceleration(Frame::World(), refFrame); math::LinearJacobian comLinearJac = skel->getCOMLinearJacobian(refFrame); math::LinearJacobian comLinearJacDeriv = skel->getCOMLinearJacobianDeriv(refFrame); Vector3d comLinearVelJac = comLinearJac * dq; Vector3d comLinearAccJac = comLinearJac * ddq + comLinearJacDeriv * dq; bool linearVelEqual = equals(comLinearVelFk, comLinearVelJac); EXPECT_TRUE(linearVelEqual); if (!linearVelEqual) printComparisonError( "COM linear velocity", skel->getName(), refFrame->getName(), comLinearVelFk, comLinearVelJac); bool linearAccEqual = equals(comLinearAccFk, comLinearAccJac); EXPECT_TRUE(linearAccEqual); if (!linearAccEqual) printComparisonError( "COM linear acceleration", skel->getName(), refFrame->getName(), comLinearAccFk, comLinearAccJac); } //============================================================================== void DynamicsTest::testCenterOfMass(const common::Uri& uri) { using namespace std; using namespace Eigen; using namespace dart; using namespace math; using namespace dynamics; using namespace simulation; using namespace utils; //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeletons #ifndef NDEBUG // Debug mode std::size_t nRandomItr = 2; #else std::size_t nRandomItr = 100; #endif // Lower and upper bound of configuration for system double lb = -1.5 * constantsd::pi(); double ub = 1.5 * constantsd::pi(); // Lower and upper bound of joint damping and stiffness double lbD = 0.0; double ubD = 10.0; double lbK = 0.0; double ubK = 10.0; simulation::WorldPtr myWorld; //----------------------------- Tests ---------------------------------------- // Check whether multiplication of mass matrix and its inverse is identity // matrix. myWorld = utils::SkelParser::readWorld(uri); EXPECT_TRUE(myWorld != nullptr); for (std::size_t i = 0; i < myWorld->getNumSkeletons(); ++i) { dynamics::SkeletonPtr skeleton = myWorld->getSkeleton(i); std::size_t dof = skeleton->getNumDofs(); if (dof == 0) { dtmsg << "Skeleton [" << skeleton->getName() << "] is skipped since it " << "has 0 DOF." << endl; continue; } for (std::size_t j = 0; j < nRandomItr; ++j) { // For the second half of the tests, scramble up the Skeleton if (j > ceil(nRandomItr / 2)) { SkeletonPtr copy = skeleton->cloneSkeleton(); std::size_t maxNode = skeleton->getNumBodyNodes() - 1; BodyNode* bn1 = skeleton->getBodyNode( math::Random::uniform(0, maxNode)); BodyNode* bn2 = skeleton->getBodyNode( math::Random::uniform(0, maxNode)); if (bn1 != bn2) { BodyNode* child = bn1->descendsFrom(bn2) ? bn1 : bn2; BodyNode* parent = child == bn1 ? bn2 : bn1; child->moveTo(parent); } EXPECT_TRUE(skeleton->getNumBodyNodes() == copy->getNumBodyNodes()); EXPECT_TRUE(skeleton->getNumDofs() == copy->getNumDofs()); } // Random joint stiffness and damping coefficient for (std::size_t k = 0; k < skeleton->getNumBodyNodes(); ++k) { BodyNode* body = skeleton->getBodyNode(k); Joint* joint = body->getParentJoint(); int localDof = joint->getNumDofs(); for (int l = 0; l < localDof; ++l) { joint->setDampingCoefficient(l, Random::uniform(lbD, ubD)); joint->setSpringStiffness(l, Random::uniform(lbK, ubK)); double lbRP = joint->getPositionLowerLimit(l); double ubRP = joint->getPositionUpperLimit(l); if (lbRP < -constantsd::pi()) lbRP = -constantsd::pi(); if (ubRP > constantsd::pi()) ubRP = constantsd::pi(); joint->setRestPosition(l, Random::uniform(lbRP, ubRP)); } } // Set random states VectorXd q = VectorXd(dof); VectorXd dq = VectorXd(dof); VectorXd ddq = VectorXd(dof); for (std::size_t k = 0; k < dof; ++k) { q[k] = math::Random::uniform(lb, ub); dq[k] = math::Random::uniform(lb, ub); ddq[k] = math::Random::uniform(lb, ub); } skeleton->setPositions(q); skeleton->setVelocities(dq); skeleton->setAccelerations(ddq); randomizeRefFrames(); compareCOMJacobianToFk(skeleton, Frame::World(), 1e-6); for (std::size_t r = 0; r < refFrames.size(); ++r) compareCOMJacobianToFk(skeleton, refFrames[r], 1e-6); } } } //============================================================================== void compareCOMAccelerationToGravity( SkeletonPtr skel, const Eigen::Vector3d& gravity, double tolerance) { const std::size_t numFrames = 1e+2; skel->setGravity(gravity); for (std::size_t i = 0; i < numFrames; ++i) { skel->computeForwardDynamics(); Vector3d comLinearAccFk = skel->getCOMLinearAcceleration(); bool comLinearAccFkEqual = equals(gravity, comLinearAccFk, tolerance); EXPECT_TRUE(comLinearAccFkEqual); if (!comLinearAccFkEqual) { printComparisonError( "COM linear acceleration", skel->getName(), Frame::World()->getName(), gravity, comLinearAccFk); } VectorXd dq = skel->getVelocities(); VectorXd ddq = skel->getAccelerations(); math::LinearJacobian comLinearJac = skel->getCOMLinearJacobian(); math::LinearJacobian comLinearJacDeriv = skel->getCOMLinearJacobianDeriv(); Vector3d comLinearAccJac = comLinearJac * ddq + comLinearJacDeriv * dq; bool comLinearAccJacEqual = equals(gravity, comLinearAccJac, tolerance); EXPECT_TRUE(comLinearAccJacEqual); if (!comLinearAccJacEqual) { printComparisonError( "COM linear acceleration", skel->getName(), Frame::World()->getName(), gravity, comLinearAccJac); } } } //============================================================================== void DynamicsTest::testCenterOfMassFreeFall(const common::Uri& uri) { using namespace std; using namespace Eigen; using namespace dart; using namespace math; using namespace dynamics; using namespace simulation; using namespace utils; //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeletons #ifndef NDEBUG // Debug mode std::size_t nRandomItr = 2; #else std::size_t nRandomItr = 10; #endif // ------- Debug mode // Lower and upper bound of configuration for system double lb = -1.5 * constantsd::pi(); double ub = 1.5 * constantsd::pi(); // Lower and upper bound of joint damping and stiffness double lbD = 0.0; double ubD = 10.0; double lbK = 0.0; double ubK = 10.0; simulation::WorldPtr myWorld; std::vector gravities(4); gravities[0] = Vector3d::Zero(); gravities[1] = Vector3d(-9.81, 0, 0); gravities[2] = Vector3d(0, -9.81, 0); gravities[3] = Vector3d(0, 0, -9.81); //----------------------------- Tests ---------------------------------------- // Check whether multiplication of mass matrix and its inverse is identity // matrix. myWorld = utils::SkelParser::readWorld(uri); EXPECT_TRUE(myWorld != nullptr); for (std::size_t i = 0; i < myWorld->getNumSkeletons(); ++i) { auto skel = myWorld->getSkeleton(i); auto rootJoint = skel->getJoint(0); auto rootFreeJoint = dynamic_cast(rootJoint); auto dof = skel->getNumDofs(); if (nullptr == rootFreeJoint || !skel->isMobile() || 0 == dof) { #if BUILD_TYPE_DEBUG dtmsg << "Skipping COM free fall test for Skeleton [" << skel->getName() << "] since the Skeleton doesn't have FreeJoint at the root body " << " or immobile." << endl; #endif continue; } else { rootFreeJoint->setActuatorType(dynamics::Joint::PASSIVE); } // Make sure the damping and spring forces are zero for the root FreeJoint. for (std::size_t l = 0; l < rootJoint->getNumDofs(); ++l) { rootJoint->setDampingCoefficient(l, 0.0); rootJoint->setSpringStiffness(l, 0.0); rootJoint->setRestPosition(l, 0.0); } for (std::size_t j = 0; j < nRandomItr; ++j) { // Random joint stiffness and damping coefficient for (std::size_t k = 1; k < skel->getNumBodyNodes(); ++k) { auto body = skel->getBodyNode(k); auto joint = body->getParentJoint(); auto localDof = joint->getNumDofs(); for (std::size_t l = 0; l < localDof; ++l) { joint->setDampingCoefficient(l, Random::uniform(lbD, ubD)); joint->setSpringStiffness(l, Random::uniform(lbK, ubK)); double lbRP = joint->getPositionLowerLimit(l); double ubRP = joint->getPositionUpperLimit(l); if (lbRP < -constantsd::pi()) lbRP = -constantsd::pi(); if (ubRP > constantsd::pi()) ubRP = constantsd::pi(); joint->setRestPosition(l, Random::uniform(lbRP, ubRP)); } } // Set random states VectorXd q = VectorXd(dof); VectorXd dq = VectorXd(dof); for (std::size_t k = 0; k < dof; ++k) { q[k] = math::Random::uniform(lb, ub); dq[k] = math::Random::uniform(lb, ub); } VectorXd ddq = VectorXd::Zero(dof); skel->setPositions(q); skel->setVelocities(dq); skel->setAccelerations(ddq); for (const auto& gravity : gravities) compareCOMAccelerationToGravity(skel, gravity, 1e-6); } } } //============================================================================== void DynamicsTest::testConstraintImpulse(const common::Uri& uri) { using namespace std; using namespace Eigen; using namespace dart; using namespace math; using namespace dynamics; using namespace simulation; using namespace utils; //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeletons #ifndef NDEBUG // Debug mode std::size_t nRandomItr = 1; #else std::size_t nRandomItr = 1; #endif // Lower and upper bound of configuration for system // double lb = -1.5 * constantsd::pi(); // double ub = 1.5 * constantsd::pi(); simulation::WorldPtr myWorld; //----------------------------- Tests ---------------------------------------- // Check whether multiplication of mass matrix and its inverse is identity // matrix. myWorld = utils::SkelParser::readWorld(uri); EXPECT_TRUE(myWorld != nullptr); for (std::size_t i = 0; i < myWorld->getNumSkeletons(); ++i) { dynamics::SkeletonPtr skel = myWorld->getSkeleton(i); std::size_t dof = skel->getNumDofs(); // int nBodyNodes = skel->getNumBodyNodes(); if (dof == 0 || !skel->isMobile()) { dtdbg << "Skeleton [" << skel->getName() << "] is skipped since it has " << "0 DOF or is immobile." << endl; continue; } for (std::size_t j = 0; j < nRandomItr; ++j) { // Set random configurations for (std::size_t k = 0; k < skel->getNumBodyNodes(); ++k) { BodyNode* body = skel->getBodyNode(k); Joint* joint = body->getParentJoint(); int localDof = joint->getNumDofs(); for (int l = 0; l < localDof; ++l) { double lbRP = joint->getPositionLowerLimit(l); double ubRP = joint->getPositionUpperLimit(l); if (lbRP < -constantsd::pi()) lbRP = -constantsd::pi(); if (ubRP > constantsd::pi()) ubRP = constantsd::pi(); joint->setPosition(l, Random::uniform(lbRP, ubRP)); } // Set constraint impulse on each body skel->clearConstraintImpulses(); Eigen::Vector6d impulseOnBody = Eigen::Vector6d::Random(); body->setConstraintImpulse(impulseOnBody); // Get constraint force vector Eigen::VectorXd constraintVector1 = skel->getConstraintForces(); // Get constraint force vector by using Jacobian of skeleon Eigen::MatrixXd bodyJacobian = body->getJacobian(); Eigen::VectorXd constraintVector2 = bodyJacobian.transpose() * impulseOnBody / skel->getTimeStep(); std::size_t index = 0; for (std::size_t l = 0; l < dof; ++l) { if (constraintVector1(l) == 0.0) continue; EXPECT_NEAR(constraintVector1(l), constraintVector2(index), 1e-6); index++; } assert(static_cast(bodyJacobian.cols()) == index); } } } } //============================================================================== void DynamicsTest::testImpulseBasedDynamics(const common::Uri& uri) { using namespace std; using namespace Eigen; using namespace dart; using namespace math; using namespace dynamics; using namespace simulation; using namespace utils; //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeletons #ifndef NDEBUG // Debug mode std::size_t nRandomItr = 1; #else std::size_t nRandomItr = 100; #endif double TOLERANCE = 1e-1; // Lower and upper bound of configuration for system double lb = -1.5 * constantsd::pi(); double ub = 1.5 * constantsd::pi(); simulation::WorldPtr myWorld; //----------------------------- Tests ---------------------------------------- // Check whether multiplication of mass matrix and its inverse is identity // matrix. myWorld = utils::SkelParser::readWorld(uri); EXPECT_TRUE(myWorld != nullptr); for (std::size_t i = 0; i < myWorld->getNumSkeletons(); ++i) { dynamics::SkeletonPtr skel = myWorld->getSkeleton(i); int dof = skel->getNumDofs(); // int nBodyNodes = skel->getNumBodyNodes(); if (dof == 0 || !skel->isMobile()) { dtdbg << "Skeleton [" << skel->getName() << "] is skipped since it has " << "0 DOF or is immobile." << endl; continue; } for (std::size_t j = 0; j < nRandomItr; ++j) { // Set random configurations for (std::size_t k = 0; k < skel->getNumBodyNodes(); ++k) { BodyNode* body = skel->getBodyNode(k); Joint* joint = body->getParentJoint(); int localDof = joint->getNumDofs(); for (int l = 0; l < localDof; ++l) { double lbRP = joint->getPositionLowerLimit(l); double ubRP = joint->getPositionUpperLimit(l); if (lbRP < -constantsd::pi()) lbRP = -constantsd::pi(); if (ubRP > constantsd::pi()) ubRP = constantsd::pi(); joint->setPosition(l, Random::uniform(lbRP, ubRP)); } } // skel->setPositions(VectorXd::Zero(dof)); // TODO(JS): Just clear what should be skel->clearExternalForces(); skel->clearConstraintImpulses(); // Set random impulses VectorXd impulses = VectorXd::Zero(dof); for (int k = 0; k < impulses.size(); ++k) impulses[k] = Random::uniform(lb, ub); skel->setJointConstraintImpulses(impulses); // Compute impulse-based forward dynamics skel->computeImpulseForwardDynamics(); // Compare resultant velocity change and invM * impulses VectorXd deltaVel1 = skel->getVelocityChanges(); MatrixXd invM = skel->getInvMassMatrix(); VectorXd deltaVel2 = invM * impulses; EXPECT_TRUE(equals(deltaVel1, deltaVel2, TOLERANCE)); if (!equals(deltaVel1, deltaVel2, TOLERANCE)) { cout << "deltaVel1: " << deltaVel1.transpose() << endl; cout << "deltaVel2: " << deltaVel2.transpose() << endl; cout << "error: " << (deltaVel1 - deltaVel2).norm() << endl; } } } } //============================================================================== TEST_F(DynamicsTest, testJacobians) { for (std::size_t i = 0; i < getList().size(); ++i) { #ifndef NDEBUG dtdbg << getList()[i].toString() << std::endl; #endif testJacobians(getList()[i]); } } //============================================================================== TEST_F(DynamicsTest, testFiniteDifference) { for (std::size_t i = 0; i < getList().size(); ++i) { #if BUILD_TYPE_DEBUG dtdbg << getList()[i].toString() << std::endl; #endif testFiniteDifferenceGeneralizedCoordinates(getList()[i]); testFiniteDifferenceBodyNodeVelocity(getList()[i]); testFiniteDifferenceBodyNodeAcceleration(getList()[i]); } } //============================================================================== TEST_F(DynamicsTest, testForwardKinematics) { for (std::size_t i = 0; i < getList().size(); ++i) { #ifndef NDEBUG dtdbg << getList()[i].toString() << std::endl; #endif testForwardKinematics(getList()[i]); } } //============================================================================== TEST_F(DynamicsTest, testInverseDynamics) { for (std::size_t i = 0; i < getList().size(); ++i) { #ifndef NDEBUG dtdbg << getList()[i].toString() << std::endl; #endif testInverseDynamics(getList()[i]); } } //============================================================================== TEST_F(DynamicsTest, compareEquationsOfMotion) { for (std::size_t i = 0; i < getList().size(); ++i) { //////////////////////////////////////////////////////////////////////////// // TODO(JS): Following skel files, which contain euler joints couldn't // pass EQUATIONS_OF_MOTION, are disabled. const auto uri = getList()[i]; if (uri.toString() == "dart://sample/skel/test/double_pendulum_euler_joint.skel" || uri.toString() == "dart://sample/skel/test/chainwhipa.skel" || uri.toString() == "dart://sample/skel/test/serial_chain_eulerxyz_joint.skel" || uri.toString() == "dart://sample/skel/test/simple_tree_structure_euler_joint.skel" || uri.toString() == "dart://sample/skel/test/tree_structure_euler_joint.skel" || uri.toString() == "dart://sample/skel/fullbody1.skel") { continue; } //////////////////////////////////////////////////////////////////////////// #ifndef NDEBUG dtdbg << getList()[i].toString() << std::endl; #endif compareEquationsOfMotion(getList()[i]); } } //============================================================================== TEST_F(DynamicsTest, testCenterOfMass) { for (std::size_t i = 0; i < getList().size(); ++i) { #ifndef NDEBUG dtdbg << getList()[i].toString() << std::endl; #endif testCenterOfMass(getList()[i]); } } //============================================================================== TEST_F(DynamicsTest, testCenterOfMassFreeFall) { for (std::size_t i = 0; i < getList().size(); ++i) { #ifndef NDEBUG dtdbg << getList()[i].toString() << std::endl; #endif testCenterOfMassFreeFall(getList()[i]); } } //============================================================================== TEST_F(DynamicsTest, testConstraintImpulse) { for (std::size_t i = 0; i < getList().size(); ++i) { #ifndef NDEBUG dtdbg << getList()[i].toString() << std::endl; #endif testConstraintImpulse(getList()[i]); } } //============================================================================== TEST_F(DynamicsTest, testImpulseBasedDynamics) { for (std::size_t i = 0; i < getList().size(); ++i) { #ifndef NDEBUG dtdbg << getList()[i].toString() << std::endl; #endif testImpulseBasedDynamics(getList()[i]); } } //============================================================================== TEST_F(DynamicsTest, HybridDynamics) { const double tol = 1e-8; const double timeStep = 1e-3; #ifndef NDEBUG // Debug mode const std::size_t numFrames = 50; // 0.05 secs #else const std::size_t numFrames = 5e+3; // 5 secs #endif // ------- Debug mode // Load world and skeleton WorldPtr world = utils::SkelParser::readWorld( "dart://sample/skel/test/hybrid_dynamics_test.skel"); world->setTimeStep(timeStep); EXPECT_TRUE(world != nullptr); EXPECT_NEAR(world->getTimeStep(), timeStep, tol); SkeletonPtr skel = world->getSkeleton("skeleton 1"); EXPECT_TRUE(skel != nullptr); EXPECT_NEAR(skel->getTimeStep(), timeStep, tol); const std::size_t numDofs = skel->getNumDofs(); // Zero initial states Eigen::VectorXd q0 = Eigen::VectorXd::Zero(numDofs); Eigen::VectorXd dq0 = Eigen::VectorXd::Zero(numDofs); // Initialize the skeleton with the zero initial states skel->setPositions(q0); skel->setVelocities(dq0); EXPECT_TRUE(equals(skel->getPositions(), q0)); EXPECT_TRUE(equals(skel->getVelocities(), dq0)); // Make sure all the joint actuator types EXPECT_EQ(skel->getJoint(0)->getActuatorType(), Joint::FORCE); EXPECT_EQ(skel->getJoint(1)->getActuatorType(), Joint::ACCELERATION); EXPECT_EQ(skel->getJoint(2)->getActuatorType(), Joint::VELOCITY); EXPECT_EQ(skel->getJoint(3)->getActuatorType(), Joint::ACCELERATION); EXPECT_EQ(skel->getJoint(4)->getActuatorType(), Joint::VELOCITY); // Prepare command for each joint types per simulation steps Eigen::MatrixXd command = Eigen::MatrixXd::Zero(numFrames, numDofs); Eigen::VectorXd amp = Eigen::VectorXd::Zero(numDofs); for (std::size_t i = 0; i < numDofs; ++i) amp[i] = math::Random::uniform(-1.5, 1.5); for (std::size_t i = 0; i < numFrames; ++i) { for (std::size_t j = 0; j < numDofs; ++j) command(i, j) = amp[j] * std::sin(i * timeStep); } // Record joint forces for joint[1~4] Eigen::MatrixXd forces = Eigen::MatrixXd::Zero(numFrames, numDofs); for (std::size_t i = 0; i < numFrames; ++i) { skel->setCommands(command.row(i)); world->step(false); forces.row(i) = skel->getForces(); EXPECT_NEAR(command(i, 0), skel->getForce(0), tol); EXPECT_NEAR(command(i, 1), skel->getAcceleration(1), tol); EXPECT_NEAR(command(i, 2), skel->getVelocity(2), tol); EXPECT_NEAR(command(i, 3), skel->getAcceleration(3), tol); EXPECT_NEAR(command(i, 4), skel->getVelocity(4), tol); } // Restore the skeleton to the initial state skel->setPositions(q0); skel->setVelocities(dq0); EXPECT_TRUE(equals(skel->getPositions(), q0)); EXPECT_TRUE(equals(skel->getVelocities(), dq0)); // Change all the actuator types to force skel->getJoint(0)->setActuatorType(Joint::FORCE); skel->getJoint(1)->setActuatorType(Joint::FORCE); skel->getJoint(2)->setActuatorType(Joint::FORCE); skel->getJoint(3)->setActuatorType(Joint::FORCE); skel->getJoint(4)->setActuatorType(Joint::FORCE); EXPECT_EQ(skel->getJoint(0)->getActuatorType(), Joint::FORCE); EXPECT_EQ(skel->getJoint(1)->getActuatorType(), Joint::FORCE); EXPECT_EQ(skel->getJoint(2)->getActuatorType(), Joint::FORCE); EXPECT_EQ(skel->getJoint(3)->getActuatorType(), Joint::FORCE); EXPECT_EQ(skel->getJoint(4)->getActuatorType(), Joint::FORCE); // Test if the skeleton moves as the command with the joint forces Eigen::MatrixXd output = Eigen::MatrixXd::Zero(numFrames, numDofs); for (std::size_t i = 0; i < numFrames; ++i) { skel->setCommands(forces.row(i)); world->step(false); output(i, 0) = skel->getJoint(0)->getForce(0); output(i, 1) = skel->getJoint(1)->getAcceleration(0); output(i, 2) = skel->getJoint(2)->getVelocity(0); output(i, 3) = skel->getJoint(3)->getAcceleration(0); output(i, 4) = skel->getJoint(4)->getVelocity(0); EXPECT_NEAR(command(i, 0), output(i, 0), tol); EXPECT_NEAR(command(i, 1), output(i, 1), tol); EXPECT_NEAR(command(i, 2), output(i, 2), tol); EXPECT_NEAR(command(i, 3), output(i, 3), tol); EXPECT_NEAR(command(i, 4), output(i, 4), tol); } } //============================================================================== TEST_F(DynamicsTest, OffsetCOM) { WorldPtr world = World::create(); ASSERT_TRUE(world != nullptr); world->setGravity(Eigen::Vector3d(0.0, 0.0, 0.0)); const double dt = 0.001; world->setTimeStep(dt); SkeletonPtr boxSkel = createBox(Vector3d(1.0, 1.0, 1.0)); world->addSkeleton(boxSkel); BodyNode* box = boxSkel->getBodyNode(0); dynamics::Inertia inertia; inertia.setMass(60); inertia.setMoment( box->getShapeNode(0)->getShape()->computeInertia(inertia.getMass())); inertia.setLocalCOM({0, 500, 0}); box->setInertia(inertia); Eigen::Isometry3d boxInitialPose = box->getWorldTransform(); Eigen::Vector3d torque(0, 0, 100); box->addExtTorque(torque, false); { Vector3d comVel = box->getCOMLinearVelocity(); EXPECT_TRUE(equals(Vector3d(0, 0, 0), comVel)) << "comVel: " << comVel.transpose(); } { Vector3d linVel = box->getLinearVelocity(); EXPECT_TRUE(equals(Vector3d(0, 0, 0), linVel)) << "linVel: " << linVel.transpose(); } world->step(); // Velocity at COM should be zero since the object is just rotating. { Vector3d comVel = box->getCOMLinearVelocity(); EXPECT_TRUE(equals(Vector3d(0, 0, 0), comVel, 1e-4)) << "comVel: " << comVel.transpose(); } { Vector3d angVel = box->getAngularVelocity(); // We can compute expectedAngAccel like so because the initial angular // velocity is zero. Vector3d expectedAngAccel = inertia.getMoment().inverse() * torque; Vector3d expectedAngVel = expectedAngAccel * dt; EXPECT_TRUE(equals(expectedAngVel, angVel)) << "angVel: " << angVel.transpose(); Vector3d linVel = box->getLinearVelocity(); Vector3d expLinVel = angVel.cross(boxInitialPose.linear() * -box->getLocalCOM()); EXPECT_TRUE(equals(expLinVel, linVel)) << "Expected: " << expLinVel.transpose() << "\nActual: " << linVel.transpose(); Vector3d angAccel = box->getAngularAcceleration(); EXPECT_TRUE(equals(expectedAngAccel, angAccel)) << "angAccel: " << angAccel.transpose(); Vector3d linAccel = box->getLinearAcceleration(); Vector3d expLinAccel = expectedAngAccel.cross(boxInitialPose.linear() * -box->getLocalCOM()); EXPECT_TRUE(equals(expLinAccel, linAccel)) << "Expected: " << expLinAccel.transpose() << "\nActual: " << linAccel.transpose(); } }