Return the correct nearest points from `distance(...)` #215

avalenzu · 2017-05-31T20:23:09Z

This PR fixes issues in both the CCD-based and the independent GJK solvers, which caused distance to return erroneous values for the nearest points.

CCD-base solver: Currently the most recently computed support points are returned as the nearest points, which is incorrect. The nearest points lie in the convex-hull of the points contained in the simplex variable. The points on the original shapes can be computed from the simplex and the nearest point to the origin on the Minkowski difference, dir.
Independent solver: Currently, p2 is not properly transformed to its shape's (shape1) frame. toshape0 converts points from shape1 frame to shape0 frame. The points extracted from the GJK simplex are in shape0 frame, so the inverse of toshape0 needs to be applied.

The test_fcl_capsule_box_* tests contained lines that failed due to the issues described above. This PR uncomments those lines and modifies the tests to exercise both solvers.

This PR also adds a distance_tolerance member to fcl::DistanceRequest, which corresponds to the distance_tolerance and gjk_tolerance members in GJKSolver_libccd and GJKSolver_indep respectively. @jslee02, is this an appropriate way to expose this knob to the user? I didn't use abs_err because the default for that was 0.0, whereas the defaults for distance_tolerance and gjk_tolerance are 1e-6.

This allows the user to control the tolerance used in the GJK solvers without explicitly constructing a solver.

avalenzu · 2017-06-01T13:28:02Z

@jslee02, do you have any insights as to why libccd would return different results on Linux than it does on Windows and OSX? test_fcl_capsule_box_1 passes on Linux, but fails on the other two platforms.

jslee02 · 2017-06-01T13:44:37Z

@jslee02, do you have any insights as to why libccd would return different results on Linux than it does on Windows and OSX? test_fcl_capsule_box_1 passes on Linux, but fails on the other two platforms.

One possible reason would be that fcl uses different versions of libccd for the tests depending on platforms? fcl uses 1.5 on Linux and 2.0 for the other two platforms. You could test this by manually installing libccd 2.0 on Linux for sure.

avalenzu · 2017-06-01T14:19:28Z

You could test this by manually installing libccd 2.0 on Linux for sure.

Hmmm. I've been using the libccd-dev package from the Xenial apt repositories (which uses the libccd 2.0 sources) on my machine, and all the tests pass for me.

jslee02 · 2017-06-01T15:06:11Z

I realized that libccd-dev in the Ubuntu apt repository was built with double precision while macOS uses single precision. If you see the debian file, then it passes -DCCD_DOUBLE:BOOL=True option but the homebrew formula just uses the default setting that is single precision.

If this is the cause of the issue, then we might want to add a precision option to the homebrew formula.

sherm1 · 2017-06-01T15:19:56Z

Is FCL really useful in single precision? Likely it could at least default to double everywhere, but it might be worth simply declaring that only double is supported, especially if there are unit tests that won't pass in float.

edrumwri · 2017-06-01T15:25:50Z

I think Jeongseok was talking about the default build option for libccd, not FCL.

…

-------------------------------- Evan Drumwright Senior Research Scientist http://positronicslab.github.io Toyota Research Institute Palo Alto, CA

On Thu, Jun 1, 2017 at 8:19 AM, Michael Sherman ***@***.***> wrote: Is FCL really useful in single precision? Likely it could at least default to double everywhere, but it might be worth simply declaring that only double is supported, especially if there are unit tests that won't pass in float. — You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub <#215 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/ACHwz8VN2WcmsXZM7it6hz2Nff15SDDFks5r_taegaJpZM4NsJL-> .

avalenzu · 2017-06-01T15:36:39Z

@jslee02, it looks like that's the issue. I verified that test_fcl_capsule_box_1 passes on OSX when libccd is built with double precision, and fails when libccd is build with single precision.

avalenzu · 2017-06-01T15:48:41Z

Thanks for finding the precision issue, @jslee02!

I think we should update the homebrew formula to use double precision. @jslee02 do you have experience editing homebrew formulae? If so, could you put in a pull request with that change? I haven't the slightest idea how to go about testing changes to homebrew configuration. I'm willing to learn, but if you already have that know-how, it would be awesome if you could put that in 😃

Additionally, is there a way to require at build time that libccd was built with double precision (i.e. that ccd_real_t is double)? As @sherm1 mentioned, it doesn't really make sense to allow building in a configuration that's known to yield wrong answers.

SeanCurtis-TRI

Just a couple of notes. Doesn't address correctness, per se, but attempts to strengthen the code that's there.

SeanCurtis-TRI · 2017-06-01T16:07:39Z

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h

+      // p - A = s*AB
+      //
+      // This defines three equations, but we only need one. Take the x
+      // component


