bullet-featherstone: Reset joint motor constraint's velocity target after each step (#699)

Signed-off-by: Ian Chen <ichen@openrobotics.org>

Author: iche033

bullet-featherstone/src/SimulationFeatures.cc

@@ -51,6 +51,16 @@ void SimulationFeatures::WorldForwardStep(
   worldInfo->world->stepSimulation(static_cast<btScalar>(stepSize), 1,
                                    static_cast<btScalar>(stepSize));
 
+  // Reset joint velocity target after each step to be consistent with dart's
+  // joint velocity command behavior
+  for (auto & joint : this->joints)
+  {
+    if (joint.second->motor)
+    {
+      joint.second->motor->setVelocityTarget(btScalar(0));
+    }
+  }
+
   this->WriteRequiredData(_h);
   this->Write(_h.Get<ChangedWorldPoses>());
 }

test/common_test/joint_features.cc

@@ -174,14 +174,11 @@ TYPED_TEST(JointFeaturesTest, JointSetCommand)
       EXPECT_NEAR(1.0, joint->GetVelocity(0), 1e-2);
     }
 
-    if(this->PhysicsEngineName(name) == "dartsim")
+    for (std::size_t i = 0; i < numSteps; ++i)
     {
-      for (std::size_t i = 0; i < numSteps; ++i)
-      {
-        // expect joint to freeze in subsequent steps without SetVelocityCommand
-        world->Step(output, state, input);
-        EXPECT_NEAR(0.0, joint->GetVelocity(0), 1e-1);
-      }
+      // expect joint to freeze in subsequent steps without SetVelocityCommand
+      world->Step(output, state, input);
+      EXPECT_NEAR(0.0, joint->GetVelocity(0), 1e-1);
     }
 
     // Check that invalid velocity commands don't cause collisions to fail