libccd EPA expandPolytope: "this should not happen"

[wxmerkt]

As noted in #305 (comment), I have encountered the "should not happen" logic error:

RuntimeError: FCL expandPolytope(): This should not happen. Both the nearest point and the new vertex are on an edge, thus the nearest distance should be 0. This is touching contact, and we should not expand the polytope for touching contact.

Here are details on how to reproduce:

• fcl::distance call between a box (o1, GEOM_BOX) and a cylinder (o2, GEOM_CYLINDER).
• The box dimensions are: .05 0.39 1.47
• The cylinder dimensions are: length=0.18, radius=0.18
• The box transform is: xyz=0.8 -0.385 0.735; wxyz=0.707107 0 0 0.707107
• The cylinder transform is: xyz=0.4 -0.4 0.93; wxyz=0.991445 0 0 -0.130526

I have been able to reliably reproduce it (as part of a larger process - caught and extracted the above from it).

cc @hongkai-dai

[SeanCurtis-TRI]

FTR This is using double as the scalar?

[wxmerkt]

For FCL and our own code, yes. For libccd it is using the 16.04 system-provided libccd2/libccd-dev package.

[wxmerkt]

I just upgraded to the latest master and I am still getting this fairly frequently. Any new insights on this? I do not yet have more reproducible cases beyond the one above (as I am not logging the faulty cases any more). Happy to help if I can (Ubuntu 16.04, libccd from system Debians, latest master, only shape primitives, all double).

[SeanCurtis-TRI]

Are you getting the same "this shouldn't be happening" or are you getting a "zero area" error? It's an important distinction.

[wxmerkt]

From my failure logs - it is always this one:

RuntimeError: FCL expandPolytope(): This should not happen. Both the nearest point and the new vertex are on an edge, thus the nearest distance should be 0. This is touching contact, and we should not expand the polytope for touching contact.
RuntimeError: FCL expandPolytope(): This should not happen. Both the nearest point and the new vertex are on an edge, thus the nearest distance should be 0. This is touching contact, and we should not expand the polytope for touching contact.
RuntimeError: FCL expandPolytope(): This should not happen. Both the nearest point and the new vertex are on an edge, thus the nearest distance should be 0. This is touching contact, and we should not expand the polytope for touching contact.
[... more omitted...]

[SeanCurtis-TRI]

Good to know; we were hoping it was just due to the fact that we weren't handling degenerate triangles. But clearly, that is not the case. Thanks for checking with the new master.

[hongkai-dai]

Thanks, I will try to write a test to reproduce the error.

[hongkai-dai]

@wxmerkt I tried to reproduce the error, but couldn't. The test is in https://github.com/hongkai-dai/fcl/blob/epa_touching_degenerate/test/narrowphase/detail/convexity_based_algorithm/test_box_cylinder.cpp.

Actually the GJKSignedDistance thinks that the two objects are separated by 2.5 cm, so it doesn't call EPA algorithm at all.

Could you let me know if I did something wrong in this test? Thanks!

[hongkai-dai]

In your data, the box is rotated by 90 degrees about z axis, the cylinder is also rotated about z axis by a certain angle (but the rotation on the cylinder does not matter, since z axis is the axis of the cylinder). So after transformation, in the xy plane, you have a box with dimension (0.39, 0.05) centered at (0.8, -0.385), and a circle with radius 0.18, centered at (0.4, -0.4). You can see that along the x axis, the "rightmost" point of the circle is (0.58, -0.4), and the "leftmost" point of the box is (0.605, -0.4), where 0.605 = 0.8 - 0.39 / 2, so they are exactly separated by 0.605 - 0.58 = 0.025m

[hongkai-dai]

Could you post the argument in your fcl::distance function call?

[wxmerkt]

Hi Hongkai and Sean,
Apologies for the delay in getting back to you. Unfortunately I do not have an exact/minimally working reproduction case at hand and can only say according to logs it happens a lot :'-(. My arguments for fcl::distance are o1 and o2 as above (primitive cylinder, spheres, or boxes):

