Reset internal force of point mass before every simulation step (#1372)

* Reset internal force of point mass before every simulation step * Fix typos * Update changelog
dartsim · Jul 14, 2019 · 2466c01 · 2466c01
1 parent ae7f426
commit 2466c01
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 2 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -2,6 +2,10 @@
 
 ### [DART 6.10.0 (20XX-XX-XX)](https://github.com/dartsim/dart/milestone/58?closed=1)
 
+* Dynamics
+
+  * Fixed soft body simulation when command input is not resetted: [#1372](https://github.com/dartsim/dart/pull/1372)
+
 * GUI
 
   * Fixed memory leaks from dart::gui::osg::Viewer: [#1349](https://github.com/dartsim/dart/pull/1349)

diff --git a/dart/dynamics/PointMass.cpp b/dart/dynamics/PointMass.cpp
@@ -842,7 +842,7 @@ void PointMass::updateBiasForceFD(double _dt, const Eigen::Vector3d& _gravity)
   double kv = mParentSoftBodyNode->getVertexSpringStiffness();
   double ke = mParentSoftBodyNode->getEdgeSpringStiffness();
   double kd = mParentSoftBodyNode->getDampingCoefficient();
-  int nN = getNumConnectedPointMasses();
+  std::size_t nN = getNumConnectedPointMasses();
   mAlpha = state.mForces - (kv + nN * ke) * getPositions()
            - (_dt * (kv + nN * ke) + kd) * getVelocities()
            - getMass() * getPartialAccelerations() - mB;

diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp
@@ -713,8 +713,16 @@ void SoftBodyNode::updateBiasForce(
 {
   const Eigen::Matrix6d& mI
       = BodyNode::mAspectProperties.mInertia.getSpatialTensor();
-  for (auto& pointMass : mPointMasses)
+  for (PointMass* pointMass : mPointMasses)
+  {
+    // Reset internal forces of point masses before used.
+    //
+    // Once control force for point mass is introduced, assign it to the
+    // internal force instead of always resetting the internal forces to zero.
+    pointMass->resetForces();
+
     pointMass->updateBiasForceFD(_timeStep, _gravity);
+  }
 
   // Gravity force
   if (BodyNode::mAspectProperties.mGravityMode == true)