diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index 44328e4fb..c133b6639 100644 --- 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(_id); + auto *joint = this->ReferenceInterface(_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(joint->model); + auto *world = this->ReferenceInterface(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 index d8db8484b..fb92c4f7b 100644 --- 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) {