Parse SDF mimic joints and retarget mimic pendulum example (#2254)

Author: jslee02

CHANGELOG.md

@@ -50,6 +50,7 @@
   - Removed all APIs deprecated in DART 6.13 (the legacy `dart::common::Timer` utility, `ConstraintSolver::getConstraints()`/`containSkeleton()`, `ContactConstraint`'s raw constructor and material helper statistics, and the `MetaSkeleton` vector-returning `getBodyNodes()`/`getJoints()` accessors).
   - Removed the remaining 6.13 compatibility shims: deleted `dart/utils/urdf/URDFTypes.hpp`, the Eigen alias typedefs in `math/MathTypes.hpp`, the `dart7::comps::NameComponent` alias, and the legacy `dInfinity`/`dPAD` helpers, and tightened `SkelParser` plane parsing to treat `<point>` as an error.
   - Updated `dart::utils::SdfParser` to canonicalize input through libsdformat so it can parse SDF 1.7+ models without the legacy version gate: [#264](https://github.com/dartsim/dart/issues/264)
+  - `dart::utils::SdfParser` now imports SDF mimic metadata (reference joint/DoF, multiplier/offset, and constraint type), and the mimic pendulum example/tests rely on those parsed joints when reproducing gz-physics #432 instead of bespoke SDF readers.
   - Fixed Collada mesh imports ignoring `<unit>` metadata by preserving the Assimp-provided scale transform ([#287](https://github.com/dartsim/dart/issues/287)).
 
 - dartpy

dart/utils/sdf/SdfParser.cpp

@@ -44,6 +44,7 @@
 #include "dart/dynamics/CylinderShape.hpp"
 #include "dart/dynamics/FreeJoint.hpp"
 #include "dart/dynamics/MeshShape.hpp"
+#include "dart/dynamics/MimicDofProperties.hpp"
 #include "dart/dynamics/PrismaticJoint.hpp"
 #include "dart/dynamics/RevoluteJoint.hpp"
 #include "dart/dynamics/ScrewJoint.hpp"
@@ -193,8 +194,21 @@ struct SDFJoint
   std::string parentName;
   std::string childName;
   std::string type;
+  struct MimicInfo
+  {
+    std::string referenceJointName;
+    std::size_t referenceDof = 0;
+    double multiplier = 1.0;
+    double offset = 0.0;
+    dynamics::MimicConstraintType constraintType
+        = dynamics::MimicConstraintType::Motor;
+  };
+  std::vector<MimicInfo> mimicInfos;
 };
 
+std::vector<SDFJoint::MimicInfo> readMimicElements(
+    const ElementPtr& axisElement);
+
 // Maps the name of a BodyNode to its properties
 using BodyMap = common::aligned_map<std::string, SDFBodyNode>;
 
@@ -213,6 +227,9 @@ dynamics::SkeletonPtr readSkeleton(
     const common::Uri& baseUri,
     const ResolvedOptions& options);
 
+void applyMimicConstraints(
+    const dynamics::SkeletonPtr& skeleton, const JointMap& sdfJoints);
+
 bool createPair(
     dynamics::SkeletonPtr skeleton,
     dynamics::BodyNode* parent,
@@ -807,6 +824,8 @@ dynamics::SkeletonPtr readSkeleton(
   // Set positions to their initial values
   newSkeleton->resetPositions();
 
+  applyMimicConstraints(newSkeleton, sdfJoints);
+
   return newSkeleton;
 }
 
@@ -878,6 +897,62 @@ NextResult getNextJointAndNodePair(
   return VALID;
 }
 
+//==============================================================================
+void applyMimicConstraints(
+    const dynamics::SkeletonPtr& skeleton, const JointMap& sdfJoints)
+{
+  for (const auto& entry : sdfJoints) {
+    const auto& jointInfo = entry.second;
+    if (jointInfo.mimicInfos.empty())
+      continue;
+
+    auto* joint = skeleton->getJoint(jointInfo.properties->mName);
+    if (!joint)
+      continue;
+
+    std::vector<dynamics::MimicDofProperties> props
+        = joint->getMimicDofProperties();
+    props.resize(joint->getNumDofs());
+
+    bool applied = false;
+    bool useCoupler = false;
+    for (const auto& mimic : jointInfo.mimicInfos) {
+      auto* ref = skeleton->getJoint(mimic.referenceJointName);
+      if (!ref) {
+        DART_WARN(
+            "[SdfParser] Ignoring mimic joint [{}] referencing missing joint "
+            "[{}]",
+            jointInfo.properties->mName,
+            mimic.referenceJointName);
+        continue;
+      }
+
+      const std::size_t followerIndex
+          = std::min(mimic.referenceDof, joint->getNumDofs() - 1);
+      const std::size_t referenceIndex
+          = std::min(mimic.referenceDof, ref->getNumDofs() - 1);
+
+      auto& prop = props[followerIndex];
+      prop.mReferenceJoint = ref;
+      prop.mReferenceDofIndex = referenceIndex;
+      prop.mMultiplier = mimic.multiplier;
+      prop.mOffset = mimic.offset;
+      prop.mConstraintType = mimic.constraintType;
+      applied = true;
+      useCoupler
+          = useCoupler
+            || mimic.constraintType == dynamics::MimicConstraintType::Coupler;
+    }
+
+    if (!applied)
+      continue;
+
+    joint->setMimicJointDofs(props);
+    joint->setActuatorType(dynamics::Joint::MIMIC);
+    joint->setUseCouplerConstraint(useCoupler);
+  }
+}
+
 dynamics::SkeletonPtr makeSkeleton(
     const ElementPtr& _skeletonElement, Eigen::Isometry3d& skeletonFrame)
 {
@@ -1343,6 +1418,34 @@ JointMap readAllJoints(
   return sdfJoints;
 }
 
+std::vector<SDFJoint::MimicInfo> readMimicElements(
+    const ElementPtr& axisElement)
+{
+  std::vector<SDFJoint::MimicInfo> mimics;
+  if (!axisElement || !hasElement(axisElement, "mimic"))
+    return mimics;
+
+  const auto mimicElement = getElement(axisElement, "mimic");
+  if (!mimicElement)
+    return mimics;
+
+  SDFJoint::MimicInfo info;
+  info.referenceJointName = getAttributeString(mimicElement, "joint");
+  if (hasAttribute(mimicElement, "axis")) {
+    const auto axisAttr = getAttributeString(mimicElement, "axis");
+    info.referenceDof = axisAttr == "axis2" ? 1u : 0u;
+  }
+  info.multiplier = hasElement(mimicElement, "multiplier")
+                        ? getValueDouble(mimicElement, "multiplier")
+                        : 1.0;
+  info.offset = hasElement(mimicElement, "offset")
+                    ? getValueDouble(mimicElement, "offset")
+                    : 0.0;
+  mimics.push_back(info);
+
+  return mimics;
+}
+
 SDFJoint readJoint(
     const ElementPtr& _jointElement,
     const BodyMap& _sdfBodyNodes,
@@ -1410,6 +1513,23 @@ SDFJoint readJoint(
       = (parent_it == _sdfBodyNodes.end()) ? "" : parent_it->first;
   newJoint.childName = (child_it == _sdfBodyNodes.end()) ? "" : child_it->first;
 
+  //--------------------------------------------------------------------------
+  // Mimic metadata (captured before joint creation)
+  if (hasElement(_jointElement, "axis")) {
+    const ElementPtr& axisElement = getElement(_jointElement, "axis");
+    const auto mimics = readMimicElements(axisElement);
+    newJoint.mimicInfos.insert(
+        newJoint.mimicInfos.end(), mimics.begin(), mimics.end());
+  }
+  if (hasElement(_jointElement, "axis2")) {
+    const ElementPtr& axis2Element = getElement(_jointElement, "axis2");
+    auto mimics = readMimicElements(axis2Element);
+    for (auto& m : mimics)
+      m.referenceDof = 1u; // axis2 maps to the second DoF
+    newJoint.mimicInfos.insert(
+        newJoint.mimicInfos.end(), mimics.begin(), mimics.end());
+  }
+
   //--------------------------------------------------------------------------
   // transformation
   Eigen::Isometry3d parentWorld = Eigen::Isometry3d::Identity();