From 0fb5974b1ddc109d7f6c881cd5aafcd402cdfd5f Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 5 Nov 2024 10:37:47 -0800 Subject: [PATCH] bullet-featherstone: Reset joint motor constraint's velocity target after each step (#699) Signed-off-by: Ian Chen --- bullet-featherstone/src/SimulationFeatures.cc | 10 ++++++++++ test/common_test/joint_features.cc | 11 ++++------- 2 files changed, 14 insertions(+), 7 deletions(-) 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 435fdabcb..61fc95272 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