bullet-featherstone: add mimic constraint

WIP Signed-off-by: Steve Peters <[email protected]>
gazebosim · Jul 5, 2023 · 52259f3 · 52259f3
1 parent 0b4f490
commit 52259f3
Show file tree

Hide file tree

Showing 3 changed files with 67 additions and 0 deletions.
diff --git a/bullet-featherstone/src/Base.hh b/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>

diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc
@@ -385,6 +385,61 @@ Wrench3d JointFeatures::GetJointTransmittedWrenchInJointFrame(
     jointInfo->jointFeedback->m_reactionForces.getAngular());
   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 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);
+
+  // 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;
+  }
+
+  btMultiBodyGearConstraint* multibodyGear =
+    new btMultiBodyGearConstraint(
+      model->body.get(),
+      *leaderChild->indexInModel,
+      model->body.get(),
+      *followerChild->indexInModel,
+      btVector3(0, 0, 0),
+      btVector3(0, 0, 0),
+      btMatrix3x3::getIdentity(),
+      btMatrix3x3::getIdentity());
+  multibodyGear->setMaxAppliedImpulse(btScalar(1e8));
+  multibodyGear->setGearRatio(btScalar(_multiplier));
+  auto *world = this->ReferenceInterface<WorldInfo>(model->world);
+  world->world->addMultiBodyConstraint(multibodyGear);
+}
 }  // namespace bullet_featherstone
 }  // namespace physics
 }  // namespace gz
diff --git a/bullet-featherstone/src/JointFeatures.hh b/bullet-featherstone/src/JointFeatures.hh
@@ -49,6 +49,8 @@ struct JointFeatureList : FeatureList<
 
   GetJointTransmittedWrench,
 
+  SetMimicConstraintFeature,
+
   FixedJointCast
 > { };
 
@@ -141,6 +143,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;
 };
 }  // namespace bullet_featherstone
 }  // namespace physics