Extend Support for Mimic Joint

CHANGELOG.md

@@ -16,6 +16,8 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 - Allow use of `pathlib.Path | str` for paths in python bindings ([#2431](https://github.com/stack-of-tasks/pinocchio/pull/2431))
 - Add Pseudo inertia and Log-cholesky parametrization ([#2296](https://github.com/stack-of-tasks/pinocchio/pull/2296))
 - Add Pixi support ([#2459](https://github.com/stack-of-tasks/pinocchio/pull/2459))
+- Extend support for mimic joint: rnea, crba, forward kinematics, centroidal, jacobians and frames ([#2441](https://github.com/stack-of-tasks/pinocchio/pull/2441))
+- Add idx_j, nj members in joint models, with their respective column/row/block selectors. ([#2441](https://github.com/stack-of-tasks/pinocchio/pull/2441))
 
 ### Fixed
 - Fix linkage of Boost.Serialization on Windows ([#2400](https://github.com/stack-of-tasks/pinocchio/pull/2400))

bindings/python/multibody/sample-models.cpp

@@ -26,13 +26,27 @@ namespace pinocchio
       return model;
     }
 
+    Model buildSampleModelHumanoidRandom(bool usingFF, bool mimic)
+    {
+      Model model;
+      buildModels::humanoidRandom(model, usingFF, mimic);
+      return model;
+    }
+
     Model buildSampleModelManipulator()
     {
       Model model;
       buildModels::manipulator(model);
       return model;
     }
 
+    Model buildSampleModelManipulator(bool mimic)
+    {
+      Model model;
+      buildModels::manipulator(model, mimic);
+      return model;
+    }
+
 #ifdef PINOCCHIO_WITH_HPP_FCL
     GeometryModel buildSampleGeometryModelManipulator(const Model & model)
     {
@@ -80,11 +94,23 @@ namespace pinocchio
         "Generate a (hard-coded) model of a humanoid robot with 6-DOF limbs and random joint "
         "placements.\nOnly meant for unit tests.");
 
+      bp::def(
+        "buildSampleModelHumanoidRandom",
+        static_cast<Model (*)(bool, bool)>(pinocchio::python::buildSampleModelHumanoidRandom),
+        bp::args("using_free_flyer", "mimic"),
+        "Generate a (hard-coded) model of a humanoid robot with 6-DOF limbs and random joint "
+        "placements.\nOnly meant for unit tests.");
+
       bp::def(
         "buildSampleModelManipulator",
         static_cast<Model (*)()>(pinocchio::python::buildSampleModelManipulator),
         "Generate a (hard-coded) model of a simple manipulator.");
 
+      bp::def(
+        "buildSampleModelManipulator",
+        static_cast<Model (*)(bool)>(pinocchio::python::buildSampleModelManipulator),
+        bp::args("mimic"), "Generate a (hard-coded) model of a simple manipulator.");
+
 #ifdef PINOCCHIO_WITH_HPP_FCL
       bp::def(
         "buildSampleGeometryModelManipulator",

bindings/python/parsers/urdf/model.cpp

@@ -20,94 +20,107 @@ namespace pinocchio
 
 #ifdef PINOCCHIO_WITH_URDFDOM
 
-    Model buildModelFromUrdf(const bp::object & filename)
+    Model buildModelFromUrdf(const bp::object & filename, const bool mimic = false)
     {
       Model model;
-      pinocchio::urdf::buildModel(path(filename), model);
+      pinocchio::urdf::buildModel(path(filename), model, mimic);
       return model;
     }
 
-    Model & buildModelFromUrdf(const bp::object & filename, Model & model)
+    Model & buildModelFromUrdf(const bp::object & filename, Model & model, const bool mimic = false)
     {
-      return pinocchio::urdf::buildModel(path(filename), model);
+      return pinocchio::urdf::buildModel(path(filename), model, mimic);
     }
 
-    Model buildModelFromUrdf(const bp::object & filename, const JointModel & root_joint)
+    Model buildModelFromUrdf(
+      const bp::object & filename, const JointModel & root_joint, const bool mimic = false)
     {
       Model model;
-      pinocchio::urdf::buildModel(path(filename), root_joint, model);
+      pinocchio::urdf::buildModel(path(filename), root_joint, model, mimic);
       return model;
     }
 
     Model buildModelFromUrdf(
       const bp::object & filename,
       const JointModel & root_joint,
-      const std::string & root_joint_name)
+      const std::string & root_joint_name,
+      const bool mimic = false)
     {
       Model model;
-      pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model);
+      pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model, mimic);
       return model;
     }
 
-    Model &
-    buildModelFromUrdf(const bp::object & filename, const JointModel & root_joint, Model & model)
+    Model & buildModelFromUrdf(
+      const bp::object & filename,
+      const JointModel & root_joint,
+      Model & model,
+      const bool mimic = false)
     {
-      return pinocchio::urdf::buildModel(path(filename), root_joint, model);
+      return pinocchio::urdf::buildModel(path(filename), root_joint, model, mimic);
     }
 
     Model & buildModelFromUrdf(
       const bp::object & filename,
       const JointModel & root_joint,
       const std::string & root_joint_name,
-      Model & model)
+      Model & model,
+      const bool mimic = false)
     {
-      return pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model);
+      return pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model, mimic);
     }
 
-    Model buildModelFromXML(const std::string & xml_stream, const JointModel & root_joint)
+    Model buildModelFromXML(
+      const std::string & xml_stream, const JointModel & root_joint, const bool mimic = false)
     {
       Model model;
-      pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model);
+      pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model, mimic);
       return model;
     }
 
     Model buildModelFromXML(
       const std::string & xml_stream,
       const JointModel & root_joint,
-      const std::string & root_joint_name)
+      const std::string & root_joint_name,
+      const bool mimic = false)
     {
       Model model;
-      pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model);
+      pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model, mimic);
       return model;
     }
 
-    Model &
-    buildModelFromXML(const std::string & xml_stream, const JointModel & root_joint, Model & model)
+    Model & buildModelFromXML(
+      const std::string & xml_stream,
+      const JointModel & root_joint,
+      Model & model,
+      const bool mimic = false)
     {
-      pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model);
+      pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model, mimic);
       return model;
     }
 
     Model & buildModelFromXML(
       const std::string & xml_stream,
       const JointModel & root_joint,
       const std::string & root_joint_name,
-      Model & model)
+      Model & model,
+      const bool mimic = false)
     {
-      pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model);
+      pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model, mimic);
       return model;
     }
 
