Merge pull request #407 from jmirabel/devel

Add overload of buildModel and buildGeom

src/CMakeLists.txt

@@ -49,6 +49,7 @@ SET(${PROJECT_NAME}_MULTIBODY_SOURCES
 
 SET(${PROJECT_NAME}_PARSERS_SOURCES
   parsers/sample-models.cpp
+  parsers/srdf.cpp
   )
 
 IF(URDFDOM_FOUND)

src/parsers/srdf.cpp

+//
+// Copyright (c) 2017 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_parser_srdf_hxx__
+#define __se3_parser_srdf_hxx__
+
+#include "pinocchio/parsers/srdf.hpp"
+
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/geometry.hpp"
+#include <iostream>
+
+// Read XML file with boost
+#include <boost/property_tree/xml_parser.hpp>
+#include <boost/property_tree/ptree.hpp>
+#include <fstream>
+#include <sstream>
+#include <boost/foreach.hpp>
+
+
+namespace se3
+{
+  namespace srdf
+  {
+    
+#ifdef WITH_HPP_FCL
+    namespace details
+    {
+      inline void removeCollisionPairs(const Model& model,
+          GeometryModel & geomModel,
+          std::istream & stream,
+          const bool verbose = false)
+      {
+        // Read xml stream
+        using boost::property_tree::ptree;
+        ptree pt;
+        read_xml(stream, pt);
+
+        // Iterate over collision pairs
+        BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot"))
+        {
+          if (v.first == "disable_collisions")
+          {
+            const std::string link1 = v.second.get<std::string>("<xmlattr>.link1");
+            const std::string link2 = v.second.get<std::string>("<xmlattr>.link2");
+
+            // Check first if the two bodies exist in model
+            if (!model.existBodyName(link1) || !model.existBodyName(link2))
+            {
+              if (verbose)
+                std::cout << "It seems that " << link1 << " or " << link2 << " do not exist in model. Skip." << std::endl;
+              continue;
+            }
+
+            const Model::JointIndex frame_id1 = model.getBodyId(link1);
+            const Model::JointIndex frame_id2 = model.getBodyId(link2);
+
+            // Malformed SRDF
+            if (frame_id1 == frame_id2)
+            {
+              if (verbose)
+                std::cout << "Cannot disable collision between " << link1 << " and " << link2 << std::endl;
+              continue;
+            }
+
+            typedef std::vector<CollisionPair> CollisionPairs_t;
+            bool didRemove = false;
+            for(CollisionPairs_t::iterator _colPair = geomModel.collisionPairs.begin();
+                _colPair != geomModel.collisionPairs.end(); ) {
+              const CollisionPair& colPair (*_colPair);
+              bool remove =
+                (
+                 (geomModel.geometryObjects[colPair.first ].parentFrame == frame_id1)
+                 && (geomModel.geometryObjects[colPair.second].parentFrame == frame_id2)
+                ) || (
+                  (geomModel.geometryObjects[colPair.second].parentFrame == frame_id1)
+                  && (geomModel.geometryObjects[colPair.first ].parentFrame == frame_id2)
+                  );
+
+              if (remove) {
+                _colPair = geomModel.collisionPairs.erase(_colPair);
+                didRemove = true;
+              } else {
+                ++_colPair;
+              }
+            }
+            if(didRemove && verbose)
+              std::cout << "Remove collision pair (" << link1 << "," << link2 << ")" << std::endl;
+
+          }
+        } // BOOST_FOREACH
+      }
+    } // namespace details
+
+    void removeCollisionPairsFromSrdf(const Model& model,
+                                      GeometryModel & geomModel,
+                                      const std::string & filename,
+                                      const bool verbose) throw (std::invalid_argument)
+    {
+      // Check extension
+      const std::string extension = filename.substr(filename.find_last_of('.')+1);
+      if (extension != "srdf")
+      {
+        const std::string exception_message (filename + " does not have the right extension.");
+        throw std::invalid_argument(exception_message);
+      }
+      
+      // Open file
+      std::ifstream srdf_stream(filename.c_str());
+      if (! srdf_stream.is_open())
+      {
+        const std::string exception_message (filename + " does not seem to be a valid file.");
+        throw std::invalid_argument(exception_message);
+      }
+
+      details::removeCollisionPairs(model, geomModel, srdf_stream, verbose);
+    }
+
+    void removeCollisionPairsFromSrdfString(
+        const Model& model,
+        GeometryModel & geomModel,
+        const std::string & xmlString,
+        const bool verbose)
+    {
+      std::istringstream srdf_stream(xmlString);
+      details::removeCollisionPairs(model, geomModel, srdf_stream, verbose);
+    }
+    
+#endif // ifdef WITH_HPP_FCL
+
+    Eigen::VectorXd getNeutralConfigurationFromSrdf(Model & model,
+                                                           const std::string & filename,
+                                                           const bool verbose) throw (std::invalid_argument)
+    {
+      // Check extension
+      const std::string extension = filename.substr(filename.find_last_of('.')+1);
+      if (extension != "srdf")
+      {
+        const std::string exception_message (filename + " does not have the right extension.");
+        throw std::invalid_argument(exception_message);
+      }
+      
+      // Open file
+      std::ifstream srdf_stream(filename.c_str());
+      if (! srdf_stream.is_open())
+      {
+        const std::string exception_message (filename + " does not seem to be a valid file.");
+        throw std::invalid_argument(exception_message);
+      }
+      
+      // Read xml stream
+      using boost::property_tree::ptree;
+      ptree pt;
+      read_xml(srdf_stream, pt);
+      
+      // Iterate over all tags directly children of robot
+      BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot"))
+      {
+        // if we encounter a tag group_state
+        if (v.first == "group_state")
+        {
+          const std::string name = v.second.get<std::string>("<xmlattr>.name");
+          // Ensure that it is the half_sitting tag
+          if( name == "half_sitting")
+          {
+            // Iterate over all the joint tags
+            BOOST_FOREACH(const ptree::value_type & joint, v.second)
+            {
+              if (joint.first == "joint")
+              {
+                std::string joint_name = joint.second.get<std::string>("<xmlattr>.name");
+                double joint_config = joint.second.get<double>("<xmlattr>.value");
+                if (verbose)
+                {
+                  std::cout << "(" << joint_name << " , " << joint_config << ")" << std::endl;
+                }
+                // Search in model the joint and its config id
+                Model::JointIndex joint_id = model.getJointId(joint_name);
+
+                if (joint_id != model.joints.size()) // != model.njoints
+                {
+                  const JointModel & joint = model.joints[joint_id];
+                  model.neutralConfiguration(joint.idx_q()) = joint_config; // joint with 1 dof
+                  // model.neutralConfiguration.segment(joint.idx_q(),joint.nq()) = joint_config; // joint with more than 1 dof
+                }
+                else
+                {
+                  if (verbose) std::cout << "The Joint " << joint_name << " was not found in model" << std::endl;
+                }
+              }
+            }
+            return model.neutralConfiguration;
+          }
+          
+        }
+      } // BOOST_FOREACH
+      assert(false && "no half_sitting configuration found in the srdf file"); // Should we throw something here ?  
+      return Eigen::VectorXd::Constant(model.nq,NAN); // warning : uninitialized vector is returned
+    }
+  }
+} // namespace se3
+
+#endif // ifndef __se3_parser_srdf_hxx__

src/parsers/srdf.hpp

@@ -20,13 +20,6 @@
 
 #include "pinocchio/multibody/model.hpp"
 #include "pinocchio/multibody/geometry.hpp"
-#include <iostream>
-
-// Read XML file with boost
-#include <boost/property_tree/xml_parser.hpp>
-#include <boost/property_tree/ptree.hpp>
-#include <fstream>
-#include <boost/foreach.hpp>
 
 
 namespace se3
@@ -45,86 +38,23 @@ namespace se3
     /// \param[in] filename The complete path to the SRDF file.
     /// \param[in] verbose Verbosity mode (print removed collision pairs and undefined link inside the model).
     ///
-    inline void removeCollisionPairsFromSrdf(const Model& model,
-                                             GeometryModel & geomModel,
-                                             const std::string & filename,
-                                             const bool verbose = false) throw (std::invalid_argument)
-    {
-      // Check extension
-      const std::string extension = filename.substr(filename.find_last_of('.')+1);
-      if (extension != "srdf")
-      {
-        const std::string exception_message (filename + " does not have the right extension.");
-        throw std::invalid_argument(exception_message);
-      }
-      
-      // Open file
-      std::ifstream srdf_stream(filename.c_str());
-      if (! srdf_stream.is_open())
-      {
-        const std::string exception_message (filename + " does not seem to be a valid file.");
-        throw std::invalid_argument(exception_message);
-      }
-      
-      // Read xml stream
-      using boost::property_tree::ptree;
-      ptree pt;
-      read_xml(srdf_stream, pt);
-      
-      // Iterate over collision pairs
-      BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot"))
-      {
-        if (v.first == "disable_collisions")
-        {
-          const std::string link1 = v.second.get<std::string>("<xmlattr>.link1");
-          const std::string link2 = v.second.get<std::string>("<xmlattr>.link2");
-          
-          // Check first if the two bodies exist in model
-          if (!model.existBodyName(link1) || !model.existBodyName(link2))
-          {
-            if (verbose)
-              std::cout << "It seems that " << link1 << " or " << link2 << " do not exist in model. Skip." << std::endl;
-            continue;
-          }
-          
-          const Model::JointIndex frame_id1 = model.getBodyId(link1);
-          const Model::JointIndex frame_id2 = model.getBodyId(link2);
-
-          // Malformed SRDF
-          if (frame_id1 == frame_id2)
-          {
-            if (verbose)
-              std::cout << "Cannot disable collision between " << link1 << " and " << link2 << std::endl;
-            continue;
-          }
-          
-          typedef std::vector<CollisionPair> CollisionPairs_t;
-          bool didRemove = false;
-          for(CollisionPairs_t::iterator _colPair = geomModel.collisionPairs.begin();
-              _colPair != geomModel.collisionPairs.end(); ) {
-            const CollisionPair& colPair (*_colPair);
-            bool remove =
-              (
-                  (geomModel.geometryObjects[colPair.first ].parentFrame == frame_id1)
-               && (geomModel.geometryObjects[colPair.second].parentFrame == frame_id2)
-              ) || (
-                   (geomModel.geometryObjects[colPair.second].parentFrame == frame_id1)
-                && (geomModel.geometryObjects[colPair.first ].parentFrame == frame_id2)
-              );
-
-            if (remove) {
-              _colPair = geomModel.collisionPairs.erase(_colPair);
-              didRemove = true;
-            } else {
-              ++_colPair;
-            }
-          }
-          if(didRemove && verbose)
-            std::cout << "Remove collision pair (" << link1 << "," << link2 << ")" << std::endl;
-          
-        }
-      } // BOOST_FOREACH
-    }
+    void removeCollisionPairsFromSrdf(const Model& model,
+                                      GeometryModel & geomModel,
+                                      const std::string & filename,
+                                      const bool verbose = false) throw (std::invalid_argument);
+    ///
+    /// \brief Deactive all possible collision pairs mentioned in the SRDF file.
+    ///
+    /// \param[in] model Model of the kinematic tree.
+    /// \param[in] geomModel Model of the geometries.
+    /// \param[out] data_geom Data containing the active collision pairs.
+    /// \param[in] xmlString the SRDF string.
+    /// \param[in] verbose Verbosity mode (print removed collision pairs and undefined link inside the model).
+    ///
+    void removeCollisionPairsFromSrdfString(const Model& model,
+                                            GeometryModel & geomModel,
+                                            const std::string & xmlString,
+                                            const bool verbose = false);
     
 #endif // ifdef WITH_HPP_FCL
     
@@ -138,75 +68,9 @@ namespace se3
     /// \param[in] verbose Verbosity mode.
     ///
     /// \return The neutral configuration as an eigen vector
-    inline Eigen::VectorXd getNeutralConfigurationFromSrdf(Model & model,
-                                                           const std::string & filename,
-                                                           const bool verbose = false) throw (std::invalid_argument)
-    {
-      // Check extension
-      const std::string extension = filename.substr(filename.find_last_of('.')+1);
-      if (extension != "srdf")
-      {
-        const std::string exception_message (filename + " does not have the right extension.");
-        throw std::invalid_argument(exception_message);
-      }
-      
-      // Open file
-      std::ifstream srdf_stream(filename.c_str());
-      if (! srdf_stream.is_open())
-      {
-        const std::string exception_message (filename + " does not seem to be a valid file.");
-        throw std::invalid_argument(exception_message);
-      }
-      
-      // Read xml stream
-      using boost::property_tree::ptree;
-      ptree pt;
-      read_xml(srdf_stream, pt);
-      
-      // Iterate over all tags directly children of robot
-      BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot"))
-      {
-        // if we encounter a tag group_state
-        if (v.first == "group_state")
-        {
-          const std::string name = v.second.get<std::string>("<xmlattr>.name");
-          // Ensure that it is the half_sitting tag
-          if( name == "half_sitting")
-          {
-            // Iterate over all the joint tags
-            BOOST_FOREACH(const ptree::value_type & joint, v.second)
-            {
-              if (joint.first == "joint")
-              {
-                std::string joint_name = joint.second.get<std::string>("<xmlattr>.name");
-                double joint_config = joint.second.get<double>("<xmlattr>.value");
-                if (verbose)
-                {
-                  std::cout << "(" << joint_name << " , " << joint_config << ")" << std::endl;
-                }
-                // Search in model the joint and its config id
-                Model::JointIndex joint_id = model.getJointId(joint_name);
-
-                if (joint_id != model.joints.size()) // != model.njoints
-                {
-                  const JointModel & joint = model.joints[joint_id];
-                  model.neutralConfiguration(joint.idx_q()) = joint_config; // joint with 1 dof
-                  // model.neutralConfiguration.segment(joint.idx_q(),joint.nq()) = joint_config; // joint with more than 1 dof
-                }
-                else
-                {
-                  if (verbose) std::cout << "The Joint " << joint_name << " was not found in model" << std::endl;
-                }
-              }
-            }
-            return model.neutralConfiguration;
-          }
-          
-        }
-      } // BOOST_FOREACH
-      assert(false && "no half_sitting configuration found in the srdf file"); // Should we throw something here ?  
-      return Eigen::VectorXd::Constant(model.nq,NAN); // warning : uninitialized vector is returned
-    }
+    Eigen::VectorXd getNeutralConfigurationFromSrdf(Model & model,
+                                                    const std::string & filename,
+                                                    const bool verbose = false) throw (std::invalid_argument);
   }
 } // namespace se3
 

