diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5d650d0e0e6d0f8eaea7d5d2a77c451d85067320..52d36559fc41ccba74e5d9dca7a7e82bb9ae09de 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -130,6 +130,7 @@ SET(${PROJECT_NAME}_MULTIBODY_HEADERS
multibody/model.hpp
multibody/model.hxx
multibody/visitor.hpp
+ multibody/parser/srdf.hpp
)
SET(${PROJECT_NAME}_ALGORITHM_HEADERS
diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx
index 5f0aa0e2d0887a17d94005434eadda16bdd1fa30..b325d6c65e858c7b70b1d767c036f098a08708e2 100644
--- a/src/multibody/geometry.hxx
+++ b/src/multibody/geometry.hxx
@@ -27,6 +27,7 @@
#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/joint/joint-variant.hpp"
+#include "pinocchio/multibody/parser/srdf.hpp"
#include <iostream>
#include <hpp/fcl/collision_object.h>
@@ -36,12 +37,6 @@
#include <list>
#include <utility>
-// Read XML file with boost
-#include <boost/property_tree/xml_parser.hpp>
-#include <boost/property_tree/ptree.hpp>
-#include <fstream>
-#include <boost/foreach.hpp>
-
/// @cond DEV
namespace se3
@@ -304,68 +299,11 @@ namespace se3
void GeometryData::addCollisionPairsFromSrdf(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);
- }
-
// Add all collision pairs
addAllCollisionPairs();
- // Read xml stream
- using boost::property_tree::ptree;
- ptree pt;
- read_xml(srdf_stream, pt);
-
- // Iterate over collision pairs
- const se3::Model & model = data_ref.model;
- 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 id1 = model.getBodyId(link1);
- const Model::JointIndex id2 = model.getBodyId(link2);
-
- typedef GeometryModel::GeomIndexList GeomIndexList;
- const GeomIndexList & innerObject1 = model_geom.innerObjects.at(id1);
- const GeomIndexList & innerObject2 = model_geom.innerObjects.at(id2);
-
- for(GeomIndexList::const_iterator it1 = innerObject1.begin();
- it1 != innerObject1.end();
- ++it1)
- {
- for(GeomIndexList::const_iterator it2 = innerObject2.begin();
- it2 != innerObject2.end();
- ++it2)
- {
- removeCollisionPair(CollisionPair(*it1, *it2));
- }
- }
-
- } // BOOST_FOREACH
- }
+ // Read SRDF file
+ srdf::removeCollisionPairsFromSrdf(model_geom, *this, filename, verbose);
}
diff --git a/src/multibody/parser/srdf.hpp b/src/multibody/parser/srdf.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..511d4fb2acb0181b2439bcc1117ad42bb7e6bfec
--- /dev/null
+++ b/src/multibody/parser/srdf.hpp
@@ -0,0 +1,115 @@
+//
+// Copyright (c) 2016 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_hpp__
+#define __se3_parser_srdf_hpp__
+
+#include "pinocchio/multibody/model.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
+{
+ namespace srdf
+ {
+
+#ifdef WITH_HPP_FCL
+ ///
+ /// \brief Deactive all possible collision pairs mentioned in the SRDF file.
+ /// It throws if the SRDF file is incorrect.
+ ///
+ /// \param[in] filename The complete path to the SRDF file.
+ /// \param[in] verbose Verbosity mode.
+ ///
+ inline void removeCollisionPairsFromSrdf(const GeometryModel & model_geom,
+ GeometryData & data_geom,
+ 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);
+ 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
+ const se3::Model & model = model_geom.model;
+ 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 id1 = model.getBodyId(link1);
+ const Model::JointIndex id2 = model.getBodyId(link2);
+
+ typedef GeometryModel::GeomIndexList GeomIndexList;
+ const GeomIndexList & innerObject1 = model_geom.innerObjects.at(id1);
+ const GeomIndexList & innerObject2 = model_geom.innerObjects.at(id2);
+
+ for(GeomIndexList::const_iterator it1 = innerObject1.begin();
+ it1 != innerObject1.end();
+ ++it1)
+ {
+ for(GeomIndexList::const_iterator it2 = innerObject2.begin();
+ it2 != innerObject2.end();
+ ++it2)
+ {
+ data_geom.removeCollisionPair(CollisionPair(*it1, *it2));
+ }
+ }
+
+ }
+ } // BOOST_FOREACH
+ }
+
+#endif // ifdef WITH_HPP_FCL
+
+ }
+} // namespace se3
+
+#endif // ifndef __se3_parser_srdf_hpp__