diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc index c3509721d..e74cdbebc 100644 --- a/bullet-featherstone/src/SimulationFeatures.cc +++ b/bullet-featherstone/src/SimulationFeatures.cc @@ -51,6 +51,16 @@ void SimulationFeatures::WorldForwardStep( worldInfo->world->stepSimulation(static_cast(stepSize), 1, static_cast(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()); } diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 129465d95..d8db8484b 100644 --- a/test/common_test/joint_features.cc +++ b/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