GCC Code Coverage Report

Line	Branch	Exec	Source
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
18			namespace pinocchio
19			{
20
21			struct CollisionPair : public std::pair<GeomIndex, GeomIndex>
22			{
23
24			typedef std::pair<GeomIndex, GeomIndex> Base;
25
26			/// \brief Empty constructor
27			CollisionPair();
28
29			///
30			/// \brief Default constructor of a collision pair from two collision object indexes.
31			/// \remarks The two indexes must be different, otherwise the constructor throws.
32			///
33			/// \param[in] co1 Index of the first collision object.
34			/// \param[in] co2 Index of the second collision object.
35			///
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<>
45			struct traits<GeometryModel>
46			{
47			typedef context::Scalar Scalar;
48			};
49
50			struct GeometryModel
51			: NumericalBase<GeometryModel>
52			, serialization::Serializable<GeometryModel>
53			{
54			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
55
56			typedef context::Scalar Scalar;
57			enum
58			{
59			Options = context::Options
60			};
61
62			typedef SE3Tpl<Scalar, Options> SE3;
63
64			typedef ::pinocchio::GeometryObject GeometryObject;
65			typedef PINOCCHIO_ALIGNED_STD_VECTOR(GeometryObject) GeometryObjectVector;
66			typedef std::vector<CollisionPair> CollisionPairVector;
67			typedef Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXb;
68			typedef Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXi;
69
70			typedef pinocchio::GeomIndex GeomIndex;
71
72		35	GeometryModel()
73		35	: ngeoms(0)
74		35	, geometryObjects()
75	1/2 ✓ Branch 2 taken 35 times. ✗ Branch 3 not taken.	35	, collisionPairs()
76			{
77		35	}
78
79	2/4 ✓ Branch 2 taken 27 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 27 times. ✗ Branch 6 not taken.	27	GeometryModel(const GeometryModel & other) = default;
80
81		61	~GeometryModel() {};
82
83			/**
84			* @brief Add a geometry object to a GeometryModel and set its parent joint.
85			*
86			* @param[in] object Object
87			* @param[in] model Corresponding model, used to assert the attributes of object.
88			*
89			* @return The index of the new added GeometryObject in geometryObjects
90			* @note object is a nonconst copy to ease the insertion code.
91			*/
92			template<typename S2, int O2, template<typename, int> class _JointCollectionTpl>
93			GeomIndex addGeometryObject(
94			const GeometryObject & object, const ModelTpl<S2, O2, _JointCollectionTpl> & model);
95
96			/**
97			* @brief Add a geometry object to a GeometryModel.
98			*
99			* @param[in] object Object
100			*
101			* @return The index of the new added GeometryObject in geometryObjects.
102			*/
103			GeomIndex addGeometryObject(const GeometryObject & object);
104
105			/**
106			* @brief Remove a GeometryObject
107			*
108			* @param[in] name Name of the GeometryObject
109			*
110			* @node Remove also the collision pairs that contain the object.
111			*/
112			void removeGeometryObject(const std::string & name);
113
114			/**
115			* @brief Return the index of a GeometryObject given by its name.
116			*
117			* @param[in] name Name of the GeometryObject
118			*
119			* @return Index of the corresponding GeometryObject
120			*/
121			GeomIndex getGeometryId(const std::string & name) const;
122
123			/**
124			* @brief Check if a GeometryObject given by its name exists.
125			*
126			* @param[in] name Name of the GeometryObject
127			*
128			* @return True if the GeometryObject exists in the geometryObjects.
129			*/
130			bool existGeometryName(const std::string & name) const;
131
132			///
133			/// \brief Add a collision pair into the vector of collision_pairs.
134			/// The method check before if the given CollisionPair is already included.
135			///
136			/// \param[in] pair The CollisionPair to add.
137			///
138			void addCollisionPair(const CollisionPair & pair);
139
140			///
141			/// \brief Add all possible collision pairs.
142			///
143			/// \note Collision pairs between geometries having the same parent joint are not added.
144			///
145			void addAllCollisionPairs();
146
147			///
148			/// \brief Set the collision pairs from a given input array.
149			/// Each entry of the input matrix defines the activation of a given collision pair
150			/// (map[i,j] == true means that the pair (i,j) is active).
151			///
152			/// \param[in] collision_map Associative array.
153			/// \param[in] upper Wheter the collision_map is an upper or lower triangular filled array.
154			///
155			void setCollisionPairs(const MatrixXb & collision_map, const bool upper = true);
156
157			///
158			/// \brief Remove if exists the CollisionPair from the vector collision_pairs.
159			///
160			/// \param[in] pair The CollisionPair to remove.
161			///
162			void removeCollisionPair(const CollisionPair & pair);
163
164			///
165			/// \brief Remove all collision pairs from collisionPairs. Same as collisionPairs.clear().
166			///
167			void removeAllCollisionPairs();
168
169			///
170			/// \brief Check if a collision pair exists in collisionPairs.
171			/// See also findCollisitionPair(const CollisionPair & pair).
172			///
173			/// \param[in] pair The CollisionPair.
174			///
175			/// \return True if the CollisionPair exists, false otherwise.
176			///
177			bool existCollisionPair(const CollisionPair & pair) const;
178
179			///
180			/// \brief Return the index of a given collision pair in collisionPairs.
181			///
182			/// \param[in] pair The CollisionPair.
183			///
184			/// \return The index of the CollisionPair in collisionPairs.
185			///
186			PairIndex findCollisionPair(const CollisionPair & pair) const;
187
188			///
189			/// \brief Create a deep copy of *this.
190			///
191			GeometryModel clone() const;
192
193			///
194			/// \brief Returns true if *this and other are equal.
195			///
196		✗	bool operator==(const GeometryModel & other) const
197			{
198		✗	return ngeoms == other.ngeoms && geometryObjects == other.geometryObjects
199		✗	&& collisionPairs == other.collisionPairs
200		✗	&& collisionPairMapping == other.collisionPairMapping;
201			}
202
203			///
204			/// \brief Returns true if *this and other are not equal.
205			///
206		✗	bool operator!=(const GeometryModel & other) const
207			{
208		✗	return !(*this == other);
209			}
210
211			friend std::ostream & operator<<(std::ostream & os, const GeometryModel & model_geom);
212
213			/// \brief The number of GeometryObjects
214			Index ngeoms;
215
216			/// \brief Vector of GeometryObjects used for collision computations
217			GeometryObjectVector geometryObjects;
218
219			/// \brief Vector of collision pairs.
220			CollisionPairVector collisionPairs;
221
222			/// \brief Matrix relating the collision pair ID to a pair of two GeometryObject indexes
223			MatrixXi collisionPairMapping;
224
225			}; // struct GeometryModel
226
227			template<>
228			struct traits<GeometryData>
229			{
230			typedef context::Scalar Scalar;
231			};
232
233			struct GeometryData
234			: NumericalBase<GeometryData>
235			, serialization::Serializable<GeometryData>
236			{
237			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
238
239			typedef context::Scalar Scalar;
240			enum
241			{
242			Options = context::Options
243			};
244
245			typedef SE3Tpl<Scalar, Options> SE3;
246			typedef std::vector<GeomIndex> GeomIndexList;
247			typedef Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXb;
248			typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
249
250			#ifdef PINOCCHIO_WITH_HPP_FCL
251			typedef ::pinocchio::ComputeCollision ComputeCollision;
252			typedef ::pinocchio::ComputeDistance ComputeDistance;
253			#endif
254
255			///
256			/// \brief Vector gathering the SE3 placements of the geometry objects relative to the world.
257			/// See updateGeometryPlacements to update the placements.
258			///
259			/// oMg is used for pinocchio (kinematics) computation but is translated to fcl type
260			/// for fcl (collision) computation. The copy is done in collisionObjects[i]->setTransform(.)
261			///
262			PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMg;
263
264			///
265			/// \brief Vector of collision pairs.
266			///
267			std::vector<bool> activeCollisionPairs;
268
269			#ifdef PINOCCHIO_WITH_HPP_FCL
270
271			///
272			/// \brief Defines what information should be computed by distance computation.
273			/// There is one request per pair of geometries.
274			std::vector<fcl::DistanceRequest> distanceRequests;
275
276			///
277			/// \brief Vector gathering the result of the distance computation for all the collision pairs.
278			///
279			std::vector<fcl::DistanceResult> distanceResults;
280
281			///
282			/// \brief Defines what information should be computed by collision test.
283			/// There is one request per pair of geometries.
284			std::vector<fcl::CollisionRequest> collisionRequests;
285
286			///
287			/// \brief Vector gathering the result of the collision computation for all the collision pairs.
288			///
289			std::vector<fcl::CollisionResult> collisionResults;
290
291			///
292			/// \brief Radius of the bodies, i.e. distance of the further point of the geometry model
293			/// attached to the body from the joint center.
294			///
295			std::vector<Scalar> radius;
296
297			///
298			/// \brief Index of the collision pair
299			///
300			/// It is used by some method to return additional information. For instance,
301			/// the algo computeCollisions() sets it to the first colliding pair.
302			///
303			PairIndex collisionPairIndex;
304
305			/// \brief Functor associated to the computation of collisions.
306			PINOCCHIO_ALIGNED_STD_VECTOR(ComputeCollision) collision_functors;
307
308			/// \brief Functor associated to the computation of distances.
309			PINOCCHIO_ALIGNED_STD_VECTOR(ComputeDistance) distance_functors;
310
311			#endif // PINOCCHIO_WITH_HPP_FCL
312
313			/// \brief Map over vector GeomModel::geometryObjects, indexed by joints.
314			///
315			/// The map lists the collision GeometryObjects associated to a given joint Id.
316			/// Inner objects can be seen as geometry objects that directly move when the associated joint
317			/// moves
318			std::map<JointIndex, GeomIndexList> innerObjects;
319
320			/// \brief A list of associated collision GeometryObjects to a given joint Id
321			///
322			/// Outer objects can be seen as geometry objects that may often be
323			/// obstacles to the Inner objects of a given joint
324			std::map<JointIndex, GeomIndexList> outerObjects;
325
326			///
327			/// \brief Default constructor from a GeometryModel
328			///
329			/// \param[in] geom_model GeometryModel associated to the new GeometryData
330			///
331			explicit GeometryData(const GeometryModel & geom_model);
332
333			///
334			/// \brief Copy constructor
335			///
336			/// \param[in] other GeometryData to copy
337			///
338			GeometryData(const GeometryData & other);
339
340			///
341			/// \brief Copy operator
342			///
343			/// \param[in] other GeometryData to copy
344			///
345			GeometryData & operator=(const GeometryData & other);
346
347			/// \brief Empty constructor
348		✗	GeometryData() {};
349
350			/// \brief Destructor
351			~GeometryData();
352
353			/// Fill both innerObjects and outerObjects maps, from vectors collisionObjects and
354			/// collisionPairs.
355			///
356			/// This simply corresponds to storing in a re-arranged manner the information stored
357			/// in geomModel.geometryObjects and geomModel.collisionPairs.
358			/// \param[in] geomModel the geometry model (const)
359			///
360			/// \warning Outer objects are not duplicated (i.e. if a is in outerObjects[b], then
361			/// b is not in outerObjects[a]).
362			void fillInnerOuterObjectMaps(const GeometryModel & geomModel);
363
364			///
365			/// Activate a collision pair, for which collisions and distances would now be computed.
366			///
367			/// The collision (resp distance) between two geometries of GeomModel::geometryObjects
368			/// is computed iff the corresponding pair has been added in GeomModel::collisionPairs AND
369			/// it is active, i.e. the corresponding boolean in GeomData::activePairs is true. The second
370			/// condition can be used to temporarily remove a pair without touching the model, in a
371			/// versatile manner.
372			///
373			/// \param[in] pair_id the index of the pair in GeomModel::collisionPairs vector.
374			///
375			/// \sa GeomData
376			///
377			void activateCollisionPair(const PairIndex pair_id);
378
379			///
380			/// \brief Activate all collision pairs.
381			///
382			/// \sa GeomData::deactivateAllCollisionPairs, GeomData::activateCollisionPair,
383			/// GeomData::deactivateCollisionPair
384			///
385			void activateAllCollisionPairs();
386
387			///
388			/// \brief Set the collision pair association from a given input array.
389			/// Each entry of the input matrix defines the activation of a given collision pair.
390			///
391			/// \param[in] geom_model Geometry model associated to the data.
392			/// \param[in] collision_map Associative array.
393			/// \param[in] upper Wheter the collision_map is an upper or lower triangular filled array.
394			///
395			void setActiveCollisionPairs(
396			const GeometryModel & geom_model, const MatrixXb & collision_map, const bool upper = true);
397
398			///
399			/// \brief Enable or disable collision for the given geometry given by its geometry id with all
400			/// the other geometries registered in the list of collision pairs.
401			///
402			/// \param[in] geom_model Geometry model associated to the data.
403			/// \param[in] geom_id Index of the geometry.
404			/// \param[in] enable_collision If true, the collision will be enable, otherwise disable.
405			///
406			void setGeometryCollisionStatus(
407			const GeometryModel & geom_model, const GeomIndex geom_id, bool enable_collision);
408
409			///
410			/// Deactivate a collision pair.
411			///
412			/// Calls indeed GeomData::activateCollisionPair(pair_id)
413			///
414			/// \param[in] pair_id the index of the pair in GeomModel::collisionPairs vector.
415			///
416			/// \sa GeomData::activateCollisionPair
417			///
418			void deactivateCollisionPair(const PairIndex pair_id);
419
420			///
421			/// \brief Deactivate all collision pairs.
422			///
423			/// \sa GeomData::activateAllCollisionPairs, GeomData::activateCollisionPair,
424			/// GeomData::deactivateCollisionPair
425			///
426			void deactivateAllCollisionPairs();
427
428			#ifdef PINOCCHIO_WITH_HPP_FCL
429			///
430			/// \brief Set the security margin of all the collision request in a row, according to the
431			/// values stored in the associative map.
432			///
433			/// \param[in] geom_model Geometry model associated to the data.
434			/// \param[in] security_margin_map Associative map related the security margin of a given input
435			/// collision pair (i,j). \param[in] upper Wheter the security_margin_map is an upper or lower
436			/// triangular filled array. \param[in] sync_distance_upper_bound Wheter distance_upper_bound
437			/// have fields to be updated with the security margin value.
438			///
439			void setSecurityMargins(
440			const GeometryModel & geom_model,
441			const MatrixXs & security_margin_map,
442			const bool upper = true,
443			const bool sync_distance_upper_bound = false);
444
445			#endif // ifdef PINOCCHIO_WITH_HPP_FCL
446
447			friend std::ostream & operator<<(std::ostream & os, const GeometryData & geomData);
448
449			///
450			/// \brief Returns true if *this and other are equal.
451			///
452		✗	bool operator==(const GeometryData & other) const
453			{
454		✗	return oMg == other.oMg && activeCollisionPairs == other.activeCollisionPairs
455			#ifdef PINOCCHIO_WITH_HPP_FCL
456		✗	&& distanceRequests == other.distanceRequests
457		✗	&& distanceResults == other.distanceResults
458		✗	&& collisionRequests == other.collisionRequests
459		✗	&& collisionResults == other.collisionResults && radius == other.radius
460		✗	&& collisionPairIndex == other.collisionPairIndex
461			#endif
462		✗	&& innerObjects == other.innerObjects && outerObjects == other.outerObjects;
463			}
464
465			///
466			/// \brief Returns true if *this and other are not equal.
467			///
468		✗	bool operator!=(const GeometryData & other) const
469			{
470		✗	return !(*this == other);
471			}
472
473			}; // struct GeometryData
474
475			} // namespace pinocchio
476
477			/* --- Details -------------------------------------------------------------- */
478			/* --- Details -------------------------------------------------------------- */
479			/* --- Details -------------------------------------------------------------- */
480			#include "pinocchio/multibody/geometry.hxx"
481
482			#endif // ifndef __pinocchio_multibody_geometry_hpp__
483