GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/geometry-object.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 42 115 36.5%
Branches: 15 106 14.2%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2023 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_multibody_geometry_object_hpp__
6 #define __pinocchio_multibody_geometry_object_hpp__
7
8 #include "pinocchio/utils/shared-ptr.hpp"
9 #include "pinocchio/spatial/se3.hpp"
10 #include "pinocchio/multibody/fwd.hpp"
11 #include "pinocchio/multibody/model-item.hpp"
12 #include "pinocchio/multibody/fcl.hpp"
13 #include "pinocchio/serialization/serializable.hpp"
14
15 /// Be carefull to include this header after fwd.hpp.
16 /// fwd.hpp contains some define to change the boost::variant max size.
17 /// If we don't include it before, default size is choosed that can
18 /// make all the build fail.
19 #include <boost/variant.hpp>
20
21 namespace pinocchio
22 {
23
24 enum GeometryType
25 {
26 VISUAL,
27 COLLISION
28 };
29
30 /// No material associated to a geometry.
31 struct GeometryNoMaterial
32 {
33 ✗ bool operator==(const GeometryNoMaterial &) const
34 {
35 ✗ return true;
36 }
37 };
38
39 /// Mesh material based on the Phong lighting model.
40 /// Diffuse color is stored in \p GeometryObject::meshColor.
41 struct GeometryPhongMaterial
42 {
43 60 GeometryPhongMaterial() = default;
44 ✗ GeometryPhongMaterial(
45 const Eigen::Vector4d & meshEmissionColor,
46 const Eigen::Vector4d & meshSpecularColor,
47 double meshShininess)
48 ✗ : meshEmissionColor(meshEmissionColor)
49 ✗ , meshSpecularColor(meshSpecularColor)
50 ✗ , meshShininess(meshShininess)
51 {
52 }
53
54 ✗ bool operator==(const GeometryPhongMaterial & other) const
55 {
56 ✗ return meshEmissionColor == other.meshEmissionColor
57 ✗ && meshSpecularColor == other.meshSpecularColor
58 ✗ && meshShininess == other.meshShininess;
59 }
60
61 /// \brief RGBA emission (ambient) color value of the GeometryObject::geometry object.
62 20 Eigen::Vector4d meshEmissionColor{Eigen::Vector4d(0., 0., 0., 1.)};
63
64 /// \brief RGBA specular color value of the GeometryObject::geometry object.
65 20 Eigen::Vector4d meshSpecularColor{Eigen::Vector4d(0., 0., 0., 1.)};
66
67 /// \brief Shininess associated to the specular lighting model.
68 ///
69 /// This value must normalized between 0 and 1.
70 double meshShininess{0.};
71 };
72
73 typedef boost::variant<GeometryNoMaterial, GeometryPhongMaterial> GeometryMaterial;
74
75 struct GeometryObject; // fwd
76
77 template<>
78 struct traits<GeometryObject>
79 {
80 typedef context::Scalar Scalar;
81 enum
82 {
83 Options = context::Options
84 };
85 };
86
87 struct GeometryObject
88 : public ModelItem<GeometryObject>
89 , serialization::Serializable<GeometryObject>
90 {
91 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
92
93 typedef ModelItem<GeometryObject> Base;
94 typedef typename traits<GeometryObject>::Scalar Scalar;
95 enum
96 {
97 Options = traits<GeometryObject>::Options
98 };
99
100 typedef SE3Tpl<Scalar, Options> SE3;
101
102 typedef std::shared_ptr<fcl::CollisionGeometry> CollisionGeometryPtr;
103
104 using Base::name;
105 using Base::parentFrame;
106 using Base::parentJoint;
107 using Base::placement;
108
109 /// \brief The FCL CollisionGeometry (might be a Mesh, a Geometry Primitive, etc.)
110 CollisionGeometryPtr geometry;
111
112 /// \brief Absolute path to the mesh file (if the geometry pointee is also a Mesh)
113 std::string meshPath;
114
115 /// \brief Scaling vector applied to the GeometryObject::geometry object.
116 Eigen::Vector3d meshScale;
117
118 /// \brief Decide whether to override the Material.
119 bool overrideMaterial;
120
121 /// \brief RGBA color value of the GeometryObject::geometry object.
122 Eigen::Vector4d meshColor;
123
124 /// \brief Material associated to the mesh.
125 /// This material should be used only if overrideMaterial is set to true.
126 /// In other case, the mesh default material must be used.
127 GeometryMaterial meshMaterial;
128
129 /// \brief Absolute path to the mesh texture file.
130 std::string meshTexturePath;
131
132 /// \brief If true, no collision or distance check will be done between the Geometry and any
133 /// other geometry
134 bool disableCollision;
135
136 ///
137 /// \brief Full constructor.
138 ///
139 /// \param[in] name Name of the geometry object.
140 /// \param[in] parent_joint Index of the parent joint (that supports the geometry).
141 /// \param[in] parent_frame Index of the parent frame.
142 /// \param[in] placement Placement of the geometry with respect to the joint frame.
143 /// \param[in] collision_geometry The FCL collision geometry object.
144 /// \param[in] meshPath Path of the mesh (may be needed extarnally to load the mesh inside a
145 /// viewer for instance) [if applicable]. \param[in] meshScale Scale of the mesh [if
146 /// applicable]. \param[in] overrideMaterial If true, this option allows to overrite the
147 /// material [if applicable]. \param[in] meshColor Color of the mesh [if applicable]. \param[in]
148 /// meshTexturePath Path to the file containing the texture information [if applicable].
149 /// \param[in] meshMaterial Material of the mesh [if applicable].
150 ///
151 890 GeometryObject(
152 const std::string & name,
153 const JointIndex parent_joint,
154 const FrameIndex parent_frame,
155 const SE3 & placement,
156 const CollisionGeometryPtr & collision_geometry,
157 const std::string & meshPath = "",
158 const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(),
159 const bool overrideMaterial = false,
160 const Eigen::Vector4d & meshColor = Eigen::Vector4d(0, 0, 0, 1),
161 const std::string & meshTexturePath = "",
162 const GeometryMaterial & meshMaterial = GeometryNoMaterial())
163 890 : Base(name, parent_joint, parent_frame, placement)
164 890 , geometry(collision_geometry)
165
1/2
✓ Branch 1 taken 890 times.
✗ Branch 2 not taken.
 890 , meshPath(meshPath)
166
1/2
✓ Branch 1 taken 890 times.
✗ Branch 2 not taken.
 890 , meshScale(meshScale)
167 890 , overrideMaterial(overrideMaterial)
168
1/2
✓ Branch 1 taken 890 times.
✗ Branch 2 not taken.
 890 , meshColor(meshColor)
169
1/2
✓ Branch 1 taken 890 times.
✗ Branch 2 not taken.
 890 , meshMaterial(meshMaterial)
170
1/2
✓ Branch 1 taken 890 times.
✗ Branch 2 not taken.
 890 , meshTexturePath(meshTexturePath)
171 890 , disableCollision(false)
172 {
173 890 }
174
175 ///
176 /// \brief Reduced constructor.
177 /// \remarks Compared to the other constructor, this one assumes that there is no parentFrame
178 /// associated to the geometry.
179 ///
180 /// \param[in] name Name of the geometry object.
181 /// \param[in] parent_joint Index of the parent joint (that supports the geometry).
182 /// \param[in] placement Placement of the geometry with respect to the joint frame.
183 /// \param[in] collision_geometry The FCL collision geometry object.
184 /// \param[in] meshPath Path of the mesh (may be needed extarnally to load the mesh inside a
185 /// viewer for instance) [if applicable]. \param[in] meshScale Scale of the mesh [if
186 /// applicable]. \param[in] overrideMaterial If true, this option allows to overrite the
187 /// material [if applicable]. \param[in] meshColor Color of the mesh [if applicable]. \param[in]
188 /// meshTexturePath Path to the file containing the texture information [if applicable].
189 /// \param[in] meshMaterial Material of the mesh [if applicable].
190 ///
191 7 GeometryObject(
192 const std::string & name,
193 const JointIndex parent_joint,
194 const SE3 & placement,
195 const CollisionGeometryPtr & collision_geometry,
196 const std::string & meshPath = "",
197 const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(),
198 const bool overrideMaterial = false,
199 const Eigen::Vector4d & meshColor = Eigen::Vector4d(0, 0, 0, 1),
200 const std::string & meshTexturePath = "",
201 const GeometryMaterial & meshMaterial = GeometryNoMaterial())
202 7 : Base(name, parent_joint, std::numeric_limits<FrameIndex>::max(), placement)
203 7 , geometry(collision_geometry)
204
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 , meshPath(meshPath)
205
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 , meshScale(meshScale)
206 7 , overrideMaterial(overrideMaterial)
207
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 , meshColor(meshColor)
208
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 , meshMaterial(meshMaterial)
209
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 , meshTexturePath(meshTexturePath)
210 7 , disableCollision(false)
211 {
212 7 }
213
214 ///
215 /// \brief Full constructor.
216 ///
217 /// \param[in] name Name of the geometry object.
218 /// \param[in] parent_frame Index of the parent frame.
219 /// \param[in] parent_joint Index of the parent joint (that supports the geometry).
220 /// \param[in] collision_geometry The FCL collision geometry object.
221 /// \param[in] placement Placement of the geometry with respect to the joint frame.
222 /// \param[in] meshPath Path of the mesh (may be needed extarnally to load the mesh inside a
223 /// viewer for instance) [if applicable]. \param[in] meshScale Scale of the mesh [if
224 /// applicable]. \param[in] overrideMaterial If true, this option allows to overrite the
225 /// material [if applicable]. \param[in] meshColor Color of the mesh [if applicable]. \param[in]
226 /// meshTexturePath Path to the file containing the texture information [if applicable].
227 /// \param[in] meshMaterial Material of the mesh [if applicable].
228 ///
229 /// \deprecated This constructor is now deprecated, and its argument order has been changed.
230 ///
231 ✗ PINOCCHIO_DEPRECATED GeometryObject(
232 const std::string & name,
233 const FrameIndex parent_frame,
234 const JointIndex parent_joint,
235 const CollisionGeometryPtr & collision_geometry,
236 const SE3 & placement,
237 const std::string & meshPath = "",
238 const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(),
239 const bool overrideMaterial = false,
240 const Eigen::Vector4d & meshColor = Eigen::Vector4d(0, 0, 0, 1),
241 const std::string & meshTexturePath = "",
242 const GeometryMaterial & meshMaterial = GeometryNoMaterial())
243 ✗ : Base(name, parent_joint, parent_frame, placement)
244 ✗ , geometry(collision_geometry)
245 ✗ , meshPath(meshPath)
246 ✗ , meshScale(meshScale)
247 ✗ , overrideMaterial(overrideMaterial)
248 ✗ , meshColor(meshColor)
249 ✗ , meshMaterial(meshMaterial)
250 ✗ , meshTexturePath(meshTexturePath)
251 ✗ , disableCollision(false)
252 {
253 }
254
255 ///
256 /// \brief Reduced constructor.
257 /// \remarks Compared to the other constructor, this one assumes that there is no parentFrame
258 /// associated to the geometry.
259 ///
260 /// \param[in] name Name of the geometry object.
261 /// \param[in] parent_joint Index of the parent joint (that supports the geometry).
262 /// \param[in] collision_geometry The FCL collision geometry object.
263 /// \param[in] placement Placement of the geometry with respect to the joint frame.
264 /// \param[in] meshPath Path of the mesh (may be needed extarnally to load the mesh inside a
265 /// viewer for instance) [if applicable]. \param[in] meshScale Scale of the mesh [if
266 /// applicable]. \param[in] overrideMaterial If true, this option allows to overrite the
267 /// material [if applicable]. \param[in] meshColor Color of the mesh [if applicable]. \param[in]
268 /// meshTexturePath Path to the file containing the texture information [if applicable].
269 /// \param[in] meshMaterial Material of the mesh [if applicable].
270 ///
271 /// \deprecated This constructor is now deprecated, and its argument order has been changed.
272 ///
273 ✗ PINOCCHIO_DEPRECATED GeometryObject(
274 const std::string & name,
275 const JointIndex parent_joint,
276 const CollisionGeometryPtr & collision_geometry,
277 const SE3 & placement,
278 const std::string & meshPath = "",
279 const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(),
280 const bool overrideMaterial = false,
281 const Eigen::Vector4d & meshColor = Eigen::Vector4d(0, 0, 0, 1),
282 const std::string & meshTexturePath = "",
283 const GeometryMaterial & meshMaterial = GeometryNoMaterial())
284 ✗ : Base(name, parent_joint, std::numeric_limits<FrameIndex>::max(), placement)
285 ✗ , geometry(collision_geometry)
286 ✗ , meshPath(meshPath)
287 ✗ , meshScale(meshScale)
288 ✗ , overrideMaterial(overrideMaterial)
289 ✗ , meshColor(meshColor)
290 ✗ , meshMaterial(meshMaterial)
291 ✗ , meshTexturePath(meshTexturePath)
292 ✗ , disableCollision(false)
293 {
294 }
295
296
5/10
✓ Branch 3 taken 1963 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1963 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1963 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1963 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1963 times.
✗ Branch 16 not taken.
 1963 GeometryObject(const GeometryObject & other) = default;
297 ✗ GeometryObject & operator=(const GeometryObject & other) = default;
298
299 ///
300 /// \brief Perform a deep copy of this. It will create a copy of the underlying FCL geometry.
301 ///
302 ✗ GeometryObject clone() const
303 {
304 ✗ GeometryObject res(*this);
305
306 #ifdef PINOCCHIO_WITH_HPP_FCL
307 ✗ if (geometry)
308 ✗ res.geometry = CollisionGeometryPtr(geometry->clone());
309 #endif
310
311 ✗ return res;
312 }
313
314 ✗ bool operator==(const GeometryObject & other) const
315 {
316 ✗ if (this == &other)
317 ✗ return true;
318 ✗ return name == other.name && parentFrame == other.parentFrame
319 ✗ && parentJoint == other.parentJoint && placement == other.placement
320 ✗ && meshPath == other.meshPath && meshScale == other.meshScale
321 ✗ && overrideMaterial == other.overrideMaterial && meshColor == other.meshColor
322 ✗ && meshMaterial == other.meshMaterial && meshTexturePath == other.meshTexturePath
323 ✗ && disableCollision == other.disableCollision
324 ✗ && compare_shared_ptr(geometry, other.geometry);
325 }
326
327 ✗ bool operator!=(const GeometryObject & other) const
328 {
329 ✗ return !(*this == other);
330 }
331
332 friend std::ostream & operator<<(std::ostream & os, const GeometryObject & geomObject);
333 };
334
335 #ifdef PINOCCHIO_WITH_HPP_FCL
336
337 struct CollisionObject : ::hpp::fcl::CollisionObject
338 {
339 typedef ::hpp::fcl::CollisionObject Base;
340 typedef SE3Tpl<double> SE3;
341
342 CollisionObject()
343 : Base(nullptr, false)
344 , geometryObjectIndex((std::numeric_limits<size_t>::max)())
345 {
346 }
347
348 ✗ explicit CollisionObject(
349 const std::shared_ptr<::hpp::fcl::CollisionGeometry> & collision_geometry,
350 const size_t geometryObjectIndex = (std::numeric_limits<size_t>::max)(),
351 bool compute_local_aabb = true)
352 ✗ : Base(collision_geometry, compute_local_aabb)
353 ✗ , geometryObjectIndex(geometryObjectIndex)
354 {
355 }
356
357 ✗ CollisionObject(
358 const std::shared_ptr<::hpp::fcl::CollisionGeometry> & collision_geometry,
359 const SE3 & transform,
360 const size_t geometryObjectIndex = (std::numeric_limits<size_t>::max)(),
361 bool compute_local_aabb = true)
362 ✗ : Base(collision_geometry, toFclTransform3f(transform), compute_local_aabb)
363 ✗ , geometryObjectIndex(geometryObjectIndex)
364 {
365 }
366
367 ✗ bool operator==(const CollisionObject & other) const
368 {
369 ✗ return Base::operator==(other) && geometryObjectIndex == other.geometryObjectIndex;
370 }
371
372 bool operator!=(const CollisionObject & other) const
373 {
374 return !(*this == other);
375 }
376
377 /// @brief Geometry object index related to the current collision object.
378 size_t geometryObjectIndex;
379 };
380
381 struct ComputeCollision : ::hpp::fcl::ComputeCollision
382 {
383 typedef ::hpp::fcl::ComputeCollision Base;
384
385 70 ComputeCollision(const GeometryObject & go1, const GeometryObject & go2)
386 70 : Base(go1.geometry.get(), go2.geometry.get())
387 70 , go1_ptr(&go1)
388 140 , go2_ptr(&go2)
389 {
390 70 }
391
392 280 virtual ~ComputeCollision() {};
393
394 71 virtual std::size_t run(
395 const fcl::Transform3f & tf1,
396 const fcl::Transform3f & tf2,
397 const fcl::CollisionRequest & request,
398 fcl::CollisionResult & result) const
399 {
400 typedef ::hpp::fcl::CollisionGeometry const * Pointer;
401 71 const_cast<Pointer &>(Base::o1) = go1_ptr->geometry.get();
402 71 const_cast<Pointer &>(Base::o2) = go2_ptr->geometry.get();
403 71 return Base::run(tf1, tf2, request, result);
404 }
405
406 ✗ bool operator==(const ComputeCollision & other) const
407 {
408 ✗ return Base::operator==(other) && go1_ptr == other.go1_ptr
409 ✗ && go2_ptr == other.go2_ptr; // Maybe, it would be better to just check *go2_ptr ==
410 // *other.go2_ptr
411 }
412
413 bool operator!=(const ComputeCollision & other) const
414 {
415 return !(*this == other);
416 }
417
418 ✗ const GeometryObject & getGeometryObject1() const
419 {
420 ✗ return *go1_ptr;
421 }
422 ✗ const GeometryObject & getGeometryObject2() const
423 {
424 ✗ return *go2_ptr;
425 }
426
427 protected:
428 const GeometryObject * go1_ptr;
429 const GeometryObject * go2_ptr;
430 };
431
432 struct ComputeDistance : ::hpp::fcl::ComputeDistance
433 {
434 typedef ::hpp::fcl::ComputeDistance Base;
435
436 70 ComputeDistance(const GeometryObject & go1, const GeometryObject & go2)
437 70 : Base(go1.geometry.get(), go2.geometry.get())
438 70 , go1_ptr(&go1)
439 140 , go2_ptr(&go2)
440 {
441 70 }
442
443 280 virtual ~ComputeDistance() {};
444
445 ✗ virtual hpp::fcl::FCL_REAL run(
446 const fcl::Transform3f & tf1,
447 const fcl::Transform3f & tf2,
448 const fcl::DistanceRequest & request,
449 fcl::DistanceResult & result) const
450 {
451 typedef ::hpp::fcl::CollisionGeometry const * Pointer;
452 ✗ const_cast<Pointer &>(Base::o1) = go1_ptr->geometry.get();
453 ✗ const_cast<Pointer &>(Base::o2) = go2_ptr->geometry.get();
454 ✗ return Base::run(tf1, tf2, request, result);
455 }
456
457 ✗ bool operator==(const ComputeDistance & other) const
458 {
459 ✗ return Base::operator==(other) && go1_ptr == other.go1_ptr && go2_ptr == other.go2_ptr;
460 }
461
462 bool operator!=(const ComputeDistance & other) const
463 {
464 return !(*this == other);
465 }
466
467 ✗ const GeometryObject & getGeometryObject1() const
468 {
469 ✗ return *go1_ptr;
470 }
471 ✗ const GeometryObject & getGeometryObject2() const
472 {
473 ✗ return *go2_ptr;
474 }
475
476 protected:
477 const GeometryObject * go1_ptr;
478 const GeometryObject * go2_ptr;
479 };
480
481 #endif
482
483 } // namespace pinocchio
484
485 /* --- Details -------------------------------------------------------------- */
486 /* --- Details -------------------------------------------------------------- */
487 /* --- Details -------------------------------------------------------------- */
488 #include "pinocchio/multibody/geometry-object.hxx"
489
490 #endif // ifndef __pinocchio_multibody_geometry_object_hpp__
491