Mimic constraint feature

dartsim/src/JointFeatures.cc

@@ -723,6 +723,56 @@ Wrench3d JointFeatures::GetJointTransmittedWrenchInJointFrame(
   wrenchOut.force = transmittedWrenchInJoint.tail<3>();
   return wrenchOut;
 }
+
+/////////////////////////////////////////////////
+void JointFeatures::SetJointMimicConstraint(
+    const Identity &_id,
+    const std::string _joint,
+    const std::string,
+    const double _multiplier,
+    const double _offset,
+    const double _reference)
+{
+  auto joint = this->ReferenceInterface<JointInfo>(_id)->joint;
+  auto jointMimic = joint->getSkeleton()->getJoint(_joint);
+
+  // Take extra care that the value is valid. A nan can cause the DART
+  // constraint solver to fail, which will in turn either cause a crash or
+  // collisions to fail
+  if (std::isnan(_multiplier))
+  {
+    gzerr << "Invalid value found in multiplier [" << _multiplier
+           << "] for joint [" << joint->getName()
+           << "]. The command will be ignored\n";
+    return;
+  }
+  if (std::isnan(_offset))
+  {
+    gzerr << "Invalid value found in offset [" << _offset
+           << "] for joint [" << joint->getName()
+           << "]. The command will be ignored\n";
+    return;
+  }
+  // Check if the joint tries to mimic itself.
+  if (joint == jointMimic)
+  {
+    gzerr << "Leader and follower joints for the mimic constraint"
+      << " should not be the same. The constraint will be ignored"
+      << std::endl;
+  }
+  // Check for degrees of freedom.
+  if (joint->getNumDofs() != jointMimic->getNumDofs())
+  {
+    gzerr << "Mimic constraint pair of joints should have the same"
+      << " degress of freedom. The constraint will be ignored"
+      << std::endl;
+  }
+
+  joint->setActuatorType(dart::dynamics::Joint::MIMIC);
+  joint->setMimicJoint(jointMimic, _multiplier,
+      _offset - _multiplier * _reference);
+}
+
 }
 }
 }

dartsim/src/JointFeatures.hh

@@ -56,7 +56,9 @@ struct JointFeatureList : FeatureList<
   SetJointPositionLimitsFeature,
   SetJointVelocityLimitsFeature,
   SetJointEffortLimitsFeature,
-  GetJointTransmittedWrench
+  GetJointTransmittedWrench,
+
+  SetMimicConstraintFeature
 > { };
 
 class JointFeatures :
@@ -203,6 +205,15 @@ class JointFeatures :
   // ----- Transmitted wrench -----
   public: Wrench3d GetJointTransmittedWrenchInJointFrame(
       const Identity &_id) const override;
+
+  // ----- Mimic joint constraint -----
+  public: void SetJointMimicConstraint(
+      const Identity &_id,
+      const std::string _joint,
+      const std::string _axis,
+      const double _multiplier,
+      const double _offset,
+      const double _reference) override;
 };
 
 }

include/gz/physics/Joint.hh

@@ -22,6 +22,8 @@
 #include <gz/physics/FrameSemantics.hh>
 #include <gz/physics/Geometry.hh>
 
+#include <string>
+
 namespace gz
 {
   namespace physics
@@ -584,6 +586,46 @@ namespace gz
       };
     };
 
+    /////////////////////////////////////////////////
+    /// \brief This feature sets the mimic constraint for this Joint.
+    class GZ_PHYSICS_VISIBLE SetMimicConstraintFeature
+        : public virtual Feature
+    {
+      /// \brief The Joint API for setting the mimic joint constraint.
+      /// \param[in] _joint
+      ///   name of the joint to be mimicked, i.e. the leader joint.
+      /// \param[in] _multiplier
+      ///   The multiplier to be applied to poistion of leader joint.
+      /// \param[in] _offset
+      ///   The offset to be applied to position of leader joint after
+      ///   the multiplier is applied.
+      public: template <typename PolicyT, typename FeaturesT>
+      class Joint : public virtual Feature::Joint<PolicyT, FeaturesT>
+      {
+        public: using Scalar = typename PolicyT::Scalar;
+
+        public: void SetMimicConstraint(
+                    const std::string _joint,
+                    const std::string _axis,
+                    Scalar _multiplier,
+                    Scalar _offset,
+                    Scalar _reference);
+      };
+
+      /// \private The implementation API for setting the mimic constraint.
+      public: template <typename PolicyT>
+      class Implementation : public virtual Feature::Implementation<PolicyT>
+      {
+        public: using Scalar = typename PolicyT::Scalar;
+
+        // See Joint::MimicConstraint above
+        public: virtual void SetJointMimicConstraint(
+            const Identity &_id,
+            const std::string _joint,
+            const std::string _axis,
+            Scalar _multiplier, Scalar _offset, Scalar _reference) = 0;
+      };
+    };
   }
 }
 

include/gz/physics/detail/Joint.hh

@@ -21,6 +21,8 @@
 #include <gz/physics/Joint.hh>
 #include "gz/physics/detail/FrameSemantics.hh"
 
+#include <string>
+
 namespace gz
 {
   namespace physics
@@ -214,6 +216,19 @@ namespace gz
         ->SetJointMaxEffort(this->identity, _dof, _value);
     }
 
+    /////////////////////////////////////////////////
+    template <typename PolicyT, typename FeaturesT>
+    void SetMimicConstraintFeature::Joint<PolicyT, FeaturesT>::
+    SetMimicConstraint(
+      const std::string _joint,
+      const std::string _axis,
+      Scalar _multiplier, Scalar _offset, Scalar _reference)
+    {
+      this->template Interface<SetMimicConstraintFeature>()
+        ->SetJointMimicConstraint(this->identity, _joint,
+          _axis, _multiplier, _offset, _reference);
+    }
+
     /////////////////////////////////////////////////
     template <typename PolicyT, typename FeaturesT>
     void DetachJointFeature::Joint<PolicyT, FeaturesT>::Detach()

test/common_test/CMakeLists.txt

@@ -9,6 +9,7 @@ set(tests
   construct_empty_world
   free_joint_features
   joint_features
+  joint_mimic_features
   joint_transmitted_wrench_features
   kinematic_features
   link_features