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);
39 BOOST_FOREACH(
const ptree::value_type & link, tree_.get_child(
"robot")) {
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));
47 bool isCapsule(
const std::string & linkName,
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;
70 bool isMeshConvex(
const std::string & linkName,
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>
102 static void retrieveMeshScale(const ::urdf::MeshSharedPtr & mesh,
103 const Eigen::MatrixBase<Vector3> & scale)
105 Vector3 & scale_ = PINOCCHIO_EIGEN_CONST_CAST(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 boost::shared_ptr<fcl::CollisionGeometry>
132 fcl::MeshLoaderPtr& meshLoader,
133 const std::string& linkName,
134 const std::string& geomName,
135 const ::urdf::GeometrySharedPtr urdf_geometry,
136 const std::vector<std::string> & package_dirs,
137 std::string & meshPath,
138 Eigen::Vector3d & meshScale)
140 boost::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,
160 retrieveMeshScale(urdf_mesh, meshScale);
163 #ifdef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
164 hpp::fcl::BVHModelPtr_t bvh = meshLoader->load (meshPath, scale);
165 bool convex = tree.isMeshConvex (linkName, geomName);
167 bvh->buildConvexRepresentation (
false);
168 geometry = bvh->convex;
172 geometry = meshLoader->load (meshPath, scale);
178 else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER)
180 const bool is_capsule = tree.isCapsule(linkName, geomName);
182 const ::urdf::CylinderSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry);
184 double radius = collisionGeometry->radius;
185 double length = collisionGeometry->length;
189 meshPath =
"CAPSULE";
190 geometry = boost::shared_ptr < fcl::CollisionGeometry >(
new fcl::Capsule (radius, length));
192 meshPath =
"CYLINDER";
193 geometry = boost::shared_ptr < fcl::CollisionGeometry >(
new fcl::Cylinder (radius, length));
197 else if (urdf_geometry->type == ::urdf::Geometry::BOX)
201 const ::urdf::BoxSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Box> (urdf_geometry);
203 double x = collisionGeometry->dim.x;
204 double y = collisionGeometry->dim.y;
205 double z = collisionGeometry->dim.z;
207 geometry = boost::shared_ptr < fcl::CollisionGeometry > (
new fcl::Box (x, y, z));
210 else if (urdf_geometry->type == ::urdf::Geometry::SPHERE)
214 const ::urdf::SphereSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Sphere> (urdf_geometry);
216 double radius = collisionGeometry->radius;
218 geometry = boost::shared_ptr < fcl::CollisionGeometry > (
new fcl::Sphere (radius));
220 else throw std::invalid_argument(
"Unknown geometry type :");
224 throw std::invalid_argument(
"The polyhedron retrived is empty");
229 #endif // PINOCCHIO_WITH_HPP_FCL
239 inline PINOCCHIO_URDF_SHARED_PTR(
const T)
240 getLinkGeometry(const ::urdf::LinkConstSharedPtr link);
243 inline ::urdf::CollisionConstSharedPtr
244 getLinkGeometry< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
246 return link->collision;
250 inline ::urdf::VisualConstSharedPtr
251 getLinkGeometry< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
266 template<
typename urdfObject>
267 inline bool getVisualMaterial(
const PINOCCHIO_URDF_SHARED_PTR(urdfObject) urdf_object,std::string & meshTexturePath,
268 Eigen::Vector4d & meshColor,
const std::vector<std::string> & package_dirs);
271 inline bool getVisualMaterial< ::urdf::Collision>
272 (const ::urdf::CollisionSharedPtr, std::string& meshTexturePath,
273 Eigen::Vector4d & meshColor,
const std::vector<std::string> &)
275 meshColor << 0.9, 0.9, 0.9, 1.;
276 meshTexturePath =
"";
281 inline bool getVisualMaterial< ::urdf::Visual>
282 (const ::urdf::VisualSharedPtr urdf_visual, std::string& meshTexturePath,
283 Eigen::Vector4d & meshColor,
const std::vector<std::string> & package_dirs)
285 meshColor << 0.9, 0.9, 0.9, 1.;
286 meshTexturePath =
"";
287 bool overrideMaterial =
false;
288 if(urdf_visual->material) {
289 overrideMaterial =
true;
290 meshColor << urdf_visual->material->color.r, urdf_visual->material->color.g,
291 urdf_visual->material->color.b, urdf_visual->material->color.a;
292 if(urdf_visual->material->texture_filename!=
"")
293 meshTexturePath =
retrieveResourcePath((urdf_visual)->material->texture_filename, package_dirs);
295 return overrideMaterial;
306 inline const std::vector< PINOCCHIO_URDF_SHARED_PTR(T) > &
310 inline const std::vector< ::urdf::CollisionSharedPtr> &
311 getLinkGeometryArray< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
313 return link->collision_array;
317 inline const std::vector< ::urdf::VisualSharedPtr> &
318 getLinkGeometryArray< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
320 return link->visual_array;
335 template<
typename GeometryType>
337 ::hpp::fcl::MeshLoaderPtr & meshLoader,
338 ::urdf::LinkConstSharedPtr link,
339 UrdfGeomVisitorBase& visitor,
341 const std::vector<std::string> & package_dirs)
343 #ifndef PINOCCHIO_WITH_HPP_FCL
344 PINOCCHIO_UNUSED_VARIABLE(tree);
345 PINOCCHIO_UNUSED_VARIABLE(meshLoader);
346 #endif // PINOCCHIO_WITH_HPP_FCL
348 typedef std::vector< PINOCCHIO_URDF_SHARED_PTR(GeometryType) > VectorSharedT;
351 if(getLinkGeometry<GeometryType>(link))
353 std::string meshPath =
"";
355 Eigen::Vector3d meshScale(Eigen::Vector3d::Ones());
357 const std::string & link_name = link->name;
359 VectorSharedT geometries_array = getLinkGeometryArray<GeometryType>(link);
362 UrdfGeomVisitorBase::Frame frame = visitor.getBodyFrame (link_name, frame_id);
363 SE3 body_placement = frame.placement;
365 std::size_t objectId = 0;
366 for (
typename VectorSharedT::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i)
369 #ifdef PINOCCHIO_WITH_HPP_FCL
371 #ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
372 const std::string & geom_name = (*i)->group_name;
374 const std::string & geom_name = (*i)->name;
375 #endif // PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
376 const GeometryObject::CollisionGeometryPtr geometry =
378 (*i)->geometry, package_dirs, meshPath, meshScale);
380 ::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> ((*i)->geometry);
384 retrieveMeshScale(urdf_mesh, meshScale);
387 const boost::shared_ptr<fcl::CollisionGeometry> geometry(
new fcl::CollisionGeometry());
388 #endif // PINOCCHIO_WITH_HPP_FCL
390 Eigen::Vector4d meshColor;
391 std::string meshTexturePath;
392 bool overrideMaterial = getVisualMaterial<GeometryType>((*i), meshTexturePath, meshColor, package_dirs);
395 std::ostringstream geometry_object_suffix;
396 geometry_object_suffix <<
"_" << objectId;
397 const std::string & geometry_object_name = std::string(link_name + geometry_object_suffix.str());
399 frame_id, frame.parent,
401 geomPlacement, meshPath, meshScale,
402 overrideMaterial, meshColor, meshTexturePath);
423 ::hpp::fcl::MeshLoaderPtr& meshLoader,
424 ::urdf::LinkConstSharedPtr link,
425 UrdfGeomVisitorBase& visitor,
427 const std::vector<std::string> & package_dirs,
428 const GeometryType type)
434 addLinkGeometryToGeomModel< ::urdf::Collision >(tree, meshLoader, link, visitor, geomModel, package_dirs);
437 addLinkGeometryToGeomModel< ::urdf::Visual >(tree, meshLoader, link, visitor, geomModel, package_dirs);
443 BOOST_FOREACH(::urdf::LinkConstSharedPtr child,link->child_links)
450 void parseTreeForGeom(UrdfGeomVisitorBase& visitor,
451 const std::istream& xmlStream,
452 const GeometryType type,
454 const std::vector<std::string> & package_dirs,
455 ::hpp::fcl::MeshLoaderPtr meshLoader)
459 std::ostringstream os;
460 os << xmlStream.rdbuf();
464 details::UrdfTree tree;
467 std::vector<std::string> hint_directories(package_dirs);
470 std::vector<std::string> ros_pkg_paths =
rosPaths();
471 hint_directories.insert(hint_directories.end(), ros_pkg_paths.begin(), ros_pkg_paths.end());
473 #ifdef PINOCCHIO_WITH_HPP_FCL
474 if (!meshLoader) meshLoader = fcl::MeshLoaderPtr(
new fcl::MeshLoader);
475 #endif // ifdef PINOCCHIO_WITH_HPP_FCL
478 visitor, geomModel, hint_directories,type);