Joint axis mimic constraints: add sdf element (#1166)

This adds to SDFormat 1.11 a new Mimic joint constraint
that enforces a linear relationship between the output
position of two joint axes. This is functionally equivalent to
the Gearbox joint but is more flexible and easier to specify.

The linear relationship is defined with the following equation:
follower_position = multiplier * (leader_position - reference) + offset
where the joint axis containing the mimic tag is the follower,
and the leader is specified using the //mimic/@joint and
//mimic/@axis attributes. The multiplier parameter specifies
the ratio between changes in follower axis position relative to
changes in leader axis position.

Signed-off-by: Aditya <aditya050995@gmail.com>
Signed-off-by: Steve Peters <scpeters@openrobotics.org>
Co-authored-by: Steve Peters <scpeters@openrobotics.org>

Migration.md

@@ -572,6 +572,19 @@ ABI was broken for `sdf::Element`, and restored on version 11.2.1.
 
 ### Additions
 
+1. **mimic.sdf**, **joint.sdf**: a mimic tag can be added to `//joint/axis` and
+    `//joint/axis2` to specify a linear relationship between the position of two
+    joint axes according to the following equation:
+    `follower_position = multiplier * (leader_position - reference) + offset`.
+    The joint axis containing the mimic tag is the follwer and the leader is
+    specified using the `@joint` and `@axis` attributes.
+    + `//mimic/@joint`: name of joint containing the leader axis.
+    + `//mimic/@axis`: name of the leader axis. Only valid values are "axis" or "axis2".
+    + `//mimic/multiplier`: parameter representing ratio between changes in follower axis position relative to changes in leader axis position.
+    + `//mimic/offset`: parameter representing offset to follower position.
+    + `//mimic/reference`: parameter representing reference for leader position before applying the multiplier.
+
+1. **joint.sdf**:
 ## SDFormat specification 1.9 to 1.10
 
 ### Additions

include/sdf/Error.hh

@@ -179,6 +179,10 @@ namespace sdf
 
     /// \brief Generic error during parsing.
     PARSING_ERROR,
+
+    /// \brief The joint axis mimic does not refer to a valid joint in the
+    /// current scope.
+    JOINT_AXIS_MIMIC_INVALID,
   };
 
   class SDFORMAT_VISIBLE Error

include/sdf/JointAxis.hh

@@ -19,6 +19,7 @@
 
 #include <memory>
 #include <string>
+#include <utility>
 #include <gz/math/Vector3.hh>
 #include <gz/utils/ImplPtr.hh>
 #include "sdf/Element.hh"
@@ -37,6 +38,84 @@ namespace sdf
   struct PoseRelativeToGraph;
   template <typename T> class ScopedGraph;
 
+  /// \brief Helper class to hold contents of a joint axis mimic tag, which
+  /// define a Mimic constraint. A Mimic constraint encodes a linear
+  /// relationship between the 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 SDFORMAT_VISIBLE MimicConstraint
+  {
+    /// \brief Constructor with arguments.
+    /// \param[in] _joint Name of Joint containing the leader axis.
+    /// \param[in] _axis Name of the leader axis, either "axis" or "axis2".
+    /// \param[in] _multiplier Multiplier parameter, which represents the ratio
+    /// between changes in the follower position relative to changes in the
+    /// leader position.
+    /// \param[in] _offset Offset to the follower position in the linear
+    /// constraint.
+    /// \param[in] _reference Reference for the leader position before applying
+    /// the multiplier in the linear constraint.
+    public: MimicConstraint(
+              const std::string &_joint = "",
+              const std::string &_axis = "axis",
+              double _multiplier = 0.0,
+              double _offset = 0.0,
+              double _reference = 0.0);
+
+    /// \brief Set the leader joint name.
+    /// \param[in] _joint Name of the the joint containing the leader axis.
+    public: void SetJoint(const std::string &_joint);
+
+    /// \brief Set the leader axis name, either "axis" or "axis2".
+    /// \param[in] _axis Name of the leader axis.
+    public: void SetAxis(const std::string &_axis);
+
+    /// \brief Set the multiplier parameter, which represents the ratio
+    /// between changes in the follower position relative to changes in the
+    /// leader position.
+    /// \param[in] _multiplier Multiplier for the Mimic constraint.
+    public: void SetMultiplier(double _multiplier);
+
+    /// \brief Set the offset to the follower position in the linear constraint.
+    /// \param[in] _offset Offset for the Mimic constraint.
+    public: void SetOffset(double _offset);
+
+    /// \brief Set the reference for the leader position before applying
+    /// the multiplier in the linear constraint.
+    /// \param[in] _reference Reference for the Mimic constraint.
+    public: void SetReference(double _reference);
+
+    /// \brief Retrieve the name of the leader joint.
+    /// \return Name of the joint containing the leader axis.
+    public: const std::string &Joint() const;
+
+    /// \brief Retrieve the name of the leader axis, either "axis" or "axis2".
+    /// \return Name of the leader axis.
+    public: const std::string &Axis() const;
+
+    /// \brief Retrieve the multiplier parameter, which represents the ratio
+    /// between changes in the follower position relative to changes in the
+    /// leader position.
+    /// \return Multiplier for the Mimic constraint.
+    public: double Multiplier() const;
+
+    /// \brief Retrieve the offset to the follower position in the linear
+    /// constraint.
+    /// \return Offset for the Mimic constraint.
+    public: double Offset() const;
+
+    /// \brief Retrieve the reference for the leader position before applying
+    /// the multiplier in the linear constraint.
+    /// \param[in] _reference Reference for the Mimic constraint.
+    public: double Reference() const;
+
+    /// \brief The implementation pointer.
+    GZ_UTILS_IMPL_PTR(dataPtr)
+  };
+
   /// \brief Parameters related to the axis of rotation for rotational joints,
   /// and the axis of translation for prismatic joints.
   class SDFORMAT_VISIBLE JointAxis
