Program Listing for File geometry.hpp

↰ Return to documentation for file (include/pinocchio/multibody/geometry.hpp)

//
// Copyright (c) 2015-2021 CNRS INRIA
//

#ifndef __pinocchio_multibody_geometry_hpp__
#define __pinocchio_multibody_geometry_hpp__

#include "pinocchio/multibody/fcl.hpp"
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/container/aligned-vector.hpp"

#include "pinocchio/serialization/serializable.hpp"

#include <map>
#include <list>
#include <utility>
#include <assert.h>

namespace pinocchio
{

  struct GeometryModel
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef double Scalar;
    enum { Options = 0 };

    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 pinocchio::GeomIndex GeomIndex;

    GeometryModel()
    : ngeoms(0)
    , geometryObjects()
    , collisionPairs()
    {}

    ~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;

    bool operator==(const GeometryModel & other) const
    {
      return
         ngeoms == other.ngeoms
      && geometryObjects == other.geometryObjects
      && collisionPairs == other.collisionPairs
      ;
    }

    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;

  }; // struct GeometryModel

  struct GeometryData
  : serialization::Serializable<GeometryData>
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef double Scalar;
    enum { Options = 0 };

    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);
#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__