From fd2374f5943061098a0d02c3f910cd2fee88ad59 Mon Sep 17 00:00:00 2001
From: Valenza Florian <fvalenza@laas.fr>
Date: Tue, 12 Jul 2016 13:20:05 +0200
Subject: [PATCH] [C++][Cmake] Reorganize urdf parsers - moved parsers one
level above
---
CMakeLists.txt | 41 +-
benchmark/timings-geometry.cpp | 5 +-
benchmark/timings.cpp | 4 +-
src/CMakeLists.txt | 20 +-
src/multibody/geometry.hxx | 2 +-
src/multibody/parser/urdf-with-geometry.hpp | 98 ---
src/multibody/parser/urdf-with-geometry.hxx | 193 ------
src/multibody/parser/urdf.hxx | 624 ------------------
src/multibody/parser/utils.hpp | 49 --
src/{multibody/parser => parsers}/lua.cpp | 4 +-
src/{multibody/parser => parsers}/lua.hpp | 0
.../parser => parsers}/lua/lua_tables.cpp | 2 +-
.../parser => parsers}/lua/lua_tables.hpp | 0
src/{multibody/parser => parsers}/python.cpp | 2 +-
src/{multibody/parser => parsers}/python.hpp | 0
.../parser => parsers}/sample-models.cpp | 2 +-
.../parser => parsers}/sample-models.hpp | 0
src/{multibody/parser => parsers}/srdf.hpp | 0
src/{multibody/parser => parsers}/urdf.hpp | 70 +-
src/parsers/urdf/geometry.hxx | 224 +++++++
src/parsers/urdf/model.hxx | 584 ++++++++++++++++
src/parsers/urdf/utils.hpp | 104 +++
.../utils.hpp} | 42 +-
src/python/model.hpp | 2 +-
src/python/parsers.hpp | 7 +-
src/tools/file-explorer.hpp | 11 +
unittest/aba.cpp | 2 +-
unittest/cholesky.cpp | 2 +-
unittest/com.cpp | 2 +-
unittest/compute-all-terms.cpp | 2 +-
unittest/crba.cpp | 2 +-
unittest/dynamics.cpp | 2 +-
unittest/energy.cpp | 2 +-
unittest/frames.cpp | 2 +-
unittest/geom.cpp | 5 +-
unittest/jacobian.cpp | 2 +-
unittest/joint-configurations.cpp | 2 +-
unittest/joint.cpp | 4 +-
unittest/lua.cpp | 2 +-
unittest/python_parser.cpp | 2 +-
unittest/rnea.cpp | 2 +-
unittest/urdf.cpp | 2 +-
unittest/value.cpp | 2 +-
utils/pinocchio_read_model.cpp | 6 +-
44 files changed, 1061 insertions(+), 1074 deletions(-)
delete mode 100644 src/multibody/parser/urdf-with-geometry.hpp
delete mode 100644 src/multibody/parser/urdf-with-geometry.hxx
delete mode 100644 src/multibody/parser/urdf.hxx
delete mode 100644 src/multibody/parser/utils.hpp
rename src/{multibody/parser => parsers}/lua.cpp (98%)
rename src/{multibody/parser => parsers}/lua.hpp (100%)
rename src/{multibody/parser => parsers}/lua/lua_tables.cpp (99%)
rename src/{multibody/parser => parsers}/lua/lua_tables.hpp (100%)
rename src/{multibody/parser => parsers}/python.cpp (97%)
rename src/{multibody/parser => parsers}/python.hpp (100%)
rename src/{multibody/parser => parsers}/sample-models.cpp (99%)
rename src/{multibody/parser => parsers}/sample-models.hpp (100%)
rename src/{multibody/parser => parsers}/srdf.hpp (100%)
rename src/{multibody/parser => parsers}/urdf.hpp (64%)
create mode 100644 src/parsers/urdf/geometry.hxx
create mode 100644 src/parsers/urdf/model.hxx
create mode 100644 src/parsers/urdf/utils.hpp
rename src/{multibody/parser/from-collada-to-fcl.hpp => parsers/utils.hpp} (75%)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 74fa8613a..13f8e5333 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -153,10 +153,6 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
multibody/joint/joint-basic-visitors.hxx
)
-SET(${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS
- multibody/parser/sample-models.hpp
- multibody/parser/utils.hpp
- )
SET(${PROJECT_NAME}_MULTIBODY_HEADERS
multibody/fwd.hpp
@@ -166,9 +162,14 @@ SET(${PROJECT_NAME}_MULTIBODY_HEADERS
multibody/model.hpp
multibody/model.hxx
multibody/visitor.hpp
- multibody/parser/srdf.hpp
)
+SET(${PROJECT_NAME}_PARSERS_HEADERS
+ parsers/sample-models.hpp
+ parsers/utils.hpp
+ parsers/srdf.hpp
+ )
+
SET(${PROJECT_NAME}_ALGORITHM_HEADERS
algorithm/aba.hpp
algorithm/aba.hxx
@@ -213,8 +214,8 @@ IF(BUILD_PYTHON_INTERFACE)
python/explog.hpp
)
- LIST(APPEND ${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS
- multibody/parser/python.hpp
+ LIST(APPEND ${PROJECT_NAME}_PARSERS_HEADERS
+ parsers/python.hpp
)
ENDIF(BUILD_PYTHON_INTERFACE)
@@ -228,16 +229,15 @@ IF(HPP_FCL_FOUND AND BUILD_PYTHON_INTERFACE)
ENDIF(HPP_FCL_FOUND AND BUILD_PYTHON_INTERFACE)
IF(URDFDOM_FOUND)
- LIST(APPEND ${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS
- multibody/parser/urdf.hpp
- multibody/parser/urdf.hxx
+ 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}_MULTIBODY_PARSER_HEADERS
- multibody/parser/from-collada-to-fcl.hpp
- multibody/parser/urdf-with-geometry.hpp
- multibody/parser/urdf-with-geometry.hxx
+ LIST(APPEND ${PROJECT_NAME}_PARSERS_HEADERS
+ parsers/urdf/geometry.hxx
)
ENDIF(HPP_FCL_FOUND)
@@ -258,9 +258,9 @@ IF(HPP_FCL_FOUND)
ENDIF(HPP_FCL_FOUND)
IF(LUA5_1_FOUND)
- LIST(APPEND ${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS
- multibody/parser/lua.hpp
- multibody/parser/lua/lua_tables.hpp
+ LIST(APPEND ${PROJECT_NAME}_PARSERS_HEADERS
+ parsers/lua.hpp
+ parsers/lua/lua_tables.hpp
)
ADD_DEFINITIONS(-DWITH_LUA)
@@ -272,8 +272,8 @@ SET(HEADERS
${${PROJECT_NAME}_TOOLS_HEADERS}
${${PROJECT_NAME}_SPATIAL_HEADERS}
${${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS}
- ${${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS}
${${PROJECT_NAME}_MULTIBODY_HEADERS}
+ ${${PROJECT_NAME}_PARSERS_HEADERS}
${${PROJECT_NAME}_ALGORITHM_HEADERS}
${${PROJECT_NAME}_PYTHON_HEADERS}
exception.hpp
@@ -289,8 +289,9 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/math")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/spatial")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/joint")
-MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/parser")
-MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/parser/lua")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers/lua")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers/urdf")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/tools")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/algorithm")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/python")
diff --git a/benchmark/timings-geometry.cpp b/benchmark/timings-geometry.cpp
index 66bc7073e..6faa9ab49 100644
--- a/benchmark/timings-geometry.cpp
+++ b/benchmark/timings-geometry.cpp
@@ -27,12 +27,11 @@
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/collisions.hpp"
-#include "pinocchio/multibody/parser/urdf.hpp"
-#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/multibody/geometry.hpp"
-#include "pinocchio/multibody/parser/urdf-with-geometry.hpp"
#ifdef WITH_HPP_MODEL_URDF
#include <hpp/util/debug.hh>
#include <hpp/model/device.hh>
diff --git a/benchmark/timings.cpp b/benchmark/timings.cpp
index e23c37c65..55107238d 100644
--- a/benchmark/timings.cpp
+++ b/benchmark/timings.cpp
@@ -27,8 +27,8 @@
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
-#include "pinocchio/multibody/parser/urdf.hpp"
-#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
#include <iostream>
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 38c29f83e..0bc13b375 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -45,25 +45,29 @@ ENDMACRO(ADD_TARGET_CFLAGS)
SET(${PROJECT_NAME}_MULTIBODY_SOURCES
multibody/model.cpp
- multibody/parser/sample-models.cpp
+ )
+
+SET(${PROJECT_NAME}_PARSERS_SOURCES
+ parsers/sample-models.cpp
)
IF(BUILD_PYTHON_INTERFACE)
- SET(${PROJECT_NAME}_MULTIBODY_PARSER_PYTHON_SOURCES
- multibody/parser/python.cpp
- )
+SET(${PROJECT_NAME}_PARSERS_PYTHON_SOURCES
+ parsers/python.cpp
+ )
ENDIF(BUILD_PYTHON_INTERFACE)
SET(${PROJECT_NAME}_SOURCES ${${PROJECT_NAME}_MULTIBODY_SOURCES})
IF(BUILD_PYTHON_INTERFACE)
- LIST(APPEND ${PROJECT_NAME}_SOURCES ${${PROJECT_NAME}_MULTIBODY_PARSER_PYTHON_SOURCES})
+ LIST(APPEND ${PROJECT_NAME}_SOURCES ${${PROJECT_NAME}_PARSERS_SOURCES}
+ ${${PROJECT_NAME}_PARSERS_PYTHON_SOURCES})
ENDIF(BUILD_PYTHON_INTERFACE)
IF(LUA5_1_FOUND)
SET(${PROJECT_NAME}_MULTIBODY_PARSER_LUA_SOURCES
- multibody/parser/lua/lua_tables.cpp
- multibody/parser/lua.cpp
+ parsers/lua/lua_tables.cpp
+ parsers/lua.cpp
)
LIST(APPEND ${PROJECT_NAME}_SOURCES ${${PROJECT_NAME}_MULTIBODY_PARSER_LUA_SOURCES})
ENDIF(LUA5_1_FOUND)
@@ -145,7 +149,7 @@ IF(BUILD_PYTHON_INTERFACE)
python/explog.py
)
- FOREACH(python ${PYTHON_FILES})
+ FOREACH(python ${PYTHON_FILES})
GET_FILENAME_COMPONENT(pythonFile ${python} NAME)
EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E ${LINK}
${${PROJECT_NAME}_SOURCE_DIR}/src/${python}
diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx
index fa1a08b70..7c69015b9 100644
--- a/src/multibody/geometry.hxx
+++ b/src/multibody/geometry.hxx
@@ -27,7 +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 "pinocchio/parsers/srdf.hpp"
#include <iostream>
#include <hpp/fcl/collision_object.h>
diff --git a/src/multibody/parser/urdf-with-geometry.hpp b/src/multibody/parser/urdf-with-geometry.hpp
deleted file mode 100644
index d0aecf248..000000000
--- a/src/multibody/parser/urdf-with-geometry.hpp
+++ /dev/null
@@ -1,98 +0,0 @@
-//
-// Copyright (c) 2015-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_urdf_geom_hpp__
-#define __se3_urdf_geom_hpp__
-
-#include <iostream>
-#include <exception>
-#include "pinocchio/multibody/model.hpp"
-
-#include <hpp/fcl/collision_object.h>
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/shape/geometric_shapes.h>
-
-#include "pinocchio/multibody/parser/urdf.hpp"
-
-namespace se3
-{
- namespace urdf
- {
-
-
- /**
- * @brief Get a fcl::CollisionObject from an urdf geometry, searching
- * for it in specified package directories
- *
- * @param[in] urdf_geometry The input urdf geometry
- * @param[in] package_dirs A vector containing the different directories where to search for packages
- * @param[out] mesh_path The Absolute path of the mesh currently read
- *
- * @return The geometry converted as a fcl::CollisionObject
- */
- inline fcl::CollisionObject retrieveCollisionGeometry(const boost::shared_ptr < ::urdf::Geometry > urdf_geometry,
- const std::vector < std::string > & package_dirs,
- std::string & mesh_path);
-
-
- /**
- * @brief Recursive procedure for reading the URDF tree, looking for geometries
- * This function fill the geometric model whith geometry objects retrieved from the URDF tree
- *
- * @param[in] link The current URDF link
- * @param model The model to which is the GeometryModel associated
- * @param model_geom The GeometryModel where the Collision Objects must be added
- * @param[in] package_dirs A vector containing the different directories where to search for packages
- * @param[in] rootJointAdded If a root joint was added at the begining of the urdf kinematic chain by user when constructing the Model
- */
- inline void parseTreeForGeom( ::urdf::LinkConstPtr link,
- const Model & model,
- GeometryModel & model_geom,
- const std::vector<std::string> & package_dirs) 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] package_dirs A vector containing the different directories
- * where to search for models and meshes.
- *
- * @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> ()) throw (std::invalid_argument);
-
- } // namespace urdf
-
-} // namespace se3
-
-
-/* --- Details -------------------------------------------------------------- */
-/* --- Details -------------------------------------------------------------- */
-/* --- Details -------------------------------------------------------------- */
-#include "pinocchio/multibody/parser/urdf-with-geometry.hxx"
-
-#endif // ifndef __se3_urdf_geom_hpp__
-
diff --git a/src/multibody/parser/urdf-with-geometry.hxx b/src/multibody/parser/urdf-with-geometry.hxx
deleted file mode 100644
index 49dd57a4e..000000000
--- a/src/multibody/parser/urdf-with-geometry.hxx
+++ /dev/null
@@ -1,193 +0,0 @@
-//
-// Copyright (c) 2015-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_urdf_geom_hxx__
-#define __se3_urdf_geom_hxx__
-
-#include <urdf_model/model.h>
-#include <urdf_parser/urdf_parser.h>
-
-#include <boost/foreach.hpp>
-#include "pinocchio/multibody/model.hpp"
-
-#include "pinocchio/multibody/parser/from-collada-to-fcl.hpp"
-#include <hpp/fcl/mesh_loader/assimp.h>
-
-
- namespace se3
-{
- namespace urdf
- {
- typedef fcl::BVHModel< fcl::OBBRSS > PolyhedronType;
- typedef boost::shared_ptr <PolyhedronType> PolyhedronPtrType;
-
- inline fcl::CollisionObject 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;
-
- // Handle the case where collision geometry is a mesh
- if (urdf_geometry->type == ::urdf::Geometry::MESH)
- {
- boost::shared_ptr < ::urdf::Mesh> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Mesh> (urdf_geometry);
- std::string collisionFilename = collisionGeometry->filename;
-
- mesh_path = convertURDFMeshPathToAbsolutePath(collisionFilename, package_dirs);
-
- fcl::Vec3f scale = fcl::Vec3f(collisionGeometry->scale.x,
- collisionGeometry->scale.y,
- collisionGeometry->scale.z
- );
-
- // Create FCL mesh by parsing Collada file.
- PolyhedronPtrType polyhedron (new PolyhedronType);
-
- fcl::loadPolyhedronFromResource (mesh_path, scale, polyhedron);
- geometry = polyhedron;
- }
-
- // Handle the case where collision geometry is a cylinder
- // Use FCL capsules for cylinders
- else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER)
- {
- mesh_path = "CYLINDER";
- boost::shared_ptr < ::urdf::Cylinder> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry);
-
- double radius = collisionGeometry->radius;
- double length = collisionGeometry->length;
-
- // Create fcl capsule geometry.
- geometry = boost::shared_ptr < fcl::CollisionGeometry >(new fcl::Capsule (radius, length));
- }
- // Handle the case where collision geometry is a box.
- else if (urdf_geometry->type == ::urdf::Geometry::BOX)
- {
- mesh_path = "BOX";
- boost::shared_ptr < ::urdf::Box> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Box> (urdf_geometry);
-
- double x = collisionGeometry->dim.x;
- double y = collisionGeometry->dim.y;
- double z = collisionGeometry->dim.z;
-
- geometry = boost::shared_ptr < fcl::CollisionGeometry > (new fcl::Box (x, y, z));
- }
- // Handle the case where collision geometry is a sphere.
- else if (urdf_geometry->type == ::urdf::Geometry::SPHERE)
- {
- mesh_path = "SPHERE";
- boost::shared_ptr < ::urdf::Sphere> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Sphere> (urdf_geometry);
-
- double radius = collisionGeometry->radius;
-
- geometry = boost::shared_ptr < fcl::CollisionGeometry > (new fcl::Sphere (radius));
- }
- else throw std::runtime_error (std::string ("Unknown geometry type :"));
-
- if (!geometry)
- {
- throw std::runtime_error(std::string("The polyhedron retrived is empty"));
- }
- fcl::CollisionObject collisionObject (geometry, fcl::Transform3f());
-
- return collisionObject;
- }
-
-
- inline void parseTreeForGeom(::urdf::LinkConstPtr link,
- const Model & model,
- GeometryModel & geom_model,
- const std::vector<std::string> & package_dirs) throw (std::invalid_argument)
- {
-
- std::string mesh_path = "";
-
- std::string link_name = link->name;
- // start with first link that is not empty
- if(link->collision)
- {
- assert(link->getParent()!=NULL);
- if (link->getParent() == NULL)
- {
- const std::string exception_message (link->name + " - joint information missing.");
- throw std::invalid_argument(exception_message);
- }
-
- for (std::vector< boost::shared_ptr< ::urdf::Collision> >::const_iterator i = link->collision_array.begin();i != link->collision_array.end(); ++i)
- {
- fcl::CollisionObject collision_object = retrieveCollisionGeometry((*i)->geometry, package_dirs, mesh_path);
- SE3 geomPlacement = convertFromUrdf((*i)->origin);
- const std::string & collision_object_name = link_name;
- geom_model.addCollisionObject(model.getFrameParent(link_name), collision_object, geomPlacement, collision_object_name, mesh_path);
- }
- } // if(link->collision)
-
- if(link->visual)
- {
- assert(link->getParent()!=NULL);
- if (link->getParent() == NULL)
- {
- const std::string exception_message (link->name + " - joint information missing.");
- throw std::invalid_argument(exception_message);
- }
-
- for (std::vector< boost::shared_ptr< ::urdf::Visual> >::const_iterator i = link->visual_array.begin(); i != link->visual_array.end(); ++i)
- {
- fcl::CollisionObject visual_object = retrieveCollisionGeometry((*i)->geometry, package_dirs, mesh_path);
- SE3 geomPlacement = convertFromUrdf((*i)->origin);
- const std::string & visual_object_name = link_name;
- geom_model.addVisualObject(model.getFrameParent(link_name), visual_object, geomPlacement, visual_object_name, mesh_path);
- }
- } // if(link->visual)
-
-
- BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
- {
- parseTreeForGeom(child, model, geom_model, package_dirs);
- }
-
- }
-
-
- inline GeometryModel buildGeom(const Model & model,
- const std::string & filename,
- const std::vector<std::string> & package_dirs) throw(std::invalid_argument)
- {
- GeometryModel model_geom(model);
-
- std::vector<std::string> hint_directories(package_dirs);
-
- // Append the ROS_PACKAGE_PATH
- std::vector<std::string> ros_pkg_paths = extractPathFromEnvVar("ROS_PACKAGE_PATH");
- hint_directories.insert(hint_directories.end(), ros_pkg_paths.begin(), ros_pkg_paths.end());
-
- if(hint_directories.empty())
- {
- throw std::runtime_error("You did not specify any package directory and ROS_PACKAGE_PATH is empty. Geometric parsing will crash");
- }
-
- ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
- parseTreeForGeom(urdfTree->getRoot(), model, model_geom, hint_directories);
- return model_geom;
- }
-
-
- } // namespace urdf
-} // namespace se3
-
-#endif // ifndef __se3_urdf_geom_hxx__
-
diff --git a/src/multibody/parser/urdf.hxx b/src/multibody/parser/urdf.hxx
deleted file mode 100644
index baab62097..000000000
--- a/src/multibody/parser/urdf.hxx
+++ /dev/null
@@ -1,624 +0,0 @@
- //
-// Copyright (c) 2015-2016 CNRS
-// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
-//
-// 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_urdf_hxx__
-#define __se3_urdf_hxx__
-
-#include <urdf_model/model.h>
-#include <urdf_parser/urdf_parser.h>
-
-#include <iostream>
-#include <sstream>
-#include <iomanip>
-#include <boost/foreach.hpp>
-#include "pinocchio/multibody/model.hpp"
-
-#include <exception>
-
-/// @cond DEV
-
-namespace se3
-{
- namespace urdf
- {
-
- ///
- /// \brief Convert URDF Inertial quantity to Spatial Inertia.
- ///
- /// \param[in] Y The input URDF Inertia.
- ///
- /// \return The converted Spatial Inertia se3::Inertia.
- ///
- inline Inertia convertFromUrdf (const ::urdf::Inertial & Y)
- {
- const ::urdf::Vector3 & p = Y.origin.position;
- const ::urdf::Rotation & q = Y.origin.rotation;
-
- const Eigen::Vector3d com(p.x,p.y,p.z);
- const Eigen::Matrix3d & R = Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix();
-
- Eigen::Matrix3d I; I << Y.ixx,Y.ixy,Y.ixz
- , Y.ixy,Y.iyy,Y.iyz
- , Y.ixz,Y.iyz,Y.izz;
- return Inertia(Y.mass,com,R*I*R.transpose());
- }
-
- ///
- /// \brief Convert URDF Pose quantity to SE3.
- ///
- /// \param[in] M The input URDF Pose.
- ///
- /// \return The converted pose/transform se3::SE3.
- ///
- inline SE3 convertFromUrdf (const ::urdf::Pose & M)
- {
- const ::urdf::Vector3 & p = M.position;
- const ::urdf::Rotation & q = M.rotation;
- return SE3( Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix(), Eigen::Vector3d(p.x,p.y,p.z));
- }
-
- ///
- /// \brief The four possible cartesian types of an 3D axis.
- ///
- enum AxisCartesian { AXIS_X, AXIS_Y, AXIS_Z, AXIS_UNALIGNED };
-
- ///
- /// \brief Extract the cartesian property of a particular 3D axis.
- ///
- /// \param[in] axis The input URDF axis.
- ///
- /// \return The property of the particular axis se3::urdf::AxisCartesian.
- ///
- inline AxisCartesian extractCartesianAxis (const ::urdf::Vector3 & axis)
- {
- if( (axis.x==1.0)&&(axis.y==0.0)&&(axis.z==0.0) )
- return AXIS_X;
- else if( (axis.x==0.0)&&(axis.y==1.0)&&(axis.z==0.0) )
- return AXIS_Y;
- else if( (axis.x==0.0)&&(axis.y==0.0)&&(axis.z==1.0) )
- return AXIS_Z;
- else
- return AXIS_UNALIGNED;
- }
-
- ///
- /// \brief Recursive procedure for reading the URDF tree.
- /// The function returns an exception as soon as a necessary Inertia or Joint information are missing.
- ///
- /// \param[in] link The current URDF link.
- /// \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)
- {
-
- // Parent joint of the current body
- ::urdf::JointConstPtr joint = link->parent_joint;
-
- // OffSet of the next link. In case we encounter a fixed joint, we need to propagate the offset placement of its attached body to next joint.
- SE3 nextPlacementOffset = SE3::Identity();
-
- if(joint != NULL) // if the link is not the root of the tree
- {
- assert(link->getParent()!=NULL);
-
- const std::string & joint_name = joint->name;
- const std::string & link_name = link->name;
- const std::string & parent_link_name = link->getParent()->name;
- std::string joint_info = "";
-
- // check if inertial information is provided
- if (!link->inertial && joint->type != ::urdf::Joint::FIXED)
- {
- const std::string exception_message (link->name + " - spatial inertial information missing.");
- throw std::invalid_argument(exception_message);
- }
-
- Model::JointIndex parent_joint_id = (link->getParent()->parent_joint==NULL) ? (model.existJointName("root_joint") ? model.getJointId("root_joint") : 0) :
- model.getJointId( link->getParent()->parent_joint->name );
-
- // Transformation from the parent link to the joint origin
- const SE3 & jointPlacement = placementOffset*convertFromUrdf(joint->parent_to_joint_origin_transform);
-
- const Inertia & Y = (link->inertial) ? convertFromUrdf(*link->inertial) :
- Inertia::Zero();
-
-
- switch(joint->type)
- {
- case ::urdf::Joint::FLOATING:
- {
- joint_info = "joint FreeFlyer";
-
- typedef JointModelFreeFlyer::ConfigVector_t ConfigVector_t;
- typedef JointModelFreeFlyer::TangentVector_t TangentVector_t;
- typedef ConfigVector_t::Scalar Scalar;
-
-
- ConfigVector_t lower_position;
- lower_position.fill(std::numeric_limits<Scalar>::min());
-
- ConfigVector_t upper_position;
- upper_position.fill(std::numeric_limits<Scalar>::max());
-
- TangentVector_t max_effort;
- max_effort.fill(std::numeric_limits<Scalar>::max());
-
- TangentVector_t max_velocity;
- max_velocity.fill(std::numeric_limits<Scalar>::max());
-
- model.addJointAndBody(parent_joint_id, JointModelFreeFlyer(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name, link->name);
- break;
- }
- case ::urdf::Joint::REVOLUTE:
- {
- joint_info = "joint REVOLUTE with axis";
-
- typedef JointModelRX::ConfigVector_t ConfigVector_t;
- typedef JointModelRX::TangentVector_t TangentVector_t;
-
- TangentVector_t max_effort;
- TangentVector_t max_velocity;
- ConfigVector_t lower_position;
- ConfigVector_t upper_position;
-
- if (joint->limits)
- {
- max_effort << joint->limits->effort;
- max_velocity << joint->limits->velocity;
- lower_position << joint->limits->lower;
- upper_position << joint->limits->upper;
- }
-
- Eigen::Vector3d jointAxis(Eigen::Vector3d::Zero());
- AxisCartesian axis = extractCartesianAxis(joint->axis);
-
- switch(axis)
- {
- case AXIS_X:
- {
- joint_info += " along X";
- model.addJointAndBody(parent_joint_id, JointModelRX(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name );
- break;
- }
- case AXIS_Y:
- {
- joint_info += " along Y";
- model.addJointAndBody(parent_joint_id, JointModelRY(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name );
- break;
- }
- case AXIS_Z:
- {
- joint_info += " along Z";
- model.addJointAndBody(parent_joint_id, JointModelRZ(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name );
- break;
- }
- case AXIS_UNALIGNED:
- {
- std::stringstream axis_value;
- axis_value << std::setprecision(5);
- axis_value << "(" << joint->axis.x <<"," << joint->axis.y << "," << joint->axis.z << ")";
- joint_info += " unaligned " + axis_value.str();
-
- jointAxis = Eigen::Vector3d( joint->axis.x,joint->axis.y,joint->axis.z );
- jointAxis.normalize();
- model.addJointAndBody( parent_joint_id, JointModelRevoluteUnaligned(jointAxis),
- jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name );
- break;
- }
- default:
- {
- assert(false && "The axis type of the revolute joint is of wrong type.");
- break;
- }
- }
- break;
- }
- case ::urdf::Joint::CONTINUOUS: // Revolute joint with no joint limits
- {
- joint_info = "joint CONTINUOUS with axis";
-
- typedef JointModelRX::ConfigVector_t ConfigVector_t;
- typedef JointModelRX::TangentVector_t TangentVector_t;
-
- TangentVector_t max_effort;
- TangentVector_t max_velocity;
- ConfigVector_t lower_position;
- ConfigVector_t upper_position;
-
- if (joint->limits)
- {
- max_effort << joint->limits->effort;
- max_velocity << joint->limits->velocity;
- lower_position << joint->limits->lower;
- upper_position << joint->limits->upper;
- }
-
- Eigen::Vector3d jointAxis(Eigen::Vector3d::Zero());
- AxisCartesian axis = extractCartesianAxis(joint->axis);
-
- switch(axis)
- {
- case AXIS_X:
- {
- joint_info += " along X";
- model.addJointAndBody(parent_joint_id, JointModelRX(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name );
- break;
- }
- case AXIS_Y:
- {
- joint_info += " along Y";
- model.addJointAndBody(parent_joint_id, JointModelRY(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name );
- break;
- }
- case AXIS_Z:
- {
- joint_info += " along Z";
- model.addJointAndBody(parent_joint_id, JointModelRZ(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name );
- break;
- }
- case AXIS_UNALIGNED:
- {
- std::stringstream axis_value;
- axis_value << std::setprecision(5);
- axis_value << "(" << joint->axis.x <<"," << joint->axis.y << "," << joint->axis.z << ")";
- joint_info += " unaligned " + axis_value.str();
-
- jointAxis = Eigen::Vector3d( joint->axis.x,joint->axis.y,joint->axis.z );
- jointAxis.normalize();
- model.addJointAndBody( parent_joint_id, JointModelRevoluteUnaligned(jointAxis),
- jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name );
- break;
- }
- default:
- {
- assert(false && "The axis type of the revolute joint is of wrong type.");
- break;
- }
- }
- break;
- }
- case ::urdf::Joint::PRISMATIC:
- {
- joint_info = "joint PRISMATIC with axis";
-
- typedef JointModelRX::ConfigVector_t ConfigVector_t;
- typedef JointModelRX::TangentVector_t TangentVector_t;
-
- TangentVector_t max_effort;
- TangentVector_t max_velocity;
- ConfigVector_t lower_position;
- ConfigVector_t upper_position;
-
- if (joint->limits)
- {
- max_effort << joint->limits->effort;
- max_velocity << joint->limits->velocity;
- lower_position << joint->limits->lower;
- upper_position << joint->limits->upper;
- }
-
- AxisCartesian axis = extractCartesianAxis(joint->axis);
- switch(axis)
- {
- case AXIS_X:
- {
- joint_info += " along X";
- model.addJointAndBody(parent_joint_id, JointModelPX(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name );
- break;
- }
- case AXIS_Y:
- {
- joint_info += " along Y";
- model.addJointAndBody(parent_joint_id, JointModelPY(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name);
- break;
- }
- case AXIS_Z:
- {
- joint_info += " along Z";
- model.addJointAndBody(parent_joint_id, JointModelPZ(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name );
- break;
- }
- case AXIS_UNALIGNED:
- {
- std::stringstream axis_value;
- axis_value << std::setprecision(5);
- axis_value << "(" << joint->axis.x <<"," << joint->axis.y << "," << joint->axis.z << ")";
- joint_info += " unaligned " + axis_value.str();
-
- Eigen::Vector3d jointAxis(Eigen::Vector3d(joint->axis.x,joint->axis.y,joint->axis.z));
- jointAxis.normalize();
- model.addJointAndBody(parent_joint_id, JointModelPrismaticUnaligned(jointAxis),
- jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name);
-
- break;
- }
- default:
- {
- assert(false && "The axis type of the prismatic joint is of wrong type.");
- break;
- }
- }
- break;
- }
- case ::urdf::Joint::PLANAR:
- {
- joint_info = "joint PLANAR with normal axis along Z";
-
- typedef JointModelPlanar::ConfigVector_t ConfigVector_t;
- typedef JointModelPlanar::TangentVector_t TangentVector_t;
-
- TangentVector_t max_effort;
- TangentVector_t max_velocity;
- ConfigVector_t lower_position;
- ConfigVector_t upper_position;
-
- if (joint->limits)
- {
- max_effort << joint->limits->effort;
- max_velocity << joint->limits->velocity;
- lower_position << joint->limits->lower;
- upper_position << joint->limits->upper;
- }
-
- model.addJointAndBody(parent_joint_id, JointModelPlanar(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name );
-
- break;
- }
- case ::urdf::Joint::FIXED:
- {
- // In case of fixed joint, if link has inertial tag:
- // -add the inertia of the link to his parent in the model
- // Otherwise do nothing.
- // In all cases:
- // -let all the children become children of parent
- // -inform the parser of the offset to apply
- // -add fixed body in model to display it in gepetto-viewer
-
- joint_info = "fixed joint";
- if (link->inertial)
- {
- model.appendBodyToJoint(parent_joint_id, jointPlacement, Y, link->name); //Modify the parent inertia in the model
- }
-
- //transformation of the current placement offset
- nextPlacementOffset = jointPlacement;
-
- // Add a frame in the model to keep trace of this fixed joint
- model.addFrame(joint->name, parent_joint_id, nextPlacementOffset, FIXED_JOINT);
-
- //for the children of the current link, set their parent to be
- //the the parent of the current link.
- BOOST_FOREACH(::urdf::LinkPtr child_link, link->child_links)
- {
- child_link->setParent(link->getParent() );
- }
- break;
- }
- default:
- {
- const std::string exception_message ("The type of joint " + joint_name + " is not supported.");
- throw std::invalid_argument(exception_message);
- break;
- }
- }
-
- if (verbose)
- {
- std::cout << "Adding Body" << std::endl;
- std::cout << "\"" << link_name << "\" connected to " << "\"" << parent_link_name << "\" throw joint " << "\"" << joint_name << "\"" << std::endl;
- std::cout << "joint type: " << joint_info << std::endl;
- std::cout << "joint placement:\n" << jointPlacement;
- std::cout << "body info: " << std::endl;
- std::cout << " " << "mass: " << Y.mass() << std::endl;
- std::cout << " " << "lever: " << Y.lever().transpose() << std::endl;
- std::cout << " " << "inertia elements (Ixx,Iyx,Iyy,Izx,Izy,Izz): " << Y.inertia().data().transpose() << std::endl << std::endl;
- }
-
- }
- else if (link->getParent() != NULL)
- {
- const std::string exception_message (link->name + " - joint information missing.");
- throw std::invalid_argument(exception_message);
- }
-
-
- BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
- {
- parseTree(child, model, nextPlacementOffset, verbose);
- }
- }
-
-
- template <typename D>
- void parseRootTree (::urdf::LinkConstPtr root_link, Model & model, const JointModelBase<D> & root_joint, const bool verbose) throw (std::invalid_argument)
- {
- // If the root link has no inertial info (because it is a base_link for example),
- // we have to merge its inertial info with all its children connected to it with fixed joints
- if (!root_link->inertial)
- {
- // If the root link has only one child
- if (root_link->child_links.size() == 1)
- {
- ::urdf::LinkPtr child_link = root_link->child_links[0];
- assert(child_link->inertial != NULL && "Inertial information missing for parsing the root link.");
- const Inertia & Y = convertFromUrdf(*child_link->inertial);
- model.addJointAndBody(0, root_joint, SE3::Identity(), Y, "root_joint", child_link->name);
-
- // Change the name of the parent joint in the URDF tree
- child_link->parent_joint->name = "root_joint";
-
- BOOST_FOREACH(::urdf::LinkConstPtr child, child_link->child_links)
- {
- parseTree(child, model, SE3::Identity(), verbose);
- }
- }
- else
- {
- const std::string exception_message (root_link->name + " has no inertial information and has more than one child link. It corresponds to a disjoint tree.");
- throw std::invalid_argument(exception_message);
- }
-
- }
- else // otherwise, it is a plain body with inertial info. It processes as usual.
- {
- const Inertia & Y = convertFromUrdf(*root_link->inertial);
- model.addJointAndBody(0, root_joint, SE3::Identity(), Y , "root_joint", root_link->name);
-
- BOOST_FOREACH(::urdf::LinkPtr child, root_link->child_links)
- {
- parseTree(child, model, SE3::Identity(), verbose);
- }
- }
-
- }
-
-
- template <typename D>
- Model buildModel (const std::string & filename, const JointModelBase<D> & root_joint, bool verbose) throw (std::invalid_argument)
- {
- Model model;
-
- ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
- if (urdfTree)
- parseRootTree(urdfTree->getRoot(), model, root_joint, verbose);
- else
- {
- const std::string exception_message ("The file " + filename + " does not contain a valid URDF model.");
- throw std::invalid_argument(exception_message);
- }
-
- return model;
- }
-
- void parseRootTree (::urdf::LinkConstPtr root_link, Model & model, const bool verbose) throw (std::invalid_argument)
- {
- // If the root link has no inertial info, it may be because it is a base_link.
- // In this case, we look for its child links which indeed contribute to the dynamics, they are not fixed to the universe.
- // TODO: it may be necessary to compute joint placement variable instead of setting it to SE3::Identity()
- if (!root_link->inertial)
- {
- typedef std::vector< ::urdf::LinkPtr > LinkSharedPtrVector_t;
- LinkSharedPtrVector_t movable_child_links;
- LinkSharedPtrVector_t direct_child_links(root_link->child_links);
- LinkSharedPtrVector_t next_direct_child_links; // next child to visit
- LinkSharedPtrVector_t pathologic_child_links; // children which have inertial info but rigidly attached to the world
- do
- {
- next_direct_child_links.clear();
- BOOST_FOREACH(::urdf::LinkPtr child, direct_child_links)
- {
- if (child->parent_joint->type != ::urdf::Joint::FIXED)
- movable_child_links.push_back(child);
- else
- {
- if (child->inertial)
- pathologic_child_links.push_back(child);
- next_direct_child_links.insert (next_direct_child_links.end(), child->child_links.begin(), child->child_links.end());
- }
-
- }
- direct_child_links = next_direct_child_links;
- }
- while (!direct_child_links.empty());
-
- if (!pathologic_child_links.empty())
- {
- std::cout << "[INFO] The links:" << std::endl;
- for (LinkSharedPtrVector_t::iterator it = pathologic_child_links.begin();
- it < pathologic_child_links.end(); ++it)
- {
- std::cout << " - " << (*it)->name << std::endl;
- }
- std::cout << "are fixed regarding to the universe (base_link) and convey inertial info. They won't affect the dynamics of the output model. Maybe, a root joint is missing connecting this links to the universe." << std::endl;
-
- }
-
- BOOST_FOREACH(::urdf::LinkPtr child, movable_child_links)
- {
- child->getParent()->parent_joint->name = model.names[0];
- parseTree(child, model, SE3::Identity(), verbose);
- }
-
- }
- else // Otherwise, we have to rase an exception because the first link will no be added to the model.
- // It seems a root joint is missing.
- {
- std::cout << "[INFO] The root link " << root_link->name << " of the model tree contains inertial information. It seems that a root joint is missing connecting this root link to the universe. The root link won't affect the dynamics of the model." << std::endl;
-
- BOOST_FOREACH(::urdf::LinkPtr child, root_link->child_links)
- {
- parseTree(child, model, SE3::Identity(), verbose);
- }
- }
-
-
- }
-
-
- inline Model buildModel (const std::string & filename, const bool verbose) throw (std::invalid_argument)
- {
- Model model;
-
- ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
- if (urdfTree)
- parseRootTree(urdfTree->getRoot(),model,verbose);
- else
- {
- const std::string exception_message ("The file " + filename + " does not contain a valid URDF model.");
- throw std::invalid_argument(exception_message);
- }
-
- return model;
- }
-
- } // namespace urdf
-
-} // namespace se3
-
-/// @endcond
-
-#endif // ifndef __se3_urdf_hxx__
diff --git a/src/multibody/parser/utils.hpp b/src/multibody/parser/utils.hpp
deleted file mode 100644
index 20cc54329..000000000
--- a/src/multibody/parser/utils.hpp
+++ /dev/null
@@ -1,49 +0,0 @@
-//
-// Copyright (c) 2015 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/>.
-
-#include <iostream>
-
-namespace se3
-{
- ///
- /// \brief Supported model file extensions
- ///
- enum ModelFileExtensionType{
- UNKNOWN = 0,
- URDF,
- LUA
- };
-
- ///
- /// \brief Extract the type of the given model file according to its extension
- ///
- /// \param[in] filemane The complete path to the model file.
- ///
- /// \return The type of the extension of the model file
- ///
- ModelFileExtensionType checkModelFileExtension (const std::string & filename)
- {
- const std::string extension = filename.substr(filename.find_last_of(".") + 1);
-
- if (extension == "urdf")
- return URDF;
- else if (extension == "lua")
- return LUA;
-
- return UNKNOWN;
- }
-} // namespace se3
diff --git a/src/multibody/parser/lua.cpp b/src/parsers/lua.cpp
similarity index 98%
rename from src/multibody/parser/lua.cpp
rename to src/parsers/lua.cpp
index a45d7cdff..9a5a558ac 100644
--- a/src/multibody/parser/lua.cpp
+++ b/src/parsers/lua.cpp
@@ -1,6 +1,6 @@
-#include "pinocchio/multibody/parser/lua/lua_tables.hpp"
-#include "pinocchio/multibody/parser/lua.hpp"
+#include "pinocchio/parsers/lua/lua_tables.hpp"
+#include "pinocchio/parsers/lua.hpp"
#include <lua.hpp>
#include <iostream>
diff --git a/src/multibody/parser/lua.hpp b/src/parsers/lua.hpp
similarity index 100%
rename from src/multibody/parser/lua.hpp
rename to src/parsers/lua.hpp
diff --git a/src/multibody/parser/lua/lua_tables.cpp b/src/parsers/lua/lua_tables.cpp
similarity index 99%
rename from src/multibody/parser/lua/lua_tables.cpp
rename to src/parsers/lua/lua_tables.cpp
index 11f4b6235..e95040b61 100644
--- a/src/multibody/parser/lua/lua_tables.cpp
+++ b/src/parsers/lua/lua_tables.cpp
@@ -25,7 +25,7 @@
* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
-#include "pinocchio/multibody/parser/lua/lua_tables.hpp"
+#include "pinocchio/parsers/lua/lua_tables.hpp"
#include <assert.h>
#include <iostream>
diff --git a/src/multibody/parser/lua/lua_tables.hpp b/src/parsers/lua/lua_tables.hpp
similarity index 100%
rename from src/multibody/parser/lua/lua_tables.hpp
rename to src/parsers/lua/lua_tables.hpp
diff --git a/src/multibody/parser/python.cpp b/src/parsers/python.cpp
similarity index 97%
rename from src/multibody/parser/python.cpp
rename to src/parsers/python.cpp
index 3f1b48580..b88404fbd 100644
--- a/src/multibody/parser/python.cpp
+++ b/src/parsers/python.cpp
@@ -16,7 +16,7 @@
// <http://www.gnu.org/licenses/>.
-#include "pinocchio/multibody/parser/python.hpp"
+#include "pinocchio/parsers/python.hpp"
#include "pinocchio/python/model.hpp"
#include <iostream>
diff --git a/src/multibody/parser/python.hpp b/src/parsers/python.hpp
similarity index 100%
rename from src/multibody/parser/python.hpp
rename to src/parsers/python.hpp
diff --git a/src/multibody/parser/sample-models.cpp b/src/parsers/sample-models.cpp
similarity index 99%
rename from src/multibody/parser/sample-models.cpp
rename to src/parsers/sample-models.cpp
index d9af943db..ddb89526b 100644
--- a/src/multibody/parser/sample-models.cpp
+++ b/src/parsers/sample-models.cpp
@@ -16,7 +16,7 @@
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
-#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
#ifdef WITH_HPP_FCL
#include <hpp/fcl/shape/geometric_shapes.h>
diff --git a/src/multibody/parser/sample-models.hpp b/src/parsers/sample-models.hpp
similarity index 100%
rename from src/multibody/parser/sample-models.hpp
rename to src/parsers/sample-models.hpp
diff --git a/src/multibody/parser/srdf.hpp b/src/parsers/srdf.hpp
similarity index 100%
rename from src/multibody/parser/srdf.hpp
rename to src/parsers/srdf.hpp
diff --git a/src/multibody/parser/urdf.hpp b/src/parsers/urdf.hpp
similarity index 64%
rename from src/multibody/parser/urdf.hpp
rename to src/parsers/urdf.hpp
index 72c8d3245..8e63f552a 100644
--- a/src/multibody/parser/urdf.hpp
+++ b/src/parsers/urdf.hpp
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2016 CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
@@ -16,8 +16,8 @@
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
-#ifndef __se3_urdf_hpp__
-#define __se3_urdf_hpp__
+#ifndef __se3_parsers_urdf_hpp__
+#define __se3_parsers_urdf_hpp__
#include <urdf_model/model.h>
#include <urdf_parser/urdf_parser.h>
@@ -31,6 +31,12 @@
#include <exception>
#include <limits>
+#ifdef WITH_HPP_FCL
+#include <hpp/fcl/collision_object.h>
+#include <hpp/fcl/collision.h>
+#include <hpp/fcl/shape/geometric_shapes.h>
+#endif
+
namespace urdf
{
typedef boost::shared_ptr<ModelInterface> ModelInterfacePtr;
@@ -45,34 +51,6 @@ namespace se3
namespace urdf
{
- ///
- /// \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.
- ///
- /// \param[in] link The current URDF link.
- /// \param[in] model The model where the link must be added.
- /// \param[in] verbose Print parsing info.
- ///
- void parseRootTree (::urdf::LinkConstPtr link,
- Model & model,
- const bool verbose = false) throw (std::invalid_argument);
-
-
- ///
- /// \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.
- ///
- /// \param[in] link The current URDF link.
- /// \param[in] model The model where the link must be added.
- /// \param[in] root_joint The specific root joint.
- /// \param[in] verbose Print parsing info.
- ///
- template <typename D>
- void parseRootTree (::urdf::LinkConstPtr link,
- Model & model,
- const JointModelBase<D> & root_joint,
- const bool verbose = false) throw (std::invalid_argument);
-
///
/// \brief Build the model from a URDF file with a particular joint as root of the model tree.
///
@@ -98,12 +76,38 @@ namespace se3
inline Model buildModel (const std::string & filename,
const bool verbose = false) throw (std::invalid_argument);
+#ifdef WITH_HPP_FCL
+
+ /**
+ * @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] package_dirs A vector containing the different directories
+ * where to search for models and meshes, typically
+ * obtained from calling se3::rosPaths()
+ *
+ * @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> ()) throw (std::invalid_argument);
+
+#endif
+
} // namespace urdf
} // namespace se3
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
-#include "pinocchio/multibody/parser/urdf.hxx"
+#include "pinocchio/parsers/urdf/model.hxx"
+#ifdef WITH_HPP_FCL
+#include "pinocchio/parsers/urdf/geometry.hxx"
+#endif
-#endif // ifndef __se3_urdf_hpp__
+#endif // ifndef __se3_parsers_urdf_hpp__
diff --git a/src/parsers/urdf/geometry.hxx b/src/parsers/urdf/geometry.hxx
new file mode 100644
index 000000000..c158e948a
--- /dev/null
+++ b/src/parsers/urdf/geometry.hxx
@@ -0,0 +1,224 @@
+//
+// Copyright (c) 2015 - 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_parsers_urdf_geometry_hxx__
+#define __se3_parsers_urdf_geometry_hxx__
+
+#include <urdf_model/model.h>
+#include <urdf_parser/urdf_parser.h>
+
+#include <iostream>
+#include <sstream>
+#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
+ {
+ namespace details
+ {
+ typedef fcl::BVHModel< fcl::OBBRSS > PolyhedronType;
+ typedef boost::shared_ptr <PolyhedronType> PolyhedronPtrType;
+
+ /**
+ * @brief Get a fcl::CollisionObject from an urdf geometry, searching
+ * for it in specified package directories
+ *
+ * @param[in] urdf_geometry The input urdf geometry
+ * @param[in] package_dirs A vector containing the different directories where to search for packages
+ * @param[out] mesh_path The Absolute path of the mesh currently read
+ *
+ * @return The geometry converted as a fcl::CollisionObject
+ */
+ inline fcl::CollisionObject 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;
+
+ // Handle the case where collision geometry is a mesh
+ if (urdf_geometry->type == ::urdf::Geometry::MESH)
+ {
+ boost::shared_ptr < ::urdf::Mesh> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Mesh> (urdf_geometry);
+ std::string collisionFilename = collisionGeometry->filename;
+
+ mesh_path = convertURDFMeshPathToAbsolutePath(collisionFilename, package_dirs);
+
+ fcl::Vec3f scale = fcl::Vec3f(collisionGeometry->scale.x,
+ collisionGeometry->scale.y,
+ collisionGeometry->scale.z
+ );
+
+ // Create FCL mesh by parsing Collada file.
+ PolyhedronPtrType polyhedron (new PolyhedronType);
+
+ fcl::loadPolyhedronFromResource (mesh_path, scale, polyhedron);
+ geometry = polyhedron;
+ }
+
+ // Handle the case where collision geometry is a cylinder
+ // Use FCL capsules for cylinders
+ else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER)
+ {
+ mesh_path = "CYLINDER";
+ boost::shared_ptr < ::urdf::Cylinder> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry);
+
+ double radius = collisionGeometry->radius;
+ double length = collisionGeometry->length;
+
+ // Create fcl capsule geometry.
+ geometry = boost::shared_ptr < fcl::CollisionGeometry >(new fcl::Capsule (radius, length));
+ }
+ // Handle the case where collision geometry is a box.
+ else if (urdf_geometry->type == ::urdf::Geometry::BOX)
+ {
+ mesh_path = "BOX";
+ boost::shared_ptr < ::urdf::Box> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Box> (urdf_geometry);
+
+ double x = collisionGeometry->dim.x;
+ double y = collisionGeometry->dim.y;
+ double z = collisionGeometry->dim.z;
+
+ geometry = boost::shared_ptr < fcl::CollisionGeometry > (new fcl::Box (x, y, z));
+ }
+ // Handle the case where collision geometry is a sphere.
+ else if (urdf_geometry->type == ::urdf::Geometry::SPHERE)
+ {
+ mesh_path = "SPHERE";
+ boost::shared_ptr < ::urdf::Sphere> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Sphere> (urdf_geometry);
+
+ double radius = collisionGeometry->radius;
+
+ geometry = boost::shared_ptr < fcl::CollisionGeometry > (new fcl::Sphere (radius));
+ }
+ else throw std::runtime_error (std::string ("Unknown geometry type :"));
+
+ if (!geometry)
+ {
+ throw std::runtime_error(std::string("The polyhedron retrived is empty"));
+ }
+ fcl::CollisionObject collisionObject (geometry, fcl::Transform3f());
+
+ return collisionObject;
+ }
+
+
+ /**
+ * @brief Recursive procedure for reading the URDF tree, looking for geometries
+ * This function fill the geometric model whith geometry objects retrieved from the URDF tree
+ *
+ * @param[in] link The current URDF link
+ * @param model The model to which is the GeometryModel associated
+ * @param model_geom The GeometryModel where the Collision Objects must be added
+ * @param[in] package_dirs A vector containing the different directories where to search for packages
+ * @param[in] rootJointAdded If a root joint was added at the begining of the urdf kinematic chain by user when constructing the Model
+ */
+ inline void parseTreeForGeom(::urdf::LinkConstPtr link,
+ const Model & model,
+ GeometryModel & geom_model,
+ const std::vector<std::string> & package_dirs) throw (std::invalid_argument)
+ {
+
+ std::string mesh_path = "";
+
+ std::string link_name = link->name;
+ // start with first link that is not empty
+ if(link->collision)
+ {
+ assert(link->getParent()!=NULL);
+ if (link->getParent() == NULL)
+ {
+ const std::string exception_message (link->name + " - joint information missing.");
+ throw std::invalid_argument(exception_message);
+ }
+
+ for (std::vector< boost::shared_ptr< ::urdf::Collision> >::const_iterator i = link->collision_array.begin();i != link->collision_array.end(); ++i)
+ {
+ fcl::CollisionObject collision_object = retrieveCollisionGeometry((*i)->geometry, package_dirs, mesh_path);
+ SE3 geomPlacement = convertFromUrdf((*i)->origin);
+ const std::string & collision_object_name = link_name;
+ geom_model.addCollisionObject(model.getFrameParent(link_name), collision_object, geomPlacement, collision_object_name, mesh_path);
+ }
+ } // if(link->collision)
+
+ if(link->visual)
+ {
+ assert(link->getParent()!=NULL);
+ if (link->getParent() == NULL)
+ {
+ const std::string exception_message (link->name + " - joint information missing.");
+ throw std::invalid_argument(exception_message);
+ }
+
+ for (std::vector< boost::shared_ptr< ::urdf::Visual> >::const_iterator i = link->visual_array.begin(); i != link->visual_array.end(); ++i)
+ {
+ fcl::CollisionObject visual_object = retrieveCollisionGeometry((*i)->geometry, package_dirs, mesh_path);
+ SE3 geomPlacement = convertFromUrdf((*i)->origin);
+ const std::string & visual_object_name = link_name;
+ geom_model.addVisualObject(model.getFrameParent(link_name), visual_object, geomPlacement, visual_object_name, mesh_path);
+ }
+ } // if(link->visual)
+
+
+ BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
+ {
+ parseTreeForGeom(child, model, geom_model, package_dirs);
+ }
+
+ }
+
+
+ } // namespace details
+
+
+
+ inline GeometryModel buildGeom(const Model & model,
+ const std::string & filename,
+ const std::vector<std::string> & package_dirs) throw(std::invalid_argument)
+ {
+ GeometryModel model_geom(model);
+
+ std::vector<std::string> hint_directories(package_dirs);
+
+ // Append the ROS_PACKAGE_PATH
+ std::vector<std::string> ros_pkg_paths = rosPaths();
+ hint_directories.insert(hint_directories.end(), ros_pkg_paths.begin(), ros_pkg_paths.end());
+
+ if(hint_directories.empty())
+ {
+ throw std::runtime_error("You did not specify any package directory and ROS_PACKAGE_PATH is empty. Geometric parsing will crash");
+ }
+
+ ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
+ details::parseTreeForGeom(urdfTree->getRoot(), model, model_geom, hint_directories);
+ return model_geom;
+ }
+
+ } // namespace urdf
+} // namespace se3
+
+
+#endif // __se3_parsers_urdf_geometry_hxx__
diff --git a/src/parsers/urdf/model.hxx b/src/parsers/urdf/model.hxx
new file mode 100644
index 000000000..17573ef02
--- /dev/null
+++ b/src/parsers/urdf/model.hxx
@@ -0,0 +1,584 @@
+//
+// Copyright (c) 2016 CNRS
+// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
+//
+// 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_parsers_urdf_model_hxx__
+#define __se3_parsers_urdf_model_hxx__
+
+#include <urdf_model/model.h>
+#include <urdf_parser/urdf_parser.h>
+
+#include <iostream>
+#include <sstream>
+#include <iomanip>
+#include <boost/foreach.hpp>
+
+#include "pinocchio/parsers/urdf/utils.hpp"
+#include "pinocchio/multibody/model.hpp"
+
+#include <exception>
+
+namespace se3
+{
+ namespace urdf
+ {
+ namespace details
+ {
+///
+/// \brief Recursive procedure for reading the URDF tree.
+/// The function returns an exception as soon as a necessary Inertia or Joint information are missing.
+///
+/// \param[in] link The current URDF link.
+/// \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)
+{
+
+ // Parent joint of the current body
+ ::urdf::JointConstPtr joint = link->parent_joint;
+
+ // OffSet of the next link. In case we encounter a fixed joint, we need to propagate the offset placement of its attached body to next joint.
+ SE3 nextPlacementOffset = SE3::Identity();
+
+ if(joint != NULL) // if the link is not the root of the tree
+ {
+ assert(link->getParent()!=NULL);
+
+ const std::string & joint_name = joint->name;
+ const std::string & link_name = link->name;
+ const std::string & parent_link_name = link->getParent()->name;
+ std::string joint_info = "";
+
+ // check if inertial information is provided
+ if (!link->inertial && joint->type != ::urdf::Joint::FIXED)
+ {
+ const std::string exception_message (link->name + " - spatial inertial information missing.");
+ throw std::invalid_argument(exception_message);
+ }
+
+ Model::JointIndex parent_joint_id = (link->getParent()->parent_joint==NULL) ? (model.existJointName("root_joint") ? model.getJointId("root_joint") : 0) :
+ model.getJointId( link->getParent()->parent_joint->name );
+
+ // Transformation from the parent link to the joint origin
+ const SE3 & jointPlacement = placementOffset*convertFromUrdf(joint->parent_to_joint_origin_transform);
+
+ const Inertia & Y = (link->inertial) ? convertFromUrdf(*link->inertial) :
+ Inertia::Zero();
+
+
+ switch(joint->type)
+ {
+ case ::urdf::Joint::FLOATING:
+ {
+ joint_info = "joint FreeFlyer";
+
+ typedef JointModelFreeFlyer::ConfigVector_t ConfigVector_t;
+ typedef JointModelFreeFlyer::TangentVector_t TangentVector_t;
+ typedef ConfigVector_t::Scalar Scalar_t;
+
+
+ ConfigVector_t lower_position;
+ lower_position.fill(std::numeric_limits<Scalar_t>::min());
+
+ ConfigVector_t upper_position;
+ upper_position.fill(std::numeric_limits<Scalar_t>::max());
+
+ TangentVector_t max_effort;
+ max_effort.fill(std::numeric_limits<Scalar_t>::max());
+
+ TangentVector_t max_velocity;
+ max_velocity.fill(std::numeric_limits<Scalar_t>::max());
+
+ model.addJointAndBody(parent_joint_id, JointModelFreeFlyer(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name, link->name);
+ break;
+ }
+ case ::urdf::Joint::REVOLUTE:
+ {
+ joint_info = "joint REVOLUTE with axis";
+
+ typedef JointModelRX::ConfigVector_t ConfigVector_t;
+ typedef JointModelRX::TangentVector_t TangentVector_t;
+
+ TangentVector_t max_effort;
+ TangentVector_t max_velocity;
+ ConfigVector_t lower_position;
+ ConfigVector_t upper_position;
+
+ if (joint->limits)
+ {
+ max_effort << joint->limits->effort;
+ max_velocity << joint->limits->velocity;
+ lower_position << joint->limits->lower;
+ upper_position << joint->limits->upper;
+ }
+
+ Eigen::Vector3d jointAxis(Eigen::Vector3d::Zero());
+ AxisCartesian axis = extractCartesianAxis(joint->axis);
+
+ switch(axis)
+ {
+ case AXIS_X:
+ {
+ joint_info += " along X";
+ model.addJointAndBody(parent_joint_id, JointModelRX(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name );
+ break;
+ }
+ case AXIS_Y:
+ {
+ joint_info += " along Y";
+ model.addJointAndBody(parent_joint_id, JointModelRY(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name );
+ break;
+ }
+ case AXIS_Z:
+ {
+ joint_info += " along Z";
+ model.addJointAndBody(parent_joint_id, JointModelRZ(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name );
+ break;
+ }
+ case AXIS_UNALIGNED:
+ {
+ std::stringstream axis_value;
+ axis_value << std::setprecision(5);
+ axis_value << "(" << joint->axis.x <<"," << joint->axis.y << "," << joint->axis.z << ")";
+ joint_info += " unaligned " + axis_value.str();
+
+ jointAxis = Eigen::Vector3d( joint->axis.x,joint->axis.y,joint->axis.z );
+ jointAxis.normalize();
+ model.addJointAndBody( parent_joint_id, JointModelRevoluteUnaligned(jointAxis),
+ jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name );
+ break;
+ }
+ default:
+ {
+ assert(false && "The axis type of the revolute joint is of wrong type.");
+ break;
+ }
+ }
+ break;
+ }
+ case ::urdf::Joint::CONTINUOUS: // Revolute joint with no joint limits
+ {
+ joint_info = "joint CONTINUOUS with axis";
+
+ typedef JointModelRX::ConfigVector_t ConfigVector_t;
+ typedef JointModelRX::TangentVector_t TangentVector_t;
+
+ TangentVector_t max_effort;
+ TangentVector_t max_velocity;
+ ConfigVector_t lower_position;
+ ConfigVector_t upper_position;
+
+ if (joint->limits)
+ {
+ max_effort << joint->limits->effort;
+ max_velocity << joint->limits->velocity;
+ lower_position << joint->limits->lower;
+ upper_position << joint->limits->upper;
+ }
+
+ Eigen::Vector3d jointAxis(Eigen::Vector3d::Zero());
+ AxisCartesian axis = extractCartesianAxis(joint->axis);
+
+ switch(axis)
+ {
+ case AXIS_X:
+ {
+ joint_info += " along X";
+ model.addJointAndBody(parent_joint_id, JointModelRX(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name );
+ break;
+ }
+ case AXIS_Y:
+ {
+ joint_info += " along Y";
+ model.addJointAndBody(parent_joint_id, JointModelRY(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name );
+ break;
+ }
+ case AXIS_Z:
+ {
+ joint_info += " along Z";
+ model.addJointAndBody(parent_joint_id, JointModelRZ(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name );
+ break;
+ }
+ case AXIS_UNALIGNED:
+ {
+ std::stringstream axis_value;
+ axis_value << std::setprecision(5);
+ axis_value << "(" << joint->axis.x <<"," << joint->axis.y << "," << joint->axis.z << ")";
+ joint_info += " unaligned " + axis_value.str();
+
+ jointAxis = Eigen::Vector3d( joint->axis.x,joint->axis.y,joint->axis.z );
+ jointAxis.normalize();
+ model.addJointAndBody( parent_joint_id, JointModelRevoluteUnaligned(jointAxis),
+ jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name );
+ break;
+ }
+ default:
+ {
+ assert(false && "The axis type of the revolute joint is of wrong type.");
+ break;
+ }
+ }
+ break;
+ }
+ case ::urdf::Joint::PRISMATIC:
+ {
+ joint_info = "joint PRISMATIC with axis";
+
+ typedef JointModelRX::ConfigVector_t ConfigVector_t;
+ typedef JointModelRX::TangentVector_t TangentVector_t;
+
+ TangentVector_t max_effort;
+ TangentVector_t max_velocity;
+ ConfigVector_t lower_position;
+ ConfigVector_t upper_position;
+
+ if (joint->limits)
+ {
+ max_effort << joint->limits->effort;
+ max_velocity << joint->limits->velocity;
+ lower_position << joint->limits->lower;
+ upper_position << joint->limits->upper;
+ }
+
+ AxisCartesian axis = extractCartesianAxis(joint->axis);
+ switch(axis)
+ {
+ case AXIS_X:
+ {
+ joint_info += " along X";
+ model.addJointAndBody(parent_joint_id, JointModelPX(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name );
+ break;
+ }
+ case AXIS_Y:
+ {
+ joint_info += " along Y";
+ model.addJointAndBody(parent_joint_id, JointModelPY(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name);
+ break;
+ }
+ case AXIS_Z:
+ {
+ joint_info += " along Z";
+ model.addJointAndBody(parent_joint_id, JointModelPZ(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name );
+ break;
+ }
+ case AXIS_UNALIGNED:
+ {
+ std::stringstream axis_value;
+ axis_value << std::setprecision(5);
+ axis_value << "(" << joint->axis.x <<"," << joint->axis.y << "," << joint->axis.z << ")";
+ joint_info += " unaligned " + axis_value.str();
+
+ Eigen::Vector3d jointAxis(Eigen::Vector3d(joint->axis.x,joint->axis.y,joint->axis.z));
+ jointAxis.normalize();
+ model.addJointAndBody(parent_joint_id, JointModelPrismaticUnaligned(jointAxis),
+ jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name);
+
+ break;
+ }
+ default:
+ {
+ assert(false && "The axis type of the prismatic joint is of wrong type.");
+ break;
+ }
+ }
+ break;
+ }
+ case ::urdf::Joint::PLANAR:
+ {
+ joint_info = "joint PLANAR with normal axis along Z";
+
+ typedef JointModelPlanar::ConfigVector_t ConfigVector_t;
+ typedef JointModelPlanar::TangentVector_t TangentVector_t;
+
+ TangentVector_t max_effort;
+ TangentVector_t max_velocity;
+ ConfigVector_t lower_position;
+ ConfigVector_t upper_position;
+
+ if (joint->limits)
+ {
+ max_effort << joint->limits->effort;
+ max_velocity << joint->limits->velocity;
+ lower_position << joint->limits->lower;
+ upper_position << joint->limits->upper;
+ }
+
+ model.addJointAndBody(parent_joint_id, JointModelPlanar(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name );
+
+ break;
+ }
+ case ::urdf::Joint::FIXED:
+ {
+ // In case of fixed joint, if link has inertial tag:
+ // -add the inertia of the link to his parent in the model
+ // Otherwise do nothing.
+ // In all cases:
+ // -let all the children become children of parent
+ // -inform the parser of the offset to apply
+ // -add fixed body in model to display it in gepetto-viewer
+
+ joint_info = "fixed joint";
+ if (link->inertial)
+ {
+ model.appendBodyToJoint(parent_joint_id, jointPlacement, Y, link->name); //Modify the parent inertia in the model
+ }
+
+ //transformation of the current placement offset
+ nextPlacementOffset = jointPlacement;
+
+ // Add a frame in the model to keep trace of this fixed joint
+ model.addFrame(joint->name, parent_joint_id, nextPlacementOffset, FIXED_JOINT);
+
+ //for the children of the current link, set their parent to be
+ //the the parent of the current link.
+ BOOST_FOREACH(::urdf::LinkPtr child_link, link->child_links)
+ {
+ child_link->setParent(link->getParent() );
+ }
+ break;
+ }
+ default:
+ {
+ const std::string exception_message ("The type of joint " + joint_name + " is not supported.");
+ throw std::invalid_argument(exception_message);
+ break;
+ }
+ }
+
+ if (verbose)
+ {
+ std::cout << "Adding Body" << std::endl;
+ std::cout << "\"" << link_name << "\" connected to " << "\"" << parent_link_name << "\" throw joint " << "\"" << joint_name << "\"" << std::endl;
+ std::cout << "joint type: " << joint_info << std::endl;
+ std::cout << "joint placement:\n" << jointPlacement;
+ std::cout << "body info: " << std::endl;
+ std::cout << " " << "mass: " << Y.mass() << std::endl;
+ std::cout << " " << "lever: " << Y.lever().transpose() << std::endl;
+ std::cout << " " << "inertia elements (Ixx,Iyx,Iyy,Izx,Izy,Izz): " << Y.inertia().data().transpose() << std::endl << std::endl;
+ }
+
+ }
+ else if (link->getParent() != NULL)
+ {
+ const std::string exception_message (link->name + " - joint information missing.");
+ throw std::invalid_argument(exception_message);
+ }
+
+
+ BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
+ {
+ parseTree(child, model, nextPlacementOffset, verbose);
+ }
+}
+
+
+
+///
+/// \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.
+///
+/// \param[in] link The current URDF link.
+/// \param[in] model The model where the link must be added.
+/// \param[in] verbose Print parsing info.
+///
+void parseRootTree (::urdf::LinkConstPtr root_link, Model & model, const bool verbose) throw (std::invalid_argument)
+{
+ // If the root link has no inertial info, it may be because it is a base_link.
+ // In this case, we look for its child links which indeed contribute to the dynamics, they are not fixed to the universe.
+ // TODO: it may be necessary to compute joint placement variable instead of setting it to SE3::Identity()
+ if (!root_link->inertial)
+ {
+ typedef std::vector< ::urdf::LinkPtr > LinkSharedPtrVector_t;
+ LinkSharedPtrVector_t movable_child_links;
+ LinkSharedPtrVector_t direct_child_links(root_link->child_links);
+ LinkSharedPtrVector_t next_direct_child_links; // next child to visit
+ LinkSharedPtrVector_t pathologic_child_links; // children which have inertial info but rigidly attached to the world
+ do
+ {
+ next_direct_child_links.clear();
+ BOOST_FOREACH(::urdf::LinkPtr child, direct_child_links)
+ {
+ if (child->parent_joint->type != ::urdf::Joint::FIXED)
+ movable_child_links.push_back(child);
+ else
+ {
+ if (child->inertial)
+ pathologic_child_links.push_back(child);
+ next_direct_child_links.insert (next_direct_child_links.end(), child->child_links.begin(), child->child_links.end());
+ }
+
+ }
+ direct_child_links = next_direct_child_links;
+ }
+ while (!direct_child_links.empty());
+
+ if (!pathologic_child_links.empty())
+ {
+ std::cout << "[INFO] The links:" << std::endl;
+ for (LinkSharedPtrVector_t::iterator it = pathologic_child_links.begin();
+ it < pathologic_child_links.end(); ++it)
+ {
+ std::cout << " - " << (*it)->name << std::endl;
+ }
+ std::cout << "are fixed regarding to the universe (base_link) and convey inertial info. They won't affect the dynamics of the output model. Maybe, a root joint is missing connecting this links to the universe." << std::endl;
+
+ }
+
+ BOOST_FOREACH(::urdf::LinkPtr child, movable_child_links)
+ {
+ child->getParent()->parent_joint->name = model.names[0];
+ parseTree(child, model, SE3::Identity(), verbose);
+ }
+
+ }
+ else // Otherwise, we have to rase an exception because the first link will no be added to the model.
+ // It seems a root joint is missing.
+ {
+ std::cout << "[INFO] The root link " << root_link->name << " of the model tree contains inertial information. It seems that a root joint is missing connecting this root link to the universe. The root link won't affect the dynamics of the model." << std::endl;
+
+ BOOST_FOREACH(::urdf::LinkPtr child, root_link->child_links)
+ {
+ parseTree(child, model, SE3::Identity(), verbose);
+ }
+ }
+
+
+}
+
+///
+/// \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.
+///
+/// \param[in] link The current URDF link.
+/// \param[in] model The model where the link must be added.
+/// \param[in] root_joint The specific root joint.
+/// \param[in] verbose Print parsing info.
+///
+template <typename D>
+void parseRootTree (::urdf::LinkConstPtr root_link, Model & model, const JointModelBase<D> & root_joint, const bool verbose) throw (std::invalid_argument)
+{
+ // If the root link has no inertial info (because it is a base_link for example),
+ // we have to merge its inertial info with all its children connected to it with fixed joints
+ if (!root_link->inertial)
+ {
+ // If the root link has only one child
+ if (root_link->child_links.size() == 1)
+ {
+ ::urdf::LinkPtr child_link = root_link->child_links[0];
+ assert(child_link->inertial != NULL && "Inertial information missing for parsing the root link.");
+ const Inertia & Y = convertFromUrdf(*child_link->inertial);
+ model.addJointAndBody(0, root_joint, SE3::Identity(), Y, "root_joint", child_link->name);
+
+ // Change the name of the parent joint in the URDF tree
+ child_link->parent_joint->name = "root_joint";
+
+ BOOST_FOREACH(::urdf::LinkConstPtr child, child_link->child_links)
+ {
+ parseTree(child, model, SE3::Identity(), verbose);
+ }
+ }
+ else
+ {
+ const std::string exception_message (root_link->name + " has no inertial information and has more than one child link. It corresponds to a disjoint tree.");
+ throw std::invalid_argument(exception_message);
+ }
+
+ }
+ else // otherwise, it is a plain body with inertial info. It processes as usual.
+ {
+ const Inertia & Y = convertFromUrdf(*root_link->inertial);
+ model.addJointAndBody(0, root_joint, SE3::Identity(), Y , "root_joint", root_link->name);
+
+ BOOST_FOREACH(::urdf::LinkPtr child, root_link->child_links)
+ {
+ parseTree(child, model, SE3::Identity(), verbose);
+ }
+ }
+
+}
+ } // namespace details
+
+template <typename D>
+Model buildModel (const std::string & filename, const JointModelBase<D> & root_joint, bool verbose) throw (std::invalid_argument)
+{
+ Model model;
+
+ ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
+ if (urdfTree)
+ details::parseRootTree(urdfTree->getRoot(), model, root_joint, verbose);
+ else
+ {
+ const std::string exception_message ("The file " + filename + " does not contain a valid URDF model.");
+ throw std::invalid_argument(exception_message);
+ }
+
+ return model;
+}
+
+
+
+
+inline Model buildModel (const std::string & filename, const bool verbose) throw (std::invalid_argument)
+{
+ Model model;
+
+ ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
+ if (urdfTree)
+ details::parseRootTree(urdfTree->getRoot(),model,verbose);
+ else
+ {
+ const std::string exception_message ("The file " + filename + " does not contain a valid URDF model.");
+ throw std::invalid_argument(exception_message);
+ }
+
+ return model;
+}
+
+ } // namespace urdf
+} // namespace se3
+
+
+#endif // __se3_parsers_urdf_model_hxx__
diff --git a/src/parsers/urdf/utils.hpp b/src/parsers/urdf/utils.hpp
new file mode 100644
index 000000000..1b00571cb
--- /dev/null
+++ b/src/parsers/urdf/utils.hpp
@@ -0,0 +1,104 @@
+//
+// Copyright (c) 2016 CNRS
+// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
+//
+// 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_parsers_urdf_utils_hpp__
+#define __se3_parsers_urdf_utils_hpp__
+
+#include <urdf_model/model.h>
+#include <urdf_parser/urdf_parser.h>
+
+#include <iostream>
+#include <sstream>
+#include <iomanip>
+#include <boost/foreach.hpp>
+
+#include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/spatial/se3.hpp"
+
+// #include "pinocchio/multibody/model.hpp"
+
+#include <exception>
+#include <limits>
+
+namespace se3
+{
+ namespace urdf
+ {
+
+ ///
+ /// \brief Convert URDF Inertial quantity to Spatial Inertia.
+ ///
+ /// \param[in] Y The input URDF Inertia.
+ ///
+ /// \return The converted Spatial Inertia se3::Inertia.
+ ///
+ inline Inertia convertFromUrdf (const ::urdf::Inertial & Y)
+ {
+ const ::urdf::Vector3 & p = Y.origin.position;
+ const ::urdf::Rotation & q = Y.origin.rotation;
+
+ const Eigen::Vector3d com(p.x,p.y,p.z);
+ const Eigen::Matrix3d & R = Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix();
+
+ Eigen::Matrix3d I; I << Y.ixx,Y.ixy,Y.ixz
+ , Y.ixy,Y.iyy,Y.iyz
+ , Y.ixz,Y.iyz,Y.izz;
+ return Inertia(Y.mass,com,R*I*R.transpose());
+ }
+
+ ///
+ /// \brief Convert URDF Pose quantity to SE3.
+ ///
+ /// \param[in] M The input URDF Pose.
+ ///
+ /// \return The converted pose/transform se3::SE3.
+ ///
+ inline SE3 convertFromUrdf (const ::urdf::Pose & M)
+ {
+ const ::urdf::Vector3 & p = M.position;
+ const ::urdf::Rotation & q = M.rotation;
+ return SE3( Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix(), Eigen::Vector3d(p.x,p.y,p.z));
+ }
+
+ ///
+ /// \brief The four possible cartesian types of an 3D axis.
+ ///
+ enum AxisCartesian { AXIS_X, AXIS_Y, AXIS_Z, AXIS_UNALIGNED };
+
+ ///
+ /// \brief Extract the cartesian property of a particular 3D axis.
+ ///
+ /// \param[in] axis The input URDF axis.
+ ///
+ /// \return The property of the particular axis se3::urdf::AxisCartesian.
+ ///
+ inline AxisCartesian extractCartesianAxis (const ::urdf::Vector3 & axis)
+ {
+ if( (axis.x==1.0)&&(axis.y==0.0)&&(axis.z==0.0) )
+ return AXIS_X;
+ else if( (axis.x==0.0)&&(axis.y==1.0)&&(axis.z==0.0) )
+ return AXIS_Y;
+ else if( (axis.x==0.0)&&(axis.y==0.0)&&(axis.z==1.0) )
+ return AXIS_Z;
+ else
+ return AXIS_UNALIGNED;
+ }
+
+ } //urdf
+} // se3
+#endif // __se3_parsers_urdf_utils_hpp__
diff --git a/src/multibody/parser/from-collada-to-fcl.hpp b/src/parsers/utils.hpp
similarity index 75%
rename from src/multibody/parser/from-collada-to-fcl.hpp
rename to src/parsers/utils.hpp
index 76ee4894d..e686012a0 100644
--- a/src/multibody/parser/from-collada-to-fcl.hpp
+++ b/src/parsers/utils.hpp
@@ -1,8 +1,7 @@
//
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2015 CNRS
//
-// This file is part of Pinocchio and is mainly inspired
-// by software hpp-model-urdf
+// 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
@@ -16,9 +15,7 @@
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
-#ifndef __se3_collada_to_fcl_hpp__
-#define __se3_collada_to_fcl_hpp__
-
+#include <iostream>
#include <limits>
#include <sstream>
@@ -33,8 +30,36 @@
namespace se3
{
-
+ ///
+ /// \brief Supported model file extensions
+ ///
+ enum ModelFileExtensionType{
+ UNKNOWN = 0,
+ URDF,
+ LUA
+ };
+ ///
+ /// \brief Extract the type of the given model file according to its extension
+ ///
+ /// \param[in] filemane The complete path to the model file.
+ ///
+ /// \return The type of the extension of the model file
+ ///
+ ModelFileExtensionType checkModelFileExtension (const std::string & filename)
+ {
+ const std::string extension = filename.substr(filename.find_last_of(".") + 1);
+
+ if (extension == "urdf")
+ return URDF;
+ else if (extension == "lua")
+ return LUA;
+
+ return UNKNOWN;
+ }
+
+
+
/**
* @brief Transform a package://.. mesh path to an absolute path, searching for a valid file
* in a list of package directories
@@ -67,6 +92,3 @@ namespace se3
}
} // namespace se3
-
-
-#endif // __se3_collada_to_fcl_hpp__
diff --git a/src/python/model.hpp b/src/python/model.hpp
index ddb92b7db..7d8b507dc 100644
--- a/src/python/model.hpp
+++ b/src/python/model.hpp
@@ -24,7 +24,7 @@
#include <eigenpy/eigenpy.hpp>
#include "pinocchio/multibody/model.hpp"
-#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/python/se3.hpp"
#include "pinocchio/python/eigen_container.hpp"
#include "pinocchio/python/handler.hpp"
diff --git a/src/python/parsers.hpp b/src/python/parsers.hpp
index d2dc499ca..284cdc62e 100644
--- a/src/python/parsers.hpp
+++ b/src/python/parsers.hpp
@@ -25,19 +25,18 @@
#include "pinocchio/python/data.hpp"
#ifdef WITH_URDFDOM
- #include "pinocchio/multibody/parser/urdf.hpp"
+ #include "pinocchio/parsers/urdf.hpp"
#ifdef WITH_HPP_FCL
#include "pinocchio/python/geometry-model.hpp"
#include "pinocchio/python/geometry-data.hpp"
- #include "pinocchio/multibody/parser/urdf-with-geometry.hpp"
#endif
#endif
#ifdef WITH_LUA
- #include "pinocchio/multibody/parser/lua.hpp"
+ #include "pinocchio/parsers/lua.hpp"
#endif // #ifdef WITH_LUA
-#include "pinocchio/multibody/parser/srdf.hpp"
+#include "pinocchio/parsers/srdf.hpp"
namespace se3
{
diff --git a/src/tools/file-explorer.hpp b/src/tools/file-explorer.hpp
index 9bb588646..74a857428 100644
--- a/src/tools/file-explorer.hpp
+++ b/src/tools/file-explorer.hpp
@@ -58,6 +58,17 @@ namespace se3
return env_var_paths;
}
+
+ /**
+ * @brief Parse the environment variable ROS_PACKAGE_PATH and extract paths
+ *
+ * @return The vector of paths extracted from the environment variable ROS_PACKAGE_PATH
+ */
+ inline std::vector<std::string> rosPaths()
+ {
+ return extractPathFromEnvVar("ROS_PACKAGE_PATH");
+ }
+
}
#endif // __se3_file_explorer_hpp__
diff --git a/unittest/aba.cpp b/unittest/aba.cpp
index c5c60e6a9..ac78668c5 100644
--- a/unittest/aba.cpp
+++ b/unittest/aba.cpp
@@ -23,7 +23,7 @@
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/crba.hpp"
-#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/tools/timer.hpp"
diff --git a/unittest/cholesky.cpp b/unittest/cholesky.cpp
index 2e57d01d4..a0ca68179 100644
--- a/unittest/cholesky.cpp
+++ b/unittest/cholesky.cpp
@@ -26,7 +26,7 @@
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/cholesky.hpp"
-#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/tools/timer.hpp"
#include <iostream>
diff --git a/unittest/com.cpp b/unittest/com.cpp
index 38b000a57..200f8e42a 100644
--- a/unittest/com.cpp
+++ b/unittest/com.cpp
@@ -21,7 +21,7 @@
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/tools/timer.hpp"
-#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
#include <iostream>
diff --git a/unittest/compute-all-terms.cpp b/unittest/compute-all-terms.cpp
index 6577cdbd1..79167de74 100644
--- a/unittest/compute-all-terms.cpp
+++ b/unittest/compute-all-terms.cpp
@@ -30,7 +30,7 @@
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
-#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/tools/timer.hpp"
#define BOOST_TEST_DYN_LINK
diff --git a/unittest/crba.cpp b/unittest/crba.cpp
index 0ca38b93a..a2ea0bd41 100644
--- a/unittest/crba.cpp
+++ b/unittest/crba.cpp
@@ -27,7 +27,7 @@
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
-#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/tools/timer.hpp"
#include <iostream>
diff --git a/unittest/dynamics.cpp b/unittest/dynamics.cpp
index 56534de3f..1b0ae862f 100644
--- a/unittest/dynamics.cpp
+++ b/unittest/dynamics.cpp
@@ -19,7 +19,7 @@
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/dynamics.hpp"
-#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/tools/timer.hpp"
#include <iostream>
diff --git a/unittest/energy.cpp b/unittest/energy.cpp
index 7584c4401..59e74aa90 100644
--- a/unittest/energy.cpp
+++ b/unittest/energy.cpp
@@ -18,7 +18,7 @@
#include "pinocchio/algorithm/energy.hpp"
#include "pinocchio/algorithm/crba.hpp"
-#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE ComTest
diff --git a/unittest/frames.cpp b/unittest/frames.cpp
index afa8f0448..ff32af394 100644
--- a/unittest/frames.cpp
+++ b/unittest/frames.cpp
@@ -20,7 +20,7 @@
#include "pinocchio/algorithm/frames.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/spatial/act-on-set.hpp"
-#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/tools/timer.hpp"
#include <iostream>
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
index 82ac54f9c..1ef097e09 100644
--- a/unittest/geom.cpp
+++ b/unittest/geom.cpp
@@ -32,14 +32,13 @@
#include <iomanip>
#include "pinocchio/multibody/model.hpp"
-#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/collisions.hpp"
-#include "pinocchio/multibody/parser/urdf.hpp"
+#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/spatial/explog.hpp"
#include "pinocchio/multibody/geometry.hpp"
-#include "pinocchio/multibody/parser/urdf-with-geometry.hpp"
#include <vector>
diff --git a/unittest/jacobian.cpp b/unittest/jacobian.cpp
index 1197d4691..ff3c9dfd2 100644
--- a/unittest/jacobian.cpp
+++ b/unittest/jacobian.cpp
@@ -19,7 +19,7 @@
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/spatial/act-on-set.hpp"
-#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/tools/timer.hpp"
#include <iostream>
diff --git a/unittest/joint-configurations.cpp b/unittest/joint-configurations.cpp
index 8c3542ca0..f42657a16 100644
--- a/unittest/joint-configurations.cpp
+++ b/unittest/joint-configurations.cpp
@@ -32,7 +32,7 @@
#include <iomanip>
#include "pinocchio/multibody/model.hpp"
-#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/spatial/explog.hpp"
diff --git a/unittest/joint.cpp b/unittest/joint.cpp
index 8f1724f73..36e76337a 100644
--- a/unittest/joint.cpp
+++ b/unittest/joint.cpp
@@ -22,7 +22,7 @@
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/spatial/act-on-set.hpp"
-#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/tools/timer.hpp"
#include "pinocchio/multibody/joint/joint.hpp"
@@ -140,4 +140,4 @@ BOOST_AUTO_TEST_CASE ( test_all_joints )
}
-BOOST_AUTO_TEST_SUITE_END ()
\ No newline at end of file
+BOOST_AUTO_TEST_SUITE_END ()
diff --git a/unittest/lua.cpp b/unittest/lua.cpp
index 583785717..39cef00f5 100644
--- a/unittest/lua.cpp
+++ b/unittest/lua.cpp
@@ -18,7 +18,7 @@
#include <iostream>
#include "pinocchio/multibody/model.hpp"
-#include "pinocchio/multibody/parser/lua.hpp"
+#include "pinocchio/parsers/lua.hpp"
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE LuaTest
diff --git a/unittest/python_parser.cpp b/unittest/python_parser.cpp
index 1c59ff01b..6512cb6a1 100644
--- a/unittest/python_parser.cpp
+++ b/unittest/python_parser.cpp
@@ -18,7 +18,7 @@
#include <iostream>
#include "pinocchio/multibody/model.hpp"
-#include "pinocchio/multibody/parser/python.hpp"
+#include "pinocchio/parsers/python.hpp"
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE PythonTest
diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp
index 7a6e00757..7d4990eb6 100644
--- a/unittest/rnea.cpp
+++ b/unittest/rnea.cpp
@@ -27,7 +27,7 @@
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/rnea.hpp"
-#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/tools/timer.hpp"
#include <iostream>
diff --git a/unittest/urdf.cpp b/unittest/urdf.cpp
index 280c5a81d..5f260b62d 100644
--- a/unittest/urdf.cpp
+++ b/unittest/urdf.cpp
@@ -18,7 +18,7 @@
#include <iostream>
#include "pinocchio/multibody/model.hpp"
-#include "pinocchio/multibody/parser/urdf.hpp"
+#include "pinocchio/parsers/urdf.hpp"
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE UrdfTest
diff --git a/unittest/value.cpp b/unittest/value.cpp
index 85bf88cee..d0d9ed2da 100644
--- a/unittest/value.cpp
+++ b/unittest/value.cpp
@@ -32,7 +32,7 @@
#include <iomanip>
#include "pinocchio/multibody/model.hpp"
-#include "pinocchio/multibody/parser/urdf.hpp"
+ #include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
diff --git a/utils/pinocchio_read_model.cpp b/utils/pinocchio_read_model.cpp
index aa9da9767..e64db62d6 100644
--- a/utils/pinocchio_read_model.cpp
+++ b/utils/pinocchio_read_model.cpp
@@ -22,14 +22,14 @@
#include "pinocchio/multibody/model.hpp"
#ifdef WITH_URDFDOM
- #include "pinocchio/multibody/parser/urdf.hpp"
+ #include "pinocchio/parsers/urdf.hpp"
#endif
#ifdef WITH_LUA
- #include "pinocchio/multibody/parser/lua.hpp"
+ #include "pinocchio/parsers/lua.hpp"
#endif
-#include "pinocchio/multibody/parser/utils.hpp"
+#include "pinocchio/parsers/utils.hpp"
using namespace std;
--
GitLab