src/parsers/urdf/geometry.cpp
Go to the documentation of this file.
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  boost::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  boost::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  geometry = bvh->convex;
169  } else
170  geometry = bvh;
171 #else
172  geometry = meshLoader->load (meshPath, scale);
173 #endif
174  }
175 
176  // Handle the case where collision geometry is a cylinder
177  // Use FCL capsules for cylinders
178  else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER)
179  {
180  const bool is_capsule = tree.isCapsule(linkName, geomName);
181  meshScale << 1,1,1;
182  const ::urdf::CylinderSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry);
183 
184  double radius = collisionGeometry->radius;
185  double length = collisionGeometry->length;
186 
187  // Create fcl capsule geometry.
188  if (is_capsule) {
189  meshPath = "CAPSULE";
190  geometry = boost::shared_ptr < fcl::CollisionGeometry >(new fcl::Capsule (radius, length));
191  } else {
192  meshPath = "CYLINDER";
193  geometry = boost::shared_ptr < fcl::CollisionGeometry >(new fcl::Cylinder (radius, length));
194  }
195  }
196  // Handle the case where collision geometry is a box.
197  else if (urdf_geometry->type == ::urdf::Geometry::BOX)
198  {
199  meshPath = "BOX";
200  meshScale << 1,1,1;
201  const ::urdf::BoxSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Box> (urdf_geometry);
202 
203  double x = collisionGeometry->dim.x;
204  double y = collisionGeometry->dim.y;
205  double z = collisionGeometry->dim.z;
206 
207  geometry = boost::shared_ptr < fcl::CollisionGeometry > (new fcl::Box (x, y, z));
208  }
209  // Handle the case where collision geometry is a sphere.
210  else if (urdf_geometry->type == ::urdf::Geometry::SPHERE)
211  {
212  meshPath = "SPHERE";
213  meshScale << 1,1,1;
214  const ::urdf::SphereSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Sphere> (urdf_geometry);
215 
216  double radius = collisionGeometry->radius;
217 
218  geometry = boost::shared_ptr < fcl::CollisionGeometry > (new fcl::Sphere (radius));
219  }
220  else throw std::invalid_argument("Unknown geometry type :");
221 
222  if (!geometry)
223  {
224  throw std::invalid_argument("The polyhedron retrived is empty");
225  }
226 
227  return geometry;
228  }
229 #endif // PINOCCHIO_WITH_HPP_FCL
230 
238  template<typename T>
239  inline PINOCCHIO_URDF_SHARED_PTR(const T)
240  getLinkGeometry(const ::urdf::LinkConstSharedPtr link);
241 
242  template<>
243  inline ::urdf::CollisionConstSharedPtr
244  getLinkGeometry< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
245  {
246  return link->collision;
247  }
248 
249  template<>
250  inline ::urdf::VisualConstSharedPtr
251  getLinkGeometry< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
252  {
253  return link->visual;
254  }
255 
256 
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);
269 
270  template<>
271  inline bool getVisualMaterial< ::urdf::Collision>
272  (const ::urdf::CollisionSharedPtr, std::string& meshTexturePath,
273  Eigen::Vector4d & meshColor, const std::vector<std::string> &)
274  {
275  meshColor << 0.9, 0.9, 0.9, 1.;
276  meshTexturePath = "";
277  return false;
278  }
279 
280  template<>
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)
284  {
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);
294  }
295  return overrideMaterial;
296  }
297 
305  template<typename T>
306  inline const std::vector< PINOCCHIO_URDF_SHARED_PTR(T) > &
307  getLinkGeometryArray(const ::urdf::LinkConstSharedPtr link);
308 
309  template<>
310  inline const std::vector< ::urdf::CollisionSharedPtr> &
311  getLinkGeometryArray< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
312  {
313  return link->collision_array;
314  }
315 
316  template<>
317  inline const std::vector< ::urdf::VisualSharedPtr> &
318  getLinkGeometryArray< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
319  {
320  return link->visual_array;
321  }
322 
335  template<typename GeometryType>
336  static void addLinkGeometryToGeomModel(const UrdfTree & tree,
337  ::hpp::fcl::MeshLoaderPtr & meshLoader,
338  ::urdf::LinkConstSharedPtr link,
339  UrdfGeomVisitorBase& visitor,
340  GeometryModel & geomModel,
341  const std::vector<std::string> & package_dirs)
342  {
343 #ifndef PINOCCHIO_WITH_HPP_FCL
344  PINOCCHIO_UNUSED_VARIABLE(tree);
345  PINOCCHIO_UNUSED_VARIABLE(meshLoader);
346 #endif // PINOCCHIO_WITH_HPP_FCL
347 
348  typedef std::vector< PINOCCHIO_URDF_SHARED_PTR(GeometryType) > VectorSharedT;
349  typedef GeometryModel::SE3 SE3;
350 
351  if(getLinkGeometry<GeometryType>(link))
352  {
353  std::string meshPath = "";
354 
355  Eigen::Vector3d meshScale(Eigen::Vector3d::Ones());
356 
357  const std::string & link_name = link->name;
358 
359  VectorSharedT geometries_array = getLinkGeometryArray<GeometryType>(link);
360 
361  FrameIndex frame_id;
362  UrdfGeomVisitorBase::Frame frame = visitor.getBodyFrame (link_name, frame_id);
363  SE3 body_placement = frame.placement;
364 
365  std::size_t objectId = 0;
366  for (typename VectorSharedT::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i)
367  {
368  meshPath.clear();
369 #ifdef PINOCCHIO_WITH_HPP_FCL
370 
371 #ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
372  const std::string & geom_name = (*i)->group_name;
373 #else
374  const std::string & geom_name = (*i)->name;
375 #endif // PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
376  const GeometryObject::CollisionGeometryPtr geometry =
377  retrieveCollisionGeometry(tree, meshLoader, link_name, geom_name,
378  (*i)->geometry, package_dirs, meshPath, meshScale);
379 #else
380  ::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> ((*i)->geometry);
381  if (urdf_mesh)
382  {
383  meshPath = retrieveResourcePath(urdf_mesh->filename, package_dirs);
384  retrieveMeshScale(urdf_mesh, meshScale);
385  }
386 
387  const boost::shared_ptr<fcl::CollisionGeometry> geometry(new fcl::CollisionGeometry());
388 #endif // PINOCCHIO_WITH_HPP_FCL
389 
390  Eigen::Vector4d meshColor;
391  std::string meshTexturePath;
392  bool overrideMaterial = getVisualMaterial<GeometryType>((*i), meshTexturePath, meshColor, package_dirs);
393 
394  SE3 geomPlacement = body_placement * convertFromUrdf((*i)->origin);
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());
398  GeometryObject geometry_object(geometry_object_name,
399  frame_id, frame.parent,
400  geometry,
401  geomPlacement, meshPath, meshScale,
402  overrideMaterial, meshColor, meshTexturePath);
403  geomModel.addGeometryObject(geometry_object);
404  ++objectId;
405  }
406  }
407  }
408 
422  void recursiveParseTreeForGeom(const UrdfTree& tree,
423  ::hpp::fcl::MeshLoaderPtr& meshLoader,
424  ::urdf::LinkConstSharedPtr link,
425  UrdfGeomVisitorBase& visitor,
426  GeometryModel & geomModel,
427  const std::vector<std::string> & package_dirs,
428  const GeometryType type)
429  {
430 
431  switch(type)
432  {
433  case COLLISION:
434  addLinkGeometryToGeomModel< ::urdf::Collision >(tree, meshLoader, link, visitor, geomModel, package_dirs);
435  break;
436  case VISUAL:
437  addLinkGeometryToGeomModel< ::urdf::Visual >(tree, meshLoader, link, visitor, geomModel, package_dirs);
438  break;
439  default:
440  break;
441  }
442 
443  BOOST_FOREACH(::urdf::LinkConstSharedPtr child,link->child_links)
444  {
445  recursiveParseTreeForGeom(tree, meshLoader, child, visitor, geomModel, package_dirs,type);
446  }
447 
448  }
449 
450  void parseTreeForGeom(UrdfGeomVisitorBase& visitor,
451  const std::istream& xmlStream,
452  const GeometryType type,
453  GeometryModel & geomModel,
454  const std::vector<std::string> & package_dirs,
455  ::hpp::fcl::MeshLoaderPtr meshLoader)
456  {
457  std::string xmlStr;
458  {
459  std::ostringstream os;
460  os << xmlStream.rdbuf();
461  xmlStr = os.str();
462  }
463 
464  details::UrdfTree tree;
465  tree.parse (xmlStr);
466 
467  std::vector<std::string> hint_directories(package_dirs);
468 
469  // Append the ROS_PACKAGE_PATH
470  std::vector<std::string> ros_pkg_paths = rosPaths();
471  hint_directories.insert(hint_directories.end(), ros_pkg_paths.begin(), ros_pkg_paths.end());
472 
473 #ifdef PINOCCHIO_WITH_HPP_FCL
474  if (!meshLoader) meshLoader = fcl::MeshLoaderPtr(new fcl::MeshLoader);
475 #endif // ifdef PINOCCHIO_WITH_HPP_FCL
476 
477  recursiveParseTreeForGeom(tree, meshLoader, tree.urdf_->getRoot(),
478  visitor, geomModel, hint_directories,type);
479  }
480  } // namespace details
482  } // namespace urdf
483 } // namespace pinocchio
pinocchio::GeometryType
GeometryType
Definition: fcl.hpp:112
pinocchio::FrameIndex
Index FrameIndex
Definition: src/multibody/fwd.hpp:30
PINOCCHIO_UNUSED_VARIABLE
#define PINOCCHIO_UNUSED_VARIABLE(var)
Helper to declare that a parameter is unused.
Definition: src/macros.hpp:51
pinocchio::Frame
FrameTpl< double > Frame
Definition: src/multibody/fwd.hpp:33
pinocchio::name
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
PINOCCHIO_URDF_SHARED_PTR
#define PINOCCHIO_URDF_SHARED_PTR(type)
Definition: types.hpp:18
PINOCCHIO_EIGEN_CONST_CAST
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
Definition: eigen-macros.hpp:43
pinocchio::urdf::details::UrdfTree::LinkMap_t
std::map< std::string, const ptree & > LinkMap_t
Definition: src/parsers/urdf/geometry.cpp:26
pinocchio::urdf::details::retrieveMeshScale
static void retrieveMeshScale(const ::urdf::MeshSharedPtr &mesh, const Eigen::MatrixBase< Vector3 > &scale)
Definition: src/parsers/urdf/geometry.cpp:102
value_type
Scalar value_type
Definition: eigen_plugin.hpp:2
pinocchio::urdf::details::convertFromUrdf
static Inertia convertFromUrdf(const ::urdf::Inertial &Y)
Convert URDF Inertial quantity to Spatial Inertia.
Definition: src/parsers/urdf/model.cpp:30
pinocchio::urdf::details::UrdfTree::isCapsule
bool isCapsule(const std::string &linkName, const std::string &geomName) const
Definition: src/parsers/urdf/geometry.cpp:47
pinocchio::urdf::details::UrdfTree::urdf_
::urdf::ModelInterfaceSharedPtr urdf_
Definition: src/parsers/urdf/geometry.cpp:94
pinocchio::urdf::details::UrdfTree::parse
void parse(const std::string &xmlStr)
Definition: src/parsers/urdf/geometry.cpp:28
pinocchio::rosPaths
std::vector< std::string > rosPaths()
Parse the environment variable ROS_PACKAGE_PATH and extract paths.
Definition: file-explorer.cpp:45
urdf
Definition: types.hpp:29
pinocchio::urdf::details::UrdfTree::isMeshConvex
bool isMeshConvex(const std::string &linkName, const std::string &geomName) const
Definition: src/parsers/urdf/geometry.cpp:70
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:30
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 an URDF link to a GeometryModel, looking either for collisions or visu...
Definition: src/parsers/urdf/geometry.cpp:336
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: src/parsers/urdf/geometry.cpp:422
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::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 an url-format. Currently convert from the follow...
Definition: utils.hpp:61
continuous.x
x
— Training
Definition: continuous.py:157
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::SE3
SE3Tpl< double, 0 > SE3
Definition: src/spatial/fwd.hpp:53
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::urdf::details::parseTreeForGeom
void parseTreeForGeom(UrdfGeomVisitorBase &visitor, const std::istream &xmlStream, const GeometryType type, GeometryModel &geomModel, const std::vector< std::string > &package_dirs,::hpp::fcl::MeshLoaderPtr meshLoader)
Definition: src/parsers/urdf/geometry.cpp:450

pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:03