5#ifndef __pinocchio_multibody_geometry_object_hpp__
6#define __pinocchio_multibody_geometry_object_hpp__
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"
19#include <boost/variant.hpp>
73 typedef boost::variant<GeometryNoMaterial, GeometryPhongMaterial> GeometryMaterial;
75 struct GeometryObject;
102 typedef std::shared_ptr<fcl::CollisionGeometry> CollisionGeometryPtr;
152 const std::string &
name,
158 const Eigen::Vector3d &
meshScale = Eigen::Vector3d::Ones(),
160 const Eigen::Vector4d &
meshColor = Eigen::Vector4d(0, 0, 0, 1),
192 const std::string &
name,
197 const Eigen::Vector3d &
meshScale = Eigen::Vector3d::Ones(),
199 const Eigen::Vector4d &
meshColor = Eigen::Vector4d(0, 0, 0, 1),
232 const std::string &
name,
238 const Eigen::Vector3d &
meshScale = Eigen::Vector3d::Ones(),
240 const Eigen::Vector4d &
meshColor = Eigen::Vector4d(0, 0, 0, 1),
274 const std::string &
name,
279 const Eigen::Vector3d &
meshScale = Eigen::Vector3d::Ones(),
281 const Eigen::Vector4d &
meshColor = Eigen::Vector4d(0, 0, 0, 1),
306#ifdef PINOCCHIO_WITH_HPP_FCL
327 bool operator!=(
const GeometryObject & other)
const
329 return !(*
this == other);
332 friend std::ostream & operator<<(std::ostream & os,
const GeometryObject & geomObject);
335#ifdef PINOCCHIO_WITH_HPP_FCL
339 typedef ::hpp::fcl::CollisionObject Base;
343 : Base(
nullptr,
false)
374 return !(*
this ==
other);
383 typedef ::hpp::fcl::ComputeCollision Base;
386 : Base(
go1.geometry.get(),
go2.geometry.get())
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
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();
408 return Base::operator==(
other) && go1_ptr ==
other.go1_ptr
409 && go2_ptr ==
other.go2_ptr;
415 return !(*
this ==
other);
434 typedef ::hpp::fcl::ComputeDistance Base;
437 : Base(
go1.geometry.get(),
go2.geometry.get())
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
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();
459 return Base::operator==(
other) && go1_ptr ==
other.go1_ptr && go2_ptr ==
other.go2_ptr;
464 return !(*
this ==
other);
488#include "pinocchio/multibody/geometry-object.hxx"
Main pinocchio namespace.
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.