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