From 6a7ee85d6cb322820f9b4f161d1727b5666048fe Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 30 Jun 2025 15:57:37 +0100 Subject: [PATCH 1/3] DetachableJoint: allow physics constraint properties to be set - Add a parameter block with and sub elements. - Update the `Physics` system to use the feature physics::SetDynamicJointConstraintPropertiesFeature - Add optional fields to the detachable joint component. Signed-off-by: Rhys Mainwaring --- include/gz/sim/components/DetachableJoint.hh | 39 +++++++++++++++++-- .../detachable_joint/DetachableJoint.cc | 28 ++++++++++++- .../detachable_joint/DetachableJoint.hh | 6 +++ src/systems/physics/Physics.cc | 22 ++++++++++- 4 files changed, 88 insertions(+), 7 deletions(-) diff --git a/include/gz/sim/components/DetachableJoint.hh b/include/gz/sim/components/DetachableJoint.hh index a2286e4dad..014de7d165 100644 --- a/include/gz/sim/components/DetachableJoint.hh +++ b/include/gz/sim/components/DetachableJoint.hh @@ -39,14 +39,20 @@ namespace components Entity parentLink; /// \brief Entity of the echild link Entity childLink; - // \brief Type of joint. Only the "fixed" joint type is currently supported. + /// \brief Type of joint. Only the "fixed" joint type is currently supported. std::string jointType = {"fixed"}; + /// \brief Optional constraint force mixing parameter + std::optional cfm; + /// \brief Optional error reduction parameter + std::optional erp; public: bool operator==(const DetachableJointInfo &_info) const { return (this->parentLink == _info.parentLink) && (this->childLink == _info.childLink) && - (this->jointType == _info.jointType); + (this->jointType == _info.jointType) && + (this->cfm == _info.cfm) && + (this->erp == _info.erp); } public: bool operator!=(const DetachableJointInfo &_info) const @@ -69,8 +75,20 @@ namespace serializers std::ostream &_out, const components::DetachableJointInfo &_info) { + std::string cfmStr("nullopt"); + if (_info.cfm.has_value()) + { + cfmStr = std::to_string(_info.cfm.value()); + } + std::string erpStr("nullopt"); + if (_info.erp.has_value()) + { + erpStr = std::to_string(_info.erp.value()); + } + _out << _info.parentLink << " " << _info.childLink << " " - << _info.jointType; + << _info.jointType << " " + << cfmStr << " " << erpStr; return _out; } @@ -81,7 +99,20 @@ namespace serializers public: static std::istream &Deserialize( std::istream &_in, components::DetachableJointInfo &_info) { - _in >> _info.parentLink >> _info.childLink >> _info.jointType; + std::string cfmStr; + std::string erpStr; + _in >> _info.parentLink >> _info.childLink >> _info.jointType + >> cfmStr >> erpStr; + + if (cfmStr != "nullopt") + { + _info.cfm = std::optional(std::stod(cfmStr)); + } + if (erpStr != "nullopt") + { + _info.erp = std::optional(std::stod(erpStr)); + } + return _in; } }; diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 93e504758d..5eecd4ec22 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -95,6 +95,22 @@ void DetachableJoint::Configure(const Entity &_entity, return; } + // Optional physics parameters + if (_sdf->HasElement("physics")) + { + // Clone as GetElement() requries non-const access. + auto sdfClone = _sdf->Clone(); + auto physicsSdf = sdfClone->GetElement("physics"); + if (physicsSdf->HasElement("cfm")) + { + this->cfm = std::optional(physicsSdf->Get("cfm")); + } + if (physicsSdf->HasElement("erp")) + { + this->erp = std::optional(physicsSdf->Get("erp")); + } + } + // Setup detach topic std::vector detachTopics; if (_sdf->HasElement("detach_topic")) @@ -251,13 +267,21 @@ void DetachableJoint::PreUpdate( _ecm.CreateComponent( this->detachableJointEntity, - components::DetachableJoint({this->parentLinkEntity, - this->childLinkEntity, "fixed"})); + components::DetachableJoint({ + this->parentLinkEntity, + this->childLinkEntity, + "fixed", + this->cfm, + this->erp + })); this->attachRequested = false; this->isAttached = true; this->PublishJointState(this->isAttached); gzdbg << "Attaching entity: " << this->detachableJointEntity << std::endl; + + //! @todo create a separate component for the dynamic joint + /// constraints? } else { diff --git a/src/systems/detachable_joint/DetachableJoint.hh b/src/systems/detachable_joint/DetachableJoint.hh index c207b2d2ac..704e91e509 100644 --- a/src/systems/detachable_joint/DetachableJoint.hh +++ b/src/systems/detachable_joint/DetachableJoint.hh @@ -140,6 +140,12 @@ namespace systems /// \brief Whether child entity is attached private: std::atomic isAttached{false}; + /// \brief Optional constraint force mixing parameter + private: std::optional cfm; + + /// \brief Optional error reduction parameter + private: std::optional erp; + /// \brief Whether all parameters are valid and the system can proceed private: bool validConfig{false}; diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 6a2f3eda79..e5c86141e8 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -493,7 +493,8 @@ class gz::sim::systems::PhysicsPrivate JointFeatureList, physics::AttachFixedJointFeature, physics::DetachJointFeature, - physics::SetJointTransformFromParentFeature>{}; + physics::SetJointTransformFromParentFeature, + physics::SetDynamicJointConstraintPropertiesFeature>{}; ////////////////////////////////////////////////// // Joint transmitted wrench @@ -1948,6 +1949,25 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm, { gzerr << "DetachableJoint could not be created." << std::endl; } + + // Set joint constraint properties + if (_jointInfo->Data().cfm.has_value()) + { + gzdbg << "SetConstraintForceMixing: " + << _jointInfo->Data().cfm.value() + << std::endl; + childLinkDetachableJointFeature->SetConstraintForceMixing( + _jointInfo->Data().cfm.value()); + } + if (_jointInfo->Data().erp.has_value()) + { + gzdbg << "SetErrorReductionParameter: " + << _jointInfo->Data().erp.value() + << std::endl; + childLinkDetachableJointFeature->SetErrorReductionParameter( + _jointInfo->Data().erp.value()); + } + return true; }); From 4ae1e9c418526aee1f8d590436f4ba30637eda26 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 1 Jul 2025 09:47:26 +0100 Subject: [PATCH 2/3] DetachableJoint: include Signed-off-by: Rhys Mainwaring --- include/gz/sim/components/DetachableJoint.hh | 1 + src/systems/detachable_joint/DetachableJoint.cc | 1 + src/systems/detachable_joint/DetachableJoint.hh | 1 + src/systems/physics/Physics.cc | 1 + 4 files changed, 4 insertions(+) diff --git a/include/gz/sim/components/DetachableJoint.hh b/include/gz/sim/components/DetachableJoint.hh index 014de7d165..a7d4aa8b7d 100644 --- a/include/gz/sim/components/DetachableJoint.hh +++ b/include/gz/sim/components/DetachableJoint.hh @@ -17,6 +17,7 @@ #ifndef GZ_SIM_COMPONENTS_DETACHABLE_JOINT_HH_ #define GZ_SIM_COMPONENTS_DETACHABLE_JOINT_HH_ +#include #include #include #include diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 5eecd4ec22..6dac629485 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -15,6 +15,7 @@ * */ +#include #include #include diff --git a/src/systems/detachable_joint/DetachableJoint.hh b/src/systems/detachable_joint/DetachableJoint.hh index 704e91e509..deb067e867 100644 --- a/src/systems/detachable_joint/DetachableJoint.hh +++ b/src/systems/detachable_joint/DetachableJoint.hh @@ -21,6 +21,7 @@ #include #include +#include #include #include diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index e5c86141e8..0bb0bd06f5 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include From 54d0ebfc8ece246ec29e624bce11b901b2151e01 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 1 Jul 2025 09:51:34 +0100 Subject: [PATCH 3/3] DetachableJoint: correct spelling Signed-off-by: Rhys Mainwaring --- src/systems/detachable_joint/DetachableJoint.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 6dac629485..b73706e70d 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -99,7 +99,7 @@ void DetachableJoint::Configure(const Entity &_entity, // Optional physics parameters if (_sdf->HasElement("physics")) { - // Clone as GetElement() requries non-const access. + // Clone as GetElement() requires non-const access. auto sdfClone = _sdf->Clone(); auto physicsSdf = sdfClone->GetElement("physics"); if (physicsSdf->HasElement("cfm"))