Mimic constraint feature using bullet-featherstone (#517)

Adds the SetMimicConstraintFeature with an implementation
in bullet-featherstone using btMultiBodyGearConstraint.
The MimicMotorConstraint in dartsim does not create a
bilateral constraint between joints, so it does not satisfy
the requirements of this feature, so only the
bullet-featherstone implementation is provided for now.

Signed-off-by: Aditya Pande <adityapande@google.com>
Signed-off-by: Steve Peters <scpeters@openrobotics.org>
Co-authored-by: Aditya Pande <adityapande@google.com>

bullet-featherstone/src/Base.hh

@@ -24,6 +24,7 @@
 #include <BulletDynamics/Featherstone/btMultiBody.h>
 #include <BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h>
 #include <BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h>
+#include <BulletDynamics/Featherstone/btMultiBodyGearConstraint.h>
 #include <BulletDynamics/Featherstone/btMultiBodyJointFeedback.h>
 #include <BulletDynamics/Featherstone/btMultiBodyJointMotor.h>
 #include <BulletDynamics/Featherstone/btMultiBodyLinkCollider.h>
@@ -169,6 +170,7 @@ struct JointInfo
   btScalar effort = 0;
 
   std::shared_ptr<btMultiBodyFixedConstraint> fixedConstraint = nullptr;
+  std::shared_ptr<btMultiBodyGearConstraint> gearConstraint = nullptr;
   std::shared_ptr<btMultiBodyJointFeedback> jointFeedback = nullptr;
 };
 

bullet-featherstone/src/JointFeatures.cc

@@ -20,6 +20,7 @@
 #include <algorithm>
 #include <memory>
 
+#include <gz/common/Console.hh>
 #include <sdf/Joint.hh>
 
 namespace gz {
@@ -385,6 +386,81 @@ Wrench3d JointFeatures::GetJointTransmittedWrenchInJointFrame(
     jointInfo->jointFeedback->m_reactionForces.getAngular());
   return wrenchOut;
 }
+
+/////////////////////////////////////////////////
+bool JointFeatures::SetJointMimicConstraint(
+    const Identity &_id,
+    std::size_t _dof,
+    const BaseJoint3dPtr &_leaderJoint,
+    std::size_t _leaderAxisDof,
+    double _multiplier,
+    double _offset,
+    double _reference)
+{
+  if (_dof != 0 || _leaderAxisDof != 0)
+  {
+    gzerr << "Failed to set mimic constraint for follower axis " << _dof
+          << " and leader axis " << _leaderAxisDof
+          << " because bullet-featherstone doesn't yet support mimic "
+          << " constraints for multi-axis joints."
+          << std::endl;
+    return false;
+  }
+
+  auto followerJoint = this->ReferenceInterface<JointInfo>(_id);
+  auto followerChild = this->ReferenceInterface<LinkInfo>(
+      followerJoint->childLinkID);
+  auto followerChildIndexInModel = followerChild->indexInModel.value_or(-1);
+  const auto *model =
+      this->ReferenceInterface<ModelInfo>(followerJoint->model);
+
+  auto leaderJointId = _leaderJoint->FullIdentity();
+  auto leaderJoint = this->ReferenceInterface<JointInfo>(leaderJointId);
+  auto leaderChild = this->ReferenceInterface<LinkInfo>(
+      leaderJoint->childLinkID);
+  auto leaderChildIndexInModel = leaderChild->indexInModel.value_or(-1);
+
+  // Get world pointer and remove an existing mimic / gear constraint
+  // after follower and leader entities have been found.
+  auto *world = this->ReferenceInterface<WorldInfo>(model->world);
+  if (followerJoint->gearConstraint)
+  {
+    world->world->removeMultiBodyConstraint(
+        followerJoint->gearConstraint.get());
+    followerJoint->gearConstraint.reset();
+  }
+
+  followerJoint->gearConstraint = std::make_shared<btMultiBodyGearConstraint>(
+      model->body.get(),
+      followerChildIndexInModel,
+      model->body.get(),
+      leaderChildIndexInModel,
+      btVector3(0, 0, 0),
+      btVector3(0, 0, 0),
+      btMatrix3x3::getIdentity(),
+      btMatrix3x3::getIdentity());
+  // btMultiBodyGearConstraint isn't clearly documented, but from trial and
+  // and error, I found that the Gear Ratio should have the opposite sign
+  // of the multiplier parameter.
+  followerJoint->gearConstraint->setGearRatio(btScalar(-_multiplier));
+  // Recall the linear equation from the definition of the mimic constraint:
+  // follower_position = multiplier * (leader_position - reference) + offset
+  //
+  // The equation can be rewritten to collect the constant terms involving the
+  // reference and offset parameters together:
+  // follower_position = multiplier*leader_position
+  //                   + offset - multiplier * reference
+  // The RelativePositionTarget is then set as (offset - multiplier * reference)
+  followerJoint->gearConstraint->setRelativePositionTarget(
+      btScalar(_offset - _multiplier * _reference));
+  // TODO(scpeters): figure out what is a good value for this
+  followerJoint->gearConstraint->setMaxAppliedImpulse(btScalar(1e8));
+  // setErp is needed to correct position constraint errors
+  // this is especially relevant to the offset and reference parameters
+  followerJoint->gearConstraint->setErp(btScalar(0.3));
+  world->world->addMultiBodyConstraint(followerJoint->gearConstraint.get());
+  return true;
+}
 }  // namespace bullet_featherstone
 }  // namespace physics
 }  // namespace gz

