Commit ea48ec44 authored by Valenza Florian's avatar Valenza Florian
Browse files

[C++][Python][Parser Geom] Parse automatically the ROS_PACKAGE_PATH...

[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
parent e1c1ba0c
......@@ -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(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();
......
......@@ -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)
{
......
......@@ -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
......
......@@ -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
......
......@@ -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).");
......
......@@ -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);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment