pinocchio  3.5.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
geometry.hpp
1 //
2 // Copyright (c) 2015-2023 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_multibody_geometry_hpp__
6 #define __pinocchio_multibody_geometry_hpp__
7 
8 #include "pinocchio/multibody/geometry-object.hpp"
9 #include "pinocchio/container/aligned-vector.hpp"
10 
11 #include "pinocchio/serialization/serializable.hpp"
12 
13 #include <map>
14 #include <list>
15 #include <utility>
16 #include <assert.h>
17 
18 namespace pinocchio
19 {
20 
21  struct CollisionPair : public std::pair<GeomIndex, GeomIndex>
22  {
23 
24  typedef std::pair<GeomIndex, GeomIndex> Base;
25 
28 
36  CollisionPair(const GeomIndex co1, const GeomIndex co2);
37  bool operator==(const CollisionPair & rhs) const;
38  bool operator!=(const CollisionPair & rhs) const;
39  void disp(std::ostream & os) const;
40  friend std::ostream & operator<<(std::ostream & os, const CollisionPair & X);
41 
42  }; // struct CollisionPair
43 
44  template<>
46  {
47  typedef double Scalar;
48  enum
49  {
50  Options = 0
51  };
52  };
53 
55  : NumericalBase<GeometryModel>
56  , serialization::Serializable<GeometryModel>
57  {
58  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
59 
60  typedef typename traits<GeometryModel>::Scalar Scalar;
61  enum
62  {
64  };
65 
67 
68  typedef ::pinocchio::GeometryObject GeometryObject;
69  typedef PINOCCHIO_ALIGNED_STD_VECTOR(GeometryObject) GeometryObjectVector;
70  typedef std::vector<CollisionPair> CollisionPairVector;
71  typedef Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXb;
72  typedef Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXi;
73 
74  typedef pinocchio::GeomIndex GeomIndex;
75 
77  : ngeoms(0)
78  , geometryObjects()
79  , collisionPairs()
80  {
81  }
82 
83  GeometryModel(const GeometryModel & other) = default;
84 
85  ~GeometryModel() {};
86 
96  template<typename S2, int O2, template<typename, int> class _JointCollectionTpl>
97  GeomIndex addGeometryObject(
98  const GeometryObject & object, const ModelTpl<S2, O2, _JointCollectionTpl> & model);
99 
107  GeomIndex addGeometryObject(const GeometryObject & object);
108 
116  void removeGeometryObject(const std::string & name);
117 
125  GeomIndex getGeometryId(const std::string & name) const;
126 
134  bool existGeometryName(const std::string & name) const;
135 
142  void addCollisionPair(const CollisionPair & pair);
143 
150 
159  void setCollisionPairs(const MatrixXb & collision_map, const bool upper = true);
160 
166  void removeCollisionPair(const CollisionPair & pair);
167 
172 
181  bool existCollisionPair(const CollisionPair & pair) const;
182 
190  PairIndex findCollisionPair(const CollisionPair & pair) const;
191 
196 
200  bool operator==(const GeometryModel & other) const
201  {
202  return ngeoms == other.ngeoms && geometryObjects == other.geometryObjects
203  && collisionPairs == other.collisionPairs
205  }
206 
210  bool operator!=(const GeometryModel & other) const
211  {
212  return !(*this == other);
213  }
214 
215  friend std::ostream & operator<<(std::ostream & os, const GeometryModel & model_geom);
216 
218  Index ngeoms;
219 
221  GeometryObjectVector geometryObjects;
222 
224  CollisionPairVector collisionPairs;
225 
228 
229  }; // struct GeometryModel
230 
231  template<>
233  {
234  typedef double Scalar;
235  enum
236  {
237  Options = 0
238  };
239  };
240 
242  : NumericalBase<GeometryData>
243  , serialization::Serializable<GeometryData>
244  {
245  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
246 
247  typedef typename traits<GeometryData>::Scalar Scalar;
248  enum
249  {
251  };
252 
254  typedef std::vector<GeomIndex> GeomIndexList;
255  typedef Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXb;
256  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
257 
258 #ifdef PINOCCHIO_WITH_HPP_FCL
259  typedef ::pinocchio::ComputeCollision ComputeCollision;
260  typedef ::pinocchio::ComputeDistance ComputeDistance;
261 #endif
262 
271 
275  std::vector<bool> activeCollisionPairs;
276 
277 #ifdef PINOCCHIO_WITH_HPP_FCL
278 
282  std::vector<fcl::DistanceRequest> distanceRequests;
283 
287  std::vector<fcl::DistanceResult> distanceResults;
288 
292  std::vector<fcl::CollisionRequest> collisionRequests;
293 
297  std::vector<fcl::CollisionResult> collisionResults;
298 
303  std::vector<Scalar> radius;
304 
312 
315 
318 
319 #endif // PINOCCHIO_WITH_HPP_FCL
320 
326  std::map<JointIndex, GeomIndexList> innerObjects;
327 
332  std::map<JointIndex, GeomIndexList> outerObjects;
333 
339  explicit GeometryData(const GeometryModel & geom_model);
340 
346  GeometryData(const GeometryData & other);
347 
354 
357 
360 
370  void fillInnerOuterObjectMaps(const GeometryModel & geomModel);
371 
385  void activateCollisionPair(const PairIndex pair_id);
386 
394 
404  const GeometryModel & geom_model, const MatrixXb & collision_map, const bool upper = true);
405 
415  const GeometryModel & geom_model, const GeomIndex geom_id, bool enable_collision);
416 
426  void deactivateCollisionPair(const PairIndex pair_id);
427 
435 
436 #ifdef PINOCCHIO_WITH_HPP_FCL
448  const GeometryModel & geom_model,
449  const MatrixXs & security_margin_map,
450  const bool upper = true,
451  const bool sync_distance_upper_bound = false);
452 
453 #endif // ifdef PINOCCHIO_WITH_HPP_FCL
454 
455  friend std::ostream & operator<<(std::ostream & os, const GeometryData & geomData);
456 
460  bool operator==(const GeometryData & other) const
461  {
462  return oMg == other.oMg && activeCollisionPairs == other.activeCollisionPairs
463 #ifdef PINOCCHIO_WITH_HPP_FCL
465  && distanceResults == other.distanceResults
467  && collisionResults == other.collisionResults && radius == other.radius
469 #endif
470  && innerObjects == other.innerObjects && outerObjects == other.outerObjects;
471  }
472 
476  bool operator!=(const GeometryData & other) const
477  {
478  return !(*this == other);
479  }
480 
481  }; // struct GeometryData
482 
483 } // namespace pinocchio
484 
485 /* --- Details -------------------------------------------------------------- */
486 /* --- Details -------------------------------------------------------------- */
487 /* --- Details -------------------------------------------------------------- */
488 #include "pinocchio/multibody/geometry.hxx"
489 
490 #endif // ifndef __pinocchio_multibody_geometry_hpp__
Main pinocchio namespace.
Definition: treeview.dox:11
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
CollisionPair()
Empty constructor.
CollisionPair(const GeomIndex co1, const GeomIndex co2)
Default constructor of a collision pair from two collision object indexes.
void deactivateCollisionPair(const PairIndex pair_id)
std::vector< fcl::CollisionRequest > collisionRequests
Defines what information should be computed by collision test. There is one request per pair of geome...
Definition: geometry.hpp:292
void setActiveCollisionPairs(const GeometryModel &geom_model, const MatrixXb &collision_map, const bool upper=true)
Set the collision pair association from a given input array. Each entry of the input matrix defines t...
void activateAllCollisionPairs()
Activate all collision pairs.
GeometryData()
Empty constructor.
Definition: geometry.hpp:356
PairIndex collisionPairIndex
Index of the collision pair.
Definition: geometry.hpp:311
GeometryData & operator=(const GeometryData &other)
Copy operator.
std::vector< Scalar > radius
Radius of the bodies, i.e. distance of the further point of the geometry model attached to the body f...
Definition: geometry.hpp:303
void activateCollisionPair(const PairIndex pair_id)
void deactivateAllCollisionPairs()
Deactivate all collision pairs.
void setSecurityMargins(const GeometryModel &geom_model, const MatrixXs &security_margin_map, const bool upper=true, const bool sync_distance_upper_bound=false)
Set the security margin of all the collision request in a row, according to the values stored in the ...
std::vector< fcl::DistanceResult > distanceResults
Vector gathering the result of the distance computation for all the collision pairs.
Definition: geometry.hpp:287
std::map< JointIndex, GeomIndexList > outerObjects
A list of associated collision GeometryObjects to a given joint Id.
Definition: geometry.hpp:332
std::vector< bool > activeCollisionPairs
Vector of collision pairs.
Definition: geometry.hpp:275
GeometryData(const GeometryData &other)
Copy constructor.
std::vector< fcl::CollisionResult > collisionResults
Vector gathering the result of the collision computation for all the collision pairs.
Definition: geometry.hpp:297
bool operator!=(const GeometryData &other) const
Returns true if *this and other are not equal.
Definition: geometry.hpp:476
PINOCCHIO_ALIGNED_STD_VECTOR(ComputeDistance) distance_functors
&#160;
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMg
Vector gathering the SE3 placements of the geometry objects relative to the world....
std::vector< fcl::DistanceRequest > distanceRequests
Defines what information should be computed by distance computation. There is one request per pair of...
Definition: geometry.hpp:282
bool operator==(const GeometryData &other) const
Returns true if *this and other are equal.
Definition: geometry.hpp:460
GeometryData(const GeometryModel &geom_model)
Default constructor from a GeometryModel.
~GeometryData()
Destructor.
void setGeometryCollisionStatus(const GeometryModel &geom_model, const GeomIndex geom_id, bool enable_collision)
Enable or disable collision for the given geometry given by its geometry id with all the other geomet...
PINOCCHIO_ALIGNED_STD_VECTOR(ComputeCollision) collision_functors
&#160;
void fillInnerOuterObjectMaps(const GeometryModel &geomModel)
std::map< JointIndex, GeomIndexList > innerObjects
Map over vector GeomModel::geometryObjects, indexed by joints.
Definition: geometry.hpp:326
void setCollisionPairs(const MatrixXb &collision_map, const bool upper=true)
Set the collision pairs from a given input array. Each entry of the input matrix defines the activati...
GeomIndex addGeometryObject(const GeometryObject &object, const ModelTpl< S2, O2, _JointCollectionTpl > &model)
Add a geometry object to a GeometryModel and set its parent joint.
PairIndex findCollisionPair(const CollisionPair &pair) const
Return the index of a given collision pair in collisionPairs.
bool existCollisionPair(const CollisionPair &pair) const
Check if a collision pair exists in collisionPairs. See also findCollisitionPair(const CollisionPair ...
void removeAllCollisionPairs()
Remove all collision pairs from collisionPairs. Same as collisionPairs.clear().
void addCollisionPair(const CollisionPair &pair)
Add a collision pair into the vector of collision_pairs. The method check before if the given Collisi...
bool operator==(const GeometryModel &other) const
Returns true if *this and other are equal.
Definition: geometry.hpp:200
GeometryObjectVector geometryObjects
Vector of GeometryObjects used for collision computations.
Definition: geometry.hpp:221
GeomIndex getGeometryId(const std::string &name) const
Return the index of a GeometryObject given by its name.
bool existGeometryName(const std::string &name) const
Check if a GeometryObject given by its name exists.
MatrixXi collisionPairMapping
Matrix relating the collision pair ID to a pair of two GeometryObject indexes.
Definition: geometry.hpp:227
void addAllCollisionPairs()
Add all possible collision pairs.
void removeGeometryObject(const std::string &name)
Remove a GeometryObject.
Index ngeoms
The number of GeometryObjects.
Definition: geometry.hpp:218
GeometryModel clone() const
Create a deep copy of *this.
CollisionPairVector collisionPairs
Vector of collision pairs.
Definition: geometry.hpp:224
GeomIndex addGeometryObject(const GeometryObject &object)
Add a geometry object to a GeometryModel.
void removeCollisionPair(const CollisionPair &pair)
Remove if exists the CollisionPair from the vector collision_pairs.
bool operator!=(const GeometryModel &other) const
Returns true if *this and other are not equal.
Definition: geometry.hpp:210
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:72