Merge pull request #500 from dartsim/grey/fix_499

Dirtying Jacobian update flags whenever the parent transform is changed -- Release 5.0 patch

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();

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;
 

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;
 

unittests/testForwardKinematics.cpp

@@ -38,6 +38,8 @@
 #include <gtest/gtest.h>
 #include "TestHelpers.h"
 
+#include "dart/utils/urdf/DartLoader.h"
+
 std::vector<size_t> twoLinkIndices;
 
 //==============================================================================
@@ -124,6 +126,85 @@ TEST(FORWARD_KINEMATICS, TWO_ROLLS)
   }
 }
 
+//==============================================================================
+Eigen::MatrixXd finiteDifferenceJacobian(
+    const SkeletonPtr& skeleton,
+    const Eigen::VectorXd& q,
+    const std::vector<size_t>& 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<size_t>& 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<size_t> 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[])
 {