5 #include "pinocchio/parsers/urdf.hpp" 6 #include "pinocchio/parsers/urdf/types.hpp" 7 #include "pinocchio/parsers/urdf/utils.hpp" 8 #include "pinocchio/parsers/utils.hpp" 10 #include <boost/property_tree/xml_parser.hpp> 11 #include <boost/property_tree/ptree.hpp> 13 #include <urdf_model/model.h> 14 #include <urdf_parser/urdf_parser.h> 25 typedef boost::property_tree::ptree
ptree;
26 typedef std::map<std::string, const ptree&>
LinkMap_t;
28 void parse (
const std::string & xmlStr)
30 urdf_ = ::urdf::parseURDF(xmlStr);
32 throw std::invalid_argument(
"Unable to parse URDF");
35 std::istringstream iss(xmlStr);
36 using namespace boost::property_tree;
37 read_xml(iss,
tree_, xml_parser::no_comments);
40 if (link.first ==
"link") {
41 std::string
name = link.second.get<std::string>(
"<xmlattr>.name");
42 links_.insert(std::pair<std::string,const ptree&>(name, link.second));
48 const std::string & geomName)
const 50 LinkMap_t::const_iterator _link =
links_.find(linkName);
51 assert (_link !=
links_.end());
52 const ptree& link = _link->second;
53 if (link.count (
"collision_checking") == 0)
55 BOOST_FOREACH(
const ptree::value_type & cc, link.get_child(
"collision_checking")) {
56 if (cc.first ==
"capsule") {
57 #ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME 58 std::cerr <<
"Warning: support for tag link/collision_checking/capsule" 59 " is not available for URDFDOM < 0.3.0" << std::endl;
61 std::string
name = cc.second.get<std::string>(
"<xmlattr>.name");
62 if (geomName == name)
return true;
71 const std::string & geomName)
const 73 LinkMap_t::const_iterator _link =
links_.find(linkName);
74 assert (_link !=
links_.end());
75 const ptree& link = _link->second;
76 if (link.count (
"collision_checking") == 0)
78 BOOST_FOREACH(
const ptree::value_type & cc, link.get_child(
"collision_checking")) {
79 if (cc.first ==
"convex") {
80 #ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME 81 std::cerr <<
"Warning: support for tag link/collision_checking/convex" 82 " is not available for URDFDOM < 0.3.0" << std::endl;
84 std::string
name = cc.second.get<std::string>(
"<xmlattr>.name");
85 if (geomName == name)
return true;
94 ::urdf::ModelInterfaceSharedPtr
urdf_;
101 template<
typename Vector3>
103 const Eigen::MatrixBase<Vector3> & scale)
112 #ifdef PINOCCHIO_WITH_HPP_FCL 113 # if ( HPP_FCL_MAJOR_VERSION>1 || ( HPP_FCL_MAJOR_VERSION==1 && \ 114 ( HPP_FCL_MINOR_VERSION>1 || ( HPP_FCL_MINOR_VERSION==1 && \ 115 HPP_FCL_PATCH_VERSION>3)))) 116 # define PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3 130 shared_ptr<fcl::CollisionGeometry>
131 inline retrieveCollisionGeometry(
const UrdfTree& tree,
132 fcl::MeshLoaderPtr& meshLoader,
133 const std::string& linkName,
134 const std::string& geomName,
135 const ::urdf::GeometrySharedPtr urdf_geometry,
137 std::string & meshPath,
140 shared_ptr<fcl::CollisionGeometry>
geometry;
143 if (urdf_geometry->type == ::urdf::Geometry::MESH)
145 const ::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> (urdf_geometry);
146 std::string collisionFilename = urdf_mesh->filename;
149 if (meshPath ==
"") {
150 std::stringstream ss;
151 ss <<
"Mesh " << collisionFilename <<
" could not be found.";
152 throw std::invalid_argument (ss.str());
155 fcl::Vec3f scale = fcl::Vec3f(urdf_mesh->scale.x,
163 #ifdef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3 167 bvh->buildConvexRepresentation (
false);
168 #if HPP_FCL_MAJOR_VERSION < 2 169 geometry = bvh->convex;
171 geometry = shared_ptr<fcl::CollisionGeometry>(bvh->convex.get(), [bvh](...)
mutable { bvh->convex.reset(); });
172 #endif // HPP_FCL_MAJOR_VERSION 174 #if HPP_FCL_MAJOR_VERSION < 2 177 geometry = shared_ptr<fcl::CollisionGeometry>(bvh.get(), [bvh](...)
mutable { bvh.reset(); });
178 #endif // HPP_FCL_MAJOR_VERSION 181 geometry = meshLoader->load (meshPath, scale);
187 else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER)
189 const bool is_capsule = tree.
isCapsule(linkName, geomName);
193 double radius = collisionGeometry->radius;
194 double length = collisionGeometry->length;
198 meshPath =
"CAPSULE";
199 geometry = shared_ptr < fcl::CollisionGeometry >(
new fcl::Capsule (radius, length));
201 meshPath =
"CYLINDER";
202 geometry = shared_ptr < fcl::CollisionGeometry >(
new fcl::Cylinder (radius, length));
206 else if (urdf_geometry->type == ::urdf::Geometry::BOX)
212 double x = collisionGeometry->dim.x;
213 double y = collisionGeometry->dim.y;
214 double z = collisionGeometry->dim.z;
216 geometry = shared_ptr < fcl::CollisionGeometry > (
new fcl::Box (x, y, z));
219 else if (urdf_geometry->type == ::urdf::Geometry::SPHERE)
225 double radius = collisionGeometry->radius;
227 geometry = shared_ptr < fcl::CollisionGeometry > (
new fcl::Sphere (radius));
229 else throw std::invalid_argument(
"Unknown geometry type :");
233 throw std::invalid_argument(
"The polyhedron retrieved is empty");
238 #endif // PINOCCHIO_WITH_HPP_FCL 249 getLinkGeometry(const ::urdf::LinkConstSharedPtr link);
252 inline ::urdf::CollisionConstSharedPtr
253 getLinkGeometry< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
255 return link->collision;
259 inline ::urdf::VisualConstSharedPtr
260 getLinkGeometry< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
275 template<
typename urdfObject>
277 Eigen::Vector4d &
meshColor,
const std::vector<std::string> & package_dirs);
280 inline bool getVisualMaterial< ::urdf::Collision>
281 (const ::urdf::CollisionSharedPtr, std::string& meshTexturePath,
282 Eigen::Vector4d &
meshColor,
const std::vector<std::string> &)
284 meshColor << 0.9, 0.9, 0.9, 1.;
285 meshTexturePath =
"";
290 inline bool getVisualMaterial< ::urdf::Visual>
291 (const ::urdf::VisualSharedPtr urdf_visual, std::string& meshTexturePath,
294 meshColor << 0.9, 0.9, 0.9, 1.;
295 meshTexturePath =
"";
296 bool overrideMaterial =
false;
297 if(urdf_visual->material) {
298 overrideMaterial =
true;
299 meshColor << urdf_visual->material->color.r, urdf_visual->material->color.g,
300 urdf_visual->material->color.b, urdf_visual->material->color.a;
301 if(urdf_visual->material->texture_filename!=
"")
302 meshTexturePath =
retrieveResourcePath((urdf_visual)->material->texture_filename, package_dirs);
304 return overrideMaterial;
315 inline const std::vector< PINOCCHIO_URDF_SHARED_PTR(T) > &
319 inline const std::vector< ::urdf::CollisionSharedPtr> &
320 getLinkGeometryArray< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
322 return link->collision_array;
326 inline const std::vector< ::urdf::VisualSharedPtr> &
327 getLinkGeometryArray< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
329 return link->visual_array;
344 template<
typename GeometryType>
346 ::hpp::fcl::MeshLoaderPtr & meshLoader,
347 ::urdf::LinkConstSharedPtr link,
348 UrdfGeomVisitorBase& visitor,
350 const std::vector<std::string> & package_dirs)
352 #ifndef PINOCCHIO_WITH_HPP_FCL 355 #endif // PINOCCHIO_WITH_HPP_FCL 357 typedef std::vector< PINOCCHIO_URDF_SHARED_PTR(GeometryType) > VectorSharedT;
360 if(getLinkGeometry<GeometryType>(link))
362 std::string meshPath =
"";
364 Eigen::Vector3d
meshScale(Eigen::Vector3d::Ones());
366 const std::string & link_name = link->name;
368 VectorSharedT geometries_array = getLinkGeometryArray<GeometryType>(link);
374 std::size_t objectId = 0;
375 for (
typename VectorSharedT::const_iterator
i = geometries_array.begin();
i != geometries_array.end(); ++
i)
378 #ifdef PINOCCHIO_WITH_HPP_FCL 380 #ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME 381 const std::string & geom_name = (*i)->group_name;
383 const std::string & geom_name = (*i)->name;
384 #endif // PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME 385 const GeometryObject::CollisionGeometryPtr
geometry =
386 retrieveCollisionGeometry(tree, meshLoader, link_name, geom_name,
387 (*i)->geometry, package_dirs, meshPath, meshScale);
389 ::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> ((*i)->geometry);
397 #endif // PINOCCHIO_WITH_HPP_FCL 400 std::string meshTexturePath;
401 bool overrideMaterial = getVisualMaterial<GeometryType>((*i), meshTexturePath,
meshColor,
package_dirs);
403 const SE3 geomPlacement = body_placement *
convertFromUrdf((*i)->origin);
404 std::ostringstream geometry_object_suffix;
405 geometry_object_suffix <<
"_" << objectId;
406 const std::string & geometry_object_name = std::string(link_name + geometry_object_suffix.str());
408 frame_id, frame.parent,
410 geomPlacement, meshPath, meshScale,
411 overrideMaterial, meshColor, meshTexturePath);
432 ::hpp::fcl::MeshLoaderPtr& meshLoader,
433 ::urdf::LinkConstSharedPtr link,
434 UrdfGeomVisitorBase& visitor,
436 const std::vector<std::string> & package_dirs,
443 addLinkGeometryToGeomModel< ::urdf::Collision >(tree, meshLoader, link, visitor, geomModel,
package_dirs);
446 addLinkGeometryToGeomModel< ::urdf::Visual >(tree, meshLoader, link, visitor, geomModel,
package_dirs);
452 BOOST_FOREACH(::urdf::LinkConstSharedPtr child,link->child_links)
463 const std::vector<std::string> & package_dirs,
464 ::hpp::fcl::MeshLoaderPtr meshLoader)
468 std::ostringstream os;
469 os << xmlStream.rdbuf();
476 std::vector<std::string> hint_directories(package_dirs);
479 std::vector<std::string> ros_pkg_paths =
rosPaths();
480 hint_directories.insert(hint_directories.end(), ros_pkg_paths.begin(), ros_pkg_paths.end());
482 #ifdef PINOCCHIO_WITH_HPP_FCL 483 if (!meshLoader) meshLoader = fcl::MeshLoaderPtr(
new fcl::MeshLoader);
484 #endif // ifdef PINOCCHIO_WITH_HPP_FCL 487 visitor, geomModel, hint_directories,
type);
void recursiveParseTreeForGeom(const UrdfTree &tree, ::hpp::fcl::MeshLoaderPtr &meshLoader, ::urdf::LinkConstSharedPtr link, UrdfGeomVisitorBase &visitor, GeometryModel &geomModel, const std::vector< std::string > &package_dirs, const GeometryType type)
Recursive procedure for reading the URDF tree, looking for geometries This function fill the geometri...
#define PINOCCHIO_UNUSED_VARIABLE(var)
Helper to declare that a parameter is unused.
bool isCapsule(const std::string &linkName, const std::string &geomName) const
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
#define PINOCCHIO_URDF_SHARED_PTR(type)
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
std::map< std::string, const ptree & > LinkMap_t
static void retrieveMeshScale(const ::urdf::MeshSharedPtr &mesh, const Eigen::MatrixBase< Vector3 > &scale)
static Inertia convertFromUrdf(const ::urdf::Inertial &Y)
Convert URDF Inertial quantity to Spatial Inertia.
::urdf::ModelInterfaceSharedPtr urdf_
void parse(const std::string &xmlStr)
static void addLinkGeometryToGeomModel(const UrdfTree &tree, ::hpp::fcl::MeshLoaderPtr &meshLoader, ::urdf::LinkConstSharedPtr link, UrdfGeomVisitorBase &visitor, GeometryModel &geomModel, const std::vector< std::string > &package_dirs)
Add the geometries attached to a URDF link to a GeometryModel, looking either for collisions or visua...
bool isMeshConvex(const std::string &linkName, const std::string &geomName) const
std::vector< std::string > rosPaths()
Parse the environment variables ROS_PACKAGE_PATH / AMENT_PREFIX_PATH and extract paths.
shared_ptr< BVHModelBase > BVHModelPtr_t
const std::string & xmlStream
void parseTreeForGeom(UrdfGeomVisitorBase &visitor, const std::istream &xmlStream, const GeometryType type, GeometryModel &geomModel, const std::vector< std::string > &package_dirs, ::hpp::fcl::MeshLoaderPtr meshLoader)
const shared_ptr< const CollisionGeometry > collisionGeometry() const
Main pinocchio namespace.
const std::vector< PINOCCHIO_URDF_SHARED_PTR(T) > & getLinkGeometryArray(const ::urdf::LinkConstSharedPtr link)
Get the array of geometries attached to a link.
std::string retrieveResourcePath(const std::string &string, const std::vector< std::string > &package_dirs)
Retrieve the path of the file whose path is given in URL-format. Currently convert from the following...
boost::property_tree::ptree ptree
bool getVisualMaterial(const PINOCCHIO_URDF_SHARED_PTR(urdfObject) urdf_object, std::string &meshTexturePath, Eigen::Vector4d &meshColor, const std::vector< std::string > &package_dirs)
Get the material values from the link visual object.
GeomIndex addGeometryObject(const GeometryObject &object, const ModelTpl< S2, O2, _JointCollectionTpl > &model)
Add a geometry object to a GeometryModel and set its parent joint.
Box(FCL_REAL x, FCL_REAL y, FCL_REAL z)