test_fcl_signed_distance.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2016, Open Source Robotics Foundation
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Open Source Robotics Foundation nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #include <gtest/gtest.h>
36 
37 #include "eigen_matrix_compare.h"
41 #include "test_fcl_utility.h"
42 #include "fcl_resources/config.h"
43 
44 using namespace fcl;
45 using std::abs;
46 
47 bool verbose = false;
48 
49 //==============================================================================
50 template <typename S>
52 {
53  const S radius_1 = 20;
54  const S radius_2 = 10;
55  Sphere<S> s1{radius_1};
56  Sphere<S> s2{radius_2};
57 
60 
61  DistanceRequest<S> request;
62  request.enable_signed_distance = true;
63  request.enable_nearest_points = true;
64  request.gjk_solver_type = solver_type;
65 
66  DistanceResult<S> result;
67 
68  // Expecting distance to be 10
69  result.clear();
70  tf2.translation() = Vector3<S>(40, 0, 0);
71  distance(&s1, tf1, &s2, tf2, request, result);
72  EXPECT_NEAR(result.min_distance, 10, 1e-6);
74  request.distance_tolerance));
76  request.distance_tolerance));
77 
78  // request.distance_tolerance is actually the square of the distance
79  // tolerance, namely the difference between distance returned from FCL's EPA
80  // implementation and the actual distance, is less than
81  // sqrt(request.distance_tolerance).
82  const S distance_tolerance = std::sqrt(request.distance_tolerance);
83 
84  // Expecting distance to be -5
85  result.clear();
86  tf2.translation() = Vector3<S>(25, 0, 0);
87  distance(&s1, tf1, &s2, tf2, request, result);
88  EXPECT_NEAR(result.min_distance, -5, request.distance_tolerance);
89 
90  // TODO(JS): Only GST_LIBCCD can compute the pair of nearest points on the
91  // surface of the penetrating spheres.
92  if (solver_type == GST_LIBCCD)
93  {
95  distance_tolerance));
97  distance_tolerance));
98  }
99 
100  result.clear();
101  tf2.translation() = Vector3<S>(20, 0, 20);
102  distance(&s1, tf1, &s2, tf2, request, result);
103 
104  S expected_dist =
105  (tf1.translation() - tf2.translation()).norm() - radius_1 - radius_2;
106  EXPECT_NEAR(result.min_distance, expected_dist, distance_tolerance);
107  // TODO(JS): Only GST_LIBCCD can compute the pair of nearest points on the
108  // surface of the spheres.
109  if (solver_type == GST_LIBCCD)
110  {
111  Vector3<S> dir = (tf2.translation() - tf1.translation()).normalized();
112  Vector3<S> p0_expected = dir * radius_1;
113  EXPECT_TRUE(CompareMatrices(result.nearest_points[0], p0_expected,
114  distance_tolerance));
115  Vector3<S> p1_expected = tf2.translation() - dir * radius_2;
116  EXPECT_TRUE(CompareMatrices(result.nearest_points[1], p1_expected,
117  distance_tolerance));
118  }
119 }
120 
121 template <typename S>
123 {
124  Sphere<S> s1{20};
125  Capsule<S> s2{10, 20};
126 
129 
130  DistanceRequest<S> request;
131  request.enable_signed_distance = true;
132  request.enable_nearest_points = true;
133  request.gjk_solver_type = solver_type;
134 
135  DistanceResult<S> result;
136 
137  // Expecting distance to be 10
138  result.clear();
139  tf2.translation() = Vector3<S>(40, 0, 0);
140  distance(&s1, tf1, &s2, tf2, request, result);
141  EXPECT_NEAR(result.min_distance, 10, request.distance_tolerance);
143  request.distance_tolerance));
145  request.distance_tolerance));
146 
147  // Expecting distance to be -5
148  result.clear();
149  tf2.translation() = Vector3<S>(25, 0, 0);
150  distance(&s1, tf1, &s2, tf2, request, result);
151 
152  // request.distance_tolerance is actually the square of the distance
153  // tolerance, namely the difference between distance returned from FCL's EPA
154  // implementation and the actual distance, is less than
155  // sqrt(request.distance_tolerance).
156  const S distance_tolerance = std::sqrt(request.distance_tolerance);
157  ASSERT_NEAR(result.min_distance, -5, distance_tolerance);
158  if (solver_type == GST_LIBCCD)
159  {
160  // NOTE: Currently, only GST_LIBCCD computes the pair of nearest points.
162  distance_tolerance * 100));
164  distance_tolerance * 100));
165  }
166 }
167 
168 
169 template <typename S>
171  // This is a specific case that has cropped up in the wild that reaches the
172  // unexpected `expandPolytope()` error, where the nearest point and the new
173  // vertex both lie on an edge. It turns out that libccd incorrectly thinks
174  // that the edge is the nearest feature, while actually one of the
175  // neighbouring faces is. This test confirms that the bug is fixed, by the
176  // function validateNearestFeatureOfPolytopeBeingEdge().
177  // This case was reported in
178  // https://github.com/flexible-collision-library/fcl/issues/391.
179  using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometry<S>>;
180  const S cylinder_radius = 0.03;
181  const S cylinder_length = 0.65;
182  CollisionGeometryPtr_t cylinder_geo(
183  new fcl::Cylinder<S>(cylinder_radius, cylinder_length));
185  X_WC.translation() << 0.6, 0, 0.325;
186  fcl::CollisionObject<S> cylinder(cylinder_geo, X_WC);
187 
188  const S sphere_radius = 0.055;
189  CollisionGeometryPtr_t sphere_geo(new fcl::Sphere<S>(sphere_radius));
191  // clang-format off
192  X_WS.matrix() << -0.9954758066, -0.0295866301, 0.0902914702, 0.5419794018,
193  -0.0851034786, -0.1449450565, -0.9857729599, -0.0621175025,
194  0.0422530022, -0.9889972506, 0.1417713719, 0.6016236576,
195  0, 0, 0, 1;
196  // clang-format on
197  fcl::CollisionObject<S> sphere(sphere_geo, X_WS);
198 
199  fcl::DistanceRequest<S> request;
201  request.distance_tolerance = 1e-6;
202  request.enable_signed_distance = true;
203  fcl::DistanceResult<S> result;
204 
205  ASSERT_NO_THROW(fcl::distance(&cylinder, &sphere, request, result));
206  // The two objects are penetrating.
207  EXPECT_NEAR(-(result.nearest_points[0] - result.nearest_points[1]).norm(),
208  result.min_distance, request.distance_tolerance);
209  // p_CPc is the position of the witness point Pc on the cylinder, measured
210  // and expressed in the cylinder frame C.
211  const Vector3<S> p_CPc = X_WC.inverse() * result.nearest_points[0];
212  EXPECT_LE(abs(p_CPc(2)), cylinder_length / 2);
213  EXPECT_LE(p_CPc.template head<2>().norm(), cylinder_radius);
214  // p_SPs is the position of the witness point Ps on the sphere, measured and
215  // expressed in the sphere frame S.
216  const Vector3<S> p_SPs = X_WS.inverse() * result.nearest_points[1];
217  EXPECT_LE(p_SPs.norm(), sphere_radius);
218 }
219 
220 template <typename S>
221 void test_distance_cylinder_box_helper(S cylinder_radius, S cylinder_length,
222  const Transform3<S>& X_WC,
223  const Vector3<S>& box_size,
224  const Transform3<S>& X_WB) {
225  using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometryd>;
226  CollisionGeometryPtr_t cylinder_geo(
227  new fcl::Cylinder<S>(cylinder_radius, cylinder_length));
228  fcl::CollisionObject<S> cylinder(cylinder_geo, X_WC);
229 
230  CollisionGeometryPtr_t box_geo(
231  new fcl::Box<S>(box_size(0), box_size(1), box_size(2)));
232  fcl::CollisionObject<S> box(box_geo, X_WB);
233 
234  fcl::DistanceRequest<S> request;
236  request.distance_tolerance = 1e-6;
237  request.enable_signed_distance = true;
238  fcl::DistanceResult<S> result;
239 
240  ASSERT_NO_THROW(fcl::distance(&cylinder, &box, request, result));
241  EXPECT_NEAR(abs(result.min_distance),
242  (result.nearest_points[0] - result.nearest_points[1]).norm(),
243  request.distance_tolerance);
244  // p_CPc is the position of the witness point Pc on the cylinder, measured
245  // and expressed in the cylinder frame C.
246  const Vector3<S> p_CPc = X_WC.inverse() * result.nearest_points[0];
247  const S tol = 10 * std::numeric_limits<S>::epsilon();
248  EXPECT_LE(abs(p_CPc(2)), cylinder_length / 2 + tol);
249  EXPECT_LE(p_CPc.template head<2>().norm(), cylinder_radius);
250  // p_BPb is the position of the witness point Pb on the box, measured and
251  // expressed in the box frame B.
252  const Vector3<S> p_BPb = X_WB.inverse() * result.nearest_points[1];
253  EXPECT_TRUE((p_BPb.array().abs() <= box_size.array() / 2 + tol).all());
254 }
255 
256 template <typename S>
258  // This is a *specific* case that has cropped up in the wild that reaches the
259  // unexpected `expandPolytope()` error. This error was reported in
260  // https://github.com/flexible-collision-library/fcl/issues/319
261  // This test would fail in Debug mode without the function
262  // validateNearestFeatureOfPolytopeBeingEdge in PR #388.
263  S cylinder_radius = 0.05;
264  S cylinder_length = 0.06;
265 
267  X_WC.matrix() << -0.99999999997999022838, 6.2572835802045040178e-10,
268  6.3260669852976095481e-06, 0.57500009756757608503,
269  6.3260669851683709551e-06, -6.3943303429958554955e-10,
270  0.99999999997999056145, -0.42711963046787942977,
271  6.2573180158128459924e-10, 1, 6.3942912945996747041e-10,
272  1.1867093358746836351, 0, 0, 0, 1;
273  Vector3<S> box_size(0.025, 0.35, 1.845);
275  // clang-format off
276  X_WB.matrix() << 0, -1, 0, 0.8,
277  1, 0, 0, -0.4575,
278  0, 0, 1, 1.0225,
279  0, 0, 0, 1;
280  // clang-format on
281  test_distance_cylinder_box_helper(cylinder_radius, cylinder_length, X_WC,
282  box_size, X_WB);
283  // This is a specific case reported in
284  // https://github.com/flexible-collision-library/fcl/issues/390#issuecomment-481634606
285  X_WC.matrix() << -0.97313010759279283679, -0.12202804064972551379,
286  0.19526123781136842106, 0.87472781461138560122, 0.20950801135757171623,
287  -0.11745920593569325607, 0.97072639199619581429, -0.4038687881347159947,
288  -0.095520609678929752073, 0.98555187191549953329, 0.13986894183635001365,
289  1.5871328698116491385, 0, 0, 0, 1;
290  // clang-format off
291  X_WB.matrix() << 0, -1, 0, 0.8,
292  1, 0, 0, -0.4575,
293  0, 0, 1, 1.0225,
294  0, 0, 0, 1;
295  // clang-format on
296  test_distance_cylinder_box_helper(cylinder_radius, cylinder_length, X_WC,
297  box_size, X_WB);
298 }
299 
300 template <typename S>
302  const Transform3<S>& X_WB1,
303  const Vector3<S>& box2_size,
304  const Transform3<S>& X_WB2,
305  const S* expected_distance = nullptr) {
306  using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometryd>;
307  CollisionGeometryPtr_t box1_geo(
308  new fcl::Box<S>(box1_size(0), box1_size(1), box1_size(2)));
309  fcl::CollisionObject<S> box1(box1_geo, X_WB1);
310 
311  CollisionGeometryPtr_t box2_geo(
312  new fcl::Box<S>(box2_size(0), box2_size(1), box2_size(2)));
313  fcl::CollisionObject<S> box2(box2_geo, X_WB2);
314 
315  fcl::DistanceRequest<S> request;
317  request.distance_tolerance = 1e-6;
318  request.enable_signed_distance = true;
319  fcl::DistanceResult<S> result;
320 
321  ASSERT_NO_THROW(fcl::distance(&box1, &box2, request, result));
322  EXPECT_NEAR(abs(result.min_distance),
323  (result.nearest_points[0] - result.nearest_points[1]).norm(),
324  request.distance_tolerance);
325  // p_B1P1 is the position of the witness point P1 on box 1, measured
326  // and expressed in the box 1 frame B1.
327  const Vector3<S> p_B1P1 = X_WB1.inverse() * result.nearest_points[0];
328  constexpr double tol = 10 * constants<S>::eps();
329  const double tol_1 = tol * std::max(S(1), (box1_size / 2).maxCoeff());
330  EXPECT_TRUE(
331  (p_B1P1.array().abs() <= (box1_size / 2).array() + tol_1).all())
332  << "\n p_B1P1: " << p_B1P1.transpose()
333  << "\n box1_size / 2: " << (box1_size / 2).transpose()
334  << "\n tol: " << tol_1;
335  // p_B2P2 is the position of the witness point P2 on box 2, measured
336  // and expressed in the box 2 frame B2.
337  const double tol_2 = tol * std::max(S(1), (box2_size / 2).maxCoeff());
338  const Vector3<S> p_B2P2 = X_WB2.inverse() * result.nearest_points[1];
339  EXPECT_TRUE(
340  (p_B2P2.array().abs() <= (box2_size / 2).array() + tol_2).all())
341  << "\n p_B2P2: " << p_B2P2.transpose()
342  << "\n box2_size / 2: " << (box2_size / 2).transpose()
343  << "\n tol: " << tol_2;
344 
345  // An expected distance has been provided; let's test that the value is as
346  // expected.
347  if (expected_distance) {
350  }
351 }
352 
353 // This is a *specific* case that has cropped up in the wild that reaches the
354 // unexpected `validateNearestFeatureOfPolytopeBeingEdge` error. This error was
355 // reported in https://github.com/flexible-collision-library/fcl/issues/388
356 template <typename S>
358  SCOPED_TRACE("test_distance_box_box_regression1");
359  const Vector3<S> box1_size(0.03, 0.12, 0.1);
361  X_WB1.matrix() << -3.0627937852578681533e-08, -0.99999999999999888978,
362  -2.8893865161583314238e-08, 0.63499979627350811029, 0.9999999999999980016,
363  -3.0627939739957803544e-08, 6.4729926918527511769e-08,
364  -0.48500002215636439651, -6.4729927722963847085e-08,
365  -2.8893863029448751323e-08, 0.99999999999999711342, 1.0778146458339641356,
366  0, 0, 0, 1;
367 
368  const Vector3<S> box2_size(0.025, 0.35, 1.845);
370  // clang-format off
371  X_WB2.matrix() << 0, -1, 0, 0.8,
372  1, 0, 0, -0.4575,
373  0, 0, 1, 1.0225,
374  0, 0, 0, 1;
375  // clang-format on
376  test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2);
377 }
378 
379 // This is a *specific* case that has cropped up in the wild that reaches the
380 // unexpected `triangle_size_is_zero` error. This error was
381 // reported in https://github.com/flexible-collision-library/fcl/issues/395
382 template <typename S>
384  SCOPED_TRACE("test_distance_box_box_regression2");
385  const Vector3<S> box1_size(0.46, 0.48, 0.01);
387  X_WB1.matrix() << 1,0,0, -0.72099999999999997424,
388  0,1,0, -0.77200000000000001954,
389  0,0,1, 0.81000000000000005329,
390  0,0,0,1;
391 
392  const Vector3<S> box2_size(0.049521, 0.146, 0.0725);
394  // clang-format off
395  X_WB2.matrix() << 0.10758262492983036718, -0.6624881850015212903, -0.74130653817877356637, -0.42677133002999478872,
396  0.22682184885125472595, -0.709614040775253474, 0.6670830248314786326, -0.76596851247746788882,
397 -0.96797615037608542021, -0.23991106241273435495, 0.07392465377049164954, 0.80746731400091054098,
398  0, 0, 0, 1;
399  // clang-format on
400  test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2);
401 }
402 
403 // This is a *specific* case that has cropped up in the wild that reaches the
404 // unexpected `query point colinear with the edge` error. This error was
405 // reported in https://github.com/flexible-collision-library/fcl/issues/415
406 template <typename S>
408  SCOPED_TRACE("test_distance_box_box_regression3");
409  const Vector3<S> box1_size(0.49, 0.05, 0.21);
411  // clang-format off
412  X_WB1.matrix() << 4.8966386501092529215e-12, -1,0,-0.43999999999999994671,
413  1, 4.8966386501092529215e-12,0,-0.61499999999858001587,
414  0,0,1,0.35499999999999998224,
415  0,0,0,1;
416  // clang-format on
417  const Vector3<S> box2_size(0.035, 0.12, 0.03);
419  // clang-format off
420  X_WB2.matrix() << 0.83512153565236335595, -0.55006546945762568868, -9.4542360608233572896e-16, -0.40653441507331000704,
421  0.55006546945762568868, 0.83512153565236313391, 1.1787444236552387666e-15, -0.69166166923735727945,
422 1.2902271444330665572e-16, -1.4878153530113264589e-15, 1, 0.43057093858718892276,
423  0, 0, 0, 1;
424  // clang-format on
425  test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2);
426 }
427 
428 // This is a *specific* case that has cropped up in the wild. This error was
429 // reported in https://github.com/flexible-collision-library/fcl/issues/398
430 template <typename S>
432  SCOPED_TRACE("test_distance_box_box_regression4");
433  const Vector3<S> box1_size(0.614, 3, 0.37);
435  X_WB1.translation() << -0.675, 0, 0.9115;
436  const Vector3<S> box2_size(0.494, 0.552, 0.01);
438  X_WB2.translation() << -0.692, 0, 0.935;
439  test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2);
440 }
441 
442 // This is a *specific* case that has cropped up in the wild. This error was
443 // reported in https://github.com/flexible-collision-library/fcl/issues/428
444 template <typename S>
446  SCOPED_TRACE("test_distance_box_box_regression5");
447  const Vector3<S> box1_size(0.2, 0.33, 0.1);
449  X_WB1.translation() << -0.071000000000000035305, -0.77200000000000001954, 0.79999999999999993339;
450  const Vector3<S> box2_size(0.452, 0.27, 0.6);
452  X_WB2.translation() << 0.12099999999999999645, -0.78769605692727695523, 0.53422044196125151316;
453  test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2);
454 }
455 
456 template <typename S>
458  SCOPED_TRACE("test_distance_box_box_regression6");
459  const Vector3<S> box1_size(0.31650000000000000355, 0.22759999999999999676, 0.1768000000000000127);
461  // clang-format off
462  X_WB1.matrix() << 0.44540578475530234748, 0.89532881496493399442, -8.8937407685638678971e-09, 1.2652949075960071568,
463  -0.89532881496493377238, 0.44540578475530190339, -2.8948680226084145336e-08, 1.4551012423210101243,
464  -2.1957263975186326105e-08, 2.0856732016652919226e-08, 0.99999999999999955591, 0.49480006232932938204,
465  0, 0, 0, 1;
466  // clang-format on
467  const Vector3<S> box2_size(0.49430000000000001714, 0.35460000000000002629, 0.075200000000000002953);
469  // clang-format off
470  X_WB2.matrix() << 0.44171122913485860728, 0.8971572827861190591, -1.622764514865468214e-09, 1.1304016226141906376,
471  -0.8971572827861190591, 0.44171122913485860728, -5.1621053952306079594e-09, 1.8410802645284281009,
472  -3.9144271413829990148e-09, 3.7360349218094348098e-09, 1, 0.44400006232932492933,
473  0, 0, 0, 1;
474  // clang-format on;
475  const double expected_distance{0};
476  test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2, &expected_distance);
477 }
478 
479 // Issue #493 outlines a number of scenarios that caused signed distance
480 // failure. They consisted of two identical, stacked boxes. The boxes are
481 // slightly tilted. The boxes were essentially touching but were separated by
482 // infinitesimally small distances. The issue outlines three different examples.
483 // Rather than reproducing each of them verbatim, this test attempts to
484 // generalize those cases by testing the stacked scenario across various box
485 // sizes and separation amounts (ranging from slightly penetrating to slightly
486 // separated). These should essentially cover the variations described in the
487 // issue.
488 template <typename S>
490  SCOPED_TRACE("test_distance_box_box_regression_tilted_kissing_contact");
491  // The boxes are posed relative to each other in a common frame F (such that
492  // it is easy to reason about their separation). The stack is rotated around
493  // box A's origin and translated into the world frame.
494  Matrix3<S> R_WF;
495  R_WF <<
496  0.94096063217417758029, 0.29296840037289501035, 0.16959541586174811667,
497  -0.23569836841299879326, 0.92661523595848427348, -0.29296840037289506586,
498  -0.2429801799032638987, 0.23569836841299884878, 0.94096063217417758029;
499 
500  for (const S dim : {S(0.01), S(0.25), S(0.5), S(10), S(1000)}) {
501  const Vector3<S> box_size(dim, dim, dim);
502 
503  const Vector3<S> p_WA(0, 0, 5 * dim);
504  Transform3<S> X_WA;
505  X_WA.linear() = R_WF;
506  X_WA.translation() = p_WA;
507  Transform3<S> X_WB;
508  X_WB.linear() = R_WF;
509 
510  // Both boxes always have the same orientation and the *stack* is always
511  // located at p_WA. Only the translational component of X_WB changes with
512  // varying separation distance.
513 
514  // By design, the distances are all near epsilon. We'll scale them up for
515  // larger boxes to make sure the distance doesn't simply disappear in
516  // the rounding noise.
517  for (const S distance : {S(-1e-15), S(-2.5e-16), S(-1e-16), S(0), S(1e-16),
518  S(2.5e-16), S(1e-15)}) {
519  const S scaled_distance = distance * std::max(S(1), dim);
520  const Vector3<S> p_AB_F = Vector3<S>(0, dim + scaled_distance, 0);
521 
522  X_WB.translation() = p_WA + R_WF * p_AB_F;
523  SCOPED_TRACE("dim: " + std::to_string(dim) +
524  ", distance: " + std::to_string(distance));
525  test_distance_box_box_helper(box_size, X_WA, box_size, X_WB,
526  &scaled_distance);
527  }
528  }
529 }
530 
531 // This is a *specific* case that has cropped up in the wild that reaches the
532 // unexpected `validateNearestFeatureOfPolytopeBeingEdge` error. This error was
533 // reported in https://github.com/flexible-collision-library/fcl/issues/408
534 template <typename S>
536  SCOPED_TRACE("test_distance_sphere_box_regression1");
537  using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometry<S>>;
538  const S sphere_radius = 0.06;
539  CollisionGeometryPtr_t sphere_geo(new fcl::Sphere<S>(sphere_radius));
541  // clang-format off
542  X_WS.matrix() << -0.99999999999999955591, -4.4637642593504144998e-09, 0, 1.7855056639081962376e-10,
543  4.4637642593504144998e-09, -0.99999999999999955591, 0, 0.039999999999999993894,
544  0, 0, 1.0000000000000008882, 0.33000000000000012657,
545  0, 0, 0, 1;
546  // clang-format on
547  fcl::CollisionObject<S> sphere(sphere_geo, X_WS);
548 
549  CollisionGeometryPtr_t box_geo(new fcl::Box<S>(0.1, 0.1, 0.1));
551  // clang-format off
552  X_WB.matrix() << 1, 0, 0, 0.05,
553  0, 1, 0, 0.15,
554  0, 0, 1, 0.35,
555  0, 0, 0, 1;
556  // clang-format on
557  fcl::CollisionObject<S> box(box_geo, X_WB);
558  fcl::DistanceRequest<S> request;
560  request.distance_tolerance = 1e-6;
561  request.enable_signed_distance = true;
562  fcl::DistanceResult<S> result;
563 
564  ASSERT_NO_THROW(fcl::distance(&sphere, &box, request, result));
565  const S expected_distance = 0.06 - sphere_radius;
567  request.distance_tolerance);
568 }
569 
570 //==============================================================================
571 
572 GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_ccd) {
573  test_distance_spheresphere<double>(GST_LIBCCD);
574 }
575 
576 GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_indep) {
577  test_distance_spheresphere<double>(GST_INDEP);
578 }
579 
580 GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_ccd) {
581  test_distance_spherecapsule<double>(GST_LIBCCD);
582 }
583 
584 GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_indep) {
585  test_distance_spherecapsule<double>(GST_INDEP);
586 }
587 
588 GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_sphere1_ccd) {
589  test_distance_cylinder_sphere1<double>();
590 }
591 
592 GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_box_ccd) {
593  test_distance_cylinder_box<double>();
594 }
595 
596 GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression) {
597  // A collection of scenarios observed in practice that have created error
598  // conditions in previous commits of the code. Each test is a unique instance.
599  test_distance_box_box_regression1<double>();
600  test_distance_box_box_regression2<double>();
601  test_distance_box_box_regression3<double>();
602  test_distance_box_box_regression4<double>();
603  test_distance_box_box_regression5<double>();
604  test_distance_box_box_regression6<double>();
605  test_distance_box_box_regression_tilted_kissing_contact<double>();
606  test_distance_sphere_box_regression1<double>();
607 }
608 
609 //==============================================================================
610 int main(int argc, char* argv[])
611 {
612  ::testing::InitGoogleTest(&argc, argv);
613  return RUN_ALL_TESTS();
614 }
fcl::DistanceRequest
request to the distance computation
Definition: distance_request.h:52
test_distance_box_box_regression5
void test_distance_box_box_regression5()
Definition: test_fcl_signed_distance.cpp:445
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::GJKSolverType
GJKSolverType
Type of narrow phase GJK solver.
Definition: gjk_solver_type.h:45
test_distance_box_box_regression4
void test_distance_box_box_regression4()
Definition: test_fcl_signed_distance.cpp:431
test_distance_spherecapsule
void test_distance_spherecapsule(GJKSolverType solver_type)
Definition: test_fcl_signed_distance.cpp:122
eigen_matrix_compare.h
fcl::distance
S distance(const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const kIOS< S > &b1, const kIOS< S > &b2, Vector3< S > *P, Vector3< S > *Q)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS-inl.h:266
epsilon
S epsilon()
Definition: test_fcl_simple.cpp:56
fcl::CompareMatrices
::testing::AssertionResult CompareMatrices(const Eigen::MatrixBase< DerivedA > &m1, const Eigen::MatrixBase< DerivedB > &m2, double tolerance=0.0, MatrixCompareType compare_type=MatrixCompareType::absolute)
Definition: eigen_matrix_compare.h:63
fcl::constants::eps
static constexpr Real eps()
Returns ε for the precision of the underlying scalar type.
Definition: constants.h:151
fcl::DistanceResult::nearest_points
Vector3< S > nearest_points[2]
Nearest points in the world coordinates.
Definition: distance_result.h:69
max
static T max(T x, T y)
Definition: svm.cpp:52
test_fcl_utility.h
EXPECT_TRUE
#define EXPECT_TRUE(args)
fcl::DistanceResult
distance result
Definition: distance_request.h:48
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
verbose
bool verbose
Definition: test_fcl_signed_distance.cpp:47
test_distance_box_box_regression_tilted_kissing_contact
void test_distance_box_box_regression_tilted_kissing_contact()
Definition: test_fcl_signed_distance.cpp:489
test_distance_cylinder_sphere1
void test_distance_cylinder_sphere1()
Definition: test_fcl_signed_distance.cpp:170
distance.h
test_distance_spheresphere
void test_distance_spheresphere(GJKSolverType solver_type)
Definition: test_fcl_signed_distance.cpp:51
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
gjk_solver_libccd.h
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
fcl::DistanceRequest::gjk_solver_type
GJKSolverType gjk_solver_type
narrow phase solver type
Definition: distance_request.h:102
fcl::GST_LIBCCD
@ GST_LIBCCD
Definition: gjk_solver_type.h:45
test_distance_box_box_regression2
void test_distance_box_box_regression2()
Definition: test_fcl_signed_distance.cpp:383
fcl::DistanceRequest::enable_nearest_points
bool enable_nearest_points
whether to return the nearest points
Definition: distance_request.h:55
fcl::DistanceResult::min_distance
S min_distance
Minimum distance between two objects.
Definition: distance_result.h:64
test_distance_box_box_regression1
void test_distance_box_box_regression1()
Definition: test_fcl_signed_distance.cpp:357
fcl::Cylinder
Center at zero cylinder.
Definition: cylinder.h:51
test_distance_cylinder_box_helper
void test_distance_cylinder_box_helper(S cylinder_radius, S cylinder_length, const Transform3< S > &X_WC, const Vector3< S > &box_size, const Transform3< S > &X_WB)
Definition: test_fcl_signed_distance.cpp:221
test_distance_box_box_regression6
void test_distance_box_box_regression6()
Definition: test_fcl_signed_distance.cpp:457
test_distance_sphere_box_regression1
void test_distance_sphere_box_regression1()
Definition: test_fcl_signed_distance.cpp:535
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:51
fcl::DistanceRequest::enable_signed_distance
bool enable_signed_distance
Whether to compute exact negative distance.
Definition: distance_request.h:92
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:51
fcl::DistanceRequest::distance_tolerance
S distance_tolerance
the threshold used in GJK algorithm to stop distance iteration
Definition: distance_request.h:99
test_distance_box_box_regression3
void test_distance_box_box_regression3()
Definition: test_fcl_signed_distance.cpp:407
main
int main(int argc, char *argv[])
Definition: test_fcl_signed_distance.cpp:610
test_distance_box_box_helper
void test_distance_box_box_helper(const Vector3< S > &box1_size, const Transform3< S > &X_WB1, const Vector3< S > &box2_size, const Transform3< S > &X_WB2, const S *expected_distance=nullptr)
Definition: test_fcl_signed_distance.cpp:301
fcl::constants
Definition: constants.h:129
GTEST_TEST
GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_ccd)
Definition: test_fcl_signed_distance.cpp:572
collision_node.h
fcl::GST_INDEP
@ GST_INDEP
Definition: gjk_solver_type.h:45
expected_distance
S expected_distance
Definition: test_sphere_box.cpp:184
fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:51
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::Capsule< S >
fcl::DistanceResult::clear
void clear()
clear the result
Definition: distance_result-inl.h:127
test_distance_cylinder_box
void test_distance_cylinder_box()
Definition: test_fcl_signed_distance.cpp:257


fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:49