[parsing] Add parsing of custom drake:mimic tag for SDFormat (RobotLo…

…comotion#20503)

Also extends the URDF parser to support <mimic> elements
that specify a joint that is defined after the <joint> the tag
is a member of.

multibody/parsing/detail_sdf_parser.cc

@@ -675,6 +675,7 @@ bool AddJointFromSpecification(
     "drake:rotor_inertia",
     "drake:gear_ratio",
     "drake:controller_gains",
+    "drake:mimic",
     "parent",
     "pose",
     "screw_thread_pitch"};
@@ -895,6 +896,97 @@ bool AddJointFromSpecification(
   return true;
 }
 
+// Helper method to parse a custom drake:mimic tag.
+bool ParseMimicTag(const SDFormatDiagnostic& diagnostic,
+                   const sdf::Joint& joint_spec,
+                   ModelInstanceIndex model_instance,
+                   MultibodyPlant<double>* plant) {
+  if (!joint_spec.Element()->HasElement("drake:mimic")) return true;
+
+  if (!plant->is_discrete() ||
+      plant->get_discrete_contact_solver() != DiscreteContactSolver::kSap) {
+    diagnostic.Warning(
+        joint_spec.Element(),
+        fmt::format("Joint '{}' specifies a drake:mimic element that will be "
+                    "ignored. Mimic elements are currently only supported by "
+                    "MultibodyPlant with a discrete time step and using "
+                    "DiscreteContactSolver::kSap.",
+                    joint_spec.Name()));
+    return true;
+  }
+
+  sdf::ElementPtr mimic_node = joint_spec.Element()->GetElement("drake:mimic");
+
+  if (!mimic_node->HasAttribute("joint")) {
+    diagnostic.Error(
+        mimic_node, fmt::format("Joint '{}' drake:mimic element is missing the "
+                                "required 'joint' attribute.",
+                                joint_spec.Name()));
+    return false;
+  }
+
+  const std::string joint_to_mimic = mimic_node->Get<std::string>("joint");
+
+  if (!plant->HasJointNamed(joint_to_mimic, model_instance)) {
+    diagnostic.Error(
+        mimic_node,
+        fmt::format("Joint '{}' drake:mimic element specifies joint '{}' which"
+                    " does not exist.",
+                    joint_spec.Name(), joint_to_mimic));
+    return false;
+  }
+
+  if (joint_to_mimic == joint_spec.Name()) {
+    diagnostic.Error(mimic_node,
+                     fmt::format("Joint '{}' drake:mimic element specifies "
+                                 "joint '{}'. Joints cannot mimic themselves.",
+                                 joint_spec.Name(), joint_to_mimic));
+    return false;
+  }
+
+  if (!mimic_node->HasAttribute("multiplier")) {
+    diagnostic.Error(
+        mimic_node, fmt::format("Joint '{}' drake:mimic element is missing the "
+                                "required 'multiplier' attribute.",
+                                joint_spec.Name()));
+    return false;
+  }
+
+  if (!mimic_node->HasAttribute("offset")) {
+    diagnostic.Error(
+        mimic_node, fmt::format("Joint '{}' drake:mimic element is missing the "
+                                "required 'offset' attribute.",
+                                joint_spec.Name()));
+    return false;
+  }
+
+  const double gear_ratio = mimic_node->Get<double>("multiplier");
+  const double offset = mimic_node->Get<double>("offset");
+
+  const Joint<double>& joint0 =
+      plant->GetJointByName(joint_spec.Name(), model_instance);
+  const Joint<double>& joint1 =
+      plant->GetJointByName(joint_to_mimic, model_instance);
+
+  if (joint0.num_velocities() != 1 || joint1.num_velocities() != 1) {
+    // The URDF documentation is ambiguous as to whether multi-dof joints
+    // are supported by the mimic tag (which the drake:mimic tag is analogous
+    // to). So we only raise a warning, not an error.
+    diagnostic.Warning(
+        mimic_node,
+        fmt::format("Drake only supports the drake:mimic element for "
+                    "single-dof joints. The joint '{}' (with {} "
+                    "dofs) is attempting to mimic joint '{}' (with "
+                    "{} dofs). The drake:mimic element will be ignored.",
+                    joint0.name(), joint0.num_velocities(), joint_to_mimic,
+                    joint1.num_velocities()));
+  } else {
+    plant->AddCouplerConstraint(joint0, joint1, gear_ratio, offset);
+  }
+
+  return true;
+}
+
 // Helper method to load an SDF file and read the contents into an sdf::Root
 // object.
 [[nodiscard]] sdf::Errors LoadSdf(
@@ -1632,6 +1724,16 @@ std::vector<ModelInstanceIndex> AddModelsFromSpecification(
       }
   }
 
+  // Parse drake:mimic elements only after all joints have been added.
+  for (uint64_t joint_index = 0; joint_index < model.JointCount();
+       ++joint_index) {
+    // Get a pointer to the SDF joint, and the joint axis information.
+    const sdf::Joint& joint = *model.JointByIndex(joint_index);
+    if (!ParseMimicTag(diagnostic, joint, model_instance, plant)) {
+      return {};
+    }
+  }
+
   drake::log()->trace("sdf_parser: Add explicit frames");
   // Add frames at root-level of <model>.
   for (uint64_t frame_index = 0; frame_index < model.FrameCount();
@@ -2192,6 +2294,17 @@ std::vector<ModelInstanceIndex> AddModelsFromSdf(
         }
     }
 
+    // Parse drake:mimic elements only after all joints have been added.
+    for (uint64_t joint_index = 0; joint_index < world.JointCount();
+         ++joint_index) {
+      // Get a pointer to the SDF joint, and the joint axis information.
+      const sdf::Joint& joint = *world.JointByIndex(joint_index);
+      if (!ParseMimicTag(diagnostic, joint, world_model_instance(),
+                         workspace.plant)) {
+        return {};
+      }
+    }
+
     for (sdf::JointType joint_type : joint_types) {
       if (joint_type != sdf::JointType::FIXED) {
         std::string message =

multibody/parsing/detail_urdf_parser.cc

@@ -89,6 +89,7 @@ class UrdfParser {
   void ParseTransmission(const JointEffortLimits& joint_effort_limits,
                          XMLElement* node);
   void ParseJoint(JointEffortLimits* joint_effort_limits, XMLElement* node);
+  void ParseMimicTag(XMLElement* node);
   const Body<double>* GetBodyForElement(const std::string& element_name,
                                         const std::string& link_name);
   void ParseJointDynamics(XMLElement* node, double* damping);
@@ -626,76 +627,94 @@ void UrdfParser::ParseJoint(
   }
 
   joint_effort_limits->emplace(name, effort);
+}
 
+void UrdfParser::ParseMimicTag(XMLElement* node) {
   XMLElement* mimic_node = node->FirstChildElement("mimic");
-  if (mimic_node) {
-    if (!plant->is_discrete() ||
-        plant->get_discrete_contact_solver() != DiscreteContactSolver::kSap) {
-      Warning(
-          *mimic_node,
-          fmt::format("Joint '{}' specifies a mimic element that will be "
-                      "ignored. Mimic elements are currently only supported by "
-                      "MultibodyPlant with a discrete time step and using "
-                      "DiscreteContactSolver::kSap.",
-                      name));
-    } else {
-      std::string joint_to_mimic;
-      double gear_ratio{1.0};
-      double offset{0.0};
-      if (!ParseStringAttribute(mimic_node, "joint", &joint_to_mimic)) {
-        Error(*mimic_node,
-              fmt::format("Joint '{}' mimic element is missing the "
-                          "required 'joint' attribute.",
-                          name));
-        return;
-      }
-      if (!plant->HasJointNamed(joint_to_mimic, model_instance_)) {
-        Error(*mimic_node,
-              fmt::format("Joint '{}' mimic element specifies joint '{}' which"
-                          " does not exist.",
-                          name, joint_to_mimic));
-        return;
-      }
-      ParseScalarAttribute(mimic_node, "multiplier", &gear_ratio);
-      ParseScalarAttribute(mimic_node, "offset", &offset);
-
-      if (!index) {
-        // This can currently happen if we have a "floating" joint, which does
-        // not produce the actual QuaternionFloatingJoint above.
-        Warning(*mimic_node,
-                fmt::format("Drake only supports the mimic element for "
-                            "single-dof joints. The mimic element in joint "
-                            "'{}' will be ignored.",
-                            name));
-      } else {
-        const Joint<double>& joint0 = plant->get_joint(*index);
-        const Joint<double>& joint1 =
-            plant->GetJointByName(joint_to_mimic, model_instance_);
-        if (joint1.num_velocities() != joint0.num_velocities()) {
-          Error(*mimic_node,
-                fmt::format("Joint '{}' which has {} DOF cannot mimic "
-                            "joint '{}' which has {} DOF.",
-                            name, joint0.num_velocities(), joint_to_mimic,
-                            joint1.num_velocities()));
-          return;
-        }
-        if (joint0.num_velocities() != 1) {
-          // The URDF documentation is ambiguous as to whether multi-dof joints
-          // are supported by the mimic tag. So we only raise a warning, not an
-          // error.
-          Warning(*mimic_node,
-                  fmt::format("Drake only supports the mimic element for "
-                              "single-dof joints. The joint '{}' (with {} "
-                              "dofs) is attempting to mimic joint '{}' (with "
-                              "{} dofs). The mimic element will be ignored.",
-                              name, joint0.num_velocities(), joint_to_mimic,
-                              joint1.num_velocities()));
-        } else {
-          plant->AddCouplerConstraint(joint0, joint1, gear_ratio, offset);
-        }
-      }
-    }
+  if (!mimic_node) return;
+
+  auto plant = w_.plant;
+  std::string name;
+  ParseStringAttribute(node, "name", &name);
+
+  if (!plant->is_discrete() ||
+      plant->get_discrete_contact_solver() != DiscreteContactSolver::kSap) {
+    Warning(
+        *mimic_node,
+        fmt::format("Joint '{}' specifies a mimic element that will be "
+                    "ignored. Mimic elements are currently only supported by "
+                    "MultibodyPlant with a discrete time step and using "
+                    "DiscreteContactSolver::kSap.",
+                    name));
+    return;
+  }
+
+  std::string joint_to_mimic;
+  if (!ParseStringAttribute(mimic_node, "joint", &joint_to_mimic)) {
+    Error(*mimic_node, fmt::format("Joint '{}' mimic element is missing the "
+                                   "required 'joint' attribute.",
+                                   name));
+    return;
+  }
+  if (!plant->HasJointNamed(joint_to_mimic, model_instance_)) {
+    Error(*mimic_node,
+          fmt::format("Joint '{}' mimic element specifies joint '{}' which"
+                      " does not exist.",
+                      name, joint_to_mimic));
+    return;
   }
+
+  if (joint_to_mimic == name) {
+    Error(*mimic_node,
+          fmt::format("Joint '{}' mimic element specifies "
+                      "joint '{}'. Joints cannot mimic themselves.",
+                      name, joint_to_mimic));
+    return;
+  }
+
+  double gear_ratio{1.0};
+  double offset{0.0};
+  ParseScalarAttribute(mimic_node, "multiplier", &gear_ratio);
+  ParseScalarAttribute(mimic_node, "offset", &offset);
+
+  if (!plant->HasJointNamed(name)) {
+    // This can currently happen if we have a "floating" joint, which does
+    // not produce the actual QuaternionFloatingJoint above.
+    Warning(*mimic_node,
+            fmt::format("Drake only supports the mimic element for "
+                        "single-dof joints. The mimic element in joint "
+                        "'{}' will be ignored.",
+                        name));
+    return;
+  }
+
+  const Joint<double>& joint0 = plant->GetJointByName(name);
+  const Joint<double>& joint1 =
+      plant->GetJointByName(joint_to_mimic, model_instance_);
+
+  if (joint1.num_velocities() != joint0.num_velocities()) {
+    Error(*mimic_node, fmt::format("Joint '{}' which has {} DOF cannot mimic "
+                                   "joint '{}' which has {} DOF.",
+                                   name, joint0.num_velocities(),
+                                   joint_to_mimic, joint1.num_velocities()));
+    return;
+  }
+
+  if (joint0.num_velocities() != 1) {
+    // The URDF documentation is ambiguous as to whether multi-dof joints
+    // are supported by the mimic tag. So we only raise a warning, not an
+    // error.
+    Warning(*mimic_node,
+            fmt::format("Drake only supports the mimic element for "
+                        "single-dof joints. The joint '{}' (with {} "
+                        "dofs) is attempting to mimic joint '{}' (with "
+                        "{} dofs). The mimic element will be ignored.",
+                        name, joint0.num_velocities(), joint_to_mimic,
+                        joint1.num_velocities()));
+    return;
+  }
+
+  plant->AddCouplerConstraint(joint0, joint1, gear_ratio, offset);
 }
 
 void UrdfParser::ParseMechanicalReduction(const XMLElement& node) {
@@ -1069,6 +1088,14 @@ std::pair<std::optional<ModelInstanceIndex>, std::string> UrdfParser::Parse() {
     }
   }
 
+  for (XMLElement* joint_node = node->FirstChildElement(); joint_node;
+       joint_node = joint_node->NextSiblingElement()) {
+    const std::string node_name(joint_node->Name());
+    if (node_name == "joint" || node_name == "drake:joint") {
+      ParseMimicTag(joint_node);
+    }
+  }
+
   // Parses the model's transmission elements.
   for (XMLElement* transmission_node = node->FirstChildElement("transmission");
        transmission_node;

multibody/parsing/parsing_doxygen.h

@@ -179,6 +179,7 @@ Here is the full list of custom elements:
 - @ref tag_drake_linear_bushing_rpy
 - @ref tag_drake_member
 - @ref tag_drake_mesh_resolution_hint
+- @ref tag_drake_mimic
 - @ref tag_drake_mu_dynamic
 - @ref tag_drake_mu_static
 - @ref tag_drake_parent
@@ -677,6 +678,21 @@ values will select longer edge lengths and a coarser mesh.
 
 @see @ref tag_drake_proximity_properties, @ref hug_properties
 
+@subsection tag_drake_mimic drake:mimic
+
+- SDFormat path: `//model/joint/drake:mimic`
+- URDF path: unsupported
+- Syntax: Attributes `joint` (string), `multiplier` (double) and `offset` (double)
+
+@subsubsection tag_drake_mimic_semantics Semantics
+
+This tag has equivalent semantics to those of the native URDF <mimic> tag. If
+`q0` is the position of the `<joint>` and `q1` the position of the joint
+specified by the `joint` attribute, the two joints are constrained to enforce
+the relation: `q0 = multiplier * q1 + offset`. The units of `multiplier` and
+`offset` depend on the type of joints specified. This tag only supports single
+degree of freedom joints that exist in the same model instance.
+
 @subsection tag_drake_mu_dynamic drake:mu_dynamic
 
 - SDFormat path: `//model/link/collision/drake:proximity_properties/drake:mu_dynamic`