src/multibody/geometry.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2021 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_multibody_geometry_hpp__
6 #define __pinocchio_multibody_geometry_hpp__
7 
8 #include "pinocchio/multibody/fcl.hpp"
9 #include "pinocchio/multibody/fwd.hpp"
10 #include "pinocchio/container/aligned-vector.hpp"
11 
12 #include "pinocchio/serialization/serializable.hpp"
13 
14 #include <map>
15 #include <list>
16 #include <utility>
17 #include <assert.h>
18 
19 namespace pinocchio
20 {
21 
23  {
24  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 
26  typedef double Scalar;
27  enum { Options = 0 };
28 
30 
31  typedef ::pinocchio::GeometryObject GeometryObject;
32  typedef PINOCCHIO_ALIGNED_STD_VECTOR(GeometryObject) GeometryObjectVector;
33  typedef std::vector<CollisionPair> CollisionPairVector;
34  typedef Eigen::Matrix<bool,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXb;
35 
37 
39  : ngeoms(0)
40  , geometryObjects()
41  , collisionPairs()
42  {}
43 
45 
55  template<typename S2, int O2, template<typename,int> class _JointCollectionTpl>
56  GeomIndex addGeometryObject(const GeometryObject & object,
58 
66  GeomIndex addGeometryObject(const GeometryObject & object);
67 
75  void removeGeometryObject(const std::string& name);
76 
84  GeomIndex getGeometryId(const std::string & name) const;
85 
86 
94  bool existGeometryName(const std::string & name) const;
95 
102  void addCollisionPair(const CollisionPair & pair);
103 
109  void addAllCollisionPairs();
110 
119  void setCollisionPairs(const MatrixXb & collision_map,
120  const bool upper = true);
121 
127  void removeCollisionPair(const CollisionPair& pair);
128 
133 
142  bool existCollisionPair(const CollisionPair & pair) const;
143 
151  PairIndex findCollisionPair(const CollisionPair & pair) const;
152 
156  bool operator==(const GeometryModel & other) const
157  {
158  return
159  ngeoms == other.ngeoms
160  && geometryObjects == other.geometryObjects
161  && collisionPairs == other.collisionPairs
162  ;
163  }
164 
168  bool operator!=(const GeometryModel & other) const
169  {
170  return !(*this == other);
171  }
172 
173  friend std::ostream& operator<<(std::ostream & os,
174  const GeometryModel & model_geom);
175 
178 
180  GeometryObjectVector geometryObjects;
181 
183  CollisionPairVector collisionPairs;
184 
185  }; // struct GeometryModel
186 
188  : serialization::Serializable<GeometryData>
189  {
190  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
191 
192  typedef double Scalar;
193  enum { Options = 0 };
194 
196  typedef std::vector<GeomIndex> GeomIndexList;
197  typedef Eigen::Matrix<bool,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXb;
198  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXs;
199 
200 #ifdef PINOCCHIO_WITH_HPP_FCL
201  typedef ::pinocchio::ComputeCollision ComputeCollision;
202  typedef ::pinocchio::ComputeDistance ComputeDistance;
203 #endif
204 
213 
217  std::vector<bool> activeCollisionPairs;
218 
219 #ifdef PINOCCHIO_WITH_HPP_FCL
220 
224  std::vector<fcl::DistanceRequest> distanceRequests;
225 
229  std::vector<fcl::DistanceResult> distanceResults;
230 
234  std::vector<fcl::CollisionRequest> collisionRequests;
235 
239  std::vector<fcl::CollisionResult> collisionResults;
240 
245  std::vector<Scalar> radius;
246 
253  PairIndex collisionPairIndex;
254 
256  PINOCCHIO_ALIGNED_STD_VECTOR(ComputeCollision) collision_functors;
257 
259  PINOCCHIO_ALIGNED_STD_VECTOR(ComputeDistance) distance_functors;
260 
261 #endif // PINOCCHIO_WITH_HPP_FCL
262 
267  std::map<JointIndex,GeomIndexList> innerObjects;
268 
273  std::map<JointIndex,GeomIndexList> outerObjects;
274 
280  explicit GeometryData(const GeometryModel & geom_model);
281 
287  GeometryData(const GeometryData & other);
288 
294  GeometryData& operator=(const GeometryData & other);
295 
298 
300  ~GeometryData();
301 
311  void fillInnerOuterObjectMaps(const GeometryModel & geomModel);
312 
326  void activateCollisionPair(const PairIndex pair_id);
327 
333  void activateAllCollisionPairs();
334 
343  void setActiveCollisionPairs(const GeometryModel & geom_model,
344  const MatrixXb & collision_map,
345  const bool upper = true);
346 
354  void setGeometryCollisionStatus(const GeometryModel & geom_model,
355  const GeomIndex geom_id,
356  bool enable_collision);
357 
367  void deactivateCollisionPair(const PairIndex pair_id);
368 
374  void deactivateAllCollisionPairs();
375 
376 #ifdef PINOCCHIO_WITH_HPP_FCL
377  void setSecurityMargins(const GeometryModel & geom_model,
385  const MatrixXs & security_margin_map,
386  const bool upper = true);
387 #endif // ifdef PINOCCHIO_WITH_HPP_FCL
388 
389  friend std::ostream & operator<<(std::ostream & os, const GeometryData & geomData);
390 
394  bool operator==(const GeometryData & other) const
395  {
396  return
397  oMg == other.oMg
398  && activeCollisionPairs == other.activeCollisionPairs
399 #ifdef PINOCCHIO_WITH_HPP_FCL
400  && distanceRequests == other.distanceRequests
401  && distanceResults == other.distanceResults
402  && collisionRequests == other.collisionRequests
403  && collisionResults == other.collisionResults
404  && radius == other.radius
405  && collisionPairIndex == other.collisionPairIndex
406 #endif
407  && innerObjects == other.innerObjects
408  && outerObjects == other.outerObjects
409  ;
410  }
411 
415  bool operator!=(const GeometryData & other) const
416  {
417  return !(*this == other);
418  }
419 
420  }; // struct GeometryData
421 
422 } // namespace pinocchio
423 
424 /* --- Details -------------------------------------------------------------- */
425 /* --- Details -------------------------------------------------------------- */
426 /* --- Details -------------------------------------------------------------- */
427 #include "pinocchio/multibody/geometry.hxx"
428 
429 #endif // ifndef __pinocchio_multibody_geometry_hpp__
GeometryData()
Empty constructor.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef double Scalar
Eigen::Matrix< bool, Eigen::Dynamic, Eigen::Dynamic, Options > MatrixXb
Eigen::Matrix< bool, Eigen::Dynamic, Eigen::Dynamic, Options > MatrixXb
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef double Scalar
SE3Tpl< Scalar, Options > SE3
CollisionPairVector collisionPairs
Vector of collision pairs.
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
bool existCollisionPair(const CollisionPair &pair) const
Check if a collision pair exists in collisionPairs. See also findCollisitionPair(const CollisionPair ...
std::map< JointIndex, GeomIndexList > innerObjects
Map over vector GeomModel::geometryObjects, indexed by joints.
std::vector< bool > activeCollisionPairs
Vector of collision pairs.
bool operator==(const GeometryModel &other) const
Returns true if *this and other are equal.
PairIndex findCollisionPair(const CollisionPair &pair) const
Return the index of a given collision pair in collisionPairs.
bool operator!=(const GeometryModel &other) const
Returns true if *this and other are not equal.
void removeCollisionPair(const CollisionPair &pair)
Remove if exists the CollisionPair from the vector collision_pairs.
std::vector< GeomIndex > GeomIndexList
bool operator!=(const GeometryData &other) const
Returns true if *this and other are not equal.
void removeGeometryObject(const std::string &name)
Remove a GeometryObject.
GeometryObjectVector geometryObjects
Vector of GeometryObjects used for collision computations.
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.
std::map< JointIndex, GeomIndexList > outerObjects
A list of associated collision GeometryObjects to a given joint Id.
Index ngeoms
The number of GeometryObjects.
void addAllCollisionPairs()
Add all possible collision pairs.
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...
FCL_REAL radius
typedef PINOCCHIO_ALIGNED_STD_VECTOR(GeometryObject) GeometryObjectVector
Main pinocchio namespace.
Definition: timings.cpp:28
void addCollisionPair(const CollisionPair &pair)
Add a collision pair into the vector of collision_pairs. The method check before if the given Collisi...
std::size_t Index
AABB & operator=(const AABB &other)=default
std::vector< CollisionPair > CollisionPairVector
void removeAllCollisionPairs()
Remove all collision pairs from collisionPairs. Same as collisionPairs.clear().
friend std::ostream & operator<<(std::ostream &os, const GeometryModel &model_geom)
::pinocchio::GeometryObject GeometryObject
bool operator==(const GeometryData &other) const
Returns true if *this and other are equal.
JointCollectionTpl & model
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > MatrixXs
GeomIndex addGeometryObject(const GeometryObject &object, const ModelTpl< S2, O2, _JointCollectionTpl > &model)
Add a geometry object to a GeometryModel and set its parent joint.
SE3Tpl< Scalar, Options > SE3


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:30