Merge pull request #2402 from MegMll/topic/urdf_root_joint

Update Model parsing

CHANGELOG.md

@@ -22,6 +22,12 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 - Modernize python code base with ruff ([#2418](https://github.com/stack-of-tasks/pinocchio/pull/2418))
 
+### Changed
+-  Does not create a root_joint frame from parsed models (urdf, mjcf and sdf) when no root joint is provided ([#2402](https://github.com/stack-of-tasks/pinocchio/pull/2402))
+
+### Added
+- Added argument to let users decide of root joint name when parsing models (urdf, mjcf, sdf) ([#2402](https://github.com/stack-of-tasks/pinocchio/pull/2402))
+
 ## [3.2.0] - 2024-08-27
 
 ### Fixed

bindings/python/parsers/mjcf/model.cpp

@@ -28,6 +28,17 @@ namespace pinocchio
       return model;
     }
 
+    bp::tuple buildModelFromMJCF(
+      const std::string & filename,
+      const JointModel & root_joint,
+      const std::string & root_joint_name)
+    {
+      Model model;
+      PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
+      ::pinocchio::mjcf::buildModel(filename, root_joint, root_joint_name, model, contact_models);
+      return bp::make_tuple(model, contact_models);
+    }
+
     void exposeMJCFModel()
     {
       bp::def(
@@ -42,6 +53,14 @@ namespace pinocchio
           pinocchio::python::buildModelFromMJCF),
         bp::args("mjcf_filename", "root_joint"),
         "Parse the MJCF file and return a pinocchio Model with the given root Joint.");
+
+      bp::def(
+        "buildModelFromMJCF",
+        static_cast<bp::tuple (*)(const std::string &, const JointModel &, const std::string &)>(
+          pinocchio::python::buildModelFromMJCF),
+        bp::args("mjcf_filename", "root_joint", "root_joint_name"),
+        "Parse the MJCF file and return a pinocchio Model with the given root Joint and its "
+        "specified name as well as a constraint list if some are present in the MJCF file.");
     }
   } // namespace python
 } // namespace pinocchio

bindings/python/parsers/sdf/model.cpp

@@ -42,6 +42,21 @@ namespace pinocchio
         filename, root_joint, model, contact_models, root_link_name, parent_guidance);
       return bp::make_tuple(model, contact_models);
     }
+
+    bp::tuple buildModelFromSdf(
+      const std::string & filename,
+      const JointModel & root_joint,
+      const std::string & root_link_name,
+      const std::string & root_joint_name,
+      const std::vector<std::string> & parent_guidance)
+    {
+      Model model;
+      PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
+      pinocchio::sdf::buildModel(
+        filename, root_joint, root_joint_name, model, contact_models, root_link_name,
+        parent_guidance);
+      return bp::make_tuple(model, contact_models);
+    }
 #endif
 
     void exposeSDFModel()
@@ -65,6 +80,16 @@ namespace pinocchio
          bp::arg("parent_guidance") = bp::list()),
         "Parse the SDF file given in input and return a pinocchio Model and constraint "
         "models starting with the given root joint.");
+
+      bp::def(
+        "buildModelFromSdf",
+        static_cast<bp::tuple (*)(
+          const std::string &, const JointModel &, const std::string &, const std::string &,
+          const std::vector<std::string> &)>(pinocchio::python::buildModelFromSdf),
+        (bp::arg("sdf_filename"), bp::arg("root_joint"), bp::arg("root_link_name"),
+         bp::arg("root_joint_name"), bp::arg("parent_guidance") = bp::list()),
+        "Parse the SDF file given in input and return a pinocchio Model and constraint "
+        "models starting with the given root joint and its specified name.");
 #endif
     }
   } // namespace python

bindings/python/parsers/urdf/model.cpp

@@ -38,36 +38,75 @@ namespace pinocchio
       return model;
     }
 
+    Model buildModelFromUrdf(
+      const std::string & filename,
+      const JointModel & root_joint,
+      const std::string & root_joint_name)
+    {
+      Model model;
+      pinocchio::urdf::buildModel(filename, root_joint, root_joint_name, model);
+      return model;
+    }
+
     Model &
     buildModelFromUrdf(const std::string & filename, const JointModel & root_joint, Model & model)
     {
       return pinocchio::urdf::buildModel(filename, root_joint, model);
     }
 
-    Model buildModelFromXML(const std::string & XMLstream, const JointModel & root_joint)
+    Model & buildModelFromUrdf(
+      const std::string & filename,
+      const JointModel & root_joint,
+      const std::string & root_joint_name,
+      Model & model)
+    {
+      return pinocchio::urdf::buildModel(filename, root_joint, root_joint_name, model);
+    }
+
+    Model buildModelFromXML(const std::string & xml_stream, const JointModel & root_joint)
     {
       Model model;
-      pinocchio::urdf::buildModelFromXML(XMLstream, root_joint, model);
+      pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model);
+      return model;
+    }
+
+    Model buildModelFromXML(
+      const std::string & xml_stream,
+      const JointModel & root_joint,
+      const std::string & root_joint_name)
+    {
+      Model model;
+      pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model);
       return model;
     }
 
     Model &
-    buildModelFromXML(const std::string & XMLstream, const JointModel & root_joint, Model & model)
+    buildModelFromXML(const std::string & xml_stream, const JointModel & root_joint, Model & model)
     {
-      pinocchio::urdf::buildModelFromXML(XMLstream, root_joint, model);
+      pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model);
       return model;
     }
 
