Skip to content

Commit

Permalink
bullet-featherstone: Reset joint motor constraint's velocity target a…
Browse files Browse the repository at this point in the history
…fter each step (#699)

Signed-off-by: Ian Chen <ichen@openrobotics.org>
  • Loading branch information
iche033 committed Nov 5, 2024
1 parent 70a72cd commit dd05774
Show file tree
Hide file tree
Showing 2 changed files with 14 additions and 7 deletions.
10 changes: 10 additions & 0 deletions bullet-featherstone/src/SimulationFeatures.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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>());
}
Expand Down
11 changes: 4 additions & 7 deletions test/common_test/joint_features.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit dd05774

Please sign in to comment.