From 71a3684755926fea63a23f4d83d40ad36646f136 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 25 Jan 2025 01:31:06 +0000 Subject: [PATCH 1/2] Remove joint motor constraint on joint force cmd Signed-off-by: Ian Chen --- bullet-featherstone/src/JointFeatures.cc | 15 ++++++++++++--- test/common_test/joint_features.cc | 11 ++++++++++- 2 files changed, 22 insertions(+), 4 deletions(-) diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index 59d5c65a8..a7d74260b 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -271,6 +271,7 @@ void JointFeatures::SetJointVelocity( return; const auto *model = this->ReferenceInterface(joint->model); + model->body->getJointVelMultiDof(identifier->indexInBtModel)[_dof] = static_cast(_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(_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; @@ -301,7 +302,15 @@ void JointFeatures::SetJointForce( if (!identifier) return; - const auto *model = this->ReferenceInterface(joint->model); + 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 61fc95272..37ae8c87f 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) { From b143ca37d3ebc3d3525b02d01541db16a98d76fe Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 25 Jan 2025 01:37:56 +0000 Subject: [PATCH 2/2] undo some changes Signed-off-by: Ian Chen --- bullet-featherstone/src/JointFeatures.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index a7d74260b..9b6bc377d 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -271,7 +271,6 @@ void JointFeatures::SetJointVelocity( return; const auto *model = this->ReferenceInterface(joint->model); - model->body->getJointVelMultiDof(identifier->indexInBtModel)[_dof] = static_cast(_value); model->body->wakeUp(); @@ -302,7 +301,7 @@ void JointFeatures::SetJointForce( if (!identifier) return; - auto *model = this->ReferenceInterface(joint->model); + const auto *model = this->ReferenceInterface(joint->model); auto *world = this->ReferenceInterface(model->world); // Disable velocity control by removing joint motor constraint