Correct distance queries to report nearest point in world frame

[SeanCurtis-TRI]

Changed slightly from the original PR submission.

Now this focuses on correcting the return values in the distance queries. DistanceResult reports that the near points are reported in the world frame. However, this was not supported by the code. This PR changes the various distance queries to report world-frame values and updates the tests to reflect this.

This PR is an alternate variation of #250 -- trying to get through CI.

• The first was the convex shape's aabb was being initialized incorrectly causing it to always be infinite size.
• The second, is I noticed that when getting distance information for convex shapes the nearest points were appeared to be in the shape frame to the global. After making the changes below they are now in the global frame.

NOTE: Part of this PR introduces a signed-distance test between a sphere and capsule. The test revealed an error in the calculation. The test documents this error.

---

This change is 

[Levi-Armstrong]

Thank you for taking a look. I will test it this weekend.

[Levi-Armstrong]

@SeanCurtis-TRI Sorry it took me so long to get around to testing it. This combined with the EPA fix seems to have solved the issues I was having. 👍 to merge.

[SeanCurtis-TRI]

Updated the PR w.r.t. master. This is really 2.1 PRs all rolled into one:

1. Correction of Convex geometry (initialization shadowing and bounding box calculation). This should be in its own PR with a supporting unit test.
2. Correction of signed distance to report points on surface in world frame (as documented in distance_result.h. This has plenty of unit tests.
3. A typo I happened across.

I'll probably pull the Convex geometry out of this PR and put it into its own PR (with unit tests) and leave everything else in this PR in a curated and documented PR. But, first, I'm going to let it run through CI before I start beating up on it.

So, I'll ping for a fresh review a bit later.

Reviewable status: 0 of 11 files reviewed, all discussions resolved

[SeanCurtis-TRI]

Ok. @Levi-Armstrong , I'm ready for another review. I've pulled out the convex geometry fixes into a separate PR (#325). This is now purely about the frame the nearest points are returned in. One more review and we can move forward.

Reviewable status: 0 of 10 files reviewed, all discussions resolved

[Levi-Armstrong]

@SeanCurtis-TRI I built everything using this PR along with #325 and all of my test now pass. 👍

[SeanCurtis-TRI]

Great, thansk.

Reviewable status: 0 of 10 files reviewed, all discussions resolved

[Levi-Armstrong]

@SeanCurtis-TRI Are you wanting me to review this Reviewable?

[SeanCurtis-TRI]

I'd appreciate that. How "aggressive" can you be in your review? I don't know if you've noticed, but we tend to be pretty picky over here (in the belief that it leads to better code hitting master). I value someone catching me when I've done something foolish. :) So, if you're up for that, please do a full code review. Otherwise, I'll have fellow TRI-er handle it.

Reviewable status: 0 of 10 files reviewed, all discussions resolved

[SeanCurtis-TRI]

+@hongkai-dai for feature review, please

Reviewable status: 0 of 10 files reviewed, all discussions resolved

[hongkai-dai]

Reviewed 10 of 10 files at r1.
Reviewable status: all files reviewed, 4 unresolved discussions (waiting on @SeanCurtis-TRI)

---

test/test_fcl_distance.cpp, line 343 at r1 (raw file):

  // The "nearest" points (N1 and N2) measured and expressed in box 1's and
  // box 2's frames (B1 and B2, respectively).
  Vector3<S> expected_p_B1N1{-1.375, -0.098881502700918666,

const Vector3<S>

BTW, I am wondering how the number -0.098881502700918666 is obtained. It seems to me that we cout the result from the FCL, instead of computing this number separately.

---

test/test_fcl_distance.cpp, line 348 at r1 (raw file):

                            0.084299993303443954};
  // The nearest points in the world frame.
  Vector3<S> expected_p_WN1 = box_object_1.getTransform() * expected_p_B1N1;

Ditto

---

test/test_fcl_signed_distance.cpp, line 97 at r1 (raw file):

  }

  // Expecting distance to be -1.715728753

This is a magic number to me. How did you obtain it? I assume you can compute it in the form of |sphere_center1 - sphere_center2| - radius1 - radius2?

---

test/test_fcl_signed_distance.cpp, line 109 at r1 (raw file):

  {
    Vector3<S> dir = tf2.translation().normalized();
    Vector3<S> p0_expected = dir * 20;

Why 20 here? Is it because tf2.translation() = Vector3<S>(20, 0, 20)? If so, I would suggest to use a named variable to replace 20 in all three instances.

[SeanCurtis-TRI]

Reviewable status: 8 of 10 files reviewed, 4 unresolved discussions (waiting on @hongkai-dai and @SeanCurtis-TRI)

---

test/test_fcl_distance.cpp, line 343 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

const Vector3<S>

BTW, I am wondering how the number -0.098881502700918666 is obtained. It seems to me that we cout the result from the FCL, instead of computing this number separately.

I've modified the documentation to indicate its significance. This configuration is such that if we could just "compute the number separately", then we'd have duplicated all of the complex code that's being tested.

---

test/test_fcl_distance.cpp, line 348 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Ditto

Ditto

---

test/test_fcl_signed_distance.cpp, line 97 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

This is a magic number to me. How did you obtain it? I assume you can compute it in the form of |sphere_center1 - sphere_center2| - radius1 - radius2?

Done

---

test/test_fcl_signed_distance.cpp, line 109 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Why 20 here? Is it because tf2.translation() = Vector3<S>(20, 0, 20)? If so, I would suggest to use a named variable to replace 20 in all three instances.

I've re-expressed in terms of labeled quantities.

[hongkai-dai]

+@sherm1 for platform review please.

Reviewed 2 of 2 files at r2.
Reviewable status: complete! all files reviewed, all discussions resolved

[sherm1]

-- SO many broken things fixed here!
(one minor comment)

Reviewed 8 of 10 files at r1, 2 of 2 files at r2.
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @SeanCurtis-TRI)

---

include/fcl/narrowphase/detail/gjk_solver_indep-inl.h, line 887 at r2 (raw file):

      // The answers are produced in world coordinates. Keep them there.
      if(p1) p1->noalias() = w0;
      if(p2) p2->noalias() = w1;

Nit: noalias() is not useful here (and doesn't do anything). As I understand it, it is only meaningful for matrix multiply where a temp-and-copy would otherwise be done.

[sherm1]

CI is sad.

Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @SeanCurtis-TRI)

[SeanCurtis-TRI]

It is. And not because of code -- but because Travis is sad.

Reviewable status: 9 of 10 files reviewed, all discussions resolved (waiting on @hongkai-dai and @sherm1)

---

include/fcl/narrowphase/detail/gjk_solver_indep-inl.h, line 887 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Nit: noalias() is not useful here (and doesn't do anything). As I understand it, it is only meaningful for matrix multiply where a temp-and-copy would otherwise be done.

Done

[sherm1]

Reviewed 1 of 1 files at r3.
Reviewable status: complete! all files reviewed, all discussions resolved