Cylinder half space bug fix #255

amcastro-tri · 2018-02-14T18:05:40Z

There is a bug with the penetration query for cylinder vs half space. This is reported in RobotLocomotion/drake#8049.

This PR fixes this bug and implements a new unit test that without the fix would not pass.

This change is

amcastro-tri · 2018-02-14T18:07:24Z

@jslee02, would you be able to review this?

jslee02

It looks good to me. The unit test is very clear. 😄

I just made some minor comments.

Also, it seems there is a similar issue in the cone-halfspace intersection test. Would you mind fixing that as well?

jslee02 · 2018-02-14T18:20:13Z

test/test_fcl_cylinder_half_space.cpp

+
+  // Now perform the same test but with the cylinder's z axis Cz pointing down.
+  X_WC.linear() = AngleAxisd(M_PI, Vector3d::UnitX()).matrix();
+  X_WC.translation() = Vector3d(0, 0, 0.049);


Nit: It would be good to change Vector3d to Vector3<S> for completeness.

Excellent, that's right. I was just copying the patter in other unit tests and I templated everything on <S>. Did you have a chance to test FCL with AutoDiffXd? the automatic differentiation scalar from Eigen.

I wonder what it would take to be able to say: test_collision_cylinder_half_space<AutoDiffXd>(fcl::GJKSolverType::GST_INDEP);. Here in the development of Drake at TRI we would love to be able to do just that! :-)

I wrote a very simple test case with AutoDiffScalar here. Is that what you're looking for?

Excellent! is fcl::collide() also supported? will it work with meshes?

I don't see any particular reason fcl::collide() wouldn't work as long as everything is templated for the scalar type, which I believe so, but I haven't tested it myself.

jslee02 · 2018-02-14T18:20:41Z

test/test_fcl_cylinder_half_space.cpp

+  EXPECT_NEAR(contacts[0].penetration_depth, 0.051, kTolerance);
+
+  // Now perform the same test but with the cylinder's z axis Cz pointing down.
+  X_WC.linear() = AngleAxisd(M_PI, Vector3d::UnitX()).matrix();


Nit: It would be good to use AngleAxis<S> for completeness.

amcastro-tri · 2018-02-14T19:04:31Z

I saw the cone problem also. I don't think I'll have time to push a unit test any time soon. I'll see to write a unit test for that one on Monday.

@jslee02, is there a reason to use halfspaceIntersectTolerance<S>() in the comparisons? I don't see why not to compare against 0. halfspaceIntersectTolerance<S>() resolves to 1e-7 here.

jslee02 · 2018-02-14T20:21:27Z

I saw the cone problem also. I don't think I'll have time to push a unit test any time soon. I'll see to write a unit test for that one on Monday.

No worries. Let me create an issue for this to track.

is there a reason to use halfspaceIntersectTolerance<S>() in the comparisons? I don't see why not to compare against 0. halfspaceIntersectTolerance<S>() resolves to 1e-7 here.

I didn't write the code, but here is my hunch. Zero tolerance would be too strict criteria for an object (cylinder or cone) to be regarded laid on the ground parallel to the ground surface. Suppose you drop an object on the ground (halfspace) in a physics simulator, then the object would jitter a lot before it becomes stable due to the on-and-off contact points on the end of edges of the side (e.g., the contact points on the top and bottom of the cylinder).

amcastro-tri · 2018-02-15T00:01:17Z

Suppose you drop an object on the ground (halfspace) in a physics simulator, then the object would jitter a lot before it becomes stable due to the on-and-off contact points on the end of edges of the side (e.g., the contact points on the top and bottom of the cylinder).

That is a problem of the simulator what to do with the information you provide. The problem you mention is because real physical contact does not happen at points but on surfaces patches. I believe that as a geometry engine, the penetration queries should be purely geometrical, irrespective of what a physics engine might do with the results. Attempting to couple them with tricks as using a loose tolerance will most likely bite you back and produce difficult to find bugs.

I think the right thing to do would be doing the check against "zero", though I'd need to investigate the math in this particular case further to make sure there is no other pathological case.

jslee02 · 2018-02-15T00:08:59Z

