Merge pull request #447 from jcarpent/topic/urdf

Handle XML stream inside URDF parsers

Author: jcarpent

README.md

@@ -15,6 +15,11 @@ It is built upon Eigen for linear algebra and FCL for collision detections. **Pi
 
 **Pinocchio** can be easily installed on various Linux and Unix distributions. Please refer to the [installation procedure](http://stack-of-tasks.github.io/pinocchio/download.html).
 
+## Tutorials 
+
+**Pinocchio** is comming with a large bunch of tutorials aiming at introducting the basic tools for robotics control.
+The content of the tutorials are described [here](http://projects.laas.fr/gepetto/index.php/Teach/Supaero2018) and the code source of these tutorials is located [here](https://github.com/stack-of-tasks/pinocchio-tutorials).
+
 ## Dependencies
 
 The Pinocchio software depends on several packages which
@@ -49,7 +54,7 @@ If you want to cite **Pinocchio** in your papers, please use the following bibte
       author = {Justin Carpentier and Florian Valenza and Nicolas Mansard and others},
       title = {Pinocchio: fast forward and inverse dynamics for poly-articulated systems},
       howpublished = {https://stack-of-tasks.github.io/pinocchio},
-      year = {2015--2017}
+      year = {2015--2018}
      }
 ```
 

bindings/python/parsers/parsers.hpp

@@ -69,7 +69,23 @@ namespace se3
         se3::urdf::buildModel(filename, model);
         return model;
       }
-
+      
+      static Model buildModelFromXML(const std::string & XMLstream,
+                                     bp::object & root_joint_object
+                                     )
+      {
+        JointModelVariant root_joint = bp::extract<JointModelVariant> (root_joint_object)();
+        Model model;
+        se3::urdf::buildModelFromXML(XMLstream, root_joint, model);
+        return model;
+      }
+      
+      static Model buildModelFromXML(const std::string & XMLstream)
+      {
+        Model model;
+        se3::urdf::buildModelFromXML(XMLstream, model);
+        return model;
+      }
 
       static GeometryModel
       buildGeomFromUrdf(const Model & model,
@@ -151,36 +167,49 @@ namespace se3
       bp::def("buildModelFromUrdf",
               static_cast <Model (*) (const std::string &, bp::object &)> (&ParsersPythonVisitor::buildModelFromUrdf),
               bp::args("Filename (string)","Root Joint Model"),
-              "Parse the urdf file given in input and return a pinocchio model starting with the given root joint model"
-              "(remember to create the corresponding data structure)."
+              "Parse the URDF file given in input and return a pinocchio model starting with the given root joint model"
+              "(remember to then create the corresponding Data structure associated to the model)."
               );
 
       bp::def("buildModelFromUrdf",
               static_cast <Model (*) (const std::string &)> (&ParsersPythonVisitor::buildModelFromUrdf),
               bp::args("Filename (string)"),
-              "Parse the urdf file given in input and return a pinocchio model"
-              "(remember to create the corresponding data structure)."
+              "Parse the URDF file given in input and return a pinocchio model"
+              "(remember to then create the corresponding Data structure associated to the model)."
               );
 
+      bp::def("buildModelFromXML",
+              static_cast <Model (*) (const std::string &, bp::object &)> (&ParsersPythonVisitor::buildModelFromXML),
+              bp::args("XML stream (string)","Root Joint Model"),
+              "Parse the URDF XML stream given in input and return a pinocchio model starting with the given root joint model"
+              "(remember to then create the corresponding Data structure associated to the model)."
+              );
+      
+      bp::def("buildModelFromXML",
+              static_cast <Model (*) (const std::string &)> (&ParsersPythonVisitor::buildModelFromXML),
+              bp::args("XML stream (string)"),
+              "Parse the URDF XML stream given in input and return a pinocchio model"
+              "(remember to then create the corresponding Data structure associated to the model)."
+              );
 
       bp::def("buildGeomFromUrdf",
               static_cast <GeometryModel (*) (const Model &, const std::string &, const std::vector<std::string> &, const GeometryType)> (&ParsersPythonVisitor::buildGeomFromUrdf),
               bp::args("Model to assosiate the Geometry","filename (string)", "package_dirs (vector of strings)"
                        ),
-              "Parse the urdf file given in input looking for the geometry of the given Model and return a proper pinocchio geometry model "
-              "(remember to create the corresponding data structures).");
+              "Parse the URDF file given in input looking for the geometry of the given Model and return a proper pinocchio geometry model "
+              "(remember to then create the corresponding Data structure associated to the geometry model).");
 
       bp::def("buildGeomFromUrdf",
               static_cast <GeometryModel (*) (const Model &, const std::string &, const GeometryType)> (&ParsersPythonVisitor::buildGeomFromUrdf),
               bp::args("Model to assosiate the Geometry","filename (string)"),
-              "Parse the urdf file given in input looking for the geometry of the given Model and return a proper pinocchio  geometry model "
-              "(remember to create the corresponding data structures).");
+              "Parse the URDF file given in input looking for the geometry of the given Model and return a proper pinocchio  geometry model "
+              "(remember to then create the corresponding Data structure associated to the geometry model).");
 
 #ifdef WITH_HPP_FCL
       bp::def("removeCollisionPairsFromSrdf",removeCollisionPairsFromSrdf,
               bp::args("Model", "GeometryModel (where pairs are removed)","srdf filename (string)", "verbosity"
                        ),
-              "Parse an srdf file in order to desactivate collision pairs for a specific GeometryData and GeometryModel ");
+              "Parse an SRDF file in order to desactivate collision pairs for a specific GeometryData and GeometryModel ");
 
 #endif // #ifdef WITH_HPP_FCL
 #endif // #ifdef WITH_URDFDOM
@@ -190,7 +219,7 @@ namespace se3
               bp::args("Filename (string)",
                        "Free flyer (bool, false for a fixed robot)",
                        "Verbose option "),
-              "Parse the urdf file given in input and return a proper pinocchio model "
+              "Parse the URDF file given in input and return a proper pinocchio model "
               "(remember to create the corresponding data structure).");
 #endif // #ifdef WITH_LUA5
 
@@ -202,7 +231,7 @@ namespace se3
 
       bp::def("loadRotorParamsFromSrdf",loadRotorParamsFromSrdf,
               bp::args("Model for which we are loading the rotor parameters",
-                       "srdf filename (string)", "verbosity"),
+                       "SRDF filename (string)", "verbosity"),
               "Load the rotor parameters of a given model from an SRDF file.\n"
               "Results are stored in model.rotorInertia and model.rotorGearRatio.");
     }

src/parsers/urdf.hpp

@@ -1,5 +1,5 @@
 //
-// Copyright (c) 2015-2017 CNRS
+// Copyright (c) 2015-2018 CNRS
 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
 //
 // This file is part of Pinocchio
@@ -87,6 +87,39 @@ namespace se3
     Model & buildModel (const ::urdf::ModelInterfaceSharedPtr & urdfTree,
                         Model & model,
                         const bool verbose = false);
+    
+    ///
+    /// \brief Build the model from an XML stream with a particular joint as root of the model tree inside
+    /// the model given as reference argument.
+    ///
+    /// \param[in] xmlStream stream containing the URDF model.
+    /// \param[in] rootJoint The joint at the root of the model tree.
+    /// \param[in] verbose Print parsing info.
+    /// \param[out] model Reference model where to put the parsed information.
+    /// \return Return the reference on argument model for convenience.
+    ///
+    /// \note urdfTree can be build from ::urdf::parseURDF
+    ///       or ::urdf::parseURDFFile
+    Model & buildModelFromXML(const std::string & xmlStream,
+                              const JointModelVariant & rootJoint,
+                              Model & model,
+                              const bool verbose = false)
+    throw (std::invalid_argument);
+    
+    ///
+    /// \brief Build the model from an XML stream
+    ///
+    /// \param[in] xmlStream stream containing the URDF model.
+    /// \param[in] verbose Print parsing info.
+    /// \param[out] model Reference model where to put the parsed information.
+    /// \return Return the reference on argument model for convenience.
+    ///
+    /// \note urdfTree can be build from ::urdf::parseURDF
+    ///       or ::urdf::parseURDFFile
+    Model & buildModelFromXML(const std::string & xmlStream,
+                              Model & model,
+                              const bool verbose = false)
+    throw (std::invalid_argument);
 
 
     /**
@@ -97,7 +130,7 @@ namespace se3
      * @param[in]  model         The model of the robot, built with
      *                           urdf::buildModel
      * @param[in]  filename      The URDF complete (absolute) file path
-     * @param[in]  packageDirs  A vector containing the different directories
+     * @param[in]  packageDirs   A vector containing the different directories
      *                           where to search for models and meshes, typically 
      *                           obtained from calling se3::rosPaths()
      *
@@ -106,7 +139,7 @@ namespace se3
      *
      * @return      Returns the reference on geom model for convenience.
      *
-     * \warning     If hpp-fcl has not been found during compilation, COLLISION types can not be loaded
+     * \warning     If hpp-fcl has not been found during compilation, COLLISION objects can not be loaded
      *
      */
     GeometryModel& buildGeom(const Model & model,
@@ -115,6 +148,36 @@ namespace se3
                              GeometryModel & geomModel,
                              const std::vector<std::string> & packageDirs = std::vector<std::string> ())
     throw (std::invalid_argument);
+    
+    /**
+     * @brief      Build The GeometryModel from a URDF file. Search for meshes
+     *             in the directories specified by the user first and then in
+     *             the environment variable ROS_PACKAGE_PATH
+     *
+     * @param[in]  model         The model of the robot, built with
+     *                           urdf::buildModel
+     * @param[in]  filename      The URDF complete (absolute) file path
+     * @param[in]  packageDir    A string containing the path to the directories of the meshes,
+     *                           typically obtained from calling se3::rosPaths().
+     *
+     * @param[in]   type         The type of objects that must be loaded (must be VISUAL or COLLISION)
+     * @param[out]  geomModel    Reference where to put the parsed information.
+     *
+     * @return      Returns the reference on geom model for convenience.
+     *
+     * \warning     If hpp-fcl has not been found during compilation, COLLISION objects can not be loaded
+     *
+     */
+    inline GeometryModel& buildGeom(const Model & model,
+                                    const std::string & filename,
+                                    const GeometryType type,
+                                    GeometryModel & geomModel,
+                                    const std::string & packageDir)
+    throw (std::invalid_argument)
+    {
+      const std::vector<std::string> dirs(1,packageDir);
+      return buildGeom(model,filename,type,geomModel,dirs);
+    }
 
     /**
      * @brief      Build The GeometryModel from a URDF model. Search for meshes
@@ -124,7 +187,7 @@ namespace se3
      * @param[in]  model         The model of the robot, built with
      *                           urdf::buildModel
      * @param[in]  xmlStream     Stream containing the URDF model
-     * @param[in]  packageDirs  A vector containing the different directories
+     * @param[in]  packageDirs   A vector containing the different directories
      *                           where to search for models and meshes, typically
      *                           obtained from calling se3::rosPaths()
      *
@@ -133,15 +196,45 @@ namespace se3
      *
      * @return      Returns the reference on geom model for convenience.
      *
-     * \warning     If hpp-fcl has not been found during compilation, COLLISION types can not be loaded
+     * \warning     If hpp-fcl has not been found during compilation, COLLISION objects cannot be loaded
      *
      */
     GeometryModel& buildGeom(const Model & model,
-                             const std::istream& xmlStream,
+                             const std::istream & xmlStream,
                              const GeometryType type,
                              GeometryModel & geomModel,
                              const std::vector<std::string> & packageDirs = std::vector<std::string> ())
     throw (std::invalid_argument);
+    
+    /**
+     * @brief      Build The GeometryModel from a URDF model. Search for meshes
+     *             in the directories specified by the user first and then in
+     *             the environment variable ROS_PACKAGE_PATH
+     *
+     * @param[in]  model         The model of the robot, built with
+     *                           urdf::buildModel
+     * @param[in]  xmlStream     Stream containing the URDF model
+     * @param[in]  packageDir    A string containing the path to the directories of the meshes,
+     *                           typically obtained from calling se3::rosPaths().
+     *
+     * @param[in]   type         The type of objects that must be loaded (must be VISUAL or COLLISION)
+     * @param[out]  geomModel    Reference where to put the parsed information.
+     *
+     * @return      Returns the reference on geom model for convenience.
+     *
+     * \warning     If hpp-fcl has not been found during compilation, COLLISION objects cannot be loaded
+     *
+     */
+    inline GeometryModel & buildGeom(const Model & model,
+                                     const std::istream & xmlStream,
+                                     const GeometryType type,
+                                     GeometryModel & geomModel,
+                                     const std::string & packageDir)
+    throw (std::invalid_argument)
+    {
+      const std::vector<std::string> dirs(1,packageDir);
+      return buildGeom(model,xmlStream,type,geomModel,dirs);
+    }
 
 
   } // namespace urdf

src/parsers/urdf/model.cpp

@@ -1,5 +1,5 @@
 //
-// Copyright (c) 2015-2017 CNRS
+// Copyright (c) 2015-2018 CNRS
 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
 //
 // This file is part of Pinocchio
@@ -586,9 +586,9 @@ namespace se3
       ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename);
 
       if (urdfTree)
-	{
+      {
           return buildModel (urdfTree, root_joint, model, verbose);
-	}
+      }
       else
       {
         const std::string exception_message ("The file " + filename + " does not contain a valid URDF model.");
@@ -602,9 +602,9 @@ namespace se3
     {
       ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename);
       if (urdfTree)
-	{
+      {
           return buildModel (urdfTree, model, verbose);
-	}
+      }
       else
       {
         const std::string exception_message ("The file " + filename + " does not contain a valid URDF model.");
@@ -613,6 +613,41 @@ namespace se3
 
       return model;
     }
+              
+    Model & buildModelFromXML(const std::string & xmlStream,
+                              const JointModelVariant & rootJoint,
+                              Model & model,
+                              const bool verbose) throw (std::invalid_argument)
+    {
+      ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlStream);
+      
+      if (urdfTree)
+        return buildModel(urdfTree, rootJoint, model, verbose);
+      else
+      {
+        const std::string exception_message ("The XML stream does not contain a valid URDF model.");
+        throw std::invalid_argument(exception_message);
+      }
+      
+      return model;
+    }
+              
+    Model & buildModelFromXML(const std::string & xmlStream,
+                              Model & model,
+                              const bool verbose) throw (std::invalid_argument)
+    {
+      ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlStream);
+      
+      if (urdfTree)
+        return buildModel(urdfTree, model, verbose);
+      else
+      {
+        const std::string exception_message ("The XML stream does not contain a valid URDF model.");
+        throw std::invalid_argument(exception_message);
+      }
+      
+      return model;
+    }
 
     Model& buildModel(const ::urdf::ModelInterfaceSharedPtr & urdfTree,
                       const JointModelVariant & root_joint,