DetachableJoint: allow physics constraint properties to be set

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 <optional>
 #include <string>
 #include <gz/sim/Entity.hh>
 #include <gz/sim/components/Factory.hh>
@@ -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<double> cfm;
+    /// \brief Optional error reduction parameter
+    std::optional<double> 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<double>(std::stod(cfmStr));
+      }
+      if (erpStr != "nullopt")
+      {
+        _info.erp = std::optional<double>(std::stod(erpStr));
+      }
+
       return _in;
     }
   };

src/systems/detachable_joint/DetachableJoint.cc

@@ -15,6 +15,7 @@
  *
  */
 
+#include <optional>
 #include <vector>
 
 #include <gz/plugin/Register.hh>
@@ -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<double>(physicsSdf->Get<double>("cfm"));
+    }
+    if (physicsSdf->HasElement("erp"))
+    {
+      this->erp = std::optional<double>(physicsSdf->Get<double>("erp"));
+    }
+  }
+
   // Setup detach topic
   std::vector<std::string> 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
       {

src/systems/detachable_joint/DetachableJoint.hh

@@ -21,6 +21,7 @@
 #include <gz/msgs/empty.pb.h>
 
 #include <memory>
+#include <optional>
 #include <string>
 #include <gz/transport/Node.hh>
 
@@ -140,6 +141,12 @@ namespace systems
     /// \brief Whether child entity is attached
     private: std::atomic<bool> isAttached{false};
 
+    /// \brief Optional constraint force mixing parameter
+    private: std::optional<double> cfm;
+
+    /// \brief Optional error reduction parameter
+    private: std::optional<double> erp;
+
     /// \brief Whether all parameters are valid and the system can proceed
     private: bool validConfig{false};
 

src/systems/physics/Physics.cc

@@ -26,6 +26,7 @@
 #include <iostream>
 #include <deque>
 #include <map>
+#include <optional>
 #include <set>
 #include <string>
 #include <unordered_map>
@@ -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;
       });