That is a problem of the simulator what to do with the information you provide. The problem you mention is because real physical contact does not happen at points but on surfaces patches. I believe that as a geometry engine, the penetration queries should be purely geometrical, irrespective of what a physics engine might do with the results. Attempting to couple them with tricks as using a loose tolerance will most likely bite you back and produce difficult to find bugs.

That totally makes sense. Thinking further, I actually presume that's because "comparing floating point number to zero is not safe". Probably, we never get to the case of the if-statement is true if we compare cosa to "exact" zero except some special cases (e.g., v = (1, 0, 0) and w = (0, 1, 0)). There are many ways of comparing a floating point number to zero, and here we use one of the ways (i.e., using fixed threshold).

I'm open to using a better way for this.

amcastro-tri · 2018-02-15T00:20:40Z

Yes, on further thought I agree with you. Using a tolerance is a good idea. However, what is the best way to choose this tolerance? shouldn't it be a number closer to machine epsilon? 1e-7 seems a bit loose to me but there might be a good reason for it.

jslee02 · 2018-02-15T00:31:40Z

what is the best way to choose this tolerance?

Sorry, it's a tricky question to answer to me. 😓

sherm1 · 2018-02-15T01:03:35Z

Travis CI is failing for this PR. Is that due to this change?

SeanCurtis-TRI · 2018-02-15T16:33:15Z

I also need to ponder this.

@amcastro-tri makes a compelling point about the geometry query introducing error before the physics gets a chance to reason at all.

However, the idea of testing a float against zero feels categorically wrong. For an arbitrarily oriented plane, it seems that the odds that a cylinder would ever be considered as "lying" on the plane would be roughly 1 in 2 billion. It's like testing for vertex-vertex collision -- simply not worth it.

It comes down to the fact that the geometric query itself is subject to numerical accuracy and it should do its best effort to come up with the right geometric answer which accounts for numerical. I think operating with respect to an epsilon in this regard is inevitable. It should be well reasoned and it may be that 1e-7 is far too large for doubles.

But, at the end of the day, strictly comparing against 0 just seems suspicious.

sherm1 · 2018-02-15T16:58:47Z

test/test_fcl_cylinder_half_space.cpp

+  // Pose of half space frame H in the world frame W.
+  Transform3<S> X_WH = Transform3<S>::Identity();
+
+  CollisionObject<S> half_sapce_co(half_space, X_WH);


sapce -> space

sherm1 · 2018-02-15T17:31:29Z

One rational way to choose tolerances is to view them as the result of an optimization in which we try to make a numerical calculation as close to the analytical result as possible. A tolerance of 0 won't do that because the computation will become dominated by roundoff error. A tolerance of 0.1 won't do that because it is a bad approximation of the geometry (sometimes called "truncation error"). Somewhere in between is a tolerance that minimizes some measure of the actual error.

In the case of this cylinder vs half space algorithm, I believe the tolerance is being used to choose one of three discrete contact point possibilities: center of an end cap, edge of an end cap, or center of a long edge of the tube. If the objects couldn't interpenetrate, the contact points really would change discretely. But these actually interpenetrate, so there is an overlap volume that evolves continuously. That provides a "right answer" that we could hope to approximate: that the contact "point" is at the centroid of the overlap volume. Ideally the algorithm would just return that point! But if it has to instead make discrete choices, the ideal tolerance would be the one that minimizes a measure of the deviation between the returned point and that centroid. Of course it is easier said than done to make that determination, but at least there is a reasonable way to think about it.

SeanCurtis-TRI · 2018-02-15T17:41:02Z

I've retriggered CI - one test was clearly a flake (and has since passed). The two mac tests seem to fail on an unrelated test. I've retriggered them and may take a peek later if they still fail.

However, both mac build tests are stalled in starting.

SeanCurtis-TRI · 2018-02-15T18:49:41Z

include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h

