bullet-featherstone: Remove joint motor constraint on joint force cmd (…

…#713) Signed-off-by: Ian Chen <[email protected]> (cherry picked from commit 7597a2c)
gazebosim · Jan 27, 2025 · cf8c83f · cf8c83f
1 parent 33be3ec
commit cf8c83f
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 3 deletions.
diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc
@@ -287,11 +287,11 @@ void JointFeatures::SetJointAcceleration(
 void JointFeatures::SetJointForce(
     const Identity &_id, const std::size_t _dof, const double _value)
 {
-  const auto *joint = this->ReferenceInterface<JointInfo>(_id);
+  auto *joint = this->ReferenceInterface<JointInfo>(_id);
 
   if (!std::isfinite(_value))
   {
-    gzerr << "Invalid joint velocity value [" << _value
+    gzerr << "Invalid joint force value [" << _value
            << "] commanded on joint [" << joint->name << " DOF " << _dof
            << "]. The command will be ignored\n";
     return;
@@ -302,6 +302,14 @@ void JointFeatures::SetJointForce(
     return;
 
   const auto *model = this->ReferenceInterface<ModelInfo>(joint->model);
+  auto *world = this->ReferenceInterface<WorldInfo>(model->world);
+
+  // Disable velocity control by removing joint motor constraint
+  if (joint->motor)
+  {
+    world->world->removeMultiBodyConstraint(joint->motor.get());
+    joint->motor.reset();
+  }
 
   // clamp the values
   double force = std::clamp(_value,

diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc
@@ -146,7 +146,7 @@ TYPED_TEST(JointFeaturesTest, JointSetCommand)
     auto base_link = model->GetLink("base");
     ASSERT_NE(nullptr, base_link);
 
-    // Check that invalid velocity commands don't cause collisions to fail
+    // Check that invalid force commands don't cause collisions to fail
     for (std::size_t i = 0; i < 1000; ++i)
     {
       // Silence console spam
@@ -181,6 +181,15 @@ TYPED_TEST(JointFeaturesTest, JointSetCommand)
       EXPECT_NEAR(0.0, joint->GetVelocity(0), 1e-1);
     }
 
+    // Set joint force to 0 and expect that the velocity command is no
+    // longer enforced, i.e. joint should not freeze in subsequent steps
+    joint->SetForce(0, 0.0);
+    for (std::size_t i = 0; i < numSteps; ++i)
+    {
+      world->Step(output, state, input);
+      EXPECT_LT(0.0, std::fabs(joint->GetVelocity(0)));
+    }
+
     // Check that invalid velocity commands don't cause collisions to fail
     for (std::size_t i = 0; i < 1000; ++i)
     {