GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/multibody/fcl.hpp Lines: 48 49 98.0 %
Date: 2023-08-09 08:43:58 Branches: 14 28 50.0 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2015-2023 CNRS INRIA
3
//
4
5
#ifndef __pinocchio_multibody_fcl_hpp__
6
#define __pinocchio_multibody_fcl_hpp__
7
8
#include "pinocchio/spatial/se3.hpp"
9
#include "pinocchio/multibody/fwd.hpp"
10
#include "pinocchio/container/aligned-vector.hpp"
11
12
#ifdef PINOCCHIO_WITH_HPP_FCL
13
14
  #if(WIN32)
15
    // It appears that std::snprintf is missing for Windows.
16
    #if !(( defined(_MSC_VER) && _MSC_VER < 1900 ) || ( defined(__MINGW32__) && !defined(__MINGW64_VERSION_MAJOR) ))
17
      #include <cstdio>
18
      #include <stdarg.h>
19
      namespace std
20
      {
21
        inline int _snprintf(char* buffer, std::size_t buf_size, const char* format, ...)
22
        {
23
          int res;
24
25
          va_list args;
26
          va_start(args, format);
27
          res = vsnprintf(buffer, buf_size, format, args);
28
          va_end(args);
29
30
          return res;
31
        }
32
      }
33
    #endif
34
  #endif
35
36
  #include <hpp/fcl/collision_object.h>
37
  #include <hpp/fcl/collision.h>
38
  #include <hpp/fcl/distance.h>
39
  #include <hpp/fcl/shape/geometric_shapes.h>
40
  #include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