-    Model buildModelFromXML(const std::string & XMLstream)
+    Model & buildModelFromXML(
+      const std::string & xml_stream,
+      const JointModel & root_joint,
+      const std::string & root_joint_name,
+      Model & model)
+    {
+      pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model);
+      return model;
+    }
+
+    Model buildModelFromXML(const std::string & xml_stream)
     {
       Model model;
-      pinocchio::urdf::buildModelFromXML(XMLstream, model);
+      pinocchio::urdf::buildModelFromXML(xml_stream, model);
       return model;
     }
 
-    Model & buildModelFromXML(const std::string & XMLstream, Model & model)
+    Model & buildModelFromXML(const std::string & xml_stream, Model & model)
     {
-      pinocchio::urdf::buildModelFromXML(XMLstream, model);
+      pinocchio::urdf::buildModelFromXML(xml_stream, model);
       return model;
     }
 
@@ -86,6 +125,14 @@ namespace pinocchio
         "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 std::string &, const JointModel &, const std::string &)>(
+          pinocchio::python::buildModelFromUrdf),
+        bp::args("urdf_filename", "root_joint", "root_joint_name"),
+        "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 std::string &)>(pinocchio::python::buildModelFromUrdf),
@@ -110,6 +157,17 @@ namespace pinocchio
         "it is treated as operational frame and not as a joint of the model.",
         bp::return_internal_reference<3>());
 
+      bp::def(
+        "buildModelFromUrdf",
+        static_cast<Model & (*)(const std::string &, const JointModel &, const std::string &,
+                                Model &)>(pinocchio::python::buildModelFromUrdf),
+        bp::args("urdf_filename", "root_joint", "root_joint_name", "model"),
+        "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,"
+        "it is treated as operational frame and not as a joint of the model.",
+        bp::return_internal_reference<3>());
+
       bp::def(
         "buildModelFromXML",
         static_cast<Model (*)(const std::string &, const JointModel &)>(
@@ -118,6 +176,14 @@ namespace pinocchio
         "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 &)>(
+          pinocchio::python::buildModelFromXML),
+        bp::args("urdf_xml_stream", "root_joint", "root_joint_name"),
+        "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 &)>(
@@ -127,6 +193,15 @@ namespace pinocchio
         "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"),
+        "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),

bindings/python/pinocchio/robot_wrapper.py

@@ -14,24 +14,18 @@
 
 class RobotWrapper:
     @staticmethod
-    def BuildFromURDF(
-        filename, package_dirs=None, root_joint=None, verbose=False, meshLoader=None
-    ):
+    def BuildFromURDF(filename, *args, **kwargs):
         robot = RobotWrapper()
-        robot.initFromURDF(filename, package_dirs, root_joint, verbose, meshLoader)
+
+        robot.initFromURDF(filename, *args, **kwargs)
+
         return robot
 
-    def initFromURDF(
-        self,
-        filename,
-        package_dirs=None,
-        root_joint=None,
-        verbose=False,
-        meshLoader=None,
-    ):
+    def initFromURDF(self, filename, *args, **kwargs):
         model, collision_model, visual_model = buildModelsFromUrdf(
-            filename, package_dirs, root_joint, verbose, meshLoader
+            filename, *args, **kwargs
         )
+
         RobotWrapper.__init__(
             self,
             model=model,
@@ -40,46 +34,16 @@ def initFromURDF(
         )
 
     @staticmethod
-    def BuildFromSDF(
-        filename,
-        package_dirs=None,
-        root_joint=None,
-        root_link_name="",
-        parent_guidance=[],
-        verbose=False,
-        meshLoader=None,
-    ):
+    def BuildFromSDF(filename, *args, **kwargs):
         robot = RobotWrapper()
-        robot.initFromSDF(
-            filename,
-            package_dirs,
-            root_joint,
-            root_link_name,
-            parent_guidance,
-            verbose,
-            meshLoader,
-        )
+        robot.initFromSDF(filename, *args, **kwargs)
         return robot
 
-    def initFromSDF(
-        self,
-        filename,
-        package_dirs=None,
-        root_joint=None,
-        root_link_name="",
-        parent_guidance=[],
-        verbose=False,
-        meshLoader=None,
-    ):
+    def initFromSDF(self, filename, *args, **kwargs):
         model, constraint_models, collision_model, visual_model = buildModelsFromSdf(
-            filename,
-            package_dirs,
-            root_joint,
-            root_link_name,
-            parent_guidance,
-            verbose,
-            meshLoader,
+            filename, *args, **kwargs
         )
+
         RobotWrapper.__init__(
             self,
             model=model,
@@ -89,21 +53,17 @@ def initFromSDF(
         self.constraint_models = constraint_models
 
     @staticmethod
-    def BuildFromMJCF(filename, root_joint=None, verbose=False, meshLoader=None):
+    def BuildFromMJCF(filename, *args, **kwargs):
         robot = RobotWrapper()
-        robot.initFromMJCF(filename, root_joint, verbose, meshLoader)
+        robot.initFromMJCF(filename, *args, **kwargs)
+
         return robot
 
-    def initFromMJCF(
-        self,
-        filename,
-        root_joint=None,
-        verbose=False,
-        meshLoader=None,
-    ):
+    def initFromMJCF(self, filename, *args, **kwargs):
         model, collision_model, visual_model = buildModelsFromMJCF(
-            filename, root_joint, verbose, meshLoader
+            filename, *args, **kwargs
         )
+
         RobotWrapper.__init__(
             self,
             model=model,