-    Model buildModelFromXML(const std::string & xml_stream)
+    Model buildModelFromXML(const std::string & xml_stream, const bool mimic = true)
     {
       Model model;
-      pinocchio::urdf::buildModelFromXML(xml_stream, model);
+      pinocchio::urdf::buildModelFromXML(xml_stream, model, mimic);
       return model;
     }
 
-    Model & buildModelFromXML(const std::string & xml_stream, Model & model)
+    Model &
+    buildModelFromXML(const std::string & xml_stream, Model & model, const bool mimic = true)
     {
-      pinocchio::urdf::buildModelFromXML(xml_stream, model);
+      pinocchio::urdf::buildModelFromXML(xml_stream, model, mimic);
       return model;
     }
 
@@ -120,39 +133,41 @@ namespace pinocchio
 
       bp::def(
         "buildModelFromUrdf",
-        static_cast<Model (*)(const bp::object &, const JointModel &)>(
+        static_cast<Model (*)(const bp::object &, const JointModel &, const bool)>(
           pinocchio::python::buildModelFromUrdf),
-        bp::args("urdf_filename", "root_joint"),
+        bp::args("urdf_filename", "root_joint", "mimic"),
         "Parse the URDF file given in input and return a pinocchio Model starting with the "
         "given root joint.");
 
       bp::def(
         "buildModelFromUrdf",
-        static_cast<Model (*)(const bp::object &, const JointModel &, const std::string &)>(
+        static_cast<Model (*)(
+          const bp::object &, const JointModel &, const std::string &, const bool)>(
           pinocchio::python::buildModelFromUrdf),
-        bp::args("urdf_filename", "root_joint", "root_joint_name"),
+        bp::args("urdf_filename", "root_joint", "root_joint_name", "mimic"),
         "Parse the URDF file given in input and return a pinocchio Model starting with the "
         "given root joint with its specified name.");
 
       bp::def(
         "buildModelFromUrdf",
-        static_cast<Model (*)(const bp::object &)>(pinocchio::python::buildModelFromUrdf),
-        bp::args("urdf_filename"),
+        static_cast<Model (*)(const bp::object &, const bool)>(
+          pinocchio::python::buildModelFromUrdf),
+        bp::args("urdf_filename", "mimic"),
         "Parse the URDF file given in input and return a pinocchio Model.");
 
       bp::def(
         "buildModelFromUrdf",
-        static_cast<Model & (*)(const bp::object &, Model &)>(
+        static_cast<Model & (*)(const bp::object &, Model &, const bool)>(
           pinocchio::python::buildModelFromUrdf),
-        bp::args("urdf_filename", "model"),
+        bp::args("urdf_filename", "model", "mimic"),
         "Append to a given model a URDF structure given by its filename.",
         bp::return_internal_reference<2>());
 
       bp::def(
         "buildModelFromUrdf",
-        static_cast<Model & (*)(const bp::object &, const JointModel &, Model &)>(
+        static_cast<Model & (*)(const bp::object &, const JointModel &, Model &, const bool)>(
           pinocchio::python::buildModelFromUrdf),
-        bp::args("urdf_filename", "root_joint", "model"),
+        bp::args("urdf_filename", "root_joint", "model", "mimic"),
         "Append to a given model a URDF structure given by its filename and the root joint.\n"
         "Remark: In the URDF format, a joint of type fixed can be defined. For efficiency reasons,"
         "it is treated as operational frame and not as a joint of the model.",
@@ -161,8 +176,8 @@ namespace pinocchio
       bp::def(
         "buildModelFromUrdf",
         static_cast<Model & (*)(const bp::object &, const JointModel &, const std::string &,
-                                Model &)>(pinocchio::python::buildModelFromUrdf),
-        bp::args("urdf_filename", "root_joint", "root_joint_name", "model"),
+                                Model &, const bool)>(pinocchio::python::buildModelFromUrdf),
+        bp::args("urdf_filename", "root_joint", "root_joint_name", "model", "mimic"),
         "Append to a given model a URDF structure given by its filename and the root joint with "
         "its specified name.\n"
         "Remark: In the URDF format, a joint of type fixed can be defined. For efficiency reasons,"
@@ -171,49 +186,72 @@ namespace pinocchio
 
       bp::def(
         "buildModelFromXML",
-        static_cast<Model (*)(const std::string &, const JointModel &)>(
+        static_cast<Model (*)(const std::string &, const JointModel &, const bool)>(
           pinocchio::python::buildModelFromXML),
-        bp::args("urdf_xml_stream", "root_joint"),
+        bp::args("urdf_xml_stream", "root_joint", "mimic"),
         "Parse the URDF XML stream given in input and return a pinocchio Model starting with "
         "the given root joint.");
 
       bp::def(
         "buildModelFromXML",
-        static_cast<Model (*)(const std::string &, const JointModel &, const std::string &)>(
+        static_cast<Model (*)(
+          const std::string &, const JointModel &, const std::string &, const bool)>(
           pinocchio::python::buildModelFromXML),
-        bp::args("urdf_xml_stream", "root_joint", "root_joint_name"),
+        bp::args(
+          "urdf_xml_stream", "root_joint",
+          "root_joint_name"
+          "mimic"),
         "Parse the URDF XML stream given in input and return a pinocchio Model starting with "
         "the given root joint with its specified name.");
 
       bp::def(
         "buildModelFromXML",
-        static_cast<Model & (*)(const std::string &, const JointModel &, Model &)>(
+        static_cast<Model (*)(
+          const std::string &, const JointModel &, const std::string &, const bool)>(
           pinocchio::python::buildModelFromXML),
-        bp::args("urdf_xml_stream", "root_joint", "model"),
+        bp::args("urdf_xml_stream", "root_joint", "root_joint_name", "mimic"),
+        "Parse the URDF XML stream given in input and return a pinocchio Model starting with "
+        "the given root joint with its specified name.");
+
+      bp::def(
+        "buildModelFromXML",
+        static_cast<Model & (*)(const std::string &, const JointModel &, Model &, const bool)>(
+          pinocchio::python::buildModelFromXML),
+        bp::args("urdf_xml_stream", "root_joint", "model", "mimic"),
         "Parse the URDF XML stream given in input and append it to the input model with the "
         "given interfacing joint.",
         bp::return_internal_reference<3>());
 
       bp::def(
         "buildModelFromXML",
         static_cast<Model & (*)(const std::string &, const JointModel &, const std::string &,
-                                Model &)>(pinocchio::python::buildModelFromXML),
-        bp::args("urdf_xml_stream", "root_joint", "root_joint_name", "model"),
+                                Model &, const bool)>(pinocchio::python::buildModelFromXML),
+        bp::args("urdf_xml_stream", "root_joint", "root_joint_name", "model", "mimic"),
+        "Parse the URDF XML stream given in input and append it to the input model with the "
+        "given interfacing joint with its specified name.",
+        bp::return_internal_reference<3>());
+
+      bp::def(
+        "buildModelFromXML",
+        static_cast<Model & (*)(const std::string &, const JointModel &, const std::string &,
+                                Model &, const bool)>(pinocchio::python::buildModelFromXML),
+        bp::args("urdf_xml_stream", "root_joint", "root_joint_name", "model", "mimic"),
         "Parse the URDF XML stream given in input and append it to the input model with the "
         "given interfacing joint with its specified name.",
         bp::return_internal_reference<3>());
 
       bp::def(
         "buildModelFromXML",
-        static_cast<Model (*)(const std::string &)>(pinocchio::python::buildModelFromXML),
-        bp::args("urdf_xml_stream"),
+        static_cast<Model (*)(const std::string &, const bool)>(
+          pinocchio::python::buildModelFromXML),
+        bp::args("urdf_xml_stream", "mimic"),
         "Parse the URDF XML stream given in input and return a pinocchio Model.");
 
       bp::def(
         "buildModelFromXML",
-        static_cast<Model & (*)(const std::string &, Model &)>(
+        static_cast<Model & (*)(const std::string &, Model &, const bool)>(
           pinocchio::python::buildModelFromXML),
-        bp::args("urdf_xml_stream", "model"),
+        bp::args("urdf_xml_stream", "model", "mimic"),
         "Parse the URDF XML stream given in input and append it to the input model.",
         bp::return_internal_reference<2>());
 #endif

bindings/python/pinocchio/shortcuts.py

@@ -25,6 +25,7 @@ def buildModelsFromUrdf(
         - verbose - print information of parsing (default - False)
         - meshLoader - object used to load meshes (default - hpp::fcl::MeshLoader)
         - geometry_types - Which geometry model to load. Can be pin.GeometryType.COLLISION, pin.GeometryType.VISUAL or both. (default - [pin.GeometryType.COLLISION, pin.GeometryType.VISUAL])
+        - mimic - If urdf mimic joints should be parsed or not (default - False)
     Return:
         Tuple of the models, in this order : model, collision model, and visual model.
 
@@ -34,7 +35,14 @@ def buildModelsFromUrdf(
     Remark: In the URDF format, a joint of type fixed can be defined. For efficiency reasons, it is treated as operational frame and not as a joint of the model.
     """
     # Handle the switch from old to new api
-    arg_keys = ["package_dirs", "root_joint", "verbose", "meshLoader", "geometry_types"]
+    arg_keys = [
+        "package_dirs",
+        "root_joint",
+        "verbose",
+        "meshLoader",
+        "geometry_types",
+        "mimic",
+    ]
     if len(args) >= 3:
         if isinstance(args[2], str):
             arg_keys = [
@@ -44,6 +52,7 @@ def buildModelsFromUrdf(
                 "verbose",
                 "meshLoader",
                 "geometry_types",
+                "mimic",
             ]
 
     for key, arg in zip(arg_keys, args):
@@ -63,16 +72,17 @@ def _buildModelsFromUrdf(
     verbose=False,
     meshLoader=None,
     geometry_types=None,
+    mimic=False,
 ) -> Tuple[pin.Model, pin.GeometryModel, pin.GeometryModel]:
     if geometry_types is None:
         geometry_types = [pin.GeometryType.COLLISION, pin.GeometryType.VISUAL]
 
     if root_joint is None:
-        model = pin.buildModelFromUrdf(filename)
+        model = pin.buildModelFromUrdf(filename, mimic)
     elif root_joint is not None and root_joint_name is None:
-        model = pin.buildModelFromUrdf(filename, root_joint)
+        model = pin.buildModelFromUrdf(filename, root_joint, mimic)
     else:
-        model = pin.buildModelFromUrdf(filename, root_joint, root_joint_name)
+        model = pin.buildModelFromUrdf(filename, root_joint, root_joint_name, mimic)
 
     if verbose and not WITH_HPP_FCL and meshLoader is not None:
         print(