diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index cf2561b55b02b..19f45a4606ddc 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -1334,6 +1334,36 @@ void BodyNode::drawMarkers(renderer::RenderInterface* _ri, _ri->popMatrix(); } +//============================================================================== +void BodyNode::notifyJacobianUpdate() +{ + // mIsWorldJacobianDirty depends on mIsBodyJacobianDirty, so we only need to + // check mIsBodyJacobianDirty if we want to terminate. + if(mIsBodyJacobianDirty) + return; + + mIsBodyJacobianDirty = true; + mIsWorldJacobianDirty = true; + + for(BodyNode* child : mChildBodyNodes) + child->notifyJacobianUpdate(); +} + +//============================================================================== +void BodyNode::notifyJacobianDerivUpdate() +{ + // These two flags are independent of each other, so we must check that both + // are true if we want to terminate early. + if(mIsBodyJacobianSpatialDerivDirty && mIsWorldJacobianClassicDerivDirty) + return; + + mIsBodyJacobianSpatialDerivDirty = true; + mIsWorldJacobianClassicDerivDirty = true; + + for(BodyNode* child : mChildBodyNodes) + child->notifyJacobianDerivUpdate(); +} + //============================================================================== void BodyNode::notifyTransformUpdate() { @@ -1347,6 +1377,10 @@ void BodyNode::notifyTransformUpdate() const SkeletonPtr& skel = getSkeleton(); if(skel) { + // All of these depend on the world transform of this BodyNode, so they must + // be dirtied whenever mNeedTransformUpdate is dirtied, and if + // mTransformUpdate is already dirty, then these must already be dirty as + // well SET_FLAGS(mCoriolisForces); SET_FLAGS(mGravityForces); SET_FLAGS(mCoriolisAndGravityForces); @@ -1371,8 +1405,6 @@ void BodyNode::notifyVelocityUpdate() return; mNeedVelocityUpdate = true; - mIsBodyJacobianSpatialDerivDirty = true; - mIsWorldJacobianClassicDerivDirty = true; mIsPartialAccelerationDirty = true; const SkeletonPtr& skel = getSkeleton(); diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index a31f7c5126fc0..4ed8a822a04a6 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -781,6 +781,14 @@ class BodyNode : public Frame, public SkeletonRefCountingBase // Notifications //---------------------------------------------------------------------------- + /// Notify this BodyNode and all its descendents that their Jacobians need to + /// be updated. + void notifyJacobianUpdate(); + + /// Notify this BodyNode and all its descendents that their Jacobian + /// derivatives need to be updated. + void notifyJacobianDerivUpdate(); + // Documentation inherited void notifyTransformUpdate() override; diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index 1a731095ccae6..0866f06d464fc 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -405,10 +405,8 @@ void Joint::notifyPositionUpdate() if(mChildBodyNode) { mChildBodyNode->notifyTransformUpdate(); - mChildBodyNode->mIsBodyJacobianDirty = true; - mChildBodyNode->mIsWorldJacobianDirty = true; - mChildBodyNode->mIsBodyJacobianSpatialDerivDirty = true; - mChildBodyNode->mIsWorldJacobianClassicDerivDirty = true; + mChildBodyNode->notifyJacobianUpdate(); + mChildBodyNode->notifyJacobianDerivUpdate(); } mIsLocalJacobianDirty = true; @@ -433,7 +431,10 @@ void Joint::notifyPositionUpdate() void Joint::notifyVelocityUpdate() { if(mChildBodyNode) + { mChildBodyNode->notifyVelocityUpdate(); + mChildBodyNode->notifyJacobianDerivUpdate(); + } mIsLocalJacobianTimeDerivDirty = true; diff --git a/unittests/testForwardKinematics.cpp b/unittests/testForwardKinematics.cpp index 070d4f420e516..33982f03f805d 100644 --- a/unittests/testForwardKinematics.cpp +++ b/unittests/testForwardKinematics.cpp @@ -38,6 +38,8 @@ #include #include "TestHelpers.h" +#include "dart/utils/urdf/DartLoader.h" + std::vector twoLinkIndices; //============================================================================== @@ -124,6 +126,85 @@ TEST(FORWARD_KINEMATICS, TWO_ROLLS) } } +//============================================================================== +Eigen::MatrixXd finiteDifferenceJacobian( + const SkeletonPtr& skeleton, + const Eigen::VectorXd& q, + const std::vector& active_indices) +{ + Eigen::MatrixXd J(3, q.size()); + for(int i=0; i < q.size(); ++i) + { + const double dq = 1e-4; + + Eigen::VectorXd q_up = q; + Eigen::VectorXd q_down = q; + + q_up[i] += 0.5*dq; + q_down[i] -= 0.5*dq; + + BodyNode* last_bn = skeleton->getBodyNode(skeleton->getNumBodyNodes()-1); + + skeleton->setPositions(active_indices, q_up); + Eigen::Vector3d x_up = last_bn->getTransform().translation(); + + skeleton->setPositions(active_indices, q_down); + Eigen::Vector3d x_down = last_bn->getTransform().translation(); + + skeleton->setPositions(active_indices, q); + J.col(i) = last_bn->getWorldTransform().linear().transpose() * (x_up - x_down) / dq; + } + + return J; +} + +//============================================================================== +Eigen::MatrixXd standardJacobian( + const SkeletonPtr& skeleton, + const Eigen::VectorXd& q, + const std::vector& active_indices) +{ + skeleton->setPositions(active_indices, q); + BodyNode* last_bn = skeleton->getBodyNode(skeleton->getNumBodyNodes()-1); + + Eigen::MatrixXd J = skeleton->getJacobian(last_bn).bottomRows<3>(); + + Eigen::MatrixXd reduced_J(3, q.size()); + for(int i=0; i < q.size(); ++i) + reduced_J.col(i) = J.col(active_indices[i]); + + return reduced_J; +} + +//============================================================================== +TEST(FORWARD_KINEMATICS, JACOBIAN_PARTIAL_CHANGE) +{ + // This is a regression test for issue #499 + const double tolerance = 1e-8; + + dart::utils::DartLoader loader; + SkeletonPtr skeleton1 = + loader.parseSkeleton(DART_DATA_PATH"urdf/KR5/KR5 sixx R650.urdf"); + + SkeletonPtr skeleton2 = skeleton1->clone(); + + std::vector active_indices; + for(size_t i=0; i < 3; ++i) + active_indices.push_back(i); + + Eigen::VectorXd q = Eigen::VectorXd::Random(active_indices.size()); + Eigen::MatrixXd fd_J = finiteDifferenceJacobian(skeleton1, q, active_indices); + Eigen::MatrixXd J = standardJacobian(skeleton2, q, active_indices); + + EXPECT_TRUE((fd_J - J).norm() < tolerance); + + q = Eigen::VectorXd::Random(active_indices.size()); + fd_J = finiteDifferenceJacobian(skeleton1, q, active_indices); + J = standardJacobian(skeleton2, q, active_indices); + + EXPECT_TRUE((fd_J - J).norm() < tolerance); +} + //============================================================================== int main(int argc, char* argv[]) {