-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
penetration_as_point_pair_callback.cc
436 lines (386 loc) · 18.3 KB
/
penetration_as_point_pair_callback.cc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
#include "drake/geometry/proximity/penetration_as_point_pair_callback.h"
#include <limits>
#include <utility>
#include "drake/common/default_scalars.h"
#include "drake/common/eigen_types.h"
#include "drake/common/nice_type_name.h"
#include "drake/geometry/proximity/distance_to_point_callback.h"
#include "drake/geometry/query_results/signed_distance_to_point.h"
namespace drake {
namespace geometry {
namespace internal {
namespace penetration_as_point_pair {
using Eigen::Vector3d;
/* A functor to support ComputeNarrowPhasePenetration(). It computes the
penetration distance between a supported pair of geometries. Each overload to
the call operator reports the penetration distance (encoded in
PenetrationAsPointPair<T>) between the two given geometry arguments using the
functor's stored poses.
@tparam T Computation scalar type. */
template <typename T>
class PenetrationPairFunctor {
public:
/* Constructs the functor caching all parameters that do _not_ depend on
the shape type.
@param id_A Identifier of the first geometry passed to operator().
@param id_B Identifier of the second geometry passed to operator().
@param X_WA Pose of geometry A in the world frame.
@param X_WB Pose of geometry B in the world frame */
PenetrationPairFunctor(const GeometryId& id_A, const GeometryId& id_B,
const math::RigidTransform<T>& X_WA,
const math::RigidTransform<T>& X_WB)
: id_A_(id_A), id_B_(id_B), X_WA_(X_WA), X_WB_(X_WB) {}
/* @name Overloads in support of sphere-shape computation
Computes penetration between two shapes. If there is no penetration
`result` will not be modified.
Given a sphere A centered at Ao with radius r and another geometry B,
we want to compute
1. φ_A,B = the signed penetration between the two objects, which is positive
for overlapping objects and negative for non-overlapping objects.
2. Na, Nb = a pair of witness points, Na ∈ ∂A, Nb ∈ ∂B (not necessarily
unique), |Na-Nb| = |φ_A,B|.
Define these functions: (available from SignedDistanceToPoint)
φ_B:ℝ³→ℝ, φ_B(p) = signed distance to point p from B.
η_B:ℝ³→ℝ³, η_B(p) = a nearest point to p on the boundary ∂B (not
necessarily unique).
∇φ_B:ℝ³→ℝ³, ∇φ_B(p) = gradient vector of φ_B with respect to p.
It has unit length by construction.
Algorithm:
1. φ_A,B = r - φ_B(Ao)
2. Nb = η_B(Ao)
3. Na = Ao - r * ∇φ_B(Ao) */
//@{
void operator()(const fcl::Sphered& sphere_A, const fcl::Sphered& sphere_B,
PenetrationAsPointPair<T>* result) {
SphereShapePenetration(sphere_A, sphere_B, result);
}
void operator()(const fcl::Sphered& sphere_A, const fcl::Boxd& box_B,
PenetrationAsPointPair<T>* result) {
SphereShapePenetration(sphere_A, box_B, result);
}
void operator()(const fcl::Sphered& sphere_A,
const fcl::Cylinderd& cylinder_B,
PenetrationAsPointPair<T>* result) {
SphereShapePenetration(sphere_A, cylinder_B, result);
}
void operator()(const fcl::Sphered& sphere_A,
const fcl::Halfspaced& halfspace_B,
PenetrationAsPointPair<T>* result) {
SphereShapePenetration(sphere_A, halfspace_B, result);
}
void operator()(const fcl::Sphered& sphere_A, const fcl::Capsuled& capsule_B,
PenetrationAsPointPair<T>* result) {
SphereShapePenetration(sphere_A, capsule_B, result);
}
//@}
private:
// Penetration computation between a sphere A and a generic shape B. We use
// the overloaded call operators above to limit the kinds of queries, and
// they all call this private template function to minimize code duplication.
template <typename FclShape>
void SphereShapePenetration(const fcl::Sphered& sphere_A,
const FclShape& shape_B,
PenetrationAsPointPair<T>* result) {
DRAKE_ASSERT(result != nullptr);
const SignedDistanceToPoint<T> shape_B_to_point_Ao =
point_distance::DistanceToPoint<T>(id_B_, X_WB_,
X_WA_.translation())(shape_B);
const T depth = sphere_A.radius - shape_B_to_point_Ao.distance;
if (ExtractDoubleOrThrow(depth) >= 0) {
result->id_A = id_A_;
result->id_B = id_B_;
// Note that we use a signed penetration here.
result->depth = depth;
// p_WCb is the witness point on ∂B measured and expressed in W.
result->p_WCb = X_WB_ * (shape_B_to_point_Ao.p_GN);
result->nhat_BA_W = shape_B_to_point_Ao.grad_W;
// p_WCa is the witness point on ∂A measured and expressed in W.
result->p_WCa =
X_WA_.translation() - sphere_A.radius * shape_B_to_point_Ao.grad_W;
}
}
GeometryId id_A_;
GeometryId id_B_;
math::RigidTransform<T> X_WA_;
math::RigidTransform<T> X_WB_;
PenetrationAsPointPair<T>* result_;
};
/* @name Definition of fallback for missing primitive-primitive tests
Generally, we favor hand-crafted functions for determining the signed distance
between two shapes. However, just because we have no such function for a
particular geometry pair does not mean we have no recourse for computing
signed distance. Instead, we have the "fallback" function.
The fallback function is a simple interface that takes two fcl collision
objects (called `a` and `b`), fcl distance request parameters, and a pointer
to the SignedDistancePair which will be populated with the results of the
query.
Currently, our ability to fallback depends on the scalar type. This family
of functions defines the fallback logic for the various scalar types. We assume
there is no fallback for a scalar type unless one is explicitly provided. */
//@{
/* For all scalar types T that are not explicitly enumerated, throws an
exception declaring the unsupported combination of geometry types and scalar
type. */
template <typename T>
void CalcDistanceFallback(const fcl::CollisionObjectd& a,
const fcl::CollisionObjectd& b,
const fcl::CollisionRequestd&,
PenetrationAsPointPair<T>* /* pair_data */) {
// By default, there is no fallback. For every scalar type for which one
// actually exists, it should be specialized below.
throw std::logic_error(fmt::format(
"PenetrationAsPointPair() cannot be evaluated for shapes '{}' and '{}' "
"for scalar type {}. See the documentation for "
"QueryObject::ComputePointPairPenetration() for the full status of "
"supported geometries.",
GetGeometryName(a), GetGeometryName(b), NiceTypeName::Get<T>()));
}
/* For the double scalar, computes the signed distance between the two objects.
*/
template <>
void CalcDistanceFallback<double>(const fcl::CollisionObjectd& a,
const fcl::CollisionObjectd& b,
const fcl::CollisionRequestd& request,
PenetrationAsPointPair<double>* pair_data) {
DRAKE_DEMAND(pair_data != nullptr);
fcl::CollisionResult<double> result;
// Perform nearphase collision detection
collide(&a, &b, request, result);
if (!result.isCollision()) return;
// Process the contact points
// NOTE: This assumes that the request is configured to use a single contact.
const fcl::Contact<double>& contact = result.getContact(0);
// Signed distance is negative when penetration depth is positive.
const double depth = contact.penetration_depth;
// TODO(SeanCurtis-TRI): Remove this test when FCL issue 375 is fixed.
// FCL returns osculation as contact but doesn't guarantee a non-zero
// normal. Drake isn't really in a position to define that normal from the
// geometry or contact results so, if the geometry is sufficiently close
// to osculation, we consider the geometries to be non-penetrating.
if (depth <= std::numeric_limits<double>::epsilon()) return;
pair_data->depth = depth;
// By convention, Drake requires the contact normal to point out of B
// and into A. FCL uses the opposite convention.
pair_data->nhat_BA_W = -contact.normal;
// FCL returns a single contact point centered between the two
// penetrating surfaces. PenetrationAsPointPair expects
// two, one on the surface of body A (Ac) and one on the surface of body
// B (Bc). Choose points along the line defined by the contact point and
// normal, equidistant to the contact point. Recall that depth
// is non-negative, so depth * nhat_BA_W points out of A and into B.
pair_data->p_WCa =
contact.pos - 0.5 * pair_data->depth * pair_data->nhat_BA_W;
pair_data->p_WCb =
contact.pos + 0.5 * pair_data->depth * pair_data->nhat_BA_W;
pair_data->id_A = EncodedData(a).id();
pair_data->id_B = EncodedData(b).id();
}
//@}
/* Dispatches the narrowphase shape-shape query for the object pair (`a`, `b`)
to the appropriate primitive-primitive function (optionally defaulting to the
type- and shape-dependent fallback function).
@param a The first object in the pair.
@param X_WA The pose of object `a` expressed in the world frame.
@param b The second object in the pair.
@param X_WB The pose of object `b` expressed in the world frame.
@param request The distance request parameters.
@param result The structure to capture the computation results in.
@tparam T Computation scalar type.
@pre The pair should *not* be (Halfspace, X), unless X is Sphere. */
template <typename T>
void ComputeNarrowPhasePenetration(const fcl::CollisionObjectd& a,
const math::RigidTransform<T>& X_WA,
const fcl::CollisionObjectd& b,
const math::RigidTransform<T>& X_WB,
const fcl::CollisionRequestd& request,
PenetrationAsPointPair<T>* result) {
DRAKE_DEMAND(result != nullptr);
const fcl::CollisionGeometryd* a_geometry = a.collisionGeometry().get();
const fcl::CollisionGeometryd* b_geometry = b.collisionGeometry().get();
const bool a_is_sphere = a_geometry->getNodeType() == fcl::GEOM_SPHERE;
const bool b_is_sphere = b_geometry->getNodeType() == fcl::GEOM_SPHERE;
const bool no_sphere = !(a_is_sphere || b_is_sphere);
if (no_sphere) {
CalcDistanceFallback<T>(a, b, request, result);
return;
}
DRAKE_ASSERT(a_is_sphere || b_is_sphere);
// We write `s` for the sphere object and `o` for the other object. We
// assign either (a,b) or (b,a) to (s,o) depending on whether `a` is a
// sphere or not. Therefore, we only need the helper DistancePairGeometry
// that takes (sphere, other) but not (other, sphere). This scheme helps us
// keep the code compact; however, we might have to re-order the result
// afterwards.
const fcl::CollisionObjectd& s = a_is_sphere ? a : b;
const fcl::CollisionObjectd& o = a_is_sphere ? b : a;
const fcl::CollisionGeometryd* s_geometry = s.collisionGeometry().get();
const fcl::CollisionGeometryd* o_geometry = o.collisionGeometry().get();
const math::RigidTransform<T>& X_WS(a_is_sphere ? X_WA : X_WB);
const math::RigidTransform<T>& X_WO(a_is_sphere ? X_WB : X_WA);
const auto id_S = EncodedData(s).id();
const auto id_O = EncodedData(o).id();
PenetrationPairFunctor<T> calc_penetration_pair(id_S, id_O, X_WS, X_WO);
const auto& sphere_S = *static_cast<const fcl::Sphered*>(s_geometry);
switch (o_geometry->getNodeType()) {
case fcl::GEOM_SPHERE: {
// TODO(sean.curtis@tri.global) Here we use signed distance computation
// but we actually only need to perform penetration query. This could
// incur unnecessary computation. Depending on the data types, we can
// potentially use collision query for double type, and signed distance
// query for autodiff scalar.
const auto& sphere_O = *static_cast<const fcl::Sphered*>(o_geometry);
calc_penetration_pair(sphere_S, sphere_O, result);
break;
}
case fcl::GEOM_BOX: {
const auto& box_O = *static_cast<const fcl::Boxd*>(o_geometry);
calc_penetration_pair(sphere_S, box_O, result);
break;
}
case fcl::GEOM_CYLINDER: {
const auto& cylinder_O = *static_cast<const fcl::Cylinderd*>(o_geometry);
calc_penetration_pair(sphere_S, cylinder_O, result);
break;
}
case fcl::GEOM_HALFSPACE: {
const auto& halfspace_O =
*static_cast<const fcl::Halfspaced*>(o_geometry);
calc_penetration_pair(sphere_S, halfspace_O, result);
break;
}
case fcl::GEOM_CAPSULE: {
const auto& capsule_O = *static_cast<const fcl::Capsuled*>(o_geometry);
calc_penetration_pair(sphere_S, capsule_O, result);
break;
}
case fcl::GEOM_ELLIPSOID:
case fcl::GEOM_CONVEX:
// We don't have a closed form solution for these geometries, so we
// call FCL.
CalcDistanceFallback<T>(a, b, request, result);
break;
case fcl::GEOM_PLANE:
case fcl::BV_AABB:
case fcl::BV_OBB:
case fcl::BV_RSS:
case fcl::BV_kIOS:
case fcl::BV_OBBRSS:
case fcl::BV_KDOP16:
case fcl::BV_KDOP18:
case fcl::BV_KDOP24:
case fcl::GEOM_CONE:
case fcl::GEOM_TRIANGLE:
case fcl::GEOM_OCTREE:
case fcl::BV_UNKNOWN:
case fcl::NODE_COUNT:
// Fcl NodeTypes that are *not* currently supported by Drake.
DRAKE_UNREACHABLE();
}
// If needed, re-order the result for (s,o) back to the result for (a,b).
if (!a_is_sphere) {
result->SwapAAndB();
}
}
// TODO(SeanCurtis-TRI): Replace this clunky mechanism with a new mechanism
// which does this implicitly via ADL and templates.
/* @name Mechanism for reporting on which scalars and for which shape-pairs
shape-to-shape queries can be made.
By default, nothing is supported. For each supported scalar type, a class
specialization is provided which explicitly enumerates the supported shape
types.
@tparam T The computational scalar type. */
//@{
template <typename T>
struct ScalarSupport {
static bool is_supported(fcl::NODE_TYPE, fcl::NODE_TYPE) { return false; }
};
/* Primitive support for double-valued query. */
template <>
struct ScalarSupport<double> {
static bool is_supported(fcl::NODE_TYPE node1, fcl::NODE_TYPE node2) {
// Doubles (via its fallback) can support *almost* anything. Here, we
// enumerate the *very* limited unsupported cases. We can't meaningfully
// collide two half spaces, but fcl doesn't have an intelligent response.
// So, we simply short circuit at this point.
return !(node1 == fcl::GEOM_HALFSPACE && node2 == fcl::GEOM_HALFSPACE);
}
};
/* Primitive support for AutoDiff-valued query. */
template <>
struct ScalarSupport<AutoDiffXd> {
static bool is_supported(fcl::NODE_TYPE node1, fcl::NODE_TYPE node2) {
// Explicitly permit the following pair types (with ordering permutations):
// (sphere, sphere)
// (sphere, box)
// (sphere, cylinder)
// (sphere, halfspace)
// (sphere, capsule)
return (node1 == fcl::GEOM_SPHERE &&
(node2 == fcl::GEOM_SPHERE || node2 == fcl::GEOM_BOX ||
node2 == fcl::GEOM_HALFSPACE || node2 == fcl::GEOM_CYLINDER ||
node2 == fcl::GEOM_CAPSULE)) ||
(node2 == fcl::GEOM_SPHERE &&
(node1 == fcl::GEOM_BOX || node1 == fcl::GEOM_HALFSPACE ||
node1 == fcl::GEOM_CYLINDER || node1 == fcl::GEOM_CAPSULE));
}
};
//@}
template <typename T>
bool Callback(fcl::CollisionObjectd* fcl_object_A_ptr,
fcl::CollisionObjectd* fcl_object_B_ptr, void* callback_data) {
auto& data = *static_cast<CallbackData<T>*>(callback_data);
// Extract the collision filter keys from the fcl collision objects. These
// keys will also be used to map the fcl collision object back to the Drake
// GeometryId for colliding geometries.
EncodedData encoding_A(*fcl_object_A_ptr);
EncodedData encoding_B(*fcl_object_B_ptr);
// Guarantee for geometries A and B, we always evaluate the collision between
// them in a fixed order (e.g., the first geometry gets transformed into the
// second geometry's frame for evaluation).
if (encoding_B.id() < encoding_A.id()) {
std::swap(encoding_A, encoding_B);
std::swap(fcl_object_A_ptr, fcl_object_B_ptr);
}
// NOTE: Although this function *takes* non-const pointers to satisfy the
// fcl api, it should not exploit the non-constness to modify the collision
// objects. We ensure this by immediately assigning to a const version and
// not directly using the provided parameters.
const GeometryId id_A = encoding_A.id();
const GeometryId id_B = encoding_B.id();
const bool can_collide =
data.collision_filter.CanCollideWith(encoding_A.id(), encoding_B.id());
// NOTE: Here and below, false is returned regardless of whether collision
// is detected or not because true tells the broadphase manager to terminate.
// Since we want *all* collisions, we return false.
if (!can_collide) return false;
if (ScalarSupport<T>::is_supported(
fcl_object_A_ptr->collisionGeometry()->getNodeType(),
fcl_object_B_ptr->collisionGeometry()->getNodeType())) {
// Unpack the callback data
const fcl::CollisionRequestd& request = data.request;
// This callback only works for a single contact, this confirms a request
// hasn't been made for more contacts.
DRAKE_ASSERT(request.num_max_contacts == 1);
PenetrationAsPointPair<T> penetration;
ComputeNarrowPhasePenetration(*fcl_object_A_ptr, data.X_WGs.at(id_A),
*fcl_object_B_ptr, data.X_WGs.at(id_B),
data.request, &penetration);
if (ExtractDoubleOrThrow(penetration.depth) >= 0) {
data.point_pairs.push_back(std::move(penetration));
}
} else {
throw std::logic_error(fmt::format(
"Penetration queries between shapes '{}' and '{}' "
"are not supported for scalar type {}. See the documentation for "
"QueryObject::ComputePointPairPenetration() for the full status of "
"supported geometries.",
GetGeometryName(*fcl_object_A_ptr), GetGeometryName(*fcl_object_B_ptr),
NiceTypeName::Get<T>()));
}
return false;
}
DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS((&Callback<T>))
} // namespace penetration_as_point_pair
} // namespace internal
} // namespace geometry
} // namespace drake