src/parsers/urdf.hpp

@@ -24,8 +24,6 @@
 #include "pinocchio/multibody/geometry.hpp"
 #include "pinocchio/parsers/urdf/types.hpp"
 
-#include <urdf_model/model.h>
-
 namespace se3
 {
   namespace urdf
@@ -59,6 +57,37 @@ namespace se3
                         Model & model,
                         const bool verbose = false) throw (std::invalid_argument);
 
+    ///
+    /// \brief Build the model from a URDF model with a particular joint as root of the model tree inside
+    /// the model given as reference argument.
+    ///
+    /// \param[in] urdfTree the tree build from the URDF
+    /// \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 & buildModel (const ::urdf::ModelInterfaceSharedPtr & urdfTree,
+                        const JointModelVariant & rootJoint,
+                        Model & model,
+                        const bool verbose = false);
+
+    ///
+    /// \brief Build the model from a URDF model
+    ///
+    /// \param[in] urdfTree the tree build from the URDF
+    /// \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 & buildModel (const ::urdf::ModelInterfaceSharedPtr & urdfTree,
+                        Model & model,
+                        const bool verbose = false);
+
 
     /**
      * @brief      Build The GeometryModel from a URDF file. Search for meshes
@@ -87,6 +116,33 @@ namespace se3
                              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]  urdfTree      The URDF model
+     * @param[in]  packageDirs  A vector containing the different directories
+     *                           where to search for models and 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 types can not be loaded
+     *
+     */
+    GeometryModel& buildGeom(const Model & model,
+                             const ::urdf::ModelInterfaceSharedPtr & urdfTree,
+                             const GeometryType type,
+                             GeometryModel & geomModel,
+                             const std::vector<std::string> & packageDirs = std::vector<std::string> ())
+    throw (std::invalid_argument);
+
 
   } // namespace urdf
 } // namespace se3

