pinocchio  2.6.3
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
geometry.hpp
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 
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 
93  void addCollisionPair(const CollisionPair & pair);
94 
100  void addAllCollisionPairs();
101 
110  void setCollisionPairs(const MatrixXb & collision_map,
111  const bool upper = true);
112 
118  void removeCollisionPair(const CollisionPair& pair);
119 
124 
133  bool existCollisionPair(const CollisionPair & pair) const;
134 
142  PairIndex findCollisionPair(const CollisionPair & pair) const;
143 
147  bool operator==(const GeometryModel & other) const
148  {
149  return
150  ngeoms == other.ngeoms
151  && geometryObjects == other.geometryObjects
152  && collisionPairs == other.collisionPairs
153  ;
154  }
155 
159  bool operator!=(const GeometryModel & other) const
160  {
161  return !(*this == other);
162  }
163 
164  friend std::ostream& operator<<(std::ostream & os,
165  const GeometryModel & model_geom);
166 
168  Index ngeoms;
169 
171  GeometryObjectVector geometryObjects;
172 
174  CollisionPairVector collisionPairs;
175 
176  }; // struct GeometryModel
177 
179  : serialization::Serializable<GeometryData>
180  {
181  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
182 
183  typedef double Scalar;
184  enum { Options = 0 };
185 
186  typedef SE3Tpl<Scalar,Options> SE3;
187  typedef std::vector<GeomIndex> GeomIndexList;
188  typedef Eigen::Matrix<bool,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXb;
189  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXs;
190 
191 #ifdef PINOCCHIO_WITH_HPP_FCL
192  typedef ::pinocchio::ComputeCollision ComputeCollision;
193  typedef ::pinocchio::ComputeDistance ComputeDistance;
194 #endif
195 
204 
208  std::vector<bool> activeCollisionPairs;
209 
210 #ifdef PINOCCHIO_WITH_HPP_FCL
211 
215  std::vector<fcl::DistanceRequest> distanceRequests;
216 
220  std::vector<fcl::DistanceResult> distanceResults;
221 
225  std::vector<fcl::CollisionRequest> collisionRequests;
226 
230  std::vector<fcl::CollisionResult> collisionResults;
231 
236  std::vector<Scalar> radius;
237 
245 
248 
251 
252 #endif // PINOCCHIO_WITH_HPP_FCL
253 
258  std::map<JointIndex,GeomIndexList> innerObjects;
259 
264  std::map<JointIndex,GeomIndexList> outerObjects;
265 
271  explicit GeometryData(const GeometryModel & geom_model);
272 
278  GeometryData(const GeometryData & other);
279 
282 
284  ~GeometryData();
285 
295  void fillInnerOuterObjectMaps(const GeometryModel & geomModel);
296 
310  void activateCollisionPair(const PairIndex pair_id);
311 
318 
327  void setActiveCollisionPairs(const GeometryModel & geom_model,
328  const MatrixXb & collision_map,
329  const bool upper = true);
330 
338  void setGeometryCollisionStatus(const GeometryModel & geom_model,
339  const GeomIndex geom_id,
340  bool enable_collision);
341 
351  void deactivateCollisionPair(const PairIndex pair_id);
352 
359 
360 #ifdef PINOCCHIO_WITH_HPP_FCL
361  void setSecurityMargins(const GeometryModel & geom_model,
369  const MatrixXs & security_margin_map,
370  const bool upper = true);
371 #endif // ifdef PINOCCHIO_WITH_HPP_FCL
372 
373  friend std::ostream & operator<<(std::ostream & os, const GeometryData & geomData);
374 
378  bool operator==(const GeometryData & other) const
379  {
380  return
381  oMg == other.oMg
383 #ifdef PINOCCHIO_WITH_HPP_FCL
385  && distanceResults == other.distanceResults
388  && radius == other.radius
390 #endif
391  && innerObjects == other.innerObjects
392  && outerObjects == other.outerObjects
393  ;
394  }
395 
399  bool operator!=(const GeometryData & other) const
400  {
401  return !(*this == other);
402  }
403 
404  }; // struct GeometryData
405 
406 } // namespace pinocchio
407 
408 /* --- Details -------------------------------------------------------------- */
409 /* --- Details -------------------------------------------------------------- */
410 /* --- Details -------------------------------------------------------------- */
411 #include "pinocchio/multibody/geometry.hxx"
412 
413 #endif // ifndef __pinocchio_multibody_geometry_hpp__
pinocchio::GeometryData::fillInnerOuterObjectMaps
void fillInnerOuterObjectMaps(const GeometryModel &geomModel)
pinocchio::GeometryModel::existCollisionPair
bool existCollisionPair(const CollisionPair &pair) const
Check if a collision pair exists in collisionPairs. See also findCollisitionPair(const CollisionPair ...
pinocchio::GeometryData::activateAllCollisionPairs
void activateAllCollisionPairs()
Activate all collision pairs.
pinocchio::GeometryData::distanceRequests
std::vector< fcl::DistanceRequest > distanceRequests
Defines what information should be computed by distance computation. There is one request per pair of...
Definition: geometry.hpp:215
pinocchio::GeometryModel::getGeometryId
GeomIndex getGeometryId(const std::string &name) const
Return the index of a GeometryObject given by its name.
pinocchio::GeometryModel::existGeometryName
bool existGeometryName(const std::string &name) const
Check if a GeometryObject given by its name exists.
pinocchio::GeometryData::deactivateCollisionPair
void deactivateCollisionPair(const PairIndex pair_id)
pinocchio::SE3Tpl< Scalar, Options >
pinocchio::name
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
pinocchio::GeometryModel::geometryObjects
GeometryObjectVector geometryObjects
Vector of GeometryObjects used for collision computations.
Definition: geometry.hpp:171
pinocchio::GeometryData::activeCollisionPairs
std::vector< bool > activeCollisionPairs
Vector of collision pairs.
Definition: geometry.hpp:208
pinocchio::GeometryObject
Definition: fcl.hpp:118
pinocchio::GeometryData
Definition: geometry.hpp:178
pinocchio::GeometryModel::operator==
bool operator==(const GeometryModel &other) const
Returns true if *this and other are equal.
Definition: geometry.hpp:147
pinocchio::ComputeCollision
Definition: fcl.hpp:277
pinocchio::GeometryData::GeometryData
GeometryData()
Empty constructor.
Definition: geometry.hpp:281
pinocchio::GeometryModel::removeAllCollisionPairs
void removeAllCollisionPairs()
Remove all collision pairs from collisionPairs. Same as collisionPairs.clear().
pinocchio::GeometryModel::findCollisionPair
PairIndex findCollisionPair(const CollisionPair &pair) const
Return the index of a given collision pair in collisionPairs.
pinocchio::CollisionPair
Definition: fcl.hpp:55
pinocchio::GeometryData::setSecurityMargins
void setSecurityMargins(const GeometryModel &geom_model, const MatrixXs &security_margin_map, const bool upper=true)
Set the security margin of all the collision request in a row, according to the values stored in the ...
pinocchio::GeometryData::collisionPairIndex
PairIndex collisionPairIndex
Index of the collision pair.
Definition: geometry.hpp:244
pinocchio::GeometryModel::addAllCollisionPairs
void addAllCollisionPairs()
Add all possible collision pairs.
pinocchio::GeometryModel::ngeoms
Index ngeoms
The number of GeometryObjects.
Definition: geometry.hpp:168
pinocchio::GeometryData::setGeometryCollisionStatus
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::GeometryModel::collisionPairs
CollisionPairVector collisionPairs
Vector of collision pairs.
Definition: geometry.hpp:174
pinocchio::GeometryData::distanceResults
std::vector< fcl::DistanceResult > distanceResults
Vector gathering the result of the distance computation for all the collision pairs.
Definition: geometry.hpp:220
pinocchio::GeometryData::PINOCCHIO_ALIGNED_STD_VECTOR
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMg
Vector gathering the SE3 placements of the geometry objects relative to the world....
pinocchio::GeometryModel::addGeometryObject
GeomIndex addGeometryObject(const GeometryObject &object, const ModelTpl< S2, O2, _JointCollectionTpl > &model)
Add a geometry object to a GeometryModel and set its parent joint.
pinocchio::GeometryData::collisionRequests
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:225
pinocchio::ComputeDistance
Definition: fcl.hpp:305
pinocchio::GeometryModel::removeCollisionPair
void removeCollisionPair(const CollisionPair &pair)
Remove if exists the CollisionPair from the vector collision_pairs.
pinocchio::GeometryData::radius
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:236
pinocchio::GeometryData::operator==
bool operator==(const GeometryData &other) const
Returns true if *this and other are equal.
Definition: geometry.hpp:378
pinocchio::GeometryModel::setCollisionPairs
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...
pinocchio::GeometryData::~GeometryData
~GeometryData()
Destructor.
pinocchio::GeometryData::outerObjects
std::map< JointIndex, GeomIndexList > outerObjects
A list of associated collision GeometryObjects to a given joint Id.
Definition: geometry.hpp:264
pinocchio::GeometryData::deactivateAllCollisionPairs
void deactivateAllCollisionPairs()
Deactivate all collision pairs.
pinocchio::GeometryModel
Definition: geometry.hpp:22
pinocchio::GeometryData::innerObjects
std::map< JointIndex, GeomIndexList > innerObjects
Map over vector GeomModel::geometryObjects, indexed by joints.
Definition: geometry.hpp:258
pinocchio::ModelTpl
Definition: fwd.hpp:23
pinocchio::GeometryModel::addCollisionPair
void addCollisionPair(const CollisionPair &pair)
Add a collision pair into the vector of collision_pairs. The method check before if the given Collisi...
pinocchio::GeometryData::collisionResults
std::vector< fcl::CollisionResult > collisionResults
Vector gathering the result of the collision computation for all the collision pairs.
Definition: geometry.hpp:230
pinocchio::serialization::Serializable
Definition: serializable.hpp:16
pinocchio::GeometryData::setActiveCollisionPairs
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...
pinocchio::GeometryData::activateCollisionPair
void activateCollisionPair(const PairIndex pair_id)
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24
pinocchio::GeometryModel::operator!=
bool operator!=(const GeometryModel &other) const
Returns true if *this and other are not equal.
Definition: geometry.hpp:159
pinocchio::GeometryData::operator!=
bool operator!=(const GeometryData &other) const
Returns true if *this and other are not equal.
Definition: geometry.hpp:399