Program Listing for File geometry-object.hpp
↰ Return to documentation for file (include/pinocchio/multibody/geometry-object.hpp
)
//
// Copyright (c) 2015-2023 CNRS INRIA
//
#ifndef __pinocchio_multibody_geometry_object_hpp__
#define __pinocchio_multibody_geometry_object_hpp__
#include "pinocchio/utils/shared-ptr.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/multibody/model-item.hpp"
#include "pinocchio/multibody/fcl.hpp"
#include "pinocchio/serialization/serializable.hpp"
#include <boost/variant.hpp>
namespace pinocchio
{
enum GeometryType
{
VISUAL,
COLLISION
};
struct GeometryNoMaterial
{
bool operator==(const GeometryNoMaterial &) const
{
return true;
}
};
struct GeometryPhongMaterial
{
GeometryPhongMaterial() = default;
GeometryPhongMaterial(
const Eigen::Vector4d & meshEmissionColor,
const Eigen::Vector4d & meshSpecularColor,
double meshShininess)
: meshEmissionColor(meshEmissionColor)
, meshSpecularColor(meshSpecularColor)
, meshShininess(meshShininess)
{
}
bool operator==(const GeometryPhongMaterial & other) const
{
return meshEmissionColor == other.meshEmissionColor
&& meshSpecularColor == other.meshSpecularColor
&& meshShininess == other.meshShininess;
}
Eigen::Vector4d meshEmissionColor{Eigen::Vector4d(0., 0., 0., 1.)};
Eigen::Vector4d meshSpecularColor{Eigen::Vector4d(0., 0., 0., 1.)};
double meshShininess{0.};
};
typedef boost::variant<GeometryNoMaterial, GeometryPhongMaterial> GeometryMaterial;
struct GeometryObject; // fwd
template<>
struct traits<GeometryObject>
{
typedef context::Scalar Scalar;
enum
{
Options = context::Options
};
};
struct GeometryObject
: public ModelItem<GeometryObject>
, serialization::Serializable<GeometryObject>
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef ModelItem<GeometryObject> Base;
typedef typename traits<GeometryObject>::Scalar Scalar;
enum
{
Options = traits<GeometryObject>::Options
};
typedef SE3Tpl<Scalar, Options> SE3;
typedef std::shared_ptr<fcl::CollisionGeometry> CollisionGeometryPtr;
using Base::name;
using Base::parentFrame;
using Base::parentJoint;
using Base::placement;
CollisionGeometryPtr geometry;
std::string meshPath;
Eigen::Vector3d meshScale;
bool overrideMaterial;
Eigen::Vector4d meshColor;
GeometryMaterial meshMaterial;
std::string meshTexturePath;
bool disableCollision;
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())
: Base(name, parent_joint, parent_frame, placement)
, geometry(collision_geometry)
, meshPath(meshPath)
, meshScale(meshScale)
, overrideMaterial(overrideMaterial)
, meshColor(meshColor)
, meshMaterial(meshMaterial)
, meshTexturePath(meshTexturePath)
, disableCollision(false)
{
}
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())
: Base(name, parent_joint, std::numeric_limits<FrameIndex>::max(), placement)
, geometry(collision_geometry)
, meshPath(meshPath)
, meshScale(meshScale)
, overrideMaterial(overrideMaterial)
, meshColor(meshColor)
, meshMaterial(meshMaterial)
, meshTexturePath(meshTexturePath)
, disableCollision(false)
{
}
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())
: Base(name, parent_joint, parent_frame, placement)
, geometry(collision_geometry)
, meshPath(meshPath)
, meshScale(meshScale)
, overrideMaterial(overrideMaterial)
, meshColor(meshColor)
, meshMaterial(meshMaterial)
, meshTexturePath(meshTexturePath)
, disableCollision(false)
{
}
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())
: Base(name, parent_joint, std::numeric_limits<FrameIndex>::max(), placement)
, geometry(collision_geometry)
, meshPath(meshPath)
, meshScale(meshScale)
, overrideMaterial(overrideMaterial)
, meshColor(meshColor)
, meshMaterial(meshMaterial)
, meshTexturePath(meshTexturePath)
, disableCollision(false)
{
}
GeometryObject(const GeometryObject & other) = default;
GeometryObject & operator=(const GeometryObject & other) = default;
GeometryObject clone() const
{
GeometryObject res(*this);
#ifdef PINOCCHIO_WITH_HPP_FCL
if (geometry)
res.geometry = CollisionGeometryPtr(geometry->clone());
#endif
return res;
}
bool operator==(const GeometryObject & other) const
{
if (this == &other)
return true;
return name == other.name && parentFrame == other.parentFrame
&& parentJoint == other.parentJoint && placement == other.placement
&& meshPath == other.meshPath && meshScale == other.meshScale
&& overrideMaterial == other.overrideMaterial && meshColor == other.meshColor
&& meshMaterial == other.meshMaterial && meshTexturePath == other.meshTexturePath
&& disableCollision == other.disableCollision
&& compare_shared_ptr(geometry, other.geometry);
}
bool operator!=(const GeometryObject & other) const
{
return !(*this == other);
}
friend std::ostream & operator<<(std::ostream & os, const GeometryObject & geomObject);
};
#ifdef PINOCCHIO_WITH_HPP_FCL
struct CollisionObject : ::hpp::fcl::CollisionObject
{
typedef ::hpp::fcl::CollisionObject Base;
typedef SE3Tpl<double> SE3;
CollisionObject()
: Base(nullptr, false)
, geometryObjectIndex((std::numeric_limits<size_t>::max)())
{
}
explicit CollisionObject(
const std::shared_ptr<::hpp::fcl::CollisionGeometry> & collision_geometry,
const size_t geometryObjectIndex = (std::numeric_limits<size_t>::max)(),
bool compute_local_aabb = true)
: Base(collision_geometry, compute_local_aabb)
, geometryObjectIndex(geometryObjectIndex)
{
}
CollisionObject(
const std::shared_ptr<::hpp::fcl::CollisionGeometry> & collision_geometry,
const SE3 & transform,
const size_t geometryObjectIndex = (std::numeric_limits<size_t>::max)(),
bool compute_local_aabb = true)
: Base(collision_geometry, toFclTransform3f(transform), compute_local_aabb)
, geometryObjectIndex(geometryObjectIndex)
{
}
bool operator==(const CollisionObject & other) const
{
return Base::operator==(other) && geometryObjectIndex == other.geometryObjectIndex;
}
bool operator!=(const CollisionObject & other) const
{
return !(*this == other);
}
size_t geometryObjectIndex;
};
struct ComputeCollision : ::hpp::fcl::ComputeCollision
{
typedef ::hpp::fcl::ComputeCollision Base;
ComputeCollision(const GeometryObject & go1, const GeometryObject & go2)
: Base(go1.geometry.get(), go2.geometry.get())
, go1_ptr(&go1)
, go2_ptr(&go2)
{
}
virtual ~ComputeCollision() {};
virtual std::size_t run(
const fcl::Transform3f & tf1,
const fcl::Transform3f & tf2,
const fcl::CollisionRequest & request,
fcl::CollisionResult & result) const
{
typedef ::hpp::fcl::CollisionGeometry const * Pointer;
const_cast<Pointer &>(Base::o1) = go1_ptr->geometry.get();
const_cast<Pointer &>(Base::o2) = go2_ptr->geometry.get();
return Base::run(tf1, tf2, request, result);
}
bool operator==(const ComputeCollision & other) const
{
return Base::operator==(other) && go1_ptr == other.go1_ptr
&& go2_ptr == other.go2_ptr; // Maybe, it would be better to just check *go2_ptr ==
// *other.go2_ptr
}
bool operator!=(const ComputeCollision & other) const
{
return !(*this == other);
}
const GeometryObject & getGeometryObject1() const
{
return *go1_ptr;
}
const GeometryObject & getGeometryObject2() const
{
return *go2_ptr;
}
protected:
const GeometryObject * go1_ptr;
const GeometryObject * go2_ptr;
};
struct ComputeDistance : ::hpp::fcl::ComputeDistance
{
typedef ::hpp::fcl::ComputeDistance Base;
ComputeDistance(const GeometryObject & go1, const GeometryObject & go2)
: Base(go1.geometry.get(), go2.geometry.get())
, go1_ptr(&go1)
, go2_ptr(&go2)
{
}
virtual ~ComputeDistance() {};
virtual hpp::fcl::FCL_REAL run(
const fcl::Transform3f & tf1,
const fcl::Transform3f & tf2,
const fcl::DistanceRequest & request,
fcl::DistanceResult & result) const
{
typedef ::hpp::fcl::CollisionGeometry const * Pointer;
const_cast<Pointer &>(Base::o1) = go1_ptr->geometry.get();
const_cast<Pointer &>(Base::o2) = go2_ptr->geometry.get();
return Base::run(tf1, tf2, request, result);
}
bool operator==(const ComputeDistance & other) const
{
return Base::operator==(other) && go1_ptr == other.go1_ptr && go2_ptr == other.go2_ptr;
}
bool operator!=(const ComputeDistance & other) const
{
return !(*this == other);
}
const GeometryObject & getGeometryObject1() const
{
return *go1_ptr;
}
const GeometryObject & getGeometryObject2() const
{
return *go2_ptr;
}
protected:
const GeometryObject * go1_ptr;
const GeometryObject * go2_ptr;
};
#endif
} // namespace pinocchio
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "pinocchio/multibody/geometry-object.hxx"
#endif // ifndef __pinocchio_multibody_geometry_object_hpp__