request.enable_nearest_points = true;
request.enable_signed_distance = true; 
request.distance_tolerance = 1e-6;
request.gjk_solver_type = fcl::GST_LIBCCD;

Otherwise I can add a try-catch block to extract more details when it happens.

Best,
Wolfgang

[hongkai-dai]

Hi Wolfgang,

I tried the argument you used, and fcl still doesn't throw me the error. The code is in https://github.com/hongkai-dai/fcl/blob/epa_touching_degenerate/test/narrowphase/detail/convexity_based_algorithm/test_box_cylinder.cpp.

I am quite sure with the box and cylinder you mentioned, it cannot cause FCL to throw that error. Since the box and cylinder are separated by 2.5cm. On the other hand, the EPA algorithm is only called if the two objects are penetrating. Could you check if the box/cylinder data are correct?

Best,
Hongkai

[hongkai-dai]

@wxmerkt , we merged a new PR #336 into the master, that should fix one of the "This should not happen" problems. Is it OK if you could test your problem on master?

[wxmerkt]

@hongkai-dai Thank you, I will update and run it later this week or after IROS.

[wxmerkt]

Hi,
I just upgraded to include the latest changes from #381 and it immediately caught a failure in the wild:

RuntimeError: Error with configuration
  Original error message: /home/wxm/dev/install_ws/build/fcl_catkin/fcl_src-prefix/src/fcl_src/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h:(1305): expandPolytope(): Both the nearest point and the new vertex are on an edge, thus the nearest distance should be 0. This is touching contact, and should already have been identified
  Shape 1: Cylinder(r: 0.050000000000000002776, lz: 0.05999999999999999778)
  X_FS1
   -0.99999999997999022838  6.2572835802045040178e-10  6.3260669852976095481e-06     0.57500009756757608503
 6.3260669851683709551e-06 -6.3943303429958554955e-10     0.99999999997999056145    -0.42711963046787942977
 6.2573180158128459924e-10                          1  6.3942912945996747041e-10      1.1867093358746836351
                         0                          0                          0                          1
  Shape 2: Box0.025000000000000001388   0.3499999999999999778   1.8449999999999999734
  X_FS2
6.1232339957367660359e-17                        -1                         0    0.80000000000000004441
                        1 6.1232339957367660359e-17                         0   -0.45750000000000001776
                        0                         0                         1     1.0224999999999999645
                        0                         0                         0                         1
  Solver: GjkSolver_libccd
    collision_tolerance:      2.0097183471152322134e-14
    max collision iterations: 500
    distance tolerance:       9.9999999999999995475e-07
    max distance iterations:  1000

[SeanCurtis-TRI]

Thanks for the data. This is our second data point drawn from the wild. A previous reported failure was exercised in this branch, but using the reported data, I was unable to reproduce the error.

I used your data and was likewise unable to reproduce that error. So, the hypothesis that #381 is outputting sufficient information to reproduce the inexplicable phenomenon is taking a severe beating. Obviously, more work has to be done to figure out why this is happening. Feel free to look at the branch and let me know if you think I've missed something obvious in reproducing the circumstances that should lead to the error.

Alternatively, there must be some additional state/parameter/configuration that contributes to the error. Please let me know if you gain any particular insight.

[wxmerkt]

Thank you for the quick response @SeanCurtis-TRI. I have tried this with both the libccd-dev provided in Ubuntu 18.04 and a self-compiled version of the latest master (but without USE_DOUBLE_PRECISION) - what configurations are you recommending for use with fcl master? libccd master with double precision?

[SeanCurtis-TRI]

Yes and yes: double and libccd master. In the last 30 minutes, I've had a third data point that likewise doesn't reproduce the error. So, the evidence is mounting that the log isn't serving the purpose it was intended to.

We apparently have a "reliable" set of code to produce this bug. So, probably next week, I'm going to run that and dig down into FCL from the actual end application to see if I can't figure out first, what information is relevant and, second, what the actual bug is.

[wxmerkt]

Thank you Sean, this sounds promising - I will continue to test here and see if I can extract any more insights and will update the issue if so.

[SeanCurtis-TRI]

Thanks. Hopefully we'll track this down relatively soon.