diff --git a/CMakeLists.txt b/CMakeLists.txt
index e3cc59983f2af6a05d1341a54f66ff8756dc6703..1b8ea29b9898565ad23dad185bde261fca9be70d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -25,7 +25,7 @@ INCLUDE(cmake/python.cmake)
SET(PROJECT_NAME pinocchio)
SET(PROJECT_DESCRIPTION "Rigid multi body dynamics algorithms")
-SET(PROJECT_URL "http://github.com/stac-of-tasks/pinocchio")
+SET(PROJECT_URL "http://github.com/stack-of-tasks/pinocchio")
SET(DOXYGEN_USE_MATHJAX YES)
@@ -238,13 +238,11 @@ ENDIF(BUILD_PYTHON_INTERFACE)
IF(URDFDOM_FOUND)
LIST(APPEND ${PROJECT_NAME}_PARSERS_HEADERS
parsers/urdf.hpp
- parsers/urdf/model.hxx
parsers/urdf/utils.hpp
)
IF(HPP_FCL_FOUND)
LIST(APPEND ${PROJECT_NAME}_PARSERS_HEADERS
- parsers/urdf/geometry.hxx
)
ENDIF(HPP_FCL_FOUND)
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index c5d873b48feefffc07d4958fb2a1902f7c5db01e..28d4e58ad7f1339198066e147805efad0598d4d5 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -57,6 +57,18 @@ IF(BUILD_PYTHON_INTERFACE)
)
ENDIF(BUILD_PYTHON_INTERFACE)
+IF(URDFDOM_FOUND)
+ LIST(APPEND ${PROJECT_NAME}_PARSERS_SOURCES
+ parsers/urdf/model.cpp
+ )
+
+ IF(HPP_FCL_FOUND)
+ LIST(APPEND ${PROJECT_NAME}_PARSERS_SOURCES
+ parsers/urdf/geometry.cpp
+ )
+ ENDIF(HPP_FCL_FOUND)
+ENDIF(URDFDOM_FOUND)
+
IF(LUA5_1_FOUND)
LIST(APPEND ${PROJECT_NAME}_PARSERS_SOURCES
parsers/lua/lua_tables.cpp
diff --git a/src/parsers/urdf.hpp b/src/parsers/urdf.hpp
index 644221980ab4f3a97661f08f592b6761418ea096..3ec4385ce3c5de933875bd1fb9b08c353db0f30d 100644
--- a/src/parsers/urdf.hpp
+++ b/src/parsers/urdf.hpp
@@ -31,6 +31,7 @@
#include <string>
#include <exception>
+#include <boost/shared_ptr.hpp>
namespace urdf
{
@@ -55,10 +56,9 @@ namespace se3
///
/// \return The se3::Model of the URDF file.
///
- template <typename D>
Model buildModel (const std::string & filename,
- const JointModelBase<D> & root_joint,
- bool verbose = false) throw (std::invalid_argument);
+ const JointModelVariant & root_joint,
+ const bool verbose = false) throw (std::invalid_argument);
///
/// \brief Build the model from a URDF file with a fixed joint as root of the model tree.
@@ -68,8 +68,8 @@ namespace se3
///
/// \return The se3::Model of the URDF file.
///
- inline Model buildModel (const std::string & filename,
- const bool verbose = false) throw (std::invalid_argument);
+ Model buildModel (const std::string & filename,
+ const bool verbose = false) throw (std::invalid_argument);
#ifdef WITH_HPP_FCL
@@ -90,22 +90,14 @@ namespace se3
* @return The GeometryModel associated to the urdf file and the given Model.
*
*/
- inline GeometryModel buildGeom(const Model & model,
- const std::string & filename,
- const std::vector<std::string> & package_dirs = std::vector<std::string> (),
- const GeometryType type = NONE) throw (std::invalid_argument);
+ GeometryModel buildGeom(const Model & model,
+ const std::string & filename,
+ const std::vector<std::string> & package_dirs = std::vector<std::string> (),
+ const GeometryType type = NONE) throw (std::invalid_argument);
#endif
} // namespace urdf
} // namespace se3
-/* --- Details -------------------------------------------------------------- */
-/* --- Details -------------------------------------------------------------- */
-/* --- Details -------------------------------------------------------------- */
-#include "pinocchio/parsers/urdf/model.hxx"
-#ifdef WITH_HPP_FCL
- #include "pinocchio/parsers/urdf/geometry.hxx"
-#endif
-
#endif // ifndef __se3_parsers_urdf_hpp__
diff --git a/src/parsers/urdf/geometry.hxx b/src/parsers/urdf/geometry.cpp
similarity index 90%
rename from src/parsers/urdf/geometry.hxx
rename to src/parsers/urdf/geometry.cpp
index c3c3a6feb3f10b5aee48ac1e115b5b6060d74a88..054e578d97b784e4230fd95dd8cace4e51a55254 100644
--- a/src/parsers/urdf/geometry.hxx
+++ b/src/parsers/urdf/geometry.cpp
@@ -15,8 +15,9 @@
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
-#ifndef __se3_parsers_urdf_geometry_hxx__
-#define __se3_parsers_urdf_geometry_hxx__
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/parsers/urdf/utils.hpp"
+#include "pinocchio/parsers/utils.hpp"
#include <urdf_model/model.h>
#include <urdf_parser/urdf_parser.h>
@@ -26,14 +27,8 @@
#include <iomanip>
#include <boost/foreach.hpp>
-#include "pinocchio/parsers/urdf/utils.hpp"
-#include "pinocchio/parsers/utils.hpp"
-#include "pinocchio/multibody/model.hpp"
-
#include <hpp/fcl/mesh_loader/assimp.h>
-#include <exception>
-
namespace se3
{
namespace urdf
@@ -53,11 +48,11 @@ namespace se3
*
* @return A shared pointer on the he geometry converted as a fcl::CollisionGeometry
*/
- inline boost::shared_ptr<fcl::CollisionGeometry> retrieveCollisionGeometry(const boost::shared_ptr < ::urdf::Geometry> urdf_geometry,
- const std::vector < std::string > & package_dirs,
- std::string & mesh_path)
+ boost::shared_ptr<fcl::CollisionGeometry> retrieveCollisionGeometry(const boost::shared_ptr< ::urdf::Geometry> urdf_geometry,
+ const std::vector<std::string> & package_dirs,
+ std::string & mesh_path)
{
- boost::shared_ptr < fcl::CollisionGeometry > geometry;
+ boost::shared_ptr<fcl::CollisionGeometry> geometry;
// Handle the case where collision geometry is a mesh
if (urdf_geometry->type == ::urdf::Geometry::MESH)
@@ -224,11 +219,11 @@ namespace se3
* @param[in] package_dirs A vector containing the different directories where to search for packages
* @param[in] type The type of objects that must be loaded ( can be VISUAL or COLLISION)
*/
- inline void parseTreeForGeom(::urdf::LinkConstPtr link,
- const Model & model,
- GeometryModel & geom_model,
- const std::vector<std::string> & package_dirs,
- const GeometryType type)
+ void parseTreeForGeom(::urdf::LinkConstPtr link,
+ const Model & model,
+ GeometryModel & geom_model,
+ const std::vector<std::string> & package_dirs,
+ const GeometryType type) throw (std::invalid_argument)
{
switch(type)
@@ -254,10 +249,10 @@ namespace se3
- inline GeometryModel buildGeom(const Model & model,
- const std::string & filename,
- const std::vector<std::string> & package_dirs,
- const GeometryType type) throw(std::invalid_argument)
+ GeometryModel buildGeom(const Model & model,
+ const std::string & filename,
+ const std::vector<std::string> & package_dirs,
+ const GeometryType type) throw(std::invalid_argument)
{
if (type == NONE)
{
@@ -285,6 +280,3 @@ namespace se3
} // namespace urdf
} // namespace se3
-
-
-#endif // __se3_parsers_urdf_geometry_hxx__
diff --git a/src/parsers/urdf/model.hxx b/src/parsers/urdf/model.cpp
similarity index 94%
rename from src/parsers/urdf/model.hxx
rename to src/parsers/urdf/model.cpp
index 006b0b873d40b4a3c75c9738a5822d154598cd9a..f000cb1003ce6e1fc43d7d38843776a6bc528863 100644
--- a/src/parsers/urdf/model.hxx
+++ b/src/parsers/urdf/model.cpp
@@ -16,9 +16,7 @@
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
-#ifndef __se3_parsers_urdf_model_hxx__
-#define __se3_parsers_urdf_model_hxx__
-
+#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/urdf/utils.hpp"
#include "pinocchio/multibody/model.hpp"
@@ -80,7 +78,7 @@ namespace se3
/// \param[in] model The model where the link must be added.
/// \param[in] placementOffset The relative placement of the link relative to the closer non fixed joint in the tree.
///
- inline void parseTree (::urdf::LinkConstPtr link, Model & model, const SE3 & placementOffset, bool verbose) throw (std::invalid_argument)
+ void parseTree(::urdf::LinkConstPtr link, Model & model, const SE3 & placementOffset, bool verbose) throw (std::invalid_argument)
{
// Parent joint of the current body
@@ -442,8 +440,6 @@ namespace se3
}
}
-
-
///
/// \brief Parse a tree with a specific root joint linking the model to the environment.
/// The function returns an exception as soon as a necessary Inertia or Joint information are missing.
@@ -572,15 +568,41 @@ namespace se3
}
} // namespace details
+
+ ///
+ /// \brief Call parse root tree templated function
+ ///
+ struct ParseRootTreeVisitor : public boost::static_visitor<>
+ {
+ ::urdf::LinkConstPtr m_root_link;
+ Model & m_model;
+ const bool m_verbose;
+
+ ParseRootTreeVisitor(::urdf::LinkConstPtr root_link, Model & model, const bool verbose)
+ : m_root_link(root_link)
+ , m_model(model)
+ , m_verbose(verbose)
+ {}
+
+ template<typename Derived>
+ void operator()(const JointModelBase<Derived> & root_joint) const
+ {
+ details::parseRootTree(m_root_link,m_model,root_joint,m_verbose);
+ }
+
+ static void run(::urdf::LinkConstPtr root_link, Model & model, const JointModelVariant & root_joint, const bool verbose)
+ {
+ boost::apply_visitor(ParseRootTreeVisitor(root_link,model,verbose),root_joint);
+ }
+ }; // struct ParseRootTreeVisitor
- template <typename D>
- Model buildModel (const std::string & filename, const JointModelBase<D> & root_joint, bool verbose) throw (std::invalid_argument)
+ Model buildModel(const std::string & filename, const JointModelVariant & root_joint, const bool verbose) throw (std::invalid_argument)
{
Model model;
::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
if (urdfTree)
- details::parseRootTree(urdfTree->getRoot(), model, root_joint, verbose);
+ ParseRootTreeVisitor::run(urdfTree->getRoot(),model,root_joint,verbose);
else
{
const std::string exception_message ("The file " + filename + " does not contain a valid URDF model.");
@@ -590,7 +612,7 @@ namespace se3
return model;
}
- inline Model buildModel (const std::string & filename, const bool verbose) throw (std::invalid_argument)
+ Model buildModel(const std::string & filename, const bool verbose) throw (std::invalid_argument)
{
Model model;
@@ -608,5 +630,3 @@ namespace se3
} // namespace urdf
} // namespace se3
-
-#endif // __se3_parsers_urdf_model_hxx__