@@ -371,7 +371,7 @@ bool cylinderHalfspaceIntersect(const Cylinder<S>& s1, const Transform3<S>& tf1,
  Vector3<S> dir_z = R.col(2);
  S cosa = dir_z.dot(new_s2.n);

-  if(cosa < halfspaceIntersectTolerance<S>())
+  if(std::abs(cosa) < halfspaceIntersectTolerance<S>())


I'm putting this comment here because github won't let me put it where I want to.

On line 394 we have the line of code:

if (std::abs(cosa + 1) < halfspaceIntersectTolerance<S>() || std::abs(cosa - 1) < halfspaceIntersectTolerance<S>())

It could be replaced with:

if (std::abs(cosa) - 1 < halfspaceIntersectTolerance<S>())

I figured, as long as we're in this code, we could patch this as well (although this isn't a correctness issue, merely an efficiency/compactness issue).

SeanCurtis-TRI · 2018-02-17T04:43:40Z

@jslee02 The mac CI is failing on (what seems to be) an unrelated test. I don't have access to a mac; any idea what the best way to resolve this is?

Review status: 0 of 3 files reviewed at latest revision, 4 unresolved discussions, some commit checks failed.

Comments from Reviewable

amcastro-tri · 2018-02-23T19:05:47Z

@jslee02 the mac CI keeps failing and it doesn't seem to be an obvious problem. Should we create an issue for this? do you think you'd have an idea on what causes the problem?

SeanCurtis-TRI · 2018-02-23T19:33:31Z

I added issue #260.

jslee02 · 2018-02-23T23:44:44Z

@amcastro-tri I retriggered Travis for master branch builds, which were failed due to failing in installing homebrew-science/libccd, to see if the current failing is actually due to this change. Since libccd is now available at homebrew-core, the master branch builds should be able to install libccd.

jslee02 · 2018-02-24T00:04:07Z

Okay, it seems the failure was introduced by #236 because Build #612 passed but not Build #626 (after #236 merged).

SeanCurtis-TRI · 2018-03-02T17:58:55Z

I went ahead and addressed the comments.

Reviewed 2 of 3 files at r1, 1 of 1 files at r2, 2 of 2 files at r3.
Review status: all files reviewed at latest revision, 4 unresolved discussions, some commit checks failed.

include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h, line 374 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I'm putting this comment here because github won't let me put it where I want to.

On line 394 we have the line of code:
if (std::abs(cosa + 1) < halfspaceIntersectTolerance<S>() || std::abs(cosa - 1) < halfspaceIntersectTolerance<S>())
It could be replaced with:
if (std::abs(cosa) - 1 < halfspaceIntersectTolerance<S>())
I figured, as long as we're in this code, we could patch this as well (although this isn't a correctness issue, merely an efficiency/compactness issue).

Done

test/test_fcl_cylinder_half_space.cpp, line 82 at r1 (raw file):

Previously, jslee02 (Jeongseok Lee) wrote…

Nit: It would be good to use AngleAxis<S> for completeness.

Done

test/test_fcl_cylinder_half_space.cpp, line 83 at r1 (raw file):

Previously, jslee02 (Jeongseok Lee) wrote…

I don't see any particular reason fcl::collide() wouldn't work as long as everything is templated for the scalar type, which I believe so, but I haven't tested it myself.

Done

test/test_fcl_cylinder_half_space.cpp, line 65 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

sapce -> space

Done

Comments from Reviewable

1. Fix typos 2. Update copyright 3. Simplify mathematical expression 4. Test w.r.t. templated scalar.

amcastro-tri · 2018-03-02T19:20:34Z

Thank you @SeanCurtis-TRI!

Review status: all files reviewed at latest revision, 2 unresolved discussions.

Comments from Reviewable

sherm1 · 2018-03-03T00:41:39Z

Review status: all files reviewed at latest revision, 2 unresolved discussions, some commit checks failed.

Comments from Reviewable

amcastro-tri mentioned this pull request Feb 14, 2018

Proximity engine seems to report wrong results with cylinders RobotLocomotion/drake#8049

Closed

jslee02 reviewed Feb 14, 2018

View reviewed changes

jslee02 mentioned this pull request Feb 14, 2018

Bug in cone-halfspace intersection computation #256

Open

jslee02 mentioned this pull request Feb 15, 2018

Consider using better tolerance for floating point number comparison #257

Open

sherm1 reviewed Feb 15, 2018

