GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/parsers/urdf/geometry.cpp Lines: 146 163 89.6 %
Date: 2023-08-09 08:43:58 Branches: 197 395 49.9 %

Line Branch Exec Source
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
  {
20
    /// \internal
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
41
        void parse (const std::string & xmlStr)
29
        {
30
41
          urdf_ = ::urdf::parseURDF(xmlStr);
31
41
          if (!urdf_) {
32
            throw std::invalid_argument("Unable to parse URDF");
33
          }
34
35
82
          std::istringstream iss(xmlStr);
36
          using namespace boost::property_tree;
37
41
          read_xml(iss, tree_, xml_parser::no_comments);
38
39








8071
          BOOST_FOREACH(const ptree::value_type & link, tree_.get_child("robot")) {
40
4015
            if (link.first == "link") {
41

1570
              std::string name = link.second.get<std::string>("<xmlattr>.name");
42

1570
              links_.insert(std::pair<std::string,const ptree&>(name, link.second));
43
            }
44
          } // BOOST_FOREACH
45
41
        }
46
47
23
        bool isCapsule(const std::string & linkName,
48
                       const std::string & geomName) const
49
        {
50
23
          LinkMap_t::const_iterator _link = links_.find(linkName);
51
23
          assert (_link != links_.end());
52
23
          const ptree& link = _link->second;
53

23
          if (link.count ("collision_checking") == 0)
54
20
            return false;
55








3
          BOOST_FOREACH(const ptree::value_type & cc, link.get_child("collision_checking")) {
56
3
            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

3
              std::string name = cc.second.get<std::string>("<xmlattr>.name");
62
3
              if (geomName == name) return true;
63
#endif
64
            }
65
          } // BOOST_FOREACH
66
67
          return false;
68
        }
69
70
730
        bool isMeshConvex(const std::string & linkName,
71
                          const std::string & geomName) const
