Directory: | ./ |
---|---|
File: | include/pinocchio/multibody/geometry.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 7 | 24 | 29.2% |
Branches: | 3 | 38 | 7.9% |
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 |