World::reset clears constraint impulses

Author: jslee02

dart/simulation/World.cpp

@@ -274,6 +274,11 @@ void World::reset()
   mFrame = 0;
   mRecording->clear();
   mConstraintSolver->clearLastCollisionResult();
+
+  for (auto& skel : mSkeletons) {
+    skel->clearConstraintImpulses();
+    skel->setImpulseApplied(false);
+  }
 }
 
 //==============================================================================

dart/simulation/World.hpp

@@ -239,7 +239,10 @@ class DART_API World : public virtual common::Subject
   // Simulation
   //--------------------------------------------------------------------------
 
-  /// Reset the time, frame counter and recorded histories
+  /// Reset the time, frame counter and recorded histories.
+  ///
+  /// This also clears constraint impulses on all Skeletons in the World so
+  /// stale constraint forces do not leak across independent simulation runs.
   void reset();
 
   /// Calculate the dynamics and integrate the world for one step

tests/integration/dynamics/test_SkeletonState.cpp

@@ -169,3 +169,35 @@ TEST(Issue1089, JointImpulseState)
         joint->getConstraintImpulse(i), static_cast<double>((i + 1) * 2));
   }
 }
+
+//==============================================================================
+TEST(Issue1086, WorldResetClearsConstraintImpulses)
+{
+  auto world = std::make_shared<dart::simulation::World>();
+  auto skel = dart::dynamics::Skeleton::create("skel");
+
+  const auto pair
+      = skel->createJointAndBodyNodePair<dart::dynamics::FreeJoint>(nullptr);
+  auto* joint = pair.first;
+  auto body = pair.second;
+
+  body->setConstraintImpulse(Eigen::Vector6d::Ones());
+  for (std::size_t i = 0; i < joint->getNumDofs(); ++i) {
+    joint->setVelocityChange(i, 1.0);
+    joint->setConstraintImpulse(i, 1.0);
+  }
+  skel->setImpulseApplied(true);
+
+  EXPECT_FALSE(body->getConstraintImpulse().isZero());
+  EXPECT_TRUE(skel->isImpulseApplied());
+
+  world->addSkeleton(skel);
+  world->reset();
+
+  EXPECT_TRUE(body->getConstraintImpulse().isZero());
+  for (std::size_t i = 0; i < joint->getNumDofs(); ++i) {
+    EXPECT_DOUBLE_EQ(joint->getVelocityChange(i), 0.0);
+    EXPECT_DOUBLE_EQ(joint->getConstraintImpulse(i), 0.0);
+  }
+  EXPECT_FALSE(skel->isImpulseApplied());
+}