pinocchio  2.3.1-dirty
multibody/geometry.hpp
1 //
2 // Copyright (c) 2015-2020 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/model.hpp"
10 #include "pinocchio/container/aligned-vector.hpp"
11 
12 #include <iostream>
13 
14 #include <boost/foreach.hpp>
15 #include <map>
16 #include <list>
17 #include <utility>
18 #include <assert.h>
19 
20 namespace pinocchio
21 {
22 
24  {
25  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26 
27  typedef double Scalar;
28  enum { Options = 0 };
29 
31 
32  typedef ::pinocchio::GeometryObject GeometryObject;
33  typedef PINOCCHIO_ALIGNED_STD_VECTOR(GeometryObject) GeometryObjectVector;
34  typedef std::vector<CollisionPair> CollisionPairVector;
35 
36  typedef pinocchio::GeomIndex GeomIndex;
37 
39  : ngeoms(0)
40  , geometryObjects()
41  , collisionPairs()
42  {}
43 
44  ~GeometryModel() {};
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  GeomIndex getGeometryId(const std::string & name) const;
76 
77 
85  bool existGeometryName(const std::string & name) const;
86 
87 #ifdef PINOCCHIO_WITH_HPP_FCL
88  void addCollisionPair(const CollisionPair & pair);
95 
101  void addAllCollisionPairs();
102 
108  void removeCollisionPair(const CollisionPair& pair);
109 
114 
123  bool existCollisionPair(const CollisionPair & pair) const;
124 
132  PairIndex findCollisionPair(const CollisionPair & pair) const;
133 
134 #endif // PINOCCHIO_WITH_HPP_FCL
135 
139  bool operator==(const GeometryModel & other) const
140  {
141  return
142  ngeoms == other.ngeoms
143  && geometryObjects == other.geometryObjects
144  && collisionPairs == other.collisionPairs
145  ;
146  }
147 
151  bool operator!=(const GeometryModel & other) const
152  {
153  return !(*this == other);
154  }
155 
156  friend std::ostream& operator<<(std::ostream & os,
157  const GeometryModel & model_geom);
158 
160  Index ngeoms;
161 
163  GeometryObjectVector geometryObjects;
167  CollisionPairVector collisionPairs;
168 
169  }; // struct GeometryModel
170 
172  {
173  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
174 
175  typedef double Scalar;
176  enum { Options = 0 };
177 
178  typedef SE3Tpl<Scalar,Options> SE3;
179 
187  PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMg;
188 
189 #ifdef PINOCCHIO_WITH_HPP_FCL
190  std::vector<fcl::CollisionObject> collisionObjects;
197 
201  std::vector<bool> activeCollisionPairs;
202 
206  fcl::DistanceRequest distanceRequest;
207 
211  std::vector<fcl::DistanceResult> distanceResults;
212 
216  fcl::CollisionRequest collisionRequest;
217 
221  std::vector<fcl::CollisionResult> collisionResults;
222 
227  std::vector<double> radius;
228 
236 
237  typedef std::vector<GeomIndex> GeomIndexList;
238 
243  std::map < JointIndex, GeomIndexList > innerObjects;
244 
249  std::map < JointIndex, GeomIndexList > outerObjects;
250 #endif // PINOCCHIO_WITH_HPP_FCL
251 
252  GeometryData(const GeometryModel & geomModel);
253  ~GeometryData() {};
254 
255 #ifdef PINOCCHIO_WITH_HPP_FCL
256 
266  void fillInnerOuterObjectMaps(const GeometryModel & geomModel);
267 
281  void activateCollisionPair(const PairIndex pairId);
282 
292  void deactivateCollisionPair(const PairIndex pairId);
293 
294 #endif //PINOCCHIO_WITH_HPP_FCL
295  friend std::ostream & operator<<(std::ostream & os, const GeometryData & geomData);
296 
297  }; // struct GeometryData
298 
299 } // namespace pinocchio
300 
301 /* --- Details -------------------------------------------------------------- */
302 /* --- Details -------------------------------------------------------------- */
303 /* --- Details -------------------------------------------------------------- */
304 #include "pinocchio/multibody/geometry.hxx"
305 
306 #endif // ifndef __pinocchio_multibody_geometry_hpp__
GeomIndex getGeometryId(const std::string &name) const
Return the index of a GeometryObject given by its name.
bool operator==(const GeometryModel &other) const
Returns true if *this and other are equal.
GeomIndex addGeometryObject(const GeometryObject &object, const ModelTpl< S2, O2, _JointCollectionTpl > &model)
Add a geometry object to a GeometryModel and set its parent joint.
CollisionPairVector collisionPairs
Vector of collision pairs.
std::map< JointIndex, GeomIndexList > outerObjects
A list of associated collision GeometryObjects to a given joint Id.
std::vector< fcl::DistanceResult > distanceResults
Vector gathering the result of the distance computation for all the collision pairs.
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
std::vector< fcl::CollisionResult > collisionResults
Vector gathering the result of the collision computation for all the collision pairs.
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().
std::vector< double > radius
Radius of the bodies, i.e. distance of the further point of the geometry model attached to the body f...
void removeCollisionPair(const CollisionPair &pair)
Remove if exists the CollisionPair from the vector collision_pairs.
fcl::DistanceRequest distanceRequest
Defines what information should be computed by distance computation.
Index ngeoms
The number of GeometryObjects.
std::vector< bool > activeCollisionPairs
Vector of collision pairs.
void addAllCollisionPairs()
Add all possible collision pairs.
Main pinocchio namespace.
Definition: treeview.dox:24
fcl::CollisionRequest collisionRequest
Defines what information should be computed by collision test.
std::map< JointIndex, GeomIndexList > innerObjects
Map over vector GeomModel::geometryObjects, indexed by joints.
bool operator!=(const GeometryModel &other) const
Returns true if *this and other are not equal.
PairIndex collisionPairIndex
index of the collision pair
bool existGeometryName(const std::string &name) const
Check if a GeometryObject given by its name exists.
void addCollisionPair(const CollisionPair &pair)
Add a collision pair into the vector of collision_pairs. The method check before if the given Collisi...
PairIndex findCollisionPair(const CollisionPair &pair) const
Return the index of a given collision pair in collisionPairs.
GeometryObjectVector geometryObjects
Vector of GeometryObjects used for collision computations.