Physics: make mimic constraint an optional feature

Signed-off-by: Steve Peters <[email protected]>
gazebosim · Aug 10, 2023 · 1678861 · 1678861
1 parent dfcd35e
commit 1678861
Showing 1 changed file with 58 additions and 15 deletions.
diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
@@ -470,14 +470,18 @@ class gz::sim::systems::PhysicsPrivate
             physics::GetJointFromModel,
             physics::GetBasicJointProperties,
             physics::GetBasicJointState,
-            physics::SetBasicJointState,
-            physics::SetMimicConstraintFeature>{};
+            physics::SetBasicJointState>{};
 
   /// \brief Feature list to construct joints
   public: struct ConstructSdfJointFeatureList : gz::physics::FeatureList<
             JointFeatureList,
             gz::physics::sdf::ConstructSdfJoint>{};
 
+  /// \brief Feature list for mimic constraints
+  public: struct MimicConstraintJointFeatureList : gz::physics::FeatureList<
+            JointFeatureList,
+            physics::SetMimicConstraintFeature>{};
+
   //////////////////////////////////////////////////
   // Detachable joints
 
@@ -681,6 +685,7 @@ class gz::sim::systems::PhysicsPrivate
             physics::Joint,
             JointFeatureList,
             DetachableJointFeatureList,
+            MimicConstraintJointFeatureList,
             JointVelocityCommandFeatureList,
             JointGetTransmittedWrenchFeatureList,
             JointPositionLimitsCommandFeatureList,
@@ -1702,33 +1707,71 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           // No need to create this joint because it was already created when
           // parsing the model.
 
-          // Check if mimic joint constraint should be applied to this joint.
+          // Check if mimic constraint should be applied to this joint's axes.
           auto jointAxis = _ecm.Component<components::JointAxis>(_entity);
           auto jointAxis2 = _ecm.Component<components::JointAxis2>(_entity);
 
           if (jointAxis)
           {
             if (auto mimic = jointAxis->Data().Mimic())
             {
-              existingJoint->SetMimicConstraint(0,
-                  mimic->Joint(),
-                  mimic->Axis(),
-                  mimic->Multiplier(),
-                  mimic->Offset(),
-                  mimic->Reference());
+              auto jointPtrMimic = this->entityJointMap
+                  .EntityCast<MimicConstraintJointFeatureList>(existingJoint);
+              if (jointPtrMimic)
+              {
+                jointPtrMimic->SetMimicConstraint(0,
+                    mimic->Joint(),
+                    mimic->Axis(),
+                    mimic->Multiplier(),
+                    mimic->Offset(),
+                    mimic->Reference());
+              }
+              else
+              {
+                static bool informed{false};
+                if (!informed)
+                {
+                  gzerr << "Attempting to create a mimic constraint for joint ["
+                        <<_name->Data()
+                        << "] but the chosen physics engine does not support "
+                        << "mimic constraints, so no constraint will be "
+                        << "created."
+                        << std::endl;
+                  informed = true;
+                }
+              }
             }
           }
 
           if (jointAxis2)
           {
             if (auto mimic = jointAxis2->Data().Mimic())
             {
-              existingJoint->SetMimicConstraint(1,
-                  mimic->Joint(),
-                  mimic->Axis(),
-                  mimic->Multiplier(),
-                  mimic->Offset(),
-                  mimic->Reference());
+              auto jointPtrMimic = this->entityJointMap
+                  .EntityCast<MimicConstraintJointFeatureList>(existingJoint);
+              if (jointPtrMimic)
+              {
+                jointPtrMimic->SetMimicConstraint(1,
+                    mimic->Joint(),
+                    mimic->Axis(),
+                    mimic->Multiplier(),
+                    mimic->Offset(),
+                    mimic->Reference());
+              }
+              else
+              {
+                static bool informed{false};
+                if (!informed)
+                {
+                  gzerr << "Attempting to create a mimic constraint for joint ["
+                        <<_name->Data()
+                        << "] but the chosen physics engine does not support "
+                        << "mimic constraints, so no constraint will be "
+                        << "created."
+                        << std::endl;
+                  informed = true;
+                }
+              }
             }
           }