pinocchio  2.6.20
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
geometry.cpp
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 //
4 
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"
9 
10 #include <boost/property_tree/xml_parser.hpp>
11 #include <boost/property_tree/ptree.hpp>
12 
13 #include <urdf_model/model.h>
14 #include <urdf_parser/urdf_parser.h>
15 
16 namespace pinocchio
17 {
18  namespace urdf
19  {
21  namespace details
22  {
23  struct UrdfTree
24  {
25  typedef boost::property_tree::ptree ptree;
26  typedef std::map<std::string, const ptree&> LinkMap_t;
27 
28  void parse (const std::string & xmlStr)
29  {
30  urdf_ = ::urdf::parseURDF(xmlStr);
31  if (!urdf_) {
32  throw std::invalid_argument("Unable to parse URDF");
33  }
34 
35  std::istringstream iss(xmlStr);
36  using namespace boost::property_tree;
37  read_xml(iss, tree_, xml_parser::no_comments);
38 
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));
43  }
44  } // BOOST_FOREACH
45  }
46 
47  bool isCapsule(const std::string & linkName,
48  const std::string & geomName) const
49  {
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)
54  return false;
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;
60 #else
61  std::string name = cc.second.get<std::string>("<xmlattr>.name");
62  if (geomName == name) return true;
63 #endif
64  }
65  } // BOOST_FOREACH
66 
67  return false;
68  }
69 
70  bool isMeshConvex(const std::string & linkName,
71  const std::string & geomName) const
72  {
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)
77  return false;
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;
83 #else
84  std::string name = cc.second.get<std::string>("<xmlattr>.name");
85  if (geomName == name) return true;
86 #endif
87  }
88  } // BOOST_FOREACH
89 
90  return false;
91  }
92 
93  // For standard URDF tags
94  ::urdf::ModelInterfaceSharedPtr urdf_;
95  // For other tags
96  ptree tree_;
97  // A mapping from link name to corresponding child of tree_
98  LinkMap_t links_;
99  };
100 
101  template<typename Vector3>
102  static void retrieveMeshScale(const ::urdf::MeshSharedPtr & mesh,
103  const Eigen::MatrixBase<Vector3> & scale)
104  {
105  Vector3 & scale_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3,scale);
106  scale_ <<
107  mesh->scale.x,
108  mesh->scale.y,
109  mesh->scale.z;
110  }
111 
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
117 # endif
118 
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,
136  const std::vector<std::string> & package_dirs,
137  std::string & meshPath,
138  Eigen::Vector3d & meshScale)
139  {
140  shared_ptr<fcl::CollisionGeometry> geometry;
141 
142  // Handle the case where collision geometry is a mesh
143  if (urdf_geometry->type == ::urdf::Geometry::MESH)
144  {
145  const ::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> (urdf_geometry);
146  std::string collisionFilename = urdf_mesh->filename;
147 
148  meshPath = retrieveResourcePath(collisionFilename, package_dirs);
149  if (meshPath == "") {
150  std::stringstream ss;
151  ss << "Mesh " << collisionFilename << " could not be found.";
152  throw std::invalid_argument (ss.str());
153  }
154 
155  fcl::Vec3f scale = fcl::Vec3f(urdf_mesh->scale.x,
156  urdf_mesh->scale.y,
157  urdf_mesh->scale.z
158  );
159 
160  retrieveMeshScale(urdf_mesh, meshScale);
161 
162  // Create FCL mesh by parsing Collada file.
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);
166  if (convex) {
167  bvh->buildConvexRepresentation (false);
168 #if HPP_FCL_MAJOR_VERSION < 2
169  geometry = bvh->convex;
170 #else
171  geometry = shared_ptr<fcl::CollisionGeometry>(bvh->convex.get(), [bvh](...) mutable { bvh->convex.reset(); });
172 #endif // HPP_FCL_MAJOR_VERSION
173  } else {
174 #if HPP_FCL_MAJOR_VERSION < 2
175  geometry = bvh;
176 #else
177  geometry = shared_ptr<fcl::CollisionGeometry>(bvh.get(), [bvh](...) mutable { bvh.reset(); });
178 #endif // HPP_FCL_MAJOR_VERSION
179  }
180 #else
181  geometry = meshLoader->load (meshPath, scale);
182 #endif
183  }
184 
185  // Handle the case where collision geometry is a cylinder
186  // Use FCL capsules for cylinders
187  else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER)
188  {
189  const bool is_capsule = tree.isCapsule(linkName, geomName);
190  meshScale << 1,1,1;
191  const ::urdf::CylinderSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry);
192 
193  double radius = collisionGeometry->radius;
194  double length = collisionGeometry->length;
195 
196  // Create fcl capsule geometry.
197  if (is_capsule) {
198  meshPath = "CAPSULE";
199  geometry = shared_ptr < fcl::CollisionGeometry >(new fcl::Capsule (radius, length));
200  } else {
201  meshPath = "CYLINDER";
202  geometry = shared_ptr < fcl::CollisionGeometry >(new fcl::Cylinder (radius, length));
203  }
204  }
205  // Handle the case where collision geometry is a box.
206  else if (urdf_geometry->type == ::urdf::Geometry::BOX)
207  {
208  meshPath = "BOX";
209  meshScale << 1,1,1;
210  const ::urdf::BoxSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Box> (urdf_geometry);
211 
212  double x = collisionGeometry->dim.x;
213  double y = collisionGeometry->dim.y;
214  double z = collisionGeometry->dim.z;
215 
216  geometry = shared_ptr < fcl::CollisionGeometry > (new fcl::Box (x, y, z));
217  }
218  // Handle the case where collision geometry is a sphere.
219  else if (urdf_geometry->type == ::urdf::Geometry::SPHERE)
220  {
221  meshPath = "SPHERE";
222  meshScale << 1,1,1;
223  const ::urdf::SphereSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Sphere> (urdf_geometry);
224 
225  double radius = collisionGeometry->radius;
226 
227  geometry = shared_ptr < fcl::CollisionGeometry > (new fcl::Sphere (radius));
228  }
229  else throw std::invalid_argument("Unknown geometry type :");
230 
231  if (!geometry)
232  {
233  throw std::invalid_argument("The polyhedron retrieved is empty");
234  }
235 
236  return geometry;
237  }
238 #endif // PINOCCHIO_WITH_HPP_FCL
239 
247  template<typename T>
248  inline PINOCCHIO_URDF_SHARED_PTR(const T)
249  getLinkGeometry(const ::urdf::LinkConstSharedPtr link);
250 
251  template<>
252  inline ::urdf::CollisionConstSharedPtr
253  getLinkGeometry< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
254  {
255  return link->collision;
256  }
257 
258  template<>
259  inline ::urdf::VisualConstSharedPtr
260  getLinkGeometry< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
261  {
262  return link->visual;
263  }
264 
265 
275  template<typename urdfObject>
276  inline bool getVisualMaterial(const PINOCCHIO_URDF_SHARED_PTR(urdfObject) urdf_object,std::string & meshTexturePath,
277  Eigen::Vector4d & meshColor, const std::vector<std::string> & package_dirs);
278 
279  template<>
280  inline bool getVisualMaterial< ::urdf::Collision>
281  (const ::urdf::CollisionSharedPtr, std::string& meshTexturePath,
282  Eigen::Vector4d & meshColor, const std::vector<std::string> &)
283  {
284  meshColor << 0.9, 0.9, 0.9, 1.;
285  meshTexturePath = "";
286  return false;
287  }
288 
289  template<>
290  inline bool getVisualMaterial< ::urdf::Visual>
291  (const ::urdf::VisualSharedPtr urdf_visual, std::string& meshTexturePath,
292  Eigen::Vector4d & meshColor, const std::vector<std::string> & package_dirs)
293  {
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);
303  }
304  return overrideMaterial;
305  }
306 
314  template<typename T>
315  inline const std::vector< PINOCCHIO_URDF_SHARED_PTR(T) > &
316  getLinkGeometryArray(const ::urdf::LinkConstSharedPtr link);
317 
318  template<>
319  inline const std::vector< ::urdf::CollisionSharedPtr> &
320  getLinkGeometryArray< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
321  {
322  return link->collision_array;
323  }
324 
325  template<>
326  inline const std::vector< ::urdf::VisualSharedPtr> &
327  getLinkGeometryArray< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
328  {
329  return link->visual_array;
330  }
331 
344  template<typename GeometryType>
345  static void addLinkGeometryToGeomModel(const UrdfTree & tree,
346  ::hpp::fcl::MeshLoaderPtr & meshLoader,
347  ::urdf::LinkConstSharedPtr link,
348  UrdfGeomVisitorBase& visitor,
349  GeometryModel & geomModel,
350  const std::vector<std::string> & package_dirs)
351  {
352 #ifndef PINOCCHIO_WITH_HPP_FCL
353  PINOCCHIO_UNUSED_VARIABLE(tree);
354  PINOCCHIO_UNUSED_VARIABLE(meshLoader);
355 #endif // PINOCCHIO_WITH_HPP_FCL
356 
357  typedef std::vector< PINOCCHIO_URDF_SHARED_PTR(GeometryType) > VectorSharedT;
358  typedef GeometryModel::SE3 SE3;
359 
360  if(getLinkGeometry<GeometryType>(link))
361  {
362  std::string meshPath = "";
363 
364  Eigen::Vector3d meshScale(Eigen::Vector3d::Ones());
365 
366  const std::string & link_name = link->name;
367 
368  VectorSharedT geometries_array = getLinkGeometryArray<GeometryType>(link);
369 
370  FrameIndex frame_id;
371  UrdfGeomVisitorBase::Frame frame = visitor.getBodyFrame (link_name, frame_id);
372  const SE3 & body_placement = frame.placement;
373 
374  std::size_t objectId = 0;
375  for (typename VectorSharedT::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i)
376  {
377  meshPath.clear();
378 #ifdef PINOCCHIO_WITH_HPP_FCL
379 
380 #ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
381  const std::string & geom_name = (*i)->group_name;
382 #else
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);
388 #else
389  ::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> ((*i)->geometry);
390  if (urdf_mesh)
391  {
392  meshPath = retrieveResourcePath(urdf_mesh->filename, package_dirs);
393  retrieveMeshScale(urdf_mesh, meshScale);
394  }
395 
396  const shared_ptr<fcl::CollisionGeometry> geometry(new fcl::CollisionGeometry());
397 #endif // PINOCCHIO_WITH_HPP_FCL
398 
399  Eigen::Vector4d meshColor;
400  std::string meshTexturePath;
401  bool overrideMaterial = getVisualMaterial<GeometryType>((*i), meshTexturePath, meshColor, package_dirs);
402 
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());
407  GeometryObject geometry_object(geometry_object_name,
408  frame_id, frame.parent,
409  geometry,
410  geomPlacement, meshPath, meshScale,
411  overrideMaterial, meshColor, meshTexturePath);
412  geomModel.addGeometryObject(geometry_object);
413  ++objectId;
414  }
415  }
416  }
417 
431  void recursiveParseTreeForGeom(const UrdfTree& tree,
432  ::hpp::fcl::MeshLoaderPtr& meshLoader,
433  ::urdf::LinkConstSharedPtr link,
434  UrdfGeomVisitorBase& visitor,
435  GeometryModel & geomModel,
436  const std::vector<std::string> & package_dirs,
437  const GeometryType type)
438  {
439 
440  switch(type)
441  {
442  case COLLISION:
443  addLinkGeometryToGeomModel< ::urdf::Collision >(tree, meshLoader, link, visitor, geomModel, package_dirs);
444  break;
445  case VISUAL:
446  addLinkGeometryToGeomModel< ::urdf::Visual >(tree, meshLoader, link, visitor, geomModel, package_dirs);
447  break;
448  default:
449  break;
450  }
451 
452  BOOST_FOREACH(::urdf::LinkConstSharedPtr child,link->child_links)
453  {
454  recursiveParseTreeForGeom(tree, meshLoader, child, visitor, geomModel, package_dirs,type);
455  }
456 
457  }
458 
459  void parseTreeForGeom(UrdfGeomVisitorBase& visitor,
460  const std::istream& xmlStream,
461  const GeometryType type,
462  GeometryModel & geomModel,
463  const std::vector<std::string> & package_dirs,
464  ::hpp::fcl::MeshLoaderPtr meshLoader)
465  {
466  std::string xmlStr;
467  {
468  std::ostringstream os;
469  os << xmlStream.rdbuf();
470  xmlStr = os.str();
471  }
472 
473  details::UrdfTree tree;
474  tree.parse (xmlStr);
475 
476  std::vector<std::string> hint_directories(package_dirs);
477 
478  // Append the ROS_PACKAGE_PATH
479  std::vector<std::string> ros_pkg_paths = rosPaths();
480  hint_directories.insert(hint_directories.end(), ros_pkg_paths.begin(), ros_pkg_paths.end());
481 
482 #ifdef PINOCCHIO_WITH_HPP_FCL
483  if (!meshLoader) meshLoader = fcl::MeshLoaderPtr(new fcl::MeshLoader);
484 #endif // ifdef PINOCCHIO_WITH_HPP_FCL
485 
486  recursiveParseTreeForGeom(tree, meshLoader, tree.urdf_->getRoot(),
487  visitor, geomModel, hint_directories,type);
488  }
489  } // namespace details
491  } // namespace urdf
492 } // namespace pinocchio
pinocchio::urdf::details::addLinkGeometryToGeomModel
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...
Definition: geometry.cpp:345
pinocchio::SE3Tpl< Scalar, Options >
pinocchio::urdf::details::getVisualMaterial
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.
pinocchio::name
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
pinocchio::GeometryObject
Definition: fcl.hpp:137
pinocchio::urdf::details::convertFromUrdf
static Inertia convertFromUrdf(const ::urdf::Inertial &Y)
Convert URDF Inertial quantity to Spatial Inertia.
Definition: model.cpp:30
pinocchio::urdf::details::getLinkGeometryArray
const std::vector< PINOCCHIO_URDF_SHARED_PTR(T) > & getLinkGeometryArray(const ::urdf::LinkConstSharedPtr link)
Get the array of geometries attached to a link.
pinocchio::GeometryModel::addGeometryObject
GeomIndex addGeometryObject(const GeometryObject &object, const ModelTpl< S2, O2, _JointCollectionTpl > &model)
Add a geometry object to a GeometryModel and set its parent joint.
pinocchio::retrieveResourcePath
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...
Definition: utils.hpp:61
pinocchio::urdf::details::retrieveCollisionGeometry
shared_ptr< fcl::CollisionGeometry > retrieveCollisionGeometry(const UrdfTree &tree, fcl::MeshLoaderPtr &meshLoader, const std::string &linkName, const std::string &geomName, const ::urdf::GeometrySharedPtr urdf_geometry, const std::vector< std::string > &package_dirs, std::string &meshPath, Eigen::Vector3d &meshScale)
Get a fcl::CollisionObject from a URDF geometry, searching for it in specified package directories.
Definition: geometry.cpp:131
pinocchio::urdf::details::recursiveParseTreeForGeom
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...
Definition: geometry.cpp:431
pinocchio::GeometryModel
Definition: geometry.hpp:22
pinocchio::rosPaths
std::vector< std::string > rosPaths()
Parse the environment variables ROS_PACKAGE_PATH / AMENT_PREFIX_PATH and extract paths.
Definition: file-explorer.cpp:55
pinocchio::urdf::details::UrdfTree
Definition: geometry.cpp:23
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11