Merge pull request #818 from dartsim/fix_zero_dof

Fix that ZeroDofJoint::getIndexInTree() is called

CHANGELOG.md

@@ -6,6 +6,10 @@
 
   * Fixed bug of ContactConstraint with kinematic joints: [#809](https://github.com/dartsim/dart/pull/809)
 
+* Misc
+
+  * Fixed that ZeroDofJoint::getIndexInTree was called: [#818](https://github.com/dartsim/dart/pull/818)
+
 ### DART 6.1.1 (2016-10-14)
 
 * Build

dart/dynamics/BodyNode.cpp

@@ -2092,9 +2092,13 @@ void BodyNode::aggregateSpatialToGeneralized(Eigen::VectorXd& _generalized,
   }
 
   // Project the spatial quantity to generalized coordinates
-  std::size_t iStart = mParentJoint->getIndexInTree(0);
-  _generalized.segment(iStart, mParentJoint->getNumDofs())
-      = mParentJoint->getSpatialToGeneralized(mArbitrarySpatial);
+  const auto numDofs = mParentJoint->getNumDofs();
+  if (numDofs > 0u)
+  {
+    const std::size_t iStart = mParentJoint->getIndexInTree(0);
+    _generalized.segment(iStart, numDofs)
+        = mParentJoint->getSpatialToGeneralized(mArbitrarySpatial);
+  }
 }
 
 //==============================================================================

unittests/comprehensive/test_Skeleton.cpp

@@ -765,6 +765,26 @@ TEST(Skeleton, ZeroDofJointInReferential)
   EXPECT_FALSE(branch->getIndexOf(zeroDof2) == INVALID_INDEX);
 }
 
+TEST(Skeleton, ZeroDofJointConstraintForces)
+{
+  // This is a regression test which makes sure that the BodyNodes of
+  // ZeroDofJoints will be correctly aggregate constraint forces.
+  SkeletonPtr skel = Skeleton::create();
+
+  BodyNode* bn = skel->createJointAndBodyNodePair<RevoluteJoint>().second;
+  BodyNode* zeroDof1 = skel->createJointAndBodyNodePair<WeldJoint>(bn).second;
+  bn = skel->createJointAndBodyNodePair<PrismaticJoint>(zeroDof1).second;
+  skel->createJointAndBodyNodePair<WeldJoint>(bn);
+
+  const auto numSkelDofs = skel->getNumDofs();
+  for (auto& bodyNode : skel->getBodyNodes())
+    bodyNode->setConstraintImpulse(Eigen::Vector6d::Random());
+
+  // Make sure this does not cause seg-fault
+  Eigen::VectorXd constraintForces = skel->getConstraintForces();
+  EXPECT_EQ(constraintForces.size(), static_cast<int>(numSkelDofs));
+}
+
 TEST(Skeleton, Referential)
 {
   std::vector<SkeletonPtr> skeletons = getSkeletons();