72
        {
73
730
          LinkMap_t::const_iterator _link = links_.find(linkName);
74
730
          assert (_link != links_.end());
75
730
          const ptree& link = _link->second;
76

730
          if (link.count ("collision_checking") == 0)
77
727
            return false;
78








9
          BOOST_FOREACH(const ptree::value_type & cc, link.get_child("collision_checking")) {
79
6
            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

3
              std::string name = cc.second.get<std::string>("<xmlattr>.name");
85
3
              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
730
      static void retrieveMeshScale(const ::urdf::MeshSharedPtr & mesh,
103
                                    const Eigen::MatrixBase<Vector3> & scale)
104
      {
105
730
        Vector3 & scale_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3,scale);
106
        scale_ <<
107
1460
        mesh->scale.x,
108
730
        mesh->scale.y,
109
730
        mesh->scale.z;
110
730
      }
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
119
      /**
120
       * @brief      Get a fcl::CollisionObject from a URDF geometry, searching
121
       *             for it in specified package directories
122
       *
123
       * @param[in]  urdf_geometry  A shared pointer on the input urdf Geometry
124
       * @param[in]  package_dirs   A vector containing the different directories where to search for packages
125
       * @param[out] meshPath      The Absolute path of the mesh currently read
126
       * @param[out] meshScale     Scale of transformation currently applied to the mesh
127
       *
128
       * @return     A shared pointer on the geometry converted as a fcl::CollisionGeometry
129
       */
130
      shared_ptr<fcl::CollisionGeometry>
131
763
      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
763
        shared_ptr<fcl::CollisionGeometry> geometry;
141
142
        // Handle the case where collision geometry is a mesh
143
763
        if (urdf_geometry->type == ::urdf::Geometry::MESH)
144
        {
145
1460
          const ::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> (urdf_geometry);
146
1460
          std::string collisionFilename = urdf_mesh->filename;
147
148
730
          meshPath = retrieveResourcePath(collisionFilename, package_dirs);
149
730
          if (meshPath == "") {
150
            std::stringstream ss;
151
            ss << "Mesh " << collisionFilename << " could not be found.";
152
            throw std::invalid_argument (ss.str());
153
          }
154
155
1460
          fcl::Vec3f scale = fcl::Vec3f(urdf_mesh->scale.x,
156
730
                                        urdf_mesh->scale.y,
157
730
                                        urdf_mesh->scale.z
158
730
                                        );
159
160
730
          retrieveMeshScale(urdf_mesh, meshScale);
161
162
          // Create FCL mesh by parsing Collada file.
163
#ifdef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
164
1460
          hpp::fcl::BVHModelPtr_t bvh = meshLoader->load (meshPath, scale);
165
730
          bool convex = tree.isMeshConvex (linkName, geomName);
166
730
          if (convex) {
167
3
            bvh->buildConvexRepresentation (false);
168
#if HPP_FCL_MAJOR_VERSION < 2
169
            geometry = bvh->convex;
170
#else
171
6
            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
1069
            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
33
        else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER)
188
        {
189
23
          const bool is_capsule = tree.isCapsule(linkName, geomName);
190

23
          meshScale << 1,1,1;
191
46
          const ::urdf::CylinderSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry);
192
193
23
          double radius = collisionGeometry->radius;
194
23
          double length = collisionGeometry->length;
195
196
          // Create fcl capsule geometry.
197
23
          if (is_capsule) {
198
3
            meshPath = "CAPSULE";
199

3
            geometry = shared_ptr < fcl::CollisionGeometry >(new fcl::Capsule (radius, length));
200
          } else {
201
20
            meshPath = "CYLINDER";
202

20
            geometry = shared_ptr < fcl::CollisionGeometry >(new fcl::Cylinder (radius, length));
203
          }
204
        }
205
        // Handle the case where collision geometry is a box.
206
10
        else if (urdf_geometry->type == ::urdf::Geometry::BOX)
207
        {
208
10
          meshPath = "BOX";
209

10
          meshScale << 1,1,1;
210
10
          const ::urdf::BoxSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Box> (urdf_geometry);
211
212
10
          double x = collisionGeometry->dim.x;
213
10
          double y = collisionGeometry->dim.y;
214
10
          double z = collisionGeometry->dim.z;
215
216

10
          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
763
        if (!geometry)
232
        {
233
          throw std::invalid_argument("The polyhedron retrieved is empty");
234
        }
235
236
763
        return geometry;
237
      }
238
#endif // PINOCCHIO_WITH_HPP_FCL
239
240
     /**
241
      * @brief Get the first geometry attached to a link
242
      *
243
      * @param[in] link   The URDF link
244
      *
245
      * @return Either the first collision or visual
246
      */
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
1237
      getLinkGeometry< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
254
      {
255
1237
        return link->collision;
256
      }
257
258
      template<>
259
      inline ::urdf::VisualConstSharedPtr
260
333
      getLinkGeometry< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
261
      {
262
333
        return link->visual;
263
      }
264
265
266
     /**
267
      * @brief Get the material values from the link visual object
268
      *
269
      * @param[in]  Visual/Collision The Visual or the Collision object.
270
      * @param[out] meshTexturePath  The absolute file path containing the texture description.
271
      * @param[out] meshColor        The mesh RGBA vector.
272
      * @param[in]  package_dirs     A vector containing the different directories where to search for packages
273
      *
274
      */
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
543
      inline bool getVisualMaterial< ::urdf::Collision>
281
      (const ::urdf::CollisionSharedPtr, std::string& meshTexturePath,
282
       Eigen::Vector4d & meshColor, const std::vector<std::string> &)
283
      {
284


543
        meshColor << 0.9, 0.9, 0.9, 1.;
285
543
        meshTexturePath = "";
286
543
        return false;
287
      }
288
289
      template<>
290
220
      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


220
        meshColor << 0.9, 0.9, 0.9, 1.;
295
220
        meshTexturePath = "";
296
220
        bool overrideMaterial = false;
297
220
        if(urdf_visual->material) {
298
167
          overrideMaterial = true;
299

167
          meshColor << urdf_visual->material->color.r, urdf_visual->material->color.g,
300

167
          urdf_visual->material->color.b, urdf_visual->material->color.a;
301
167
          if(urdf_visual->material->texture_filename!="")
302
            meshTexturePath = retrieveResourcePath((urdf_visual)->material->texture_filename, package_dirs);
303
        }
304
220
        return overrideMaterial;
305
      }
306
307
     /**
308
      * @brief Get the array of geometries attached to a link
309
      *
310
      * @param[in] link   The URDF link
311
      *
312
      * @return the array of either collisions or visuals
313
      */
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
540
      getLinkGeometryArray< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
321
      {
322
540
        return link->collision_array;
323
      }
324
325
      template<>
326
      inline const std::vector< ::urdf::VisualSharedPtr> &
327
220
      getLinkGeometryArray< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
328
      {
329
220
        return link->visual_array;
330
      }
331
332
      /**
333
       * @brief      Add the geometries attached to a URDF link to a GeometryModel, looking
334
       *             either for collisions or visuals
335
       *
336
       * @param[in]  tree           The URDF kinematic tree
337
       * @param[in]  meshLoader     The FCL mesh loader to avoid duplications of already loaded geometries
338
       * @param[in]  link            The current URDF link
339
       * @param      model           The model to which is the GeometryModel associated
340
       * @param      geomModel      The GeometryModel where the Collision Objects must be added
341
       * @param[in]  package_dirs    A vector containing the different directories where to search for packages
342
       *
343
       */
344
      template<typename GeometryType>