src/parsers/urdf/geometry.cpp

@@ -343,6 +343,17 @@ namespace se3
                              GeometryModel & geomModel,
                              const std::vector<std::string> & package_dirs)
       throw(std::invalid_argument)
+    {
+      ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
+      return buildGeom (model, urdfTree, type, geomModel, package_dirs);
+    }
+
+    GeometryModel& buildGeom(const Model & model,
+                             const ::urdf::ModelInterfaceSharedPtr & urdfTree,
+                             const GeometryType type,
+                             GeometryModel & geomModel,
+                             const std::vector<std::string> & package_dirs)
+      throw(std::invalid_argument)
     {
       std::vector<std::string> hint_directories(package_dirs);
 
@@ -355,7 +366,6 @@ namespace se3
         throw std::runtime_error("You did not specify any package directory and ROS_PACKAGE_PATH is empty. Geometric parsing will crash");
       }
 
-      ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
       details::parseTreeForGeom(urdfTree->getRoot(), model, geomModel, hint_directories,type);
       return geomModel;
     }

src/parsers/urdf/model.cpp

@@ -587,8 +587,7 @@ namespace se3
 
       if (urdfTree)
 	{
-	  model.name = urdfTree->getName();
-	  ParseRootTreeVisitor::run(urdfTree->getRoot(),model,root_joint,verbose);
+          return buildModel (urdfTree, root_joint, model, verbose);
 	}
       else
       {
@@ -604,8 +603,7 @@ namespace se3
       ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename);
       if (urdfTree)
 	{
-	  model.name = urdfTree->getName();
-	  details::parseRootTree(urdfTree->getRoot(),model,verbose);
+          return buildModel (urdfTree, model, verbose);
 	}
       else
       {
@@ -616,5 +614,26 @@ namespace se3
       return model;
     }
 
+    Model& buildModel(const ::urdf::ModelInterfaceSharedPtr & urdfTree,
+                      const JointModelVariant & root_joint,
+                      Model & model,
+                      const bool verbose)
+    {
+      assert(urdfTree);
+      model.name = urdfTree->getName();
+      ParseRootTreeVisitor::run(urdfTree->getRoot(),model,root_joint,verbose);
+      return model;
+    }
+                
+    Model& buildModel(const ::urdf::ModelInterfaceSharedPtr & urdfTree,
+                      Model& model,
+                      const bool verbose)
+    {
+      assert(urdfTree);
+      model.name = urdfTree->getName();