Mimic constraint feature using bullet-featherstone (gazebosim#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