diff --git a/include/gz/sim/components/DetachableJoint.hh b/include/gz/sim/components/DetachableJoint.hh index a2286e4dad..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 @@ -39,14 +40,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 +76,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 +100,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..b73706e70d 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -15,6 +15,7 @@ * */ +#include #include #include @@ -95,6 +96,22 @@ void DetachableJoint::Configure(const Entity &_entity, return; } + // Optional physics parameters + if (_sdf->HasElement("physics")) + { + // Clone as GetElement() requires 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 +268,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..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 @@ -140,6 +141,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..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 @@ -493,7 +494,8 @@ class gz::sim::systems::PhysicsPrivate JointFeatureList, physics::AttachFixedJointFeature, physics::DetachJointFeature, - physics::SetJointTransformFromParentFeature>{}; + physics::SetJointTransformFromParentFeature, + physics::SetDynamicJointConstraintPropertiesFeature>{}; ////////////////////////////////////////////////// // Joint transmitted wrench @@ -1948,6 +1950,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; });