Skip to content
GitLab
Commit fd2374f5 authored by Valenza Florian's avatar Valenza Florian
Browse files

[C++][Cmake] Reorganize urdf parsers - moved parsers one level above

parent dbba3352
Expand all Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ fd2374f5
......@@ -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")
......
benchmark/timings-geometry.cpp
View file @ fd2374f5
......@@ -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>
......
benchmark/timings.cpp
View file @ fd2374f5
......@@ -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>
......
src/CMakeLists.txt
View file @ fd2374f5
......@@ -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}
......
src/multibody/geometry.hxx
View file @ fd2374f5
......@@ -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>
......
src/multibody/parser/urdf-with-geometry.hpp deleted 100644 → 0
View file @ dbba3352
//
// 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__
src/multibody/parser/urdf-with-geometry.hxx deleted 100644 → 0
View file @ dbba3352
//
// 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__
src/multibody/parser/urdf.hxx deleted 100644 → 0
View file @ dbba3352
This diff is collapsed.
src/multibody/parser/utils.hpp deleted 100644 → 0
View file @ dbba3352
//
// 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
src/multibody/parser/lua.cpp → src/parsers/lua.cpp
View file @ fd2374f5
#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>
......
src/multibody/parser/lua/lua_tables.cpp → src/parsers/lua/lua_tables.cpp
View file @ fd2374f5
......@@ -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>
......
src/multibody/parser/python.cpp → src/parsers/python.cpp
View file @ fd2374f5
......@@ -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>
......
src/multibody/parser/sample-models.cpp → src/parsers/sample-models.cpp
View file @ fd2374f5
......@@ -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>
......
src/multibody/parser/urdf.hpp → src/parsers/urdf.hpp
View file @ fd2374f5
//
// 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,