Commit c2bc2f56 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

Merge pull request #110 from fvalenza/topic/parse-ros-path

Recreate resource retriever behaviour by specifying the ROS_PACKAGE_PATH as environment variable.
parents e1c1ba0c 9c3a688c
......@@ -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
......
......@@ -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(model, romeo_filename, package_dirs);
Data data(model);
GeometryData geom_data (data, geom_model);
GeometryData geom_data(data, geom_model);
geom_data.initializeListOfCollisionPairs();
......
......@@ -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,22 +198,35 @@ 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 fromURDFMeshPathToAbsolutePath(const std::string & urdf_mesh_path,
const std::string & meshRootDir)
{
std::string absolutePath = std::string(meshRootDir +
urdf_mesh_path.substr(10, urdf_mesh_path.size()));
inline std::string convertURDFMeshPathToAbsolutePath(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;
}
}
} // namespace se3
......
......@@ -35,47 +35,53 @@ 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) throw (std::invalid_argument);
/**
* @brief Build the GeometryModel from a URDF file.
* @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 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] 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 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 std::vector<std::string> & package_dirs = std::vector<std::string>());
} // namespace urdf
......
......@@ -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 = convertURDFMeshPathToAbsolutePath(collisionFilename, package_dirs);
::urdf::Vector3 scale = collisionGeometry->scale;
......@@ -101,68 +100,58 @@ namespace se3
inline void parseTreeForGeom(::urdf::LinkConstPtr link,
const Model & model,
GeometryModel & geom_model,
const std::string & meshRootDir,
const bool rootJointAdded) throw (std::invalid_argument)
const std::vector<std::string> & package_dirs) throw (std::invalid_argument)
{
// start with first link that is not empty
if(link->collision)
{
::urdf::JointConstPtr joint = link->parent_joint;
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);
}
else 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);
}
else if (link->getParent() != NULL)
assert(link->getParent()!=NULL);
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.parents[model.getBodyId(collision_object_name)], collision_object, geomPlacement, collision_object_name);
if (link->getParent() == NULL)
{
const std::string exception_message (link->name + " - joint information missing.");
throw std::invalid_argument(exception_message);
}
} // if(link->inertial)
} // if(link->collision)
BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
{
parseTreeForGeom(child, model, geom_model, meshRootDir, rootJointAdded);
parseTreeForGeom(child, model, geom_model, package_dirs);
}
}
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)
{
::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
GeometryModel model_geom(model);
std::vector<std::string> hint_directories(package_dirs);
appendRosPackagePaths(hint_directories);
if(hint_directories.empty())
{
::urdf::LinkPtr child_link = root_link->child_links[0];
// Change the name of the parent joint
child_link->parent_joint->name = "root_joint";
throw std::runtime_error("You did not specify any package directory and ROS_PACKAGE_PATH is empty. Geometric parsing will crash");
}
// 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;
::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
parseTreeForGeom(urdfTree->getRoot(), model, model_geom, hint_directories);
return model_geom;
}
} // namespace urdf
} // namespace se3
......
......@@ -53,11 +53,11 @@ namespace se3
#ifdef WITH_URDFDOM
struct build_model_visitor : public boost::static_visitor<ModelHandler>
struct BuildModelVisitor : public boost::static_visitor<ModelHandler>
{
const std::string& _filename;
build_model_visitor(const std::string& filename): _filename(filename){}
BuildModelVisitor(const std::string& filename): _filename(filename){}
template <typename JointModel> ModelHandler operator()(const JointModel & root_joint) const
{
......@@ -72,7 +72,7 @@ namespace se3
)
{
JointModelVariant root_joint = bp::extract<JointModelVariant> (root_joint_object);
return boost::apply_visitor(build_model_visitor(filename), root_joint);
return boost::apply_visitor(BuildModelVisitor(filename), root_joint);
}
static ModelHandler buildModelFromUrdf(const std::string & filename)
......@@ -85,49 +85,26 @@ namespace se3
#ifdef WITH_HPP_FCL
typedef std::pair<ModelHandler, GeometryModelHandler> ModelGeometryHandlerPair_t;
struct build_model_and_geom_visitor : public boost::static_visitor<std::pair<ModelHandler, GeometryModelHandler> >
{
const std::string& _filenameUrdf;
const std::string& _filenameMeshRootDir;
build_model_and_geom_visitor(const std::string & filenameUrdf,
const std::string & filenameMeshRootDir)
: _filenameUrdf(filenameUrdf)
, _filenameMeshRootDir(filenameMeshRootDir)
{}
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));
return std::pair<ModelHandler, GeometryModelHandler> (ModelHandler(model, true),
GeometryModelHandler(geometry_model, true)
);
}
};
static ModelGeometryHandlerPair_t
buildModelAndGeomFromUrdf(const std::string & filename,
const std::string & mesh_dir,
bp::object & root_joint_object
)
static GeometryModelHandler
buildGeomFromUrdf(const ModelHandler & model,
const std::string & filename
)
{
JointModelVariant root_joint = bp::extract<JointModelVariant> (root_joint_object);
return boost::apply_visitor(build_model_and_geom_visitor(filename, mesh_dir), root_joint);
GeometryModel * geometry_model = new GeometryModel(se3::urdf::buildGeom(*model, filename));
return GeometryModelHandler(geometry_model, true);
}
static ModelGeometryHandlerPair_t
buildModelAndGeomFromUrdf(const std::string & filename,
const std::string & mesh_dir)
static GeometryModelHandler
buildGeomFromUrdf(const ModelHandler & model,
const std::string & filename,
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)
);
return GeometryModelHandler(geometry_model, true);
}
#endif // #ifdef WITH_HPP_FCL
......@@ -172,17 +149,17 @@ 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)",
"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 "
bp::def("buildGeomFromUrdf",
static_cast <GeometryModelHandler (*) (const ModelHandler &, const std::string &, std::vector<std::string> &)> (&ParsersPythonVisitor::buildGeomFromUrdf),
bp::args("Model to assosiate the Geometry","filename (string)", "package_dirs (vector of strings)"
),
"Parse the urdf file given in input looking for the geometry of the given Model and return a proper pinocchio 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)"),
"Parse the urdf file given in input and return a proper pinocchio model and geometry model "
bp::def("buildGeomFromUrdf",
static_cast <GeometryModelHandler (*) (const ModelHandler &, const std::string &)> (&ParsersPythonVisitor::buildGeomFromUrdf),
bp::args("Model to assosiate the Geometry","filename (string)"),
"Parse the urdf file given in input looking for the geometry of the given Model and return a proper pinocchio geometry model "
"(remember to create the corresponding data structures).");
#endif // #ifdef WITH_HPP_FCL
......
......@@ -22,32 +22,26 @@ import time
class RobotWrapper:
def __init__(self, filename, mesh_dir = None, root_joint = None):
if isinstance(mesh_dir, basestring):
build_model_and_geom = True
else: # Only load the model
build_model_and_geom = False
root_joint = mesh_dir
self.model_filename = filename
if build_model_and_geom:
# Check if the module geometry of Pinocchio has been compiled
if not "buildModelAndGeomFromUrdf" in dir(se3):
raise Exception('It seems that the Geometry Module has not been compiled with Pinocchio')
if(root_joint is None):
self.model, self.geometry_model = se3.buildModelAndGeomFromUrdf(filename,mesh_dir)
else:
self.model, self.geometry_model = se3.buildModelAndGeomFromUrdf(filename,mesh_dir,root_joint)
self.data = self.model.createData()
self.geometry_data = se3.GeometryData(self.data, self.geometry_model)
else:
if(root_joint is None):
def __init__(self, filename, package_dirs = None, root_joint = None):
if(root_joint is None):
self.model = se3.buildModelFromUrdf(filename)
else:
else:
self.model = se3.buildModelFromUrdf(filename, root_joint)
self.data = self.model.createData()
self.data = self.model.createData()
if not "buildGeomFromUrdf" in dir(se3):
raise Exception('It seems that the Geometry Module has not been compiled with Pinocchio. No geometry model')
else:
if (package_dirs is None):
self.geometry_model = se3.buildGeomFromUrdf(self.model, filename)
self.geometry_data = se3.GeometryData(self.data, self.geometry_model)
else:
if not all(isinstance(item,basestring) for item in package_dirs):
raise Exception('The list of package directories is wrong. At least one is not a string')
else:
self.geometry_model = se3.buildGeomFromUrdf(self.model, filename, utils.fromListToVectorOfString(package_dirs))
self.geometry_data = se3.GeometryData(self.data, self.geometry_model)
self.v0 = utils.zero(self.nv)
self.q0 = utils.zero(self.nq)
......
......@@ -104,6 +104,11 @@ def mprint(M,name = "ans"):
print ""
print ""
def fromListToVectorOfString(list):
vector = se3.StdVec_StdString()
vector.extend(item for item in list)
return vector
from rpy import *
......@@ -112,4 +117,4 @@ __all__ = [ 'np','npl','eye','zero','rand','isapprox','mprint',
'npToTTuple','npToTuple','rotate',
'rpyToMatrix','matrixToRpy',
'se3ToXYZQUAT' ,'XYZQUATToSe3',
'XYZQUATToViewerConfiguration', 'ViewerConfigurationToXYZQUAT' ]
'XYZQUATToViewerConfiguration', 'ViewerConfigurationToXYZQUAT', 'fromListToVectorOfString' ]
//
// Copyright (c) 2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_file_explorer_hpp__
#define __se3_file_explorer_hpp__
#include <string>
#include <iostream>
#include <vector>
#include <exception>
#include "boost/filesystem.hpp"
namespace se3
{
/**
* @brief Parse env variable ROS_PACKAGE_PATH to extract paths and append them
* in the package_dirs variable
*
* @param[in][out] paths { The package directories where to search for meshes }
*/
inline void appendRosPackagePaths(std::vector<std::string> & package_dirs)
{
std::string delimiter = ":";
std::string policyStr = std::getenv("ROS_PACKAGE_PATH");
size_t lastOffset = 0;
while(true)
{
size_t offset = policyStr.find_first_of(delimiter, lastOffset);
if (offset < policyStr.size())
package_dirs.push_back(policyStr.substr(lastOffset, offset - lastOffset));
if (offset == std::string::npos)
break;
else
lastOffset = offset + 1; // add one to skip the delimiter
}
}
}
#endif // __se3_file_explorer_hpp__
......@@ -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);
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);
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);