From ea48ec448784f782bde6fb18b6215e82319266b7 Mon Sep 17 00:00:00 2001
From: Valenza Florian <fvalenza@laas.fr>
Date: Thu, 3 Mar 2016 14:09:58 +0100
Subject: [PATCH] [C++][Python][Parser Geom] Parse automatically the
ROS_PACKAGE_PATH environment variable if not clue is given by the user to
where to search for meshes. Change the meshRootDir argument to a vector of
strings (package directories). Updated Python consequently
---
CMakeLists.txt | 1 +
benchmark/timings-geometry.cpp | 10 ++-
src/multibody/parser/from-collada-to-fcl.hpp | 31 +++++++-
src/multibody/parser/urdf-with-geometry.hpp | 63 ++++++++++------
src/multibody/parser/urdf-with-geometry.hxx | 79 +++++++++++---------
src/python/parsers.hpp | 24 +++---
unittest/geom.cpp | 49 +++++++-----
7 files changed, 163 insertions(+), 94 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9393ca70c..b8d17d8af 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -86,6 +86,7 @@ SET(${PROJECT_NAME}_MATH_HEADERS
SET(${PROJECT_NAME}_TOOLS_HEADERS
tools/timer.hpp
tools/string-generator.hpp
+ tools/file-explorer.hpp
)
SET(${PROJECT_NAME}_SPATIAL_HEADERS
diff --git a/benchmark/timings-geometry.cpp b/benchmark/timings-geometry.cpp
index 6d69bd232..7ee7b2939 100644
--- a/benchmark/timings-geometry.cpp
+++ b/benchmark/timings-geometry.cpp
@@ -69,11 +69,15 @@ int main()
std::string romeo_filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
+ std::vector < std::string > package_dirs;
std::string romeo_meshDir = PINOCCHIO_SOURCE_DIR"/models/";
- Model model = se3::urdf::buildModel(romeo_filename, se3::JointModelFreeFlyer());
- GeometryModel geom_model = se3::urdf::buildGeom(model, romeo_filename, romeo_meshDir);
+ package_dirs.push_back(romeo_meshDir);
+
+ se3::Model model = se3::urdf::buildModel(romeo_filename, se3::JointModelFreeFlyer());
+ se3::GeometryModel geom_model = se3::urdf::buildGeom(romeo_model, romeo_filename, package_dirs, true);
+
Data data(model);
- GeometryData geom_data (data, geom_model);
+ GeometryData geom_data(data, geom_model);
geom_data.initializeListOfCollisionPairs();
diff --git a/src/multibody/parser/from-collada-to-fcl.hpp b/src/multibody/parser/from-collada-to-fcl.hpp
index 9106e0556..dd1b419be 100644
--- a/src/multibody/parser/from-collada-to-fcl.hpp
+++ b/src/multibody/parser/from-collada-to-fcl.hpp
@@ -35,6 +35,8 @@
#include <hpp/fcl/BV/OBBRSS.h>
#include <hpp/fcl/BVH/BVH_model.h>
+#include "pinocchio/tools/file-explorer.hpp"
+
#include <exception>
namespace se3
@@ -196,13 +198,36 @@ namespace se3
/**
- * @brief Transform a cURL readable path (package://..) to an absolute path for urdf collision path
+ * @brief Transform a package://.. mesh path to an absolute path, searching for a valid file
+ * in a list of package directories
*
- * @param[in] urdf_mesh_path The path given in the urdf file (package://..)
- * @param[in] meshRootDir Root path to the directory where meshes are located
+ * @param[in] urdf_mesh_path The path given in the urdf file
+ * @param[in] package_dirs A list of packages directories where to search for meshes
*
* @return The absolute path to the mesh file
*/
+ inline std::string fromURDFMeshPathToAbsolutePathPack(const std::string & urdf_mesh_path,
+ const std::vector<std::string> & package_dirs)
+ {
+ // if exists p1/mesh, absolutePath = p1/mesh,
+ // else if exists p2/mesh, absolutePath = p2/mesh
+ // else return an empty string that will provoke an error in loadPolyhedronFromResource()
+ namespace bf = boost::filesystem;
+
+ std::string absolutePath;
+ // concatenate package_path with mesh filename
+ for (int i = 0; i < package_dirs.size(); ++i)
+ {
+ if ( bf::exists( bf::path(package_dirs[i] + urdf_mesh_path.substr(9, urdf_mesh_path.size()))))
+ {
+ absolutePath = std::string( package_dirs[i] + urdf_mesh_path.substr(9, urdf_mesh_path.size())
+ );
+ break;
+ }
+ }
+ return absolutePath;
+ }
+
inline std::string fromURDFMeshPathToAbsolutePath(const std::string & urdf_mesh_path,
const std::string & meshRootDir)
{
diff --git a/src/multibody/parser/urdf-with-geometry.hpp b/src/multibody/parser/urdf-with-geometry.hpp
index 32d420bd0..6d9c1bb82 100644
--- a/src/multibody/parser/urdf-with-geometry.hpp
+++ b/src/multibody/parser/urdf-with-geometry.hpp
@@ -35,47 +35,62 @@ namespace se3
/**
- * @brief Get a CollisionObject from an urdf link, reading the
- * corresponding mesh
+ * @brief Get a CollisionObject from an urdf link, searching
+ * for it in specified package directories
*
- * @param[in] link The input urdf link
- * @param[in] meshRootDir Root path to the directory where meshes are located
+ * @param[in] link The input urdf link
+ * @param[in] package_dirs A vector containing the different directories where to search for packages
*
* @return The mesh converted as a fcl::CollisionObject
*/
- inline fcl::CollisionObject retrieveCollisionGeometry (const ::urdf::LinkConstPtr & link,
- const std::string & meshRootDir);
+ inline fcl::CollisionObject retrieveCollisionGeometry (const ::urdf::LinkConstPtr & link, const std::vector < std::string > & package_dirs);
/**
* @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 Geometry Model associated
- * @param model_geom The Geometry Model where the Collision Objects must be added
- * @param[in] meshRootDir Root path to the directory where meshes are located
+ *
+ * @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::string & meshRootDir,
- const bool rootJointAdded) throw (std::invalid_argument);
+ inline void parseTreeForGeom( ::urdf::LinkConstPtr link,
+ const Model & model,
+ GeometryModel & model_geom,
+ const std::vector<std::string> & package_dirs,
+ const bool rootJointAdded) throw (std::invalid_argument);
+
+ /**
+ * @brief Build The GeometryModel from a URDF file
+ *
+ * @param 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 packages
+ * @param[in] root_joint If we added a root joint to the Model in addition to the urdf kinematic chain
+ *
+ * @return The GeometryModel associated to the urdf file and the Model given
+ */
+ inline GeometryModel buildGeom(const Model & model,
+ const std::string & filename,
+ const std::vector<std::string> & package_dirs,
+ const bool root_joint = false );
/**
- * @brief Build the GeometryModel from a URDF file.
+ * @brief Build The GeometryModel from a URDF file
*
- * @param[in] model The model of the robot, built with urdf::buildModel.
- * @param[in] filename The complete path to the URDF model.
- * @param[in] meshRootDir Root path pointing to the directory where meshes are located.
+ * @param model The model of the robot, built with urdf::buildModel
+ * @param[in] filename The URDF complete (absolute) file path
+ * @param[in] root_joint If we added a root joint to the Model in addition to the urdf kinematic chain
*
- * @return The geometric model.
+ * @return The GeometryModel associated to the urdf file and the Model given
*/
- GeometryModel buildGeom(const Model & model,
- const std::string & filename,
- const std::string & meshRootDir);
+ inline GeometryModel buildGeom(const Model & model,
+ const std::string & filename,
+ const bool root_joint = false );
} // namespace urdf
diff --git a/src/multibody/parser/urdf-with-geometry.hxx b/src/multibody/parser/urdf-with-geometry.hxx
index 319241218..2404310dd 100644
--- a/src/multibody/parser/urdf-with-geometry.hxx
+++ b/src/multibody/parser/urdf-with-geometry.hxx
@@ -31,8 +31,7 @@ namespace se3
namespace urdf
{
- inline fcl::CollisionObject retrieveCollisionGeometry (const ::urdf::LinkConstPtr & link,
- const std::string & meshRootDir)
+ inline fcl::CollisionObject retrieveCollisionGeometry (const ::urdf::LinkConstPtr & link, const std::vector < std::string > & package_dirs)
{
boost::shared_ptr < ::urdf::Collision> collision = link->collision;
boost::shared_ptr < fcl::CollisionGeometry > geometry;
@@ -43,7 +42,7 @@ namespace se3
boost::shared_ptr < ::urdf::Mesh> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Mesh> (collision->geometry);
std::string collisionFilename = collisionGeometry->filename;
- std::string full_path = fromURDFMeshPathToAbsolutePath(collisionFilename, meshRootDir);
+ std::string full_path = fromURDFMeshPathToAbsolutePathPack(collisionFilename, package_dirs);
::urdf::Vector3 scale = collisionGeometry->scale;
@@ -101,31 +100,38 @@ namespace se3
inline void parseTreeForGeom(::urdf::LinkConstPtr link,
const Model & model,
GeometryModel & geom_model,
- const std::string & meshRootDir,
+ const std::vector<std::string> & package_dirs,
const bool rootJointAdded) throw (std::invalid_argument)
{
// start with first link that is not empty
- if(link->collision)
+ if(link->inertial)
{
::urdf::JointConstPtr joint = link->parent_joint;
- if (joint == NULL && rootJointAdded)
+ if (joint == NULL && rootJointAdded )
{
- fcl::CollisionObject collision_object = retrieveCollisionGeometry(link, meshRootDir);
- const SE3 geomPlacement = convertFromUrdf(link->collision->origin);
- const std::string & collision_object_name = link->name ;
- geom_model.addGeomObject(model.getJointId("root_joint"), collision_object, geomPlacement, collision_object_name);
+ if (link->collision)
+ {
+ fcl::CollisionObject collision_object = retrieveCollisionGeometry(link, package_dirs);
+ SE3 geomPlacement = convertFromUrdf(link->collision->origin);
+ std::string collision_object_name = link->name ;
+ geom_model.addGeomObject(model.getJointId("root_joint"), collision_object, geomPlacement, collision_object_name);
+ }
}
- else if(joint!=NULL)
+
+ if(joint!=NULL)
{
assert(link->getParent()!=NULL);
- fcl::CollisionObject collision_object = retrieveCollisionGeometry(link, meshRootDir);
- const SE3 geomPlacement = convertFromUrdf(link->collision->origin);
- const std::string & collision_object_name = link->name ;
- geom_model.addGeomObject(model.getJointId(joint->name), collision_object, geomPlacement, collision_object_name);
+ if (link->collision)
+ {
+ fcl::CollisionObject collision_object = retrieveCollisionGeometry(link, package_dirs);
+ SE3 geomPlacement = convertFromUrdf(link->collision->origin);
+ std::string collision_object_name = link->name ;
+ geom_model.addGeomObject(model.getJointId(joint->name), collision_object, geomPlacement, collision_object_name);
+ }
}
else if (link->getParent() != NULL)
{
@@ -133,34 +139,37 @@ namespace se3
throw std::invalid_argument(exception_message);
}
- } // if(link->inertial)
+ }
BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
{
- parseTreeForGeom(child, model, geom_model, meshRootDir, rootJointAdded);
+ parseTreeForGeom(child, model, geom_model, package_dirs, rootJointAdded);
}
+
}
- GeometryModel buildGeom(const Model & model,
- const std::string & filename,
- const std::string & meshRootDir)
+
+ inline GeometryModel buildGeom(const Model & model,
+ const std::string & filename,
+ const std::vector<std::string> & package_dirs,
+ const bool root_joint)
{
+ GeometryModel model_geom(model);
+
::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
- ::urdf::LinkConstPtr root_link = urdfTree->getRoot();
- if (!root_link->inertial) // if the first body is just a base_link, i.e. with no inertial info
- {
- ::urdf::LinkPtr child_link = root_link->child_links[0];
-
- // Change the name of the parent joint
- child_link->parent_joint->name = "root_joint";
- }
-
- // Read geometries
- GeometryModel geom_model (model);
- parseTreeForGeom(urdfTree->getRoot(), model, geom_model, meshRootDir, true);
-
- // Return a pair containing the kinematic tree and the geometries
- return geom_model;
+ parseTreeForGeom(urdfTree->getRoot(), model, model_geom, package_dirs, root_joint);
+ return model_geom;
+ }
+
+ inline GeometryModel buildGeom(const Model & model,
+ const std::string & filename,
+ const bool root_joint)
+ {
+ GeometryModel model_geom(model);
+
+ ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
+ parseTreeForGeom(urdfTree->getRoot(), model, model_geom, getRosPackagePaths(), root_joint);
+ return model_geom;
}
} // namespace urdf
diff --git a/src/python/parsers.hpp b/src/python/parsers.hpp
index 76ac059db..2227dd608 100644
--- a/src/python/parsers.hpp
+++ b/src/python/parsers.hpp
@@ -88,19 +88,19 @@ namespace se3
struct build_model_and_geom_visitor : public boost::static_visitor<std::pair<ModelHandler, GeometryModelHandler> >
{
const std::string& _filenameUrdf;
- const std::string& _filenameMeshRootDir;
+ const std::vector<std::string> & _package_dirs;
build_model_and_geom_visitor(const std::string & filenameUrdf,
- const std::string & filenameMeshRootDir)
+ const std::vector<std::string> & package_dirs)
: _filenameUrdf(filenameUrdf)
- , _filenameMeshRootDir(filenameMeshRootDir)
+ , _package_dirs(package_dirs)
{}
template <typename JointModel>
ModelGeometryHandlerPair_t operator() (const JointModel & root_joint) const
{
Model * model = new Model(se3::urdf::buildModel(_filenameUrdf, root_joint));
- GeometryModel * geometry_model = new GeometryModel (se3::urdf::buildGeom(*model, _filenameUrdf, _filenameMeshRootDir));
+ GeometryModel * geometry_model = new GeometryModel (se3::urdf::buildGeom(*model, _filenameUrdf, _package_dirs));
return std::pair<ModelHandler, GeometryModelHandler> (ModelHandler(model, true),
GeometryModelHandler(geometry_model, true)
@@ -110,20 +110,20 @@ namespace se3
static ModelGeometryHandlerPair_t
buildModelAndGeomFromUrdf(const std::string & filename,
- const std::string & mesh_dir,
+ const std::vector<std::string> & package_dirs,
bp::object & root_joint_object
)
{
JointModelVariant root_joint = bp::extract<JointModelVariant> (root_joint_object);
- return boost::apply_visitor(build_model_and_geom_visitor(filename, mesh_dir), root_joint);
+ return boost::apply_visitor(build_model_and_geom_visitor(filename, package_dirs), root_joint);
}
static ModelGeometryHandlerPair_t
buildModelAndGeomFromUrdf(const std::string & filename,
- const std::string & mesh_dir)
+ const std::vector<std::string> & package_dirs)
{
Model * model = new Model(se3::urdf::buildModel(filename));
- GeometryModel * geometry_model = new GeometryModel(se3::urdf::buildGeom(*model, filename, mesh_dir));
+ GeometryModel * geometry_model = new GeometryModel(se3::urdf::buildGeom(*model, filename, package_dirs));
return ModelGeometryHandlerPair_t (ModelHandler(model, true),
GeometryModelHandler(geometry_model, true)
@@ -173,15 +173,15 @@ namespace se3
bp::to_python_converter<std::pair<ModelHandler, GeometryModelHandler>, PairToTupleConverter<ModelHandler, GeometryModelHandler> >();
bp::def("buildModelAndGeomFromUrdf",
- static_cast <ModelGeometryHandlerPair_t (*) (const std::string &, const std::string &, bp::object &)> (&ParsersPythonVisitor::buildModelAndGeomFromUrdf),
- bp::args("filename (string)", "mesh_dir (string)",
+ static_cast <ModelGeometryHandlerPair_t (*) (const std::string &, const std::vector<std::string> &, bp::object &)> (&ParsersPythonVisitor::buildModelAndGeomFromUrdf),
+ bp::args("filename (string)", "package_dirs (vector of strings)",
"Root Joint Model"),
"Parse the urdf file given in input and return a proper pinocchio model starting with a given root joint and geometry model "
"(remember to create the corresponding data structures).");
bp::def("buildModelAndGeomFromUrdf",
- static_cast <ModelGeometryHandlerPair_t (*) (const std::string &, const std::string &)> (&ParsersPythonVisitor::buildModelAndGeomFromUrdf),
- bp::args("filename (string)", "mesh_dir (string)"),
+ static_cast <ModelGeometryHandlerPair_t (*) (const std::string &, const std::vector<std::string> &)> (&ParsersPythonVisitor::buildModelAndGeomFromUrdf),
+ bp::args("filename (string)", "package_dirs (vector of strings)"),
"Parse the urdf file given in input and return a proper pinocchio model and geometry model "
"(remember to create the corresponding data structures).");
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
index 03fc2e711..80c00176d 100644
--- a/unittest/geom.cpp
+++ b/unittest/geom.cpp
@@ -205,9 +205,12 @@ BOOST_AUTO_TEST_CASE ( loading_model )
std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
+ std::vector < std::string > package_dirs;
std::string meshDir = PINOCCHIO_SOURCE_DIR"/models/";
+ package_dirs.push_back(meshDir);
+
Model model = se3::urdf::buildModel(filename, se3::JointModelFreeFlyer());
- GeometryModel geometry_model = se3::urdf::buildGeom(model, filename, meshDir);
+ GeometryModel geometry_model = se3::urdf::buildGeom(model, filename, package_dirs, true);
Data data(model);
GeometryData geometry_data(data, geometry_model);
@@ -243,16 +246,22 @@ BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions )
// Building the model in pinocchio and compute kinematics/geometry for configuration q_pino
std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
+ std::vector < std::string > package_dirs;
std::string meshDir = PINOCCHIO_SOURCE_DIR"/models/";
- std::pair < Model, GeometryModel > robot = se3::urdf::buildModelAndGeom(filename, meshDir, se3::JointModelFreeFlyer());
+ package_dirs.push_back(meshDir);
+
+ se3::Model model = se3::urdf::buildModel(filename, se3::JointModelFreeFlyer());
+ se3::GeometryModel geom = se3::urdf::buildGeom(model, filename, package_dirs, true);
+ std::cout << model << std::endl;
- Data data(robot.first);
- GeometryData data_geom(data, robot.second);
+
+ Data data(model);
+ GeometryData data_geom(data, geom);
// Configuration to be tested
- Eigen::VectorXd q_pino(Eigen::VectorXd::Random(robot.first.nq));
+ Eigen::VectorXd q_pino(Eigen::VectorXd::Random(model.nq));
q_pino.segment<4>(3) /= q_pino.segment<4>(3).norm();
Eigen::VectorXd q_hpp(q_pino);
@@ -261,9 +270,9 @@ BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions )
q_hpp.segment<4>(3) = quaternion ;
- assert(q_pino.size() == robot.first.nq && "wrong config size");
+ assert(q_pino.size() == model.nq && "wrong config size");
- se3::updateGeometryPlacements(robot.first, data, robot.second, data_geom, q_pino);
+ se3::updateGeometryPlacements(model, data, geom, data_geom, q_pino);
/// ************* HPP ************* ///
@@ -277,7 +286,7 @@ BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions )
"", "");
- assert(robot.first.nq == humanoidRobot->configSize () && "Pinocchio model & HPP model config sizes are not the same ");
+ assert(model.nq == humanoidRobot->configSize () && "Pinocchio model & HPP model config sizes are not the same ");
humanoidRobot->currentConfiguration (q_hpp);
humanoidRobot->computeForwardKinematics ();
@@ -347,16 +356,22 @@ BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance)
// Building the model in pinocchio and compute kinematics/geometry for configuration q_pino
std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
+ std::vector < std::string > package_dirs;
std::string meshDir = PINOCCHIO_SOURCE_DIR"/models/";
- std::pair < Model, GeometryModel > robot = se3::urdf::buildModelAndGeom(filename, meshDir, se3::JointModelFreeFlyer());
+ package_dirs.push_back(meshDir);
- Data data(robot.first);
- GeometryData data_geom(data, robot.second);
+ se3::Model model = se3::urdf::buildModel(filename, se3::JointModelFreeFlyer());
+ se3::GeometryModel geom = se3::urdf::buildGeom(model, filename, package_dirs, true);
+ std::cout << model << std::endl;
+
+
+ Data data(model);
+ GeometryData data_geom(data, geom);
// Configuration to be tested
- Eigen::VectorXd q_pino(Eigen::VectorXd::Random(robot.first.nq));
+ Eigen::VectorXd q_pino(Eigen::VectorXd::Random(model.nq));
q_pino.segment<4>(3) /= q_pino.segment<4>(3).norm();
Eigen::VectorXd q_hpp(q_pino);
@@ -365,9 +380,9 @@ BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance)
q_hpp.segment<4>(3) = quaternion ;
- assert(q_pino.size() == robot.first.nq && "wrong config size");
+ assert(q_pino.size() == model.nq && "wrong config size");
- se3::updateGeometryPlacements(robot.first, data, robot.second, data_geom, q_pino);
+ se3::updateGeometryPlacements(model, data, geom, data_geom, q_pino);
/// ************* HPP ************* ///
@@ -381,7 +396,7 @@ BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance)
"", "");
- assert(robot.first.nq == humanoidRobot->configSize () && "Pinocchio model & HPP model config sizes are not the same ");
+ assert(model.nq == humanoidRobot->configSize () && "Pinocchio model & HPP model config sizes are not the same ");
humanoidRobot->currentConfiguration (q_hpp);
humanoidRobot->computeForwardKinematics ();
@@ -412,8 +427,8 @@ BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance)
std::cout << "comparison between " << body1 << " and " << body2 << std::endl;
- se3::DistanceResult dist_pin = data_geom.computeDistance( robot.second.getGeomId(body1),
- robot.second.getGeomId(body2));
+ se3::DistanceResult dist_pin = data_geom.computeDistance( geom.getGeomId(body1),
+ geom.getGeomId(body2));
Distance_t distance_pin(dist_pin.fcl_distance_result);
distance_hpp.checkClose(distance_pin);
--
GitLab