The quality of this solution degrades as the line segment becomes more perpendicular to the x-axis (i.e., where B_x - A_x << 1. Ideally, you'd want to pick the axis where |B_i - A_i| is biggest.

SeanCurtis-TRI · 2017-06-01T16:07:49Z

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h

+      //
+      // Thus, s is given by
+      //
+      // s = (p_x - A_x)/(B_x - vA_x)


typo? vA_x

SeanCurtis-TRI · 2017-06-01T16:34:34Z

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h

+    // t =  n . (AB x p') / n . (AB x AC)
+    double s{-ccdVec3Dot(&n, &AC_cross_v_prime) / n_dot_AB_cross_AC};
+    double t{ccdVec3Dot(&n, &AB_cross_v_prime) / n_dot_AB_cross_AC};
+


I believe a little algebraic manipulation simplifies the math (making it cheaper and more robust -- no square roots and fewer floating point ops).

Starting with the following definitions (and focusing only on t with the understanding that it applies directly to s:

n = AB x AC n̂ = n / ||n|| t = n̂⋅(AC x p') / (n̂⋅(AB x AC))

We get the following:

t = n̂⋅(AC x p') / (n̂⋅(AB x AC)) - the given equation t = (n / ||n||)⋅(AC x p') / ((n / ||n||)⋅(AB x AC)) - substitute for n̂ t = n⋅(AC x p') / (n⋅(AB x AC)) - cancel 1 / ||n|| t = (AB x AC)⋅(AC x p') / ((AB x AC)⋅(AB x AC)) - substitute for n t = (AB x AC)⋅(AC x p') / ||AB x AC||² - simplify

Nice! Done. PTAL.

SeanCurtis-TRI · 2017-06-01T16:35:44Z

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h

+      double A_x{ccdVec3X(&(simplex->ps[0].v))};
+      double B_x{ccdVec3X(&(simplex->ps[1].v))};
+      double p_x{ccdVec3X(p)};
+      double    s{(p_x - A_x) / (B_x - A_x)};


nit: extra space proceeding s

jslee02 · 2017-06-01T17:47:45Z

@jslee02 do you have experience editing homebrew formulae? If so, could you put in a pull request with that change?

I think I could do that. Let me put that it to my list.

SeanCurtis-TRI

Looks good.

jslee02 · 2017-06-01T21:47:32Z

Submitted a PR: Homebrew/homebrew-science#5762

avalenzu · 2017-06-05T13:26:05Z

I see that the homebrew PR went through! Is there a way to re-trigger the CI checks for this PR?

avalenzu · 2017-06-09T16:30:45Z

@jslee02, am I correct in thinking that this PR is blocked by #216?

Also, could I get your take on the last paragraph of the PR description? Thank you!

jslee02 · 2017-06-14T15:20:53Z

Sorry for delayed response.

#216 is now merged. Could you merge master into this PR so that the CI checks are re-triggered as well?

jslee02 · 2017-06-14T15:24:50Z

@jslee02, is this an appropriate way to expose this knob to the user?

It looks good to me. Adding a new parameter to DistanceRequest would break the API compatibility, but it would be okay because FCL 0.6.0 breaks it anyway by other changes.

avalenzu · 2017-06-14T17:42:48Z

Is it possible that the cached libccd is still single precision on the x64 Windows machine?

jslee02 · 2017-06-14T17:58:00Z

I believe so. Let's refresh the cache by removing it and adding it back (enabling the cache).

This reverts commit 81b20e3.

avalenzu · 2017-06-14T20:48:49Z

OK, the appveyor/pr check passed once the cache was cleared. I've re-enabled the cache now.

jslee02 · 2017-06-14T22:42:31Z

Hm, it seems the cache isn't reset by doing so. Instead, I found another method (ref1, ref2). Could you change this line

cache:
  - C:\Program Files\libccd

to

cache:
  - C:\Program Files\libccd -> appveyor.yml

?

Thanks!

Now the libccd cache should be invalidated whenever `.appveyor.yml` is modified.

avalenzu · 2017-06-15T13:52:17Z

@jslee02: appveyor/pr is finally happy! What's being checked in appveyor/branch?

jslee02 · 2017-06-15T14:04:30Z

I believe appveyor/branch checks for regular commits to the branch, but it seems not to work sometimes. I just removed it from the requirements as suggested in the post.

Merging this! Thank you for the contribution!

Andrés Valenzuela added 7 commits May 30, 2017 14:59

Fix INDEP implementation

6845490

Fix closest points for libccd implementation

fb509de

Uncomment lines in test_fcl_capsule_box_2.cpp

cde4dfc

Uncomment lines in test_fcl_capsule_box_1.cpp

099d73a

Add distance_tolerance to DistanceRequest

12fb537

This allows the user to control the tolerance used in the GJK solvers without explicitly constructing a solver.

Update capsule-box tests to test both solvers

ef33864

Clean up gjk_libccd-inl.h

2004033

SeanCurtis-TRI reviewed Jun 1, 2017

View reviewed changes

This was referenced Jun 1, 2017

Build libccd with double precision on Windows #216

Merged

test_fcl_capsule_box_1 fails with single-precision libccd #217

Open

Address Sean's comments

ee065f8

SeanCurtis-TRI approved these changes Jun 1, 2017

View reviewed changes

avalenzu force-pushed the fix-nearest-points branch from 5638747 to ee065f8 Compare June 2, 2017 13:16

avalenzu mentioned this pull request Jun 2, 2017

Wrong closest points between shapes (box, sphere, cylinder) in distance computations #127

Open

jslee02 approved these changes Jun 14, 2017

View reviewed changes

Merge remote-tracking branch 'upstream/master' into fix-nearest-points

055dadd

Andrés Valenzuela added 2 commits June 14, 2017 15:24

Disable libccd cache

81b20e3

Revert "Disable libccd cache"

65b5cdc

This reverts commit 81b20e3.

Add dependency for libccd cache item

cd38db0

Now the libccd cache should be invalidated whenever `.appveyor.yml` is modified.

jslee02 merged commit f6a73c7 into flexible-collision-library:master Jun 15, 2017

jslee02 added this to the FCL 0.6.0 milestone Oct 27, 2017

j-rivero mentioned this pull request Nov 28, 2017

Update versions of ccd, fcl and octomap. Avoid their installation and hide their binary symbols from libdrake RobotLocomotion/drake#7417

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Return the correct nearest points from `distance(...)` #215

Return the correct nearest points from `distance(...)` #215

avalenzu commented May 31, 2017

avalenzu commented Jun 1, 2017

jslee02 commented Jun 1, 2017 •

edited

Loading

avalenzu commented Jun 1, 2017

jslee02 commented Jun 1, 2017

sherm1 commented Jun 1, 2017

edrumwri commented Jun 1, 2017 via email

avalenzu commented Jun 1, 2017

avalenzu commented Jun 1, 2017

SeanCurtis-TRI left a comment

SeanCurtis-TRI Jun 1, 2017

avalenzu Jun 1, 2017

SeanCurtis-TRI Jun 1, 2017

avalenzu Jun 1, 2017

SeanCurtis-TRI Jun 1, 2017

avalenzu Jun 1, 2017

SeanCurtis-TRI Jun 1, 2017

jslee02 commented Jun 1, 2017

SeanCurtis-TRI left a comment

jslee02 commented Jun 1, 2017

avalenzu commented Jun 5, 2017

avalenzu commented Jun 9, 2017

jslee02 commented Jun 14, 2017

jslee02 commented Jun 14, 2017

avalenzu commented Jun 14, 2017

jslee02 commented Jun 14, 2017

avalenzu commented Jun 14, 2017

jslee02 commented Jun 14, 2017

avalenzu commented Jun 15, 2017

jslee02 commented Jun 15, 2017

Return the correct nearest points from distance(...) #215

Return the correct nearest points from distance(...) #215

Conversation

avalenzu commented May 31, 2017

avalenzu commented Jun 1, 2017

jslee02 commented Jun 1, 2017 • edited Loading

avalenzu commented Jun 1, 2017

jslee02 commented Jun 1, 2017

sherm1 commented Jun 1, 2017

edrumwri commented Jun 1, 2017 via email

avalenzu commented Jun 1, 2017

avalenzu commented Jun 1, 2017

SeanCurtis-TRI left a comment

Choose a reason for hiding this comment

SeanCurtis-TRI Jun 1, 2017

Choose a reason for hiding this comment

avalenzu Jun 1, 2017

Choose a reason for hiding this comment

SeanCurtis-TRI Jun 1, 2017

Choose a reason for hiding this comment

avalenzu Jun 1, 2017

Choose a reason for hiding this comment

SeanCurtis-TRI Jun 1, 2017

Choose a reason for hiding this comment

avalenzu Jun 1, 2017

Choose a reason for hiding this comment

SeanCurtis-TRI Jun 1, 2017

Choose a reason for hiding this comment

jslee02 commented Jun 1, 2017

SeanCurtis-TRI left a comment

Choose a reason for hiding this comment

jslee02 commented Jun 1, 2017

avalenzu commented Jun 5, 2017

avalenzu commented Jun 9, 2017

jslee02 commented Jun 14, 2017

jslee02 commented Jun 14, 2017

avalenzu commented Jun 14, 2017

jslee02 commented Jun 14, 2017

avalenzu commented Jun 14, 2017

jslee02 commented Jun 14, 2017

avalenzu commented Jun 15, 2017

jslee02 commented Jun 15, 2017

Return the correct nearest points from `distance(...)` #215

Return the correct nearest points from `distance(...)` #215

jslee02 commented Jun 1, 2017 •

edited

Loading