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