345
3140
      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
3140
        if(getLinkGeometry<GeometryType>(link))
361
        {
362
3040
          std::string meshPath = "";
363
364

1520
          Eigen::Vector3d meshScale(Eigen::Vector3d::Ones());
365
366
1520
          const std::string & link_name = link->name;
367
368
3040
          VectorSharedT geometries_array = getLinkGeometryArray<GeometryType>(link);
369
370
          FrameIndex frame_id;
371
3040
          UrdfGeomVisitorBase::Frame frame = visitor.getBodyFrame (link_name, frame_id);
372
1520
          const SE3 & body_placement = frame.placement;
373
374
1520
          std::size_t objectId = 0;
375
3046
          for (typename VectorSharedT::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i)
376
          {
377
1526
            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
1526
            const std::string & geom_name = (*i)->name;
384
#endif // PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
385
3052
            const GeometryObject::CollisionGeometryPtr geometry =
386
            retrieveCollisionGeometry(tree, meshLoader, link_name, geom_name,
387
1526
                                      (*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
1526
            Eigen::Vector4d meshColor;
400
3052
            std::string meshTexturePath;
401
1526
            bool overrideMaterial = getVisualMaterial<GeometryType>((*i), meshTexturePath, meshColor, package_dirs);
402
403

1526
            const SE3 geomPlacement = body_placement * convertFromUrdf((*i)->origin);
404
3052
            std::ostringstream geometry_object_suffix;
405

1526
            geometry_object_suffix << "_" << objectId;
406

3052
            const std::string & geometry_object_name = std::string(link_name + geometry_object_suffix.str());
407
1526
            GeometryObject geometry_object(geometry_object_name,
408
                                           frame_id, frame.parent,
409
                                           geometry,
410
                                           geomPlacement, meshPath, meshScale,
411
                                           overrideMaterial, meshColor, meshTexturePath);
412
1526
            geomModel.addGeometryObject(geometry_object);
413
1526
            ++objectId;
414
          }
415
        }
416
3140
      }
417
418
      /**
419
       * @brief      Recursive procedure for reading the URDF tree, looking for geometries
420
       *             This function fill the geometric model with geometry objects retrieved from the URDF tree
421
       *
422
       * @param[in]  tree           The URDF kinematic tree
423
       * @param[in]  meshLoader     The FCL mesh loader to avoid duplications of already loaded geometries
424
       * @param[in]  link           The current URDF link
425
       * @param      model          The model to which is the GeometryModel associated
426
       * @param      geomModel      The GeometryModel where the Collision Objects must be added
427
       * @param[in]  package_dirs   A vector containing the different directories where to search for packages
428
       * @param[in]  type           The type of objects that must be loaded ( can be VISUAL or COLLISION)
429
       *
430
       */
431
1570
      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
1570
        switch(type)
441
        {
442
1237
          case COLLISION:
443
1237
            addLinkGeometryToGeomModel< ::urdf::Collision >(tree, meshLoader, link, visitor, geomModel, package_dirs);
444
1237
            break;
445
333
          case VISUAL:
446
333
            addLinkGeometryToGeomModel< ::urdf::Visual >(tree, meshLoader, link, visitor, geomModel, package_dirs);
447
333
            break;
448
          default:
449
            break;
450
        }
451
452







4628
        BOOST_FOREACH(::urdf::LinkConstSharedPtr child,link->child_links)
453
        {
454
1529
          recursiveParseTreeForGeom(tree, meshLoader, child, visitor, geomModel, package_dirs,type);
455
        }
456
457
1570
      }
458
459
41
      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
82
        std::string xmlStr;
467
        {
468
41
          std::ostringstream os;
469

41
          os << xmlStream.rdbuf();
470
41
          xmlStr = os.str();
471
        }
472
473
82
        details::UrdfTree tree;
474
41
        tree.parse (xmlStr);
475
476
82
        std::vector<std::string> hint_directories(package_dirs);
477
478
        // Append the ROS_PACKAGE_PATH
479
41
        std::vector<std::string> ros_pkg_paths = rosPaths();
480
41
        hint_directories.insert(hint_directories.end(), ros_pkg_paths.begin(), ros_pkg_paths.end());
481
482
#ifdef PINOCCHIO_WITH_HPP_FCL
483

41
        if (!meshLoader) meshLoader = fcl::MeshLoaderPtr(new fcl::MeshLoader);
484
#endif // ifdef PINOCCHIO_WITH_HPP_FCL
485
486
41
        recursiveParseTreeForGeom(tree, meshLoader, tree.urdf_->getRoot(),
487
            visitor, geomModel, hint_directories,type);
488
41
      }
489
    } // namespace details
490
    /// \endinternal
491
  } // namespace urdf
492
} // namespace pinocchio