41
#endif
42
43
#include <iostream>
44
#include <map>
45
#include <vector>
46
#include <utility>
47
#include <limits>
48
#include <assert.h>
49
50
#ifdef PINOCCHIO_WITH_HPP_FCL
51
#if HPP_FCL_VERSION_AT_LEAST(2,0,0)
52
#define PINOCCHIO_HPPFCL_USE_STD_SHARED_PTR
53
#endif
54
#endif
55
56
#ifdef PINOCCHIO_HPPFCL_USE_STD_SHARED_PTR
57
#include <memory>
58
#else
59
#include <boost/shared_ptr.hpp>
60
#include <boost/make_shared.hpp>
61
#endif
62
63
#include <boost/foreach.hpp>
64
65
namespace pinocchio
66
{
67
#ifdef PINOCCHIO_HPPFCL_USE_STD_SHARED_PTR
68
  using std::shared_ptr;
69
  using std::make_shared;
70
#else
71
  using boost::shared_ptr;
72
  using boost::make_shared;
73
#endif
74
  struct CollisionPair
75
  : public std::pair<GeomIndex, GeomIndex>
76
  {
77
78
    typedef std::pair<GeomIndex, GeomIndex> Base;
79
80
    /// \brief Empty constructor
81
    CollisionPair();
82
83
    ///
84
    /// \brief Default constructor of a collision pair from two collision object indexes.
85
    /// \remarks The two indexes must be different, otherwise the constructor throws.
86
    ///
87
    /// \param[in] co1 Index of the first collision object.
88
    /// \param[in] co2 Index of the second collision object.
89
    ///
90
    CollisionPair(const GeomIndex co1, const GeomIndex co2);
91
    bool                  operator == (const CollisionPair& rhs) const;
92
    bool                  operator != (const CollisionPair& rhs) const;
93
    void                  disp        (std::ostream & os)        const;
94
    friend std::ostream & operator << (std::ostream & os,const CollisionPair & X);
95
96
  }; // struct CollisionPair
97
98
#ifndef PINOCCHIO_WITH_HPP_FCL
99
100
  namespace fcl
101
  {
102
103
    struct FakeCollisionGeometry
104
    {
105
      FakeCollisionGeometry(){};
106
107
      bool operator==(const FakeCollisionGeometry &) const
108
      {
109
        return true;
110
      }
111
    };
112
113
    struct AABB
114
    {
115
      AABB(): min_(0), max_(1){};
116
117
      int min_;
118
      int max_;
119
    };
120
121
    typedef FakeCollisionGeometry CollisionGeometry;
122
123
  }
124
125
#else
126
127
  namespace fcl = hpp::fcl;
128
129
#endif // PINOCCHIO_WITH_HPP_FCL
130
131
enum GeometryType
132
{
133
  VISUAL,
134
  COLLISION
135
};
136
137
struct GeometryObject
138
{
139
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
140
141
  typedef shared_ptr<fcl::CollisionGeometry> CollisionGeometryPtr;
142
143
  /// \brief Name of the geometry object
144
  std::string name;
145
146
  /// \brief Index of the parent frame
147
  ///
148
  /// Parent frame may be unset (to the std::numeric_limits<FrameIndex>::max() value) as it is mostly used as a documentation of the tree, or in third-party libraries.
149
  /// The URDF parser of Pinocchio is setting it to the proper value according to the urdf link-joint tree.
150
  /// In particular, anchor joints of URDF would cause parent frame to be different to joint frame.
151
  FrameIndex parentFrame;
152
153
  /// \brief Index of the parent joint
154
  JointIndex parentJoint;
155
156
  /// \brief The FCL CollisionGeometry (might be a Mesh, a Geometry Primitive, etc.)
157
  CollisionGeometryPtr geometry;
158
159
  /// \brief The former pointer to the FCL CollisionGeometry.
160
  /// \deprecated It is now deprecated and has been renamed GeometryObject::geometry
161
  PINOCCHIO_DEPRECATED CollisionGeometryPtr & fcl;
162
163
  /// \brief Position of geometry object in parent joint frame
164
  SE3 placement;
165
166
  /// \brief Absolute path to the mesh file (if the fcl pointee is also a Mesh)
167
  std::string meshPath;
168
169
  /// \brief Scaling vector applied to the GeometryObject::fcl object.
170
  Eigen::Vector3d meshScale;
171
172
  /// \brief Decide whether to override the Material.
173
  bool overrideMaterial;
174
175
  /// \brief RGBA color value of the GeometryObject::fcl object.
176
  Eigen::Vector4d meshColor;
177
178
  /// \brief Absolute path to the mesh texture file.
179
  std::string meshTexturePath;
180
181
  /// \brief If true, no collision or distance check will be done between the Geometry and any other geometry
182
  bool disableCollision;
183
184
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
185
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
186
  ///
187
  /// \brief Full constructor.
188
  ///
189
  /// \param[in] name  Name of the geometry object.
190
  /// \param[in] parent_frame  Index of the parent frame.
191
  /// \param[in] parent_joint  Index of the parent joint (that supports the geometry).
192
  /// \param[in] collision_geometry The FCL collision geometry object.
193
  /// \param[in] placement Placement of the geometry with respect to the joint frame.
194
  /// \param[in] meshPath Path of the mesh (may be needed extarnally to load the mesh inside a viewer for instance) [if applicable].
195
  /// \param[in] meshScale Scale of the mesh [if applicable].
196
  /// \param[in] overrideMaterial If true, this option allows to overrite the material [if applicable].
197
  /// \param[in] meshColor Color of the mesh [if applicable].
198
  /// \param[in] meshTexturePath Path to the file containing the texture information [if applicable].
199
  ///
200
918
  GeometryObject(const std::string & name,
201
                 const FrameIndex parent_frame,
202
                 const JointIndex parent_joint,
203
                 const CollisionGeometryPtr & collision_geometry,
204
                 const SE3 & placement,
205
                 const std::string & meshPath = "",
206
                 const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(),
207
                 const bool overrideMaterial = false,
208
                 const Eigen::Vector4d & meshColor = Eigen::Vector4d(0,0,0,1),
209
                 const std::string & meshTexturePath = "")
210
918
  : name(name)
211
  , parentFrame(parent_frame)
212
  , parentJoint(parent_joint)
213
  , geometry(collision_geometry)
214
918
  , fcl(geometry)
215
  , placement(placement)
216
  , meshPath(meshPath)
217
  , meshScale(meshScale)
218
  , overrideMaterial(overrideMaterial)
219
  , meshColor(meshColor)
220
  , meshTexturePath(meshTexturePath)
221


918
  , disableCollision(false)
222
918
  {}
223
PINOCCHIO_COMPILER_DIAGNOSTIC_POP
224
225
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
226
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
227
  ///
228
  /// \brief Reduced constructor.
229
  /// \remarks Compared to the other constructor, this one assumes that there is no parentFrame associated to the geometry.
230
  ///
231
  /// \param[in] name  Name of the geometry object.
232
  /// \param[in] parent_joint  Index of the parent joint (that supports the geometry).
233
  /// \param[in] collision_geometry The FCL collision geometry object.
234
  /// \param[in] placement Placement of the geometry with respect to the joint frame.
235
  /// \param[in] meshPath Path of the mesh (may be needed extarnally to load the mesh inside a viewer for instance) [if applicable].
236
  /// \param[in] meshScale Scale of the mesh [if applicable].
237
  /// \param[in] overrideMaterial If true, this option allows to overrite the material [if applicable].
238
  /// \param[in] meshColor Color of the mesh [if applicable].
239
  /// \param[in] meshTexturePath Path to the file containing the texture information [if applicable].
240
  ///
241
7
  GeometryObject(const std::string & name,
242
                 const JointIndex parent_joint,
243
                 const CollisionGeometryPtr & collision_geometry,
244
                 const SE3 & placement,
245
                 const std::string & meshPath = "",
246
                 const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(),
247
                 const bool overrideMaterial = false,
248
                 const Eigen::Vector4d & meshColor = Eigen::Vector4d::Ones(),
249
                 const std::string & meshTexturePath = "")
250
7
  : name(name)
251
7
  , parentFrame(std::numeric_limits<FrameIndex>::max())
252
  , parentJoint(parent_joint)
253
  , geometry(collision_geometry)
254
7
  , fcl(geometry)
255
  , placement(placement)
256
  , meshPath(meshPath)
257
  , meshScale(meshScale)
258
  , overrideMaterial(overrideMaterial)
259
  , meshColor(meshColor)
260
  , meshTexturePath(meshTexturePath)
261


14
  , disableCollision(false)
262
7
  {}
263
PINOCCHIO_COMPILER_DIAGNOSTIC_POP
264
265
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
266
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
267
5366
  GeometryObject(const GeometryObject & other)
268

5366
  : fcl(geometry)
269
  {
270
5366
    *this = other;
271
272
5366
  }
273
PINOCCHIO_COMPILER_DIAGNOSTIC_POP
274
275
5367
  GeometryObject & operator=(const GeometryObject & other)
276
  {
277
5367
    name                = other.name;
278
5367
    parentFrame         = other.parentFrame;
279
5367
    parentJoint         = other.parentJoint;
280
5367
    geometry            = other.geometry;
281
5367
    placement           = other.placement;
282
5367
    meshPath            = other.meshPath;
283
5367
    meshScale           = other.meshScale;
284
5367
    overrideMaterial    = other.overrideMaterial;
285
5367
    meshColor           = other.meshColor;
286
5367
    meshTexturePath     = other.meshTexturePath;
287
5367
    disableCollision   = other.disableCollision;
288
5367
    return *this;
289
  }
290
291
  friend std::ostream & operator<< (std::ostream & os, const GeometryObject & geomObject);
292
};
293
294
#ifdef PINOCCHIO_WITH_HPP_FCL
295
296
  struct ComputeCollision
297
  : ::hpp::fcl::ComputeCollision
298
  {
299
    typedef ::hpp::fcl::ComputeCollision Base;
300
    typedef shared_ptr<const fcl::CollisionGeometry> ConstCollisionGeometryPtr;
301
302
3170
    ComputeCollision(const GeometryObject & o1, const GeometryObject & o2)
303
3170
    : Base(o1.geometry.get(),o2.geometry.get())
304
3170
    , o1(o1.geometry)
305
6340
    , o2(o2.geometry)
306
3170
    {}
307
308
12738
    virtual ~ComputeCollision() {};
309
310
  protected:
311
    ConstCollisionGeometryPtr o1;
312
    ConstCollisionGeometryPtr o2;
313
314
2131
    virtual std::size_t run(const fcl::Transform3f& tf1, const fcl::Transform3f& tf2,
315
                            const fcl::CollisionRequest& request, fcl::CollisionResult& result) const
316
    {
317
      typedef ::hpp::fcl::CollisionGeometry const * Pointer;
318
2131
      const_cast<Pointer&>(Base::o1) = o1.get();
319
2131
      const_cast<Pointer&>(Base::o2) = o2.get();
320
2131
      return Base::run(tf1, tf2, request, result);
321
    }
322
  };
323
324
  struct ComputeDistance
325
  : ::hpp::fcl::ComputeDistance
326
  {
327
    typedef ::hpp::fcl::ComputeDistance Base;
328
    typedef shared_ptr<fcl::CollisionGeometry> ConstCollisionGeometryPtr;
329
330
3170
    ComputeDistance(const GeometryObject & o1, const GeometryObject & o2)
331
3170
    : Base(o1.geometry.get(),o2.geometry.get())
332
3170
    , o1(o1.geometry)
333
6340
    , o2(o2.geometry)
334
3170
    {}
335
336
12738
    virtual ~ComputeDistance() {};
337
338
  protected:
339
    ConstCollisionGeometryPtr o1;
340
    ConstCollisionGeometryPtr o2;
341
342
141
    virtual hpp::fcl::FCL_REAL run(const fcl::Transform3f& tf1, const fcl::Transform3f& tf2,
343
                                   const fcl::DistanceRequest& request, fcl::DistanceResult& result) const
344
    {
345
      typedef ::hpp::fcl::CollisionGeometry const * Pointer;
346
141
      const_cast<Pointer&>(Base::o1) = o1.get();
347
141
      const_cast<Pointer&>(Base::o2) = o2.get();
348
141
      return Base::run(tf1, tf2, request, result);
349
    }
350
  };
351
352
#endif
353
354
} // namespace pinocchio
355
356
/* --- Details -------------------------------------------------------------- */
357
/* --- Details -------------------------------------------------------------- */
358
/* --- Details -------------------------------------------------------------- */
359
#include "pinocchio/multibody/fcl.hxx"
360
361
#endif // ifndef __pinocchio_multibody_fcl_hpp__