Update total mass when removing bodies

dartsim · May 31, 2016 · fbb6da4 · fbb6da4
1 parent f53cb4d
commit fbb6da4
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 0 deletions.
diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp
@@ -2216,6 +2216,8 @@ void Skeleton::unregisterBodyNode(BodyNode* _oldBodyNode)
                                      mSoftBodyNodes.end(), soft),
                          mSoftBodyNodes.end());
   }
+
+  updateTotalMass();
 }
 
 //==============================================================================

diff --git a/unittests/testSkeleton.cpp b/unittests/testSkeleton.cpp
@@ -1257,6 +1257,16 @@ TEST(Skeleton, Updating)
   screw->setPitch(3);
   J0f = screw->getRelativeJacobian();
   EXPECT_FALSE(equals(J0i, J0f));
+
+  // Regression test for Pull Request #
+  const double originalMass = skeleton->getMass();
+  BodyNode* lastBn = skeleton->getBodyNode(skeleton->getNumBodyNodes()-1);
+  const double removedMass = lastBn->getMass();
+  EXPECT_FALSE(removedMass == 0.0);
+  lastBn->remove();
+  const double newMass = skeleton->getMass();
+  EXPECT_FALSE(originalMass == newMass);
+  EXPECT_TRUE(newMass == originalMass - removedMass);
 }
 
 int main(int argc, char* argv[])