pinocchio  2.2.1-dirty
multibody/geometry.hpp
1 //
2 // Copyright (c) 2015-2018 CNRS
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 
33  typedef std::vector<CollisionPair> CollisionPairVector;
34 
35  typedef pinocchio::GeomIndex GeomIndex;
36 
38  Index ngeoms;
39 
41  GeometryObjectVector geometryObjects;
45  CollisionPairVector collisionPairs;
46 
48  : ngeoms(0)
49  , geometryObjects()
50  , collisionPairs()
51  {
52  const std::size_t num_max_collision_pairs = (ngeoms * (ngeoms-1))/2;
53  collisionPairs.reserve(num_max_collision_pairs);
54  }
55 
56  ~GeometryModel() {};
57 
67  template<typename S2, int O2, template<typename,int> class _JointCollectionTpl>
68  GeomIndex addGeometryObject(const GeometryObject & object,
70 
78  GeomIndex addGeometryObject(const GeometryObject & object);
79 
87  GeomIndex getGeometryId(const std::string & name) const;
88 
89 
97  bool existGeometryName(const std::string & name) const;
98 
99 #ifdef PINOCCHIO_WITH_HPP_FCL
100  void addCollisionPair(const CollisionPair & pair);
107 
114  void addAllCollisionPairs();
115 
121  void removeCollisionPair(const CollisionPair& pair);
122 
125  void removeAllCollisionPairs ();
126 
135  bool existCollisionPair(const CollisionPair & pair) const;
136 
144  PairIndex findCollisionPair(const CollisionPair & pair) const;
145 
146 #endif // PINOCCHIO_WITH_HPP_FCL
147 
148  friend std::ostream& operator<<(std::ostream & os, const GeometryModel & model_geom);
149  }; // struct GeometryModel
150 
152  {
153  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
154 
155  typedef double Scalar;
156  enum { Options = 0 };
157 
158  typedef SE3Tpl<Scalar,Options> SE3;
159 
168 
169 #ifdef PINOCCHIO_WITH_HPP_FCL
170  std::vector<fcl::CollisionObject> collisionObjects;
177 
181  std::vector<bool> activeCollisionPairs;
182 
186  fcl::DistanceRequest distanceRequest;
187 
191  std::vector<fcl::DistanceResult> distanceResults;
192 
196  fcl::CollisionRequest collisionRequest;
197 
201  std::vector<fcl::CollisionResult> collisionResults;
202 
207  std::vector<double> radius;
208 
216 
217  typedef std::vector<GeomIndex> GeomIndexList;
218 
223  std::map < JointIndex, GeomIndexList > innerObjects;
224 
229  std::map < JointIndex, GeomIndexList > outerObjects;
230 #endif // PINOCCHIO_WITH_HPP_FCL
231 
232  GeometryData(const GeometryModel & geomModel);
233  ~GeometryData() {};
234 
235 #ifdef PINOCCHIO_WITH_HPP_FCL
236 
246  void fillInnerOuterObjectMaps(const GeometryModel & geomModel);
247 
261  void activateCollisionPair(const PairIndex pairId);
262 
272  void deactivateCollisionPair(const PairIndex pairId);
273 
274 #endif //PINOCCHIO_WITH_HPP_FCL
275  friend std::ostream & operator<<(std::ostream & os, const GeometryData & geomData);
276 
277  }; // struct GeometryData
278 
279 } // namespace pinocchio
280 
281 /* --- Details -------------------------------------------------------------- */
282 /* --- Details -------------------------------------------------------------- */
283 /* --- Details -------------------------------------------------------------- */
284 #include "pinocchio/multibody/geometry.hxx"
285 
286 #endif // ifndef __pinocchio_multibody_geometry_hpp__
bool existGeometryName(const std::string &name) const
Check if a GeometryObject given by its name exists.
container::aligned_vector< SE3 > oMg
Vector gathering the SE3 placements of the geometry objects relative to the world. See updateGeometryPlacements to update the placements.
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.
GeomIndex getGeometryId(const std::string &name) const
Return the index of a GeometryObject given by its name.
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.
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.
PairIndex collisionPairIndex
index of the collision pair
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 addCollisionPair(const CollisionPair &pair)
Add a collision pair into the vector of collision_pairs. The method check before if the given Collisi...
GeometryObjectVector geometryObjects
Vector of GeometryObjects used for collision computations.