Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2022 INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#ifndef __pinocchio_collision_broadphase_callback_hpp__ |
6 |
|
|
#define __pinocchio_collision_broadphase_callback_hpp__ |
7 |
|
|
|
8 |
|
|
#include <hpp/fcl/broadphase/broadphase_callbacks.h> |
9 |
|
|
|
10 |
|
|
#include "pinocchio/multibody/fcl.hpp" |
11 |
|
|
#include "pinocchio/multibody/geometry.hpp" |
12 |
|
|
|
13 |
|
|
#include "pinocchio/collision/collision.hpp" |
14 |
|
|
|
15 |
|
|
namespace pinocchio |
16 |
|
|
{ |
17 |
|
|
|
18 |
|
|
/// @brief Interface for Pinocchio collision callback functors |
19 |
|
|
struct CollisionCallBackBase : hpp::fcl::CollisionCallBackBase |
20 |
|
|
{ |
21 |
|
✗ |
CollisionCallBackBase(const GeometryModel & geometry_model, GeometryData & geometry_data) |
22 |
|
✗ |
: geometry_model_ptr(&geometry_model) |
23 |
|
✗ |
, geometry_data_ptr(&geometry_data) |
24 |
|
✗ |
, collision(false) |
25 |
|
✗ |
, accumulate(false) |
26 |
|
|
{ |
27 |
|
|
} |
28 |
|
|
|
29 |
|
✗ |
const GeometryModel & getGeometryModel() const |
30 |
|
|
{ |
31 |
|
✗ |
return *geometry_model_ptr; |
32 |
|
|
} |
33 |
|
|
const GeometryData & getGeometryData() const |
34 |
|
|
{ |
35 |
|
|
return *geometry_data_ptr; |
36 |
|
|
} |
37 |
|
✗ |
GeometryData & getGeometryData() |
38 |
|
|
{ |
39 |
|
✗ |
return *geometry_data_ptr; |
40 |
|
|
} |
41 |
|
|
|
42 |
|
|
/// \brief If true, the stopping criteria related to the collision callback has been met and one |
43 |
|
|
/// can stop. |
44 |
|
|
virtual bool stop() const = 0; |
45 |
|
|
|
46 |
|
|
/// \brief Callback method called after the termination of a collisition detection algorithm. |
47 |
|
|
/// The default implementation does nothing. |
48 |
|
✗ |
virtual void done() {}; |
49 |
|
|
|
50 |
|
|
protected: |
51 |
|
|
/// @brief Geometry model associated to the callback |
52 |
|
|
const GeometryModel * geometry_model_ptr; |
53 |
|
|
|
54 |
|
|
/// @brief Geometry data associated to the callback |
55 |
|
|
GeometryData * geometry_data_ptr; |
56 |
|
|
|
57 |
|
|
public: |
58 |
|
|
/// @brief Whether there is a collision or not |
59 |
|
|
bool collision; |
60 |
|
|
|
61 |
|
|
/// @brief Whether the callback is used in an accumulate mode where several collide methods are |
62 |
|
|
/// called successively. |
63 |
|
|
bool accumulate; |
64 |
|
|
}; |
65 |
|
|
|
66 |
|
|
struct CollisionCallBackDefault : CollisionCallBackBase |
67 |
|
|
{ |
68 |
|
✗ |
CollisionCallBackDefault( |
69 |
|
|
const GeometryModel & geometry_model, |
70 |
|
|
GeometryData & geometry_data, |
71 |
|
|
bool stopAtFirstCollision = false) |
72 |
|
✗ |
: CollisionCallBackBase(geometry_model, geometry_data) |
73 |
|
✗ |
, stopAtFirstCollision(stopAtFirstCollision) |
74 |
|
✗ |
, count(0) |
75 |
|
|
// , visited(Eigen::MatrixXd::Zero(geometry_model.ngeoms,geometry_model.ngeoms)) |
76 |
|
|
{ |
77 |
|
|
} |
78 |
|
|
|
79 |
|
✗ |
void init() |
80 |
|
|
{ |
81 |
|
✗ |
if (accumulate) // skip reseting of the parameters |
82 |
|
✗ |
return; |
83 |
|
|
|
84 |
|
✗ |
count = 0; |
85 |
|
✗ |
collision = false; |
86 |
|
✗ |
collisionPairIndex = std::numeric_limits<PairIndex>::max(); |
87 |
|
|
// visited.setZero(); |
88 |
|
|
} |
89 |
|
|
|
90 |
|
✗ |
bool collide(hpp::fcl::CollisionObject * o1, hpp::fcl::CollisionObject * o2) |
91 |
|
|
{ |
92 |
|
|
|
93 |
|
✗ |
assert(!stop() && "must never happened"); |
94 |
|
|
|
95 |
|
✗ |
CollisionObject & co1 = reinterpret_cast<CollisionObject &>(*o1); |
96 |
|
✗ |
CollisionObject & co2 = reinterpret_cast<CollisionObject &>(*o2); |
97 |
|
|
|
98 |
|
✗ |
const Eigen::DenseIndex go1_index = (Eigen::DenseIndex)co1.geometryObjectIndex; |
99 |
|
✗ |
const Eigen::DenseIndex go2_index = (Eigen::DenseIndex)co2.geometryObjectIndex; |
100 |
|
|
|
101 |
|
✗ |
const GeometryModel & geometry_model = *geometry_model_ptr; |
102 |
|
|
|
103 |
|
✗ |
PINOCCHIO_CHECK_INPUT_ARGUMENT( |
104 |
|
|
go1_index < (Eigen::DenseIndex)geometry_model.ngeoms && go1_index >= 0); |
105 |
|
✗ |
PINOCCHIO_CHECK_INPUT_ARGUMENT( |
106 |
|
|
go2_index < (Eigen::DenseIndex)geometry_model.ngeoms && go2_index >= 0); |
107 |
|
|
|
108 |
|
✗ |
const int pair_index = geometry_model.collisionPairMapping(go1_index, go2_index); |
109 |
|
✗ |
if (pair_index == -1) |
110 |
|
✗ |
return false; |
111 |
|
|
|
112 |
|
✗ |
const GeometryData & geometry_data = *geometry_data_ptr; |
113 |
|
✗ |
const CollisionPair & cp = geometry_model.collisionPairs[(PairIndex)pair_index]; |
114 |
|
|
const bool do_collision_check = |
115 |
|
✗ |
geometry_data.activeCollisionPairs[(PairIndex)pair_index] |
116 |
|
✗ |
&& !( |
117 |
|
✗ |
geometry_model.geometryObjects[cp.first].disableCollision |
118 |
|
✗ |
|| geometry_model.geometryObjects[cp.second].disableCollision); |
119 |
|
✗ |
if (!do_collision_check) |
120 |
|
✗ |
return false; |
121 |
|
|
|
122 |
|
✗ |
count++; |
123 |
|
|
|
124 |
|
|
fcl::CollisionRequest collision_request( |
125 |
|
✗ |
geometry_data_ptr->collisionRequests[size_t(pair_index)]); |
126 |
|
✗ |
collision_request.gjk_variant = fcl::GJKVariant::NesterovAcceleration; |
127 |
|
|
// collision_request.gjk_initial_guess = fcl::GJKInitialGuess::BoundingVolumeGuess; |
128 |
|
|
|
129 |
|
✗ |
if ( |
130 |
|
✗ |
co1.collisionGeometry().get() |
131 |
|
✗ |
!= geometry_model.geometryObjects[size_t(go1_index)].geometry.get() |
132 |
|
✗ |
|| co2.collisionGeometry().get() |
133 |
|
✗ |
!= geometry_model.geometryObjects[size_t(go2_index)].geometry.get()) |
134 |
|
✗ |
PINOCCHIO_THROW_PRETTY( |
135 |
|
|
std::logic_error, "go1: " << go1_index << " or go2: " << go2_index |
136 |
|
|
<< " have not been updated and have missmatching pointers."); |
137 |
|
|
// if(!(co1.collisionGeometry()->aabb_local.volume() < 0 || |
138 |
|
|
// co2.collisionGeometry()->aabb_local.volume() <0)) { // TODO(jcarpent): check potential |
139 |
|
|
// bug |
140 |
|
|
// collision_request.gjk_initial_guess = fcl::GJKInitialGuess::BoundingVolumeGuess; |
141 |
|
|
// } |
142 |
|
|
|
143 |
|
|
bool res; |
144 |
|
|
try |
145 |
|
|
{ |
146 |
|
✗ |
res = computeCollision( |
147 |
|
✗ |
*geometry_model_ptr, *geometry_data_ptr, (PairIndex)pair_index, collision_request); |
148 |
|
|
} |
149 |
|
✗ |
catch (std::logic_error & e) |
150 |
|
|
{ |
151 |
|
✗ |
PINOCCHIO_THROW_PRETTY( |
152 |
|
|
std::logic_error, "Geometries with index go1: " |
153 |
|
|
<< go1_index << " or go2: " << go2_index |
154 |
|
|
<< " have produced an internal error within HPP-FCL.\n what:\n" |
155 |
|
|
<< e.what()); |
156 |
|
|
} |
157 |
|
|
|
158 |
|
✗ |
if (res && !collision) |
159 |
|
|
{ |
160 |
|
✗ |
collision = true; |
161 |
|
✗ |
collisionPairIndex = (PairIndex)pair_index; |
162 |
|
|
} |
163 |
|
|
|
164 |
|
✗ |
if (!stopAtFirstCollision) |
165 |
|
✗ |
return false; |
166 |
|
|
else |
167 |
|
✗ |
return res; |
168 |
|
|
} |
169 |
|
|
|
170 |
|
✗ |
bool stop() const final |
171 |
|
|
{ |
172 |
|
✗ |
if (stopAtFirstCollision && collision) |
173 |
|
✗ |
return true; |
174 |
|
|
|
175 |
|
✗ |
return false; |
176 |
|
|
} |
177 |
|
|
|
178 |
|
✗ |
void done() final |
179 |
|
|
{ |
180 |
|
✗ |
if (collision) |
181 |
|
✗ |
geometry_data_ptr->collisionPairIndex = collisionPairIndex; |
182 |
|
|
} |
183 |
|
|
|
184 |
|
|
/// @brief Whether to stop or not when localizing a first collision |
185 |
|
|
bool stopAtFirstCollision; |
186 |
|
|
|
187 |
|
|
/// @brief The collision index of the first pair in collision |
188 |
|
|
PairIndex collisionPairIndex; |
189 |
|
|
|
190 |
|
|
/// @brief Number of visits of the collide method |
191 |
|
|
size_t count; |
192 |
|
|
|
193 |
|
|
// Eigen::MatrixXd visited; |
194 |
|
|
}; |
195 |
|
|
|
196 |
|
|
} // namespace pinocchio |
197 |
|
|
|
198 |
|
|
/* --- Details -------------------------------------------------------------------- */ |
199 |
|
|
#include "pinocchio/collision/broadphase-callbacks.hxx" |
200 |
|
|
|
201 |
|
|
#endif // ifndef __pinocchio_collision_broadphase_callback_hpp__ |
202 |
|
|
|