pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
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
18namespace 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 {
59
60 typedef typename traits<GeometryModel>::Scalar Scalar;
61 enum
62 {
64 };
65
67
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)
80 {
81 }
82
83 GeometryModel(const GeometryModel & other) = default;
84
85 ~GeometryModel() {};
86
96 template<typename S2, int O2, template<typename, int> class _JointCollectionTpl>
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
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
204 && collisionPairMapping == other.collisionPairMapping;
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
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 {
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
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
340
347
354
357
360
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
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
464 && distanceRequests == other.distanceRequests
465 && distanceResults == other.distanceResults
466 && collisionRequests == other.collisionRequests
467 && collisionResults == other.collisionResults && radius == other.radius
468 && collisionPairIndex == other.collisionPairIndex
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
GeometryData & operator=(const GeometryData &other)
Copy operator.
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
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