@@ -69,6 +148,20 @@ namespace sdf
     public: [[nodiscard]] sdf::Errors SetXyz(
                 const gz::math::Vector3d &_xyz);
 
+    /// \brief Set the Mimic constraint parameters.
+    /// \param[in] _mimic A MimicConstraint object containing the leader joint
+    /// and axis names names, multiplier, offset, and reference to be used
+    /// for mimicking.
+    /// \sa MimicConstraint Mimic()
+    public: void SetMimic(const MimicConstraint &_mimic);
+
+    /// \brief Get the Mimic constraint parameters.
+    /// \return A MimicConstraint object containing the leader joint
+    /// and axis names names, multiplier, offset, and reference to be used
+    /// for mimicking.
+    /// \sa void SetMimic(const MimicConstraint)
+    public: std::optional<MimicConstraint> Mimic() const;
+
     /// \brief Get the physical velocity dependent viscous damping coefficient
     /// of the joint axis. The default value is zero (0.0).
     /// \return The physical velocity dependent viscous damping coefficient

include/sdf/parser.hh

@@ -449,6 +449,15 @@ namespace sdf
   SDFORMAT_VISIBLE
   void checkJointAxisExpressedInValues(const sdf::Root *_root, Errors &_errors);
 
+  /// \brief Check that all joint axes in contained joints that specify mimic
+  /// constraints use valid joint names.
+  /// This checks recursively and should check the files exhaustively
+  /// rather than terminating early when the first error is found.
+  /// \param[in] _root SDF Root object to check recursively.
+  /// \param[out] _errors Detected errors will be appended to this variable.
+  SDFORMAT_VISIBLE
+  void checkJointAxisMimicValues(const sdf::Root *_root, Errors &_errors);
+
   /// \brief For the world and each model, check that the attached_to graphs
   /// build without errors and have no cycles.
   /// Confirm that following directed edges from each vertex in the graph

python/src/sdf/_gz_sdformat_pybind11.cc

@@ -110,6 +110,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) {
   sdf::python::defineMagnetometer(m);
   sdf::python::defineMaterial(m);
   sdf::python::defineMesh(m);
+  sdf::python::defineMimicConstraint(m);
   sdf::python::defineModel(m);
   sdf::python::defineNavSat(m);
   sdf::python::defineNoise(m);

python/src/sdf/pyError.cc

@@ -148,7 +148,8 @@ void defineError(pybind11::object module)
     .value("WARNING", sdf::ErrorCode::WARNING)
     .value("JOINT_AXIS_EXPRESSED_IN_INVALID", sdf::ErrorCode::JOINT_AXIS_EXPRESSED_IN_INVALID)
     .value("CONVERSION_ERROR", sdf::ErrorCode::CONVERSION_ERROR)
-    .value("PARSING_ERROR", sdf::ErrorCode::PARSING_ERROR);
+    .value("PARSING_ERROR", sdf::ErrorCode::PARSING_ERROR)
+    .value("JOINT_AXIS_MIMIC_INVALID", sdf::ErrorCode::JOINT_AXIS_MIMIC_INVALID);
 }
 }  // namespace python
 }  // namespace SDF_VERSION_NAMESPACE

python/src/sdf/pyJointAxis.cc

