Mimic constraint feature using bullet-featherstone

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

@@ -385,6 +385,76 @@ Wrench3d JointFeatures::GetJointTransmittedWrenchInJointFrame(
     jointInfo->jointFeedback->m_reactionForces.getAngular());
   return wrenchOut;
 }
+
+/////////////////////////////////////////////////
+void JointFeatures::SetJointMimicConstraint(
+    const Identity &_id,
+    const std::size_t /*_dof*/,  // bullet doesn't mimic multi-axis joints yet
+    const std::string &_joint,
+    const std::string &/*_axis*/,
+    const double _multiplier,
+    const double _offset,
+    const double _reference)
+{
+  auto followerJoint = this->ReferenceInterface<JointInfo>(_id);
+  auto followerChild = this->ReferenceInterface<LinkInfo>(
+      followerJoint->childLinkID);
+  if (!followerChild->indexInModel.has_value())
+  {
+    // print error message
+    return;
+  }
+  const auto *model =
+      this->ReferenceInterface<ModelInfo>(followerJoint->model);
+
+  // Get world pointer and remove an existing mimic / gear constraint
+  auto *world = this->ReferenceInterface<WorldInfo>(model->world);
+  if (followerJoint->gearConstraint)
+  {
+    world->world->removeMultiBodyConstraint(
+        followerJoint->gearConstraint.get());
+    followerJoint->gearConstraint.reset();
+  }
+
+
+  // This would be easier with EntityManagementFeatures::GetJoint()
+  const auto leaderJointIt = model->jointNameToEntityId.find(_joint);
+  if (leaderJointIt == model->jointNameToEntityId.end())
+  {
+    // print error message
+    return;
+  }
+  auto leaderJointId =
+      this->GenerateIdentity(leaderJointIt->second,
+                             this->joints.at(leaderJointIt->second));
+  auto leaderJoint = this->ReferenceInterface<JointInfo>(leaderJointId);
+  auto leaderChild = this->ReferenceInterface<LinkInfo>(
+      leaderJoint->childLinkID);
+  if (!followerChild->indexInModel.has_value())
+  {
+    // print error message
+    return;
+  }
+
+  followerJoint->gearConstraint = std::make_shared<btMultiBodyGearConstraint>(
+      model->body.get(),
+      *followerChild->indexInModel,
+      model->body.get(),
+      *leaderChild->indexInModel,
+      btVector3(0, 0, 0),
+      btVector3(0, 0, 0),
+      btMatrix3x3::getIdentity(),
+      btMatrix3x3::getIdentity());
+  followerJoint->gearConstraint->setGearRatio(btScalar(-_multiplier));
+  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());
+}
 }  // 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: void SetJointMimicConstraint(
+      const Identity &_id,
+      const std::size_t _dof,
+      const std::string &_joint,
+      const std::string &_axis,
+      const double _multiplier,
+      const double _offset,
+      const 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,52 @@ 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] _dof
+      ///   The desired generalized coordinate corresponding to the follower
+      ///   axis within this joint. Values start from 0 and stop before
+      ///   Joint::GetDegreesOfFreedom().
+      /// \param[in] _joint
+      ///   name of the joint to be mimicked, i.e. the leader joint.
+      /// \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.
+      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::size_t _dof,
+                    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::size_t _dof,
+            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,20 @@ namespace gz
         ->SetJointMaxEffort(this->identity, _dof, _value);
     }
 
+    /////////////////////////////////////////////////
+    template <typename PolicyT, typename FeaturesT>
+    void SetMimicConstraintFeature::Joint<PolicyT, FeaturesT>::
+    SetMimicConstraint(
+      const std::size_t _dof,
+      const std::string &_joint,
+      const std::string &_axis,
+      Scalar _multiplier, Scalar _offset, Scalar _reference)
+    {
+      this->template Interface<SetMimicConstraintFeature>()
+        ->SetJointMimicConstraint(this->identity, _dof, _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