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