@@ -31,6 +31,51 @@ namespace sdf
 inline namespace SDF_VERSION_NAMESPACE {
 namespace python
 {
+/////////////////////////////////////////////////
+void defineMimicConstraint(pybind11::object module)
+{
+  pybind11::class_<sdf::MimicConstraint>(module, "MimicConstraint")
+    .def(pybind11::init<>())
+    .def(pybind11::init<const std::string &,
+                        const std::string &,
+                        double,
+                        double,
+                        double>())
+    .def(pybind11::init<sdf::MimicConstraint>())
+    .def("set_joint", &sdf::MimicConstraint::SetJoint,
+         "Set the name of the joint containing the leader axis.")
+    .def("set_axis", &sdf::MimicConstraint::SetAxis,
+         "Set the leader axis name, either \"axis\" or \"axis2\".")
+    .def("set_multiplier", &sdf::MimicConstraint::SetMultiplier,
+         "Set the multiplier parameter, which represents the ratio "
+         "between changes in the follower position relative to changes in the "
+         "leader position.")
+    .def("set_offset", &sdf::MimicConstraint::SetOffset,
+         "Set the offset to the follower position in the linear constraint.")
+    .def("set_reference", &sdf::MimicConstraint::SetReference,
+         "Set the reference for the leader position before applying the "
+         "multiplier in the linear constraint.")
+    .def("joint", &sdf::MimicConstraint::Joint,
+         "Retrieve the name of the leader joint.")
+    .def("axis", &sdf::MimicConstraint::Axis,
+         "Retrieve the name of the leader axis, either \"axis\" or \"axis2\".")
+    .def("multiplier", &sdf::MimicConstraint::Multiplier,
+         "Retrieve the multiplier parameter, which represents the ratio "
+         "between changes in the follower position relative to changes in the "
+         "leader position.")
+    .def("offset", &sdf::MimicConstraint::Offset,
+         "Retrieve the offset to the follower position in the linear constraint.")
+    .def("reference", &sdf::MimicConstraint::Reference,
+         "Retrieve the reference for the leader position before applying the "
+         "multiplier in the linear constraint.")
+    .def("__copy__", [](const sdf::MimicConstraint &self) {
+      return sdf::MimicConstraint(self);
+    })
+    .def("__deepcopy__", [](const sdf::MimicConstraint &self, pybind11::dict) {
+      return sdf::MimicConstraint(self);
+    }, "memo"_a);
+}
+
 /////////////////////////////////////////////////
 void defineJointAxis(pybind11::object module)
 {
@@ -44,6 +89,12 @@ void defineJointAxis(pybind11::object module)
            ThrowIfErrors(self.SetXyz(_xyz));
          },
          "Set the x,y,z components of the axis unit vector.")
+    .def("set_mimic", &sdf::JointAxis::SetMimic,
+         "Set the Mimic constraint parameters.")
+    .def("mimic", &sdf::JointAxis::Mimic,
+         "Get a MimicConstraint object containing the leader joint "
+         "and axis names names, multiplier, offset, and reference to be used "
+         "for mimicking.")
     .def("damping", &sdf::JointAxis::Damping,
          "Get the physical velocity dependent viscous damping coefficient "
          "of the joint axis. The default value is zero (0.0).")

python/src/sdf/pyJointAxis.hh

@@ -29,6 +29,12 @@ namespace sdf
 inline namespace SDF_VERSION_NAMESPACE {
 namespace python
 {
+/// Define a pybind11 wrapper for an sdf::MimicConstraint
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ */
+void defineMimicConstraint(pybind11::object module);
+
 /// Define a pybind11 wrapper for an sdf::JointAxis
 /**
  * \param[in] module a pybind11 module to add the definition to

python/test/pyJointAxis_TEST.py

@@ -14,7 +14,7 @@
 
 import copy
 from gz_test_deps.math import Vector3d
-from gz_test_deps.sdformat import JointAxis, Error, SDFErrorsException
+from gz_test_deps.sdformat import JointAxis, MimicConstraint, Error, SDFErrorsException
 import math
 import unittest
 
@@ -76,6 +76,17 @@ def test_default_construction(self):
         axis.set_dissipation(1.5)
         self.assertAlmostEqual(1.5, axis.dissipation())
 
+        mimic = MimicConstraint("test_joint", "axis", 5.0, 1.0, 2.0)
+
+        self.assertEqual(None, axis.mimic())
+        axis.set_mimic(mimic)
+        self.assertNotEqual(None, axis.mimic())
+        self.assertEqual("test_joint", axis.mimic().joint())
+        self.assertEqual("axis", axis.mimic().axis())
+        self.assertAlmostEqual(5.0, axis.mimic().multiplier())
+        self.assertAlmostEqual(1.0, axis.mimic().offset())
+        self.assertAlmostEqual(2.0, axis.mimic().reference())
+
 
     def test_copy_construction(self):
         jointAxis = JointAxis()

sdf/1.11/CMakeLists.txt

@@ -34,6 +34,7 @@ set (sdfs
   magnetometer.sdf
   material.sdf
   mesh_shape.sdf
+  mimic.sdf
   model.sdf
   model_state.sdf
   navsat.sdf

sdf/1.11/joint.sdf

@@ -112,6 +112,9 @@
       </element>
 
     </element> <!-- End Limit -->
+
+    <include filename="mimic.sdf" required="0"/>
+
   </element> <!-- End Axis -->
 
   <element name="axis2" required="0">
@@ -170,6 +173,9 @@
       </element>
 
     </element> <!-- End Limit -->
+
+    <include filename="mimic.sdf" required="0"/>
+
   </element> <!-- End Axis2 -->
 
   <element name="physics" required="0">

sdf/1.11/mimic.sdf

@@ -0,0 +1,37 @@
+<!-- Mimic -->
+<element name="mimic" required="0">
+  <description>Specifies a linear equality constraint between the position of two joint
+    axes. One joint axis is labeled as the leader and the other as the follower.
+    The joint axis containing a mimic tag is the follower, and the leader
+    is specified by the joint and axis attributes.
+    The multiplier, offset, and reference parameters determine the linear relationship
+    according to the following equation:
+    follower_position = multiplier * (leader_position - reference) + offset.
+    Note that the multiplier and offset parameters match the parameters of
+    the URDF mimic tag if the reference parameter is 0.
+  </description>
+  <attribute name="joint" type="string" default="" required="1">
+    <description>Name of the joint containing the leader axis, i.e. the axis to be mimicked</description>
+  </attribute>
+  <attribute name="axis" type="string" default="axis" required="0">
+    <description>Name of the leader axis, i.e. the axis to be mimicked.
+      The only valid values are "axis" and "axis2", and "axis2" may only be
+      used if the leader joint has multiple axes.
+    </description>
+  </attribute>
+  <element name="multiplier" type="double" default="1.0" required="1">
+    <description>A parameter representing the ratio between changes in the
+      follower joint axis position relative to changes in the leader joint
+      axis position. It can be expressed as follows:
+      multiplier = (follower_position - offset) / (leader_position - reference)
+    </description>
+  </element>
+  <element name="offset" type="double" default="0" required="1">
+    <description>Offset to the follower position in the linear constraint.</description>
+  </element>
+  <element name="reference" type="double" default="0" required="1">
+    <description>Reference for the leader position before applying the
+      multiplier in the linear constraint.
+    </description>
+  </element>
+</element> <!-- End Mimic -->

src/Joint.cc

@@ -166,18 +166,38 @@ Errors Joint::Load(ElementPtr _sdf)
         "] was specified for both."});
   }
 
-  if (_sdf->HasElement("axis"))
+  // Map of follower axis name to dataPtr->axis[] array index
+  // The keys of this map are also the only valid leader axis names
+  const std::map<std::string, std::size_t>
+      followerAxisNames = {{"axis", 0}, {"axis2", 1}};
+  for (const auto &[followerAxis, i] : followerAxisNames)
   {
-    this->dataPtr->axis[0].emplace();
-    Errors axisErrors = this->dataPtr->axis[0]->Load(_sdf->GetElement("axis"));
-    errors.insert(errors.end(), axisErrors.begin(), axisErrors.end());
-  }
-
-  if (_sdf->HasElement("axis2"))
-  {
-    this->dataPtr->axis[1].emplace();
-    Errors axisErrors = this->dataPtr->axis[1]->Load(_sdf->GetElement("axis2"));
-    errors.insert(errors.end(), axisErrors.begin(), axisErrors.end());
+    if (_sdf->HasElement(followerAxis))
+    {
+      auto &axis = this->dataPtr->axis[i].emplace();
+      Errors axisErrors = axis.Load(_sdf->GetElement(followerAxis));
+      errors.insert(errors.end(), axisErrors.begin(), axisErrors.end());
+
+      if (axis.Mimic())
+      {
+        const auto leaderAxis = axis.Mimic()->Axis();
+        if (axis.Mimic()->Joint() == this->Name() &&
+            leaderAxis == followerAxis)
+        {
+          errors.push_back({ErrorCode::JOINT_AXIS_MIMIC_INVALID,
+            "Axis with name [" + followerAxis + "] in " +
+            "joint with name [" + this->dataPtr->name +
+            "] cannot mimic itself."});
+        }
+        if (followerAxisNames.find(leaderAxis) == followerAxisNames.end())
+        {
+          errors.push_back({ErrorCode::JOINT_AXIS_MIMIC_INVALID,
+            "Axis with name [" + followerAxis + "] in " +
+            "joint with name [" + this->dataPtr->name +
+            "] specified an invalid leader axis name [" + leaderAxis + "]."});
+        }
+      }
+    }
   }
 
   if (_sdf->HasElement("screw_thread_pitch"))