[SRDF] Support SRDF as XML string and split declaration and definition

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