View reviewed changes

SeanCurtis-TRI reviewed Feb 15, 2018

View reviewed changes

SeanCurtis-TRI force-pushed the cylinder_half_space_bug_fix branch 2 times, most recently from 83dc1ad to 4105549 Compare March 2, 2018 17:58

amcastro-tri and others added 4 commits March 2, 2018 10:00

Fixes bug in cylinderHalfspaceIntersect

7d4a14f

Implements unit test

c3817d4

Increases test tolerance due to mac failure

e753d52

Address reviewer comments

4a1677f

1. Fix typos 2. Update copyright 3. Simplify mathematical expression 4. Test w.r.t. templated scalar.

SeanCurtis-TRI force-pushed the cylinder_half_space_bug_fix branch from 4105549 to 4a1677f Compare March 2, 2018 18:00

sherm1 merged commit 8af86af into flexible-collision-library:master Mar 3, 2018

amcastro-tri mentioned this pull request Mar 12, 2018

Revert bug introduced in cylinder/half-space intersection. #267

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Cylinder half space bug fix #255

Cylinder half space bug fix #255

amcastro-tri commented Feb 14, 2018 •

edited by sherm1

amcastro-tri commented Feb 14, 2018

jslee02 left a comment

jslee02 Feb 14, 2018

amcastro-tri Feb 14, 2018

amcastro-tri Feb 14, 2018

jslee02 Feb 14, 2018

amcastro-tri Feb 15, 2018

jslee02 Feb 15, 2018

jslee02 Feb 14, 2018 •

edited

amcastro-tri commented Feb 14, 2018

jslee02 commented Feb 14, 2018 •

edited

amcastro-tri commented Feb 15, 2018

jslee02 commented Feb 15, 2018 •

edited

amcastro-tri commented Feb 15, 2018

jslee02 commented Feb 15, 2018

sherm1 commented Feb 15, 2018

SeanCurtis-TRI commented Feb 15, 2018

sherm1 Feb 15, 2018

sherm1 commented Feb 15, 2018

SeanCurtis-TRI commented Feb 15, 2018 •

edited

SeanCurtis-TRI Feb 15, 2018

SeanCurtis-TRI commented Feb 17, 2018

amcastro-tri commented Feb 23, 2018

SeanCurtis-TRI commented Feb 23, 2018

jslee02 commented Feb 23, 2018

jslee02 commented Feb 24, 2018

SeanCurtis-TRI commented Mar 2, 2018

amcastro-tri commented Mar 2, 2018

sherm1 commented Mar 3, 2018

Cylinder half space bug fix #255

Cylinder half space bug fix #255

Conversation

amcastro-tri commented Feb 14, 2018 • edited by sherm1

amcastro-tri commented Feb 14, 2018

jslee02 left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jslee02 Feb 14, 2018 • edited

Choose a reason for hiding this comment

amcastro-tri commented Feb 14, 2018

jslee02 commented Feb 14, 2018 • edited

amcastro-tri commented Feb 15, 2018

jslee02 commented Feb 15, 2018 • edited

amcastro-tri commented Feb 15, 2018

jslee02 commented Feb 15, 2018

sherm1 commented Feb 15, 2018

SeanCurtis-TRI commented Feb 15, 2018

Choose a reason for hiding this comment

sherm1 commented Feb 15, 2018

SeanCurtis-TRI commented Feb 15, 2018 • edited

Choose a reason for hiding this comment

SeanCurtis-TRI commented Feb 17, 2018

amcastro-tri commented Feb 23, 2018

SeanCurtis-TRI commented Feb 23, 2018

jslee02 commented Feb 23, 2018

jslee02 commented Feb 24, 2018

SeanCurtis-TRI commented Mar 2, 2018

amcastro-tri commented Mar 2, 2018

sherm1 commented Mar 3, 2018

amcastro-tri commented Feb 14, 2018 •

edited by sherm1

jslee02 Feb 14, 2018 •

edited

jslee02 commented Feb 14, 2018 •

edited

jslee02 commented Feb 15, 2018 •

edited

SeanCurtis-TRI commented Feb 15, 2018 •

edited