pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
geometry-object.hpp
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
19#include <boost/variant.hpp>
20
21namespace pinocchio
22{
23
24 enum GeometryType
25 {
26 VISUAL,
27 COLLISION
28 };
29
32 {
33 bool operator==(const GeometryNoMaterial &) const
34 {
35 return true;
36 }
37 };
38
42 {
43 GeometryPhongMaterial() = default;
45 const Eigen::Vector4d & meshEmissionColor,
46 const Eigen::Vector4d & meshSpecularColor,
47 double 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
62 Eigen::Vector4d meshEmissionColor{Eigen::Vector4d(0., 0., 0., 1.)};
63
65 Eigen::Vector4d meshSpecularColor{Eigen::Vector4d(0., 0., 0., 1.)};
66
70 double meshShininess{0.};
71 };
72
73 typedef boost::variant<GeometryNoMaterial, GeometryPhongMaterial> GeometryMaterial;
74
75 struct GeometryObject; // fwd
76
77 template<>
79 {
80 typedef double Scalar;
81 enum
82 {
83 Options = 0
84 };
85 };
86
88 : public ModelItem<GeometryObject>
89 , serialization::Serializable<GeometryObject>
90 {
92
94 typedef typename traits<GeometryObject>::Scalar Scalar;
95 enum
96 {
98 };
99
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
110 CollisionGeometryPtr geometry;
111
113 std::string meshPath;
114
116 Eigen::Vector3d meshScale;
117
120
122 Eigen::Vector4d meshColor;
123
127 GeometryMaterial meshMaterial;
128
130 std::string meshTexturePath;
131
135
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())
172 {
173 }
174
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 : Base(name, parent_joint, std::numeric_limits<FrameIndex>::max(), placement)
211 {
212 }
213
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())
252 {
253 }
254
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)
293 {
294 }
295
296 GeometryObject(const GeometryObject & other) = default;
297 GeometryObject & operator=(const GeometryObject & other) = default;
298
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
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)
354 {
355 }
356
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)
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
379 };
380
381 struct ComputeCollision : ::hpp::fcl::ComputeCollision
382 {
383 typedef ::hpp::fcl::ComputeCollision Base;
384
386 : Base(go1.geometry.get(), go2.geometry.get())
387 , go1_ptr(&go1)
388 , go2_ptr(&go2)
389 {
390 }
391
392 virtual ~ComputeCollision() {};
393
394 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 const_cast<Pointer &>(Base::o1) = go1_ptr->geometry.get();
402 const_cast<Pointer &>(Base::o2) = go2_ptr->geometry.get();
403 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
437 : Base(go1.geometry.get(), go2.geometry.get())
438 , go1_ptr(&go1)
439 , go2_ptr(&go2)
440 {
441 }
442
443 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__
Main pinocchio namespace.
Definition treeview.dox:11
bool compare_shared_ptr(const std::shared_ptr< T > &ptr1, const std::shared_ptr< T > &ptr2)
Compares two std::shared_ptr.
size_t geometryObjectIndex
Geometry object index related to the current collision object.
No material associated to a geometry.
Eigen::Vector4d meshColor
RGBA color value of the GeometryObject::geometry object.
GeometryMaterial meshMaterial
Material associated to the mesh. This material should be used only if overrideMaterial is set to true...
GeometryObject(const std::string &name, const JointIndex parent_joint, const SE3 &placement, const CollisionGeometryPtr &collision_geometry, const std::string &meshPath="", const Eigen::Vector3d &meshScale=Eigen::Vector3d::Ones(), const bool overrideMaterial=false, const Eigen::Vector4d &meshColor=Eigen::Vector4d(0, 0, 0, 1), const std::string &meshTexturePath="", const GeometryMaterial &meshMaterial=GeometryNoMaterial())
Reduced constructor.
std::string meshPath
Absolute path to the mesh file (if the geometry pointee is also a Mesh)
bool overrideMaterial
Decide whether to override the Material.
bool disableCollision
If true, no collision or distance check will be done between the Geometry and any other geometry.
Eigen::Vector3d meshScale
Scaling vector applied to the GeometryObject::geometry object.
GeometryObject clone() const
Perform a deep copy of this. It will create a copy of the underlying FCL geometry.
PINOCCHIO_DEPRECATED GeometryObject(const std::string &name, const JointIndex parent_joint, const CollisionGeometryPtr &collision_geometry, const SE3 &placement, const std::string &meshPath="", const Eigen::Vector3d &meshScale=Eigen::Vector3d::Ones(), const bool overrideMaterial=false, const Eigen::Vector4d &meshColor=Eigen::Vector4d(0, 0, 0, 1), const std::string &meshTexturePath="", const GeometryMaterial &meshMaterial=GeometryNoMaterial())
Reduced constructor.
PINOCCHIO_DEPRECATED GeometryObject(const std::string &name, const FrameIndex parent_frame, const JointIndex parent_joint, const CollisionGeometryPtr &collision_geometry, const SE3 &placement, const std::string &meshPath="", const Eigen::Vector3d &meshScale=Eigen::Vector3d::Ones(), const bool overrideMaterial=false, const Eigen::Vector4d &meshColor=Eigen::Vector4d(0, 0, 0, 1), const std::string &meshTexturePath="", const GeometryMaterial &meshMaterial=GeometryNoMaterial())
Full constructor.
std::string meshTexturePath
Absolute path to the mesh texture file.
GeometryObject(const std::string &name, const JointIndex parent_joint, const FrameIndex parent_frame, const SE3 &placement, const CollisionGeometryPtr &collision_geometry, const std::string &meshPath="", const Eigen::Vector3d &meshScale=Eigen::Vector3d::Ones(), const bool overrideMaterial=false, const Eigen::Vector4d &meshColor=Eigen::Vector4d(0, 0, 0, 1), const std::string &meshTexturePath="", const GeometryMaterial &meshMaterial=GeometryNoMaterial())
Full constructor.
CollisionGeometryPtr geometry
The FCL CollisionGeometry (might be a Mesh, a Geometry Primitive, etc.)
Eigen::Vector4d meshSpecularColor
RGBA specular color value of the GeometryObject::geometry object.
double meshShininess
Shininess associated to the specular lighting model.
Eigen::Vector4d meshEmissionColor
RGBA emission (ambient) color value of the GeometryObject::geometry object.
FrameIndex parentFrame
Index of the parent frame.
JointIndex parentJoint
Index of the parent joint.
std::string name
Name of the kinematic element.
SE3 placement
Position of kinematic element in parent joint frame.
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72