Program Listing for File geometry.hpp
↰ Return to documentation for file (include/pinocchio/multibody/geometry.hpp
)
//
// Copyright (c) 2015-2023 CNRS INRIA
//
#ifndef __pinocchio_multibody_geometry_hpp__
#define __pinocchio_multibody_geometry_hpp__
#include "pinocchio/multibody/geometry-object.hpp"
#include "pinocchio/container/aligned-vector.hpp"
#include "pinocchio/serialization/serializable.hpp"
#include <map>
#include <list>
#include <utility>
#include <assert.h>
namespace pinocchio
{
struct CollisionPair : public std::pair<GeomIndex, GeomIndex>
{
typedef std::pair<GeomIndex, GeomIndex> Base;
CollisionPair();
CollisionPair(const GeomIndex co1, const GeomIndex co2);
bool operator==(const CollisionPair & rhs) const;
bool operator!=(const CollisionPair & rhs) const;
void disp(std::ostream & os) const;
friend std::ostream & operator<<(std::ostream & os, const CollisionPair & X);
}; // struct CollisionPair
template<>
struct traits<GeometryModel>
{
typedef context::Scalar Scalar;
};
struct GeometryModel
: NumericalBase<GeometryModel>
, serialization::Serializable<GeometryModel>
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef context::Scalar Scalar;
enum
{
Options = context::Options
};
typedef SE3Tpl<Scalar, Options> SE3;
typedef ::pinocchio::GeometryObject GeometryObject;
typedef PINOCCHIO_ALIGNED_STD_VECTOR(GeometryObject) GeometryObjectVector;
typedef std::vector<CollisionPair> CollisionPairVector;
typedef Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXb;
typedef Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXi;
typedef pinocchio::GeomIndex GeomIndex;
GeometryModel()
: ngeoms(0)
, geometryObjects()
, collisionPairs()
{
}
GeometryModel(const GeometryModel & other) = default;
~GeometryModel() {};
template<typename S2, int O2, template<typename, int> class _JointCollectionTpl>
GeomIndex addGeometryObject(
const GeometryObject & object, const ModelTpl<S2, O2, _JointCollectionTpl> & model);
GeomIndex addGeometryObject(const GeometryObject & object);
void removeGeometryObject(const std::string & name);
GeomIndex getGeometryId(const std::string & name) const;
bool existGeometryName(const std::string & name) const;
void addCollisionPair(const CollisionPair & pair);
void addAllCollisionPairs();
void setCollisionPairs(const MatrixXb & collision_map, const bool upper = true);
void removeCollisionPair(const CollisionPair & pair);
void removeAllCollisionPairs();
bool existCollisionPair(const CollisionPair & pair) const;
PairIndex findCollisionPair(const CollisionPair & pair) const;
GeometryModel clone() const;
bool operator==(const GeometryModel & other) const
{
return ngeoms == other.ngeoms && geometryObjects == other.geometryObjects
&& collisionPairs == other.collisionPairs
&& collisionPairMapping == other.collisionPairMapping;
}
bool operator!=(const GeometryModel & other) const
{
return !(*this == other);
}
friend std::ostream & operator<<(std::ostream & os, const GeometryModel & model_geom);
Index ngeoms;
GeometryObjectVector geometryObjects;
CollisionPairVector collisionPairs;
MatrixXi collisionPairMapping;
}; // struct GeometryModel
template<>
struct traits<GeometryData>
{
typedef context::Scalar Scalar;
};
struct GeometryData
: NumericalBase<GeometryData>
, serialization::Serializable<GeometryData>
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef context::Scalar Scalar;
enum
{
Options = context::Options
};
typedef SE3Tpl<Scalar, Options> SE3;
typedef std::vector<GeomIndex> GeomIndexList;
typedef Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXb;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
#ifdef PINOCCHIO_WITH_HPP_FCL
typedef ::pinocchio::ComputeCollision ComputeCollision;
typedef ::pinocchio::ComputeDistance ComputeDistance;
#endif
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMg;
std::vector<bool> activeCollisionPairs;
#ifdef PINOCCHIO_WITH_HPP_FCL
std::vector<fcl::DistanceRequest> distanceRequests;
std::vector<fcl::DistanceResult> distanceResults;
std::vector<fcl::CollisionRequest> collisionRequests;
std::vector<fcl::CollisionResult> collisionResults;
std::vector<Scalar> radius;
PairIndex collisionPairIndex;
PINOCCHIO_ALIGNED_STD_VECTOR(ComputeCollision) collision_functors;
PINOCCHIO_ALIGNED_STD_VECTOR(ComputeDistance) distance_functors;
#endif // PINOCCHIO_WITH_HPP_FCL
std::map<JointIndex, GeomIndexList> innerObjects;
std::map<JointIndex, GeomIndexList> outerObjects;
explicit GeometryData(const GeometryModel & geom_model);
GeometryData(const GeometryData & other);
GeometryData & operator=(const GeometryData & other);
GeometryData() {};
~GeometryData();
void fillInnerOuterObjectMaps(const GeometryModel & geomModel);
void activateCollisionPair(const PairIndex pair_id);
void activateAllCollisionPairs();
void setActiveCollisionPairs(
const GeometryModel & geom_model, const MatrixXb & collision_map, const bool upper = true);
void setGeometryCollisionStatus(
const GeometryModel & geom_model, const GeomIndex geom_id, bool enable_collision);
void deactivateCollisionPair(const PairIndex pair_id);
void deactivateAllCollisionPairs();
#ifdef PINOCCHIO_WITH_HPP_FCL
void setSecurityMargins(
const GeometryModel & geom_model,
const MatrixXs & security_margin_map,
const bool upper = true,
const bool sync_distance_upper_bound = false);
#endif // ifdef PINOCCHIO_WITH_HPP_FCL
friend std::ostream & operator<<(std::ostream & os, const GeometryData & geomData);
bool operator==(const GeometryData & other) const
{
return oMg == other.oMg && activeCollisionPairs == other.activeCollisionPairs
#ifdef PINOCCHIO_WITH_HPP_FCL
&& distanceRequests == other.distanceRequests
&& distanceResults == other.distanceResults
&& collisionRequests == other.collisionRequests
&& collisionResults == other.collisionResults && radius == other.radius
&& collisionPairIndex == other.collisionPairIndex
#endif
&& innerObjects == other.innerObjects && outerObjects == other.outerObjects;
}
bool operator!=(const GeometryData & other) const
{
return !(*this == other);
}
}; // struct GeometryData
} // namespace pinocchio
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "pinocchio/multibody/geometry.hxx"
#endif // ifndef __pinocchio_multibody_geometry_hpp__