bullet-featherstone/src/JointFeatures.hh

@@ -49,6 +49,8 @@ struct JointFeatureList : FeatureList<
 
   GetJointTransmittedWrench,
 
+  SetMimicConstraintFeature,
+
   FixedJointCast
 > { };
 
@@ -141,6 +143,16 @@ class JointFeatures :
   // ----- Transmitted wrench -----
   public: Wrench3d GetJointTransmittedWrenchInJointFrame(
       const Identity &_id) const override;
+
+  // ----- Mimic joint constraint -----
+  public: bool SetJointMimicConstraint(
+      const Identity &_id,
+      std::size_t _dof,
+      const BaseJoint3dPtr &_leaderJoint,
+      std::size_t _leaderAxisDof,
+      double _multiplier,
+      double _offset,
+      double _reference) override;
 };
 }  // namespace bullet_featherstone
 }  // namespace physics

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,72 @@ namespace gz
       };
     };
 
+    /////////////////////////////////////////////////
+    /// \brief This feature applies a Mimic constraint to an axis of this Joint.
+    /// This constraint encodes a linear relationship between the output
+    /// position of two joint axes. One joint axis is labelled as the *leader*
+    /// and the other as the *follower*.
+    /// The multiplier, offset, and reference parameters determine the linear
+    /// relationship according to the following equation:
+    ///
+    /// follower_position = multiplier * (leader_position - reference) + offset
+    class GZ_PHYSICS_VISIBLE SetMimicConstraintFeature
+        : public virtual Feature
+    {
+      public: template <typename PolicyT, typename FeaturesT>
+      class Joint : public virtual Feature::Joint<PolicyT, FeaturesT>
+      {
+        public: using Scalar = typename PolicyT::Scalar;
+
+        /// \brief The Joint API for applying a mimic constraint to an axis of
+        /// this joint.
+        /// \param[in] _dof
+        ///   The desired generalized coordinate corresponding to the follower
+        ///   axis within this joint. Values start from 0 and stop before
+        ///   Joint::GetDegreesOfFreedom().
+        /// \param[in] _leaderJoint
+        ///   pointer to the joint containing the axis to be mimicked,
+        ///   i.e. the leader joint.
+        /// \param[in] _leaderAxisDof
+        ///   The desired generalized coordinate corresponding to the leader
+        ///   axis within the _leaderJoint. Values start from 0 and stop before
+        ///   Joint::GetDegreesOfFreedom().
+        /// \param[in] _multiplier
+        ///   The multiplier to be applied to position of leader joint.
+        /// \param[in] _offset
+        ///   The offset to be applied to position of leader joint after
+        ///   the multiplier is applied.
+        /// \param[in] _reference
+        ///   Reference for the leader position before applying the multiplier
+        ///   in the linear constraint.
+        /// \return True if mimic constraint was set successfully,
+        /// false otherwise.
+        public: bool SetMimicConstraint(
+                    std::size_t _dof,
+                    const BaseJointPtr<PolicyT> &_leaderJoint,
+                    std::size_t _leaderAxisDof,
+                    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 bool SetJointMimicConstraint(
+            const Identity &_id,
+            std::size_t _dof,
+            const BaseJointPtr<PolicyT> &_leaderJoint,
+            std::size_t _leaderAxisDof,
+            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,22 @@ namespace gz
         ->SetJointMaxEffort(this->identity, _dof, _value);
     }
 
+    /////////////////////////////////////////////////
+    template <typename PolicyT, typename FeaturesT>
+    bool SetMimicConstraintFeature::Joint<PolicyT, FeaturesT>::
+    SetMimicConstraint(
+      std::size_t _dof,
+      const BaseJointPtr<PolicyT> &_leaderJoint,
+      std::size_t _leaderAxisDof,
+      Scalar _multiplier,
+      Scalar _offset,
+      Scalar _reference)
+    {
+      return this->template Interface<SetMimicConstraintFeature>()
+        ->SetJointMimicConstraint(this->identity, _dof, _leaderJoint,
+          _leaderAxisDof, _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