Remove joint motor constraint on joint force cmd

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

Author: iche033

bullet-featherstone/src/JointFeatures.cc

@@ -271,6 +271,7 @@ void JointFeatures::SetJointVelocity(
     return;
 
   const auto *model = this->ReferenceInterface<ModelInfo>(joint->model);
+
   model->body->getJointVelMultiDof(identifier->indexInBtModel)[_dof] =
       static_cast<btScalar>(_value);
   model->body->wakeUp();
@@ -287,11 +288,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;
@@ -301,7 +302,15 @@ void JointFeatures::SetJointForce(
   if (!identifier)
     return;
 
-  const auto *model = this->ReferenceInterface<ModelInfo>(joint->model);
+  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,

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)
     {