diff --git a/dart/dynamics/BallJoint.cpp b/dart/dynamics/BallJoint.cpp index c04559f304110..ab8fee85a3220 100644 --- a/dart/dynamics/BallJoint.cpp +++ b/dart/dynamics/BallJoint.cpp @@ -101,6 +101,8 @@ BallJoint::BallJoint(const Properties& _properties) : MultiDofJoint<3>(_properties), mR(Eigen::Isometry3d::Identity()) { + mJacobianDeriv = Eigen::Matrix::Zero(); + setProperties(_properties); updateDegreeOfFreedomNames(); } @@ -113,47 +115,28 @@ Joint* BallJoint::clone() const //============================================================================== Eigen::Matrix BallJoint::getLocalJacobianStatic( - const Eigen::Vector3d& _positions) const + const Eigen::Vector3d& /*positions*/) const { - // Jacobian expressed in the Joint frame - Eigen::Matrix J; - J.topRows<3>() = math::expMapJac(-_positions); - J.bottomRows<3>() = Eigen::Matrix3d::Zero(); - - // Transform the reference frame to the child BodyNode frame - J = math::AdTJacFixed(mJointP.mT_ChildBodyToJoint, J); - - assert(!math::isNan(J)); - - return J; + return mJacobian; } //============================================================================== Eigen::Vector3d BallJoint::getPositionDifferencesStatic( const Eigen::Vector3d& _q2, const Eigen::Vector3d& _q1) const { - Eigen::Vector3d dq; - - const Eigen::Matrix3d Jw = getLocalJacobianStatic(_q1).topRows<3>(); - const Eigen::Matrix3d R1T = math::expMapRot(-_q1); - const Eigen::Matrix3d R2 = math::expMapRot( _q2); + const Eigen::Matrix3d R1 = convertToRotation(_q1); + const Eigen::Matrix3d R2 = convertToRotation(_q2); - dq = Jw.inverse() * math::logMap(R1T * R2); - - return dq; + return convertToPositions(R1.transpose() * R2); } //============================================================================== void BallJoint::integratePositions(double _dt) { - const Eigen::Isometry3d& R = getR(); - Eigen::Isometry3d Rnext(Eigen::Isometry3d::Identity()); - - Rnext.linear() = R.linear() - * convertToRotation(getLocalJacobianStatic().topRows<3>() - * getVelocitiesStatic() * _dt); + Eigen::Matrix3d Rnext + = getR().linear() * convertToRotation(getVelocitiesStatic() * _dt); - setPositionsStatic(convertToPositions(Rnext.linear())); + setPositionsStatic(convertToPositions(Rnext)); } //============================================================================== @@ -179,22 +162,16 @@ void BallJoint::updateLocalTransform() const } //============================================================================== -void BallJoint::updateLocalJacobian(bool) const +void BallJoint::updateLocalJacobian(bool _mandatory) const { - mJacobian = getLocalJacobianStatic(getPositionsStatic()); + if (_mandatory) + mJacobian = math::getAdTMatrix(mJointP.mT_ChildBodyToJoint).leftCols<3>(); } //============================================================================== void BallJoint::updateLocalJacobianTimeDeriv() const { - Eigen::Matrix dJ; - dJ.topRows<3>() = math::expMapJacDot(getPositionsStatic(), - getVelocitiesStatic()).transpose(); - dJ.bottomRows<3>() = Eigen::Matrix3d::Zero(); - - mJacobianDeriv = math::AdTJacFixed(mJointP.mT_ChildBodyToJoint, dJ); - - assert(!math::isNan(mJacobianDeriv)); + assert(Eigen::Matrix6d::Zero().leftCols<3>() == mJacobianDeriv); } //============================================================================== diff --git a/dart/dynamics/BallJoint.h b/dart/dynamics/BallJoint.h index ea44fbf2f16c2..77e2beb4df109 100644 --- a/dart/dynamics/BallJoint.h +++ b/dart/dynamics/BallJoint.h @@ -62,13 +62,13 @@ class BallJoint : public MultiDofJoint<3> virtual ~BallJoint(); // Documentation inherited - virtual const std::string& getType() const override; + const std::string& getType() const override; /// Get joint type for this class static const std::string& getStaticType(); // Documentation inherited - virtual bool isCyclic(size_t _index) const override; + bool isCyclic(size_t _index) const override; /// Get the Properties of this BallJoint Properties getBallJointProperties() const; @@ -105,24 +105,24 @@ class BallJoint : public MultiDofJoint<3> BallJoint(const Properties& _properties); // Documentation inherited - virtual Joint* clone() const override; + Joint* clone() const override; using MultiDofJoint::getLocalJacobianStatic; // Documentation inherited - virtual void integratePositions(double _dt) override; + void integratePositions(double _dt) override; // Documentation inherited - virtual void updateDegreeOfFreedomNames() override; + void updateDegreeOfFreedomNames() override; // Documentation inherited - virtual void updateLocalTransform() const override; + void updateLocalTransform() const override; // Documentation inherited - virtual void updateLocalJacobian(bool =true) const override; + void updateLocalJacobian(bool _mandatory = true) const override; // Documentation inherited - virtual void updateLocalJacobianTimeDeriv() const override; + void updateLocalJacobianTimeDeriv() const override; protected: diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index 107ff0f792842..46533bfb5d39f 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/dart/dynamics/FreeJoint.cpp @@ -508,25 +508,9 @@ void FreeJoint::setAngularAcceleration( //============================================================================== Eigen::Matrix6d FreeJoint::getLocalJacobianStatic( - const Eigen::Vector6d& _positions) const + const Eigen::Vector6d& /*positions*/) const { - // Jacobian expressed in the Joint frame - Eigen::Matrix6d J = Eigen::Matrix6d::Identity(); - J.topLeftCorner<3,3>() = math::expMapJac(-_positions.head<3>()); - - // Transform the reference frame to the child BodyNode frame - J.leftCols<3>() = math::AdTJacFixed(mJointP.mT_ChildBodyToJoint, - J.leftCols<3>()); - J.bottomRightCorner<3,3>() - = mJointP.mT_ChildBodyToJoint.linear() - * math::expMapRot(-_positions.head<3>()); - - // Note that the top right 3x3 block of J is always zero - assert((J.topRightCorner<3,3>()) == Eigen::Matrix3d::Zero()); - - assert(!math::isNan(J)); - - return J; + return mJacobian; } //============================================================================== @@ -534,16 +518,10 @@ Eigen::Vector6d FreeJoint::getPositionDifferencesStatic( const Eigen::Vector6d& _q2, const Eigen::Vector6d& _q1) const { - Eigen::Vector6d dq; - - const Eigen::Matrix3d Jw = getLocalJacobianStatic(_q1).topLeftCorner<3,3>(); - const Eigen::Matrix3d R1T = math::expMapRot(-_q1.head<3>()); - const Eigen::Matrix3d R2 = math::expMapRot( _q2.head<3>()); + const Eigen::Isometry3d T1 = convertToTransform(_q1); + const Eigen::Isometry3d T2 = convertToTransform(_q2); - dq.head<3>() = Jw.inverse() * math::logMap(R1T * R2); - dq.tail<3>() = _q2.tail<3>() - _q1.tail<3>(); - - return dq; + return convertToPositions(T1.inverse() * T2); } //============================================================================== @@ -551,6 +529,8 @@ FreeJoint::FreeJoint(const Properties& _properties) : MultiDofJoint<6>(_properties), mQ(Eigen::Isometry3d::Identity()) { + mJacobianDeriv = Eigen::Matrix6d::Zero(); + setProperties(_properties); updateDegreeOfFreedomNames(); } @@ -584,15 +564,8 @@ bool FreeJoint::isCyclic(size_t _index) const //============================================================================== void FreeJoint::integratePositions(double _dt) { - const Eigen::Vector6d& velocities = getVelocitiesStatic(); - - const Eigen::Isometry3d& Q = getQ(); - Eigen::Isometry3d Qnext(Eigen::Isometry3d::Identity()); - - Qnext.linear() = Q.linear() - * math::expMapRot(getLocalJacobianStatic().topLeftCorner<3,3>() - * velocities.head<3>() * _dt); - Qnext.translation() = Q.translation() + velocities.tail<3>() * _dt; + const Eigen::Isometry3d Qnext + = getQ() * convertToTransform(getVelocitiesStatic() * _dt); setPositionsStatic(convertToPositions(Qnext)); } @@ -626,40 +599,16 @@ void FreeJoint::updateLocalTransform() const } //============================================================================== -void FreeJoint::updateLocalJacobian(bool) const +void FreeJoint::updateLocalJacobian(bool _mandatory) const { - mJacobian = getLocalJacobian(getPositionsStatic()); + if (_mandatory) + mJacobian = math::getAdTMatrix(mJointP.mT_ChildBodyToJoint); } //============================================================================== void FreeJoint::updateLocalJacobianTimeDeriv() const { - Eigen::Matrix J; - J.topRows<3>() = Eigen::Matrix3d::Zero(); - J.bottomRows<3>() = Eigen::Matrix3d::Identity(); - - const Eigen::Vector6d& positions = getPositionsStatic(); - const Eigen::Vector6d& velocities = getVelocitiesStatic(); - Eigen::Matrix dJ; - dJ.topRows<3>() = math::expMapJacDot(positions.head<3>(), - velocities.head<3>()).transpose(); - dJ.bottomRows<3>() = Eigen::Matrix3d::Zero(); - - const Eigen::Isometry3d T = mJointP.mT_ChildBodyToJoint - * math::expAngular(-positions.head<3>()); - - mJacobianDeriv.leftCols<3>() = - math::AdTJacFixed(mJointP.mT_ChildBodyToJoint, dJ); - const Eigen::Matrix& Jacobian = getLocalJacobianStatic(); - mJacobianDeriv.col(3) - = -math::ad(Jacobian.leftCols<3>() * velocities.head<3>(), - math::AdT(T, J.col(0))); - mJacobianDeriv.col(4) - = -math::ad(Jacobian.leftCols<3>() * velocities.head<3>(), - math::AdT(T, J.col(1))); - mJacobianDeriv.col(5) - = -math::ad(Jacobian.leftCols<3>() * velocities.head<3>(), - math::AdT(T, J.col(2))); + assert(Eigen::Matrix6d::Zero() == mJacobianDeriv); } //============================================================================== diff --git a/dart/dynamics/FreeJoint.h b/dart/dynamics/FreeJoint.h index e4ab8d7dc17ed..ac393d51a8d86 100644 --- a/dart/dynamics/FreeJoint.h +++ b/dart/dynamics/FreeJoint.h @@ -272,24 +272,24 @@ class FreeJoint : public MultiDofJoint<6> FreeJoint(const Properties& _properties); // Documentation inherited - virtual Joint* clone() const override; + Joint* clone() const override; using MultiDofJoint::getLocalJacobianStatic; // Documentation inherited - virtual void integratePositions(double _dt) override; + void integratePositions(double _dt) override; // Documentation inherited - virtual void updateDegreeOfFreedomNames() override; + void updateDegreeOfFreedomNames() override; // Documentation inherited - virtual void updateLocalTransform() const override; + void updateLocalTransform() const override; // Documentation inherited - virtual void updateLocalJacobian(bool =true) const override; + void updateLocalJacobian(bool _mandatory = true) const override; // Documentation inherited - virtual void updateLocalJacobianTimeDeriv() const override; + void updateLocalJacobianTimeDeriv() const override; protected: diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index fd405820f7805..a5fd58b0cd99a 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -386,13 +386,13 @@ void Joint::setTransformFromChildBodyNode(const Eigen::Isometry3d& _T) } //============================================================================== -const Eigen::Isometry3d&Joint::getTransformFromParentBodyNode() const +const Eigen::Isometry3d& Joint::getTransformFromParentBodyNode() const { return mJointP.mT_ParentBodyToJoint; } //============================================================================== -const Eigen::Isometry3d&Joint::getTransformFromChildBodyNode() const +const Eigen::Isometry3d& Joint::getTransformFromChildBodyNode() const { return mJointP.mT_ChildBodyToJoint; } diff --git a/dart/math/Geometry.cpp b/dart/math/Geometry.cpp index ff7ed49bd492c..87210bc445fd4 100644 --- a/dart/math/Geometry.cpp +++ b/dart/math/Geometry.cpp @@ -635,6 +635,20 @@ Eigen::Vector6d AdT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V) { return res; } +//============================================================================== +Eigen::Matrix6d getAdTMatrix(const Eigen::Isometry3d& T) +{ + Eigen::Matrix6d AdT; + + AdT.topLeftCorner<3, 3>() = T.linear(); + AdT.topRightCorner<3, 3>().setZero(); + AdT.bottomLeftCorner<3, 3>() + = makeSkewSymmetric(T.translation()) * T.linear(); + AdT.bottomRightCorner<3, 3>() = T.linear(); + + return AdT; +} + Eigen::Vector6d AdR(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V) { //-------------------------------------------------------------------------- // w' = R*w diff --git a/dart/math/Geometry.h b/dart/math/Geometry.h index 1fbe6578769eb..f60581cc22ce4 100644 --- a/dart/math/Geometry.h +++ b/dart/math/Geometry.h @@ -39,7 +39,6 @@ #define DART_MATH_GEOMETRY_H_ #include -#include #include "dart/math/MathTypes.h" @@ -200,6 +199,9 @@ Eigen::Vector6d logMap(const Eigen::Isometry3d& _T); /// where @f$T=(R,p)@in SE(3), @quad V=(w,v)@in se(3) @f$. Eigen::Vector6d AdT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V); +/// \brief Get linear transformation matrix of Adjoint mapping +Eigen::Matrix6d getAdTMatrix(const Eigen::Isometry3d& T); + /// Adjoint mapping for dynamic size Jacobian template typename Derived::PlainObject AdTJac(const Eigen::Isometry3d& _T, diff --git a/dart/math/Helpers.h b/dart/math/Helpers.h index f6dca5809a425..aef0273027604 100644 --- a/dart/math/Helpers.h +++ b/dart/math/Helpers.h @@ -281,6 +281,23 @@ Eigen::Matrix randomVector(double _limit) return randomVector(-std::abs(_limit), std::abs(_limit)); } +//============================================================================== +inline Eigen::VectorXd randomVectorXd(size_t size, double min, double max) +{ + Eigen::VectorXd v = Eigen::VectorXd::Zero(size); + + for (size_t i = 0; i < size; ++i) + v[i] = random(min, max); + + return v; +} + +//============================================================================== +inline Eigen::VectorXd randomVectorXd(size_t size, double limit) +{ + return randomVectorXd(size, -std::abs(limit), std::abs(limit)); +} + } // namespace math namespace Color diff --git a/unittests/testDynamics.cpp b/unittests/testDynamics.cpp index 70cf4dcff58e5..afcfdd781f7c6 100644 --- a/unittests/testDynamics.cpp +++ b/unittests/testDynamics.cpp @@ -83,10 +83,18 @@ class DynamicsTest : public ::testing::Test // using finite differece method. void testFiniteDifferenceGeneralizedCoordinates(const std::string& _fileName); + // Compare spatial velocities computed by forward kinematics and finite + // difference. + void testFiniteDifferenceBodyNodeVelocity(const std::string& _fileName); + // Compare accelerations computed by recursive method, Jacobian, and finite // difference. void testFiniteDifferenceBodyNodeAcceleration(const std::string& _fileName); + // Test if the recursive forward kinematics algorithm computes + // transformations, spatial velocities, and spatial accelerations correctly. + void testForwardKinematics(const std::string& _fileName); + // Compare dynamics terms in equations of motion such as mass matrix, mass // inverse matrix, Coriolis force vector, gravity force vector, and external // force vector. @@ -775,6 +783,103 @@ void DynamicsTest::testFiniteDifferenceGeneralizedCoordinates( } } +//============================================================================== +void DynamicsTest::testFiniteDifferenceBodyNodeVelocity( + const std::string& _fileName) +{ + 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; + size_t numSteps = 1e+1; +#else + int nRandomItr = 10; + size_t numSteps = 1e+3; +#endif + double qLB = -0.5 * DART_PI; + double qUB = 0.5 * DART_PI; + double dqLB = -0.5 * DART_PI; + double dqUB = 0.5 * DART_PI; + double ddqLB = -0.5 * DART_PI; + double ddqUB = 0.5 * DART_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(_fileName); + assert(world != nullptr); + world->setGravity(gravity); + world->setTimeStep(timeStep); + + //------------------------------ Tests --------------------------------------- + for (int i = 0; i < nRandomItr; ++i) + { + for (size_t j = 0; j < world->getNumSkeletons(); ++j) + { + SkeletonPtr skeleton = world->getSkeleton(j); + EXPECT_NE(skeleton, nullptr); + + size_t dof = skeleton->getNumDofs(); + size_t numBodies = skeleton->getNumBodyNodes(); + + // Generate random states + VectorXd q = randomVectorXd(dof, qLB, qUB); + VectorXd dq = randomVectorXd(dof, dqLB, dqUB); + VectorXd ddq = randomVectorXd(dof, ddqLB, ddqUB); + + skeleton->setPositions(q); + skeleton->setVelocities(dq); + skeleton->setAccelerations(ddq); + + std::map Tmap; + for (auto k = 0u; k < numBodies; ++k) + { + auto body = skeleton->getBodyNode(k); + Tmap[body] = body->getTransform(); + } + + for (size_t k = 0; k < numSteps; ++k) + { + skeleton->integrateVelocities(skeleton->getTimeStep()); + skeleton->integratePositions(skeleton->getTimeStep()); + + for (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 std::string& _fileName) @@ -829,9 +934,6 @@ void DynamicsTest::testFiniteDifferenceBodyNodeAcceleration( ddq[k] = math::random(ddqLB, ddqUB); } - VectorXd qNext = q + dq * timeStep; - VectorXd dqNext = dq + ddq * timeStep; - // For each body node for (size_t k = 0; k < skeleton->getNumBodyNodes(); ++k) { @@ -855,9 +957,8 @@ void DynamicsTest::testFiniteDifferenceBodyNodeAcceleration( Vector3d WorldAngAcc1 = bn->getAngularAcceleration(); // Calculation of velocities and Jacobian at (k+1)-th time step - skeleton->setPositions(qNext); - skeleton->setVelocities(dqNext); - skeleton->setAccelerations(ddq); + skeleton->integrateVelocities(skeleton->getTimeStep()); + skeleton->integratePositions(skeleton->getTimeStep()); Vector3d BodyLinVel2 = bn->getLinearVelocity(Frame::World(), bn); Vector3d BodyAngVel2 = bn->getAngularVelocity(Frame::World(), bn); @@ -933,6 +1034,146 @@ void DynamicsTest::testFiniteDifferenceBodyNodeAcceleration( } } +//============================================================================== +void testForwardKinematicsSkeleton(const dynamics::SkeletonPtr& skel) +{ +#ifndef NDEBUG // Debug mode + size_t nRandomItr = 1e+1; + size_t numSteps = 1e+1; +#else + size_t nRandomItr = 1e+2; + size_t numSteps = 1e+2; +#endif + double qLB = -0.5 * DART_PI; + double qUB = 0.5 * DART_PI; + double dqLB = -0.3 * DART_PI; + double dqUB = 0.3 * DART_PI; + double ddqLB = -0.1 * DART_PI; + double ddqUB = 0.1 * DART_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; + + std::map Tmap; + std::map Vmap; + std::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 = randomVectorXd(dof, qLB, qUB); + dq = randomVectorXd(dof, dqLB, dqUB); + ddq = randomVectorXd(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->getLocalJacobian(); + Eigen::MatrixXd dS = joint->getLocalJacobianTimeDeriv(); + 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 std::string& fileName) +{ + auto world = utils::SkelParser::readWorld(fileName); + EXPECT_TRUE(world != nullptr); + + auto numSkeletons = world->getNumSkeletons(); + for (auto i = 0u; i < numSkeletons; ++i) + { + auto skeleton = world->getSkeleton(i); + testForwardKinematicsSkeleton(skeleton); + } +} + //============================================================================== void DynamicsTest::compareEquationsOfMotion(const std::string& _fileName) { @@ -1686,14 +1927,27 @@ TEST_F(DynamicsTest, testFiniteDifference) { for (size_t i = 0; i < getList().size(); ++i) { -#ifndef NDEBUG +#ifdef BUILD_TYPE_DEBUG dtdbg << getList()[i] << std::endl; #endif testFiniteDifferenceGeneralizedCoordinates(getList()[i]); + testFiniteDifferenceBodyNodeVelocity(getList()[i]); testFiniteDifferenceBodyNodeAcceleration(getList()[i]); } } +//============================================================================== +TEST_F(DynamicsTest, testForwardKinematics) +{ + for (size_t i = 0; i < getList().size(); ++i) + { +#ifndef NDEBUG + dtdbg << getList()[i] << std::endl; +#endif + testForwardKinematics(getList()[i]); + } +} + //============================================================================== TEST_F(DynamicsTest, compareEquationsOfMotion) { diff --git a/unittests/testJoints.cpp b/unittests/testJoints.cpp index b55ad76e638bc..7d6ec24e0f138 100644 --- a/unittests/testJoints.cpp +++ b/unittests/testJoints.cpp @@ -309,10 +309,14 @@ TEST_F(JOINTS, UNIVERSAL_JOINT) } // 3-dof joint -TEST_F(JOINTS, BALL_JOINT) -{ - kinematicsTest(); -} +//TEST_F(JOINTS, BALL_JOINT) +//{ +// kinematicsTest(); +//} +// TODO(JS): Disabled the test compares analytical Jacobian and numerical +// Jacobian since the meaning of BallJoint Jacobian is changed per +// we now use angular velocity and angular accertions as BallJoint's generalized +// velocities and accelerations, repectively. // 3-dof joint TEST_F(JOINTS, EULER_JOINT) @@ -339,10 +343,14 @@ TEST_F(JOINTS, PLANAR_JOINT) } // 6-dof joint -TEST_F(JOINTS, FREE_JOINT) -{ - kinematicsTest(); -} +//TEST_F(JOINTS, FREE_JOINT) +//{ +// kinematicsTest(); +//} +// TODO(JS): Disabled the test compares analytical Jacobian and numerical +// Jacobian since the meaning of FreeJoint Jacobian is changed per +// we now use spatial velocity and spatial accertions as FreeJoint's generalized +// velocities and accelerations, repectively. //============================================================================== template