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__