Report configuration state for narrowphase errors

[SeanCurtis-TRI]

This allows low-level, narrowphase algorithms to indicate to higher-level callers that something went so wrong that it would be helpful to capture the configuration that led to the error.

resolves #364

Add exception

1. Add exception type.
2. Instrument some know problem call sites with the new exception.
3. Expand unit tests to cover the exceptions.
   i. Add FCL_EXPECT_THROWS_MESSAGE* functionality for tests.

Add exception catching

1. Add exception processing at an appropriate call site.
   1. Requires printing out shapes and solver data; so streaming operator
      support has been added to all corresponding parties.
   2. Added only to signed distance functions of both solvers.

Notes:

• This doesn't include any unit test for the exception being thrown in context because it has historically been difficult to author such scenarios (which is why we are creating this in the first place). I'm open to testing suggestions.
• CollisionGeometry can span multiple types and this doesn't necessarily cover them all. However, unit tests pass and this can be extended to encompass more geometry types as shown necessary.

---

This change is 

[SeanCurtis-TRI]

+@DamrongGuoy for feature review.

cc @hongkai-dai

Reviewable status: 0 of 19 files reviewed, all discussions resolved (waiting on @DamrongGuoy)

[DamrongGuoy]

with some questions.

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

---

include/fcl/geometry/shape/capsule.h, line 40 at r1 (raw file):

#ifndef FCL_SHAPE_CAPSULE_H
#define FCL_SHAPE_CAPSULE_H

BTW, #include <iostream> ?

---

include/fcl/geometry/shape/cone.h, line 40 at r1 (raw file):

#ifndef FCL_SHAPE_CONE_H
#define FCL_SHAPE_CONE_H

BTW, #include <iostream> ?

---

include/fcl/geometry/shape/convex.h, line 41 at r1 (raw file):

#ifndef FCL_SHAPE_CONVEX_H
#define FCL_SHAPE_CONVEX_H

BTW, #include <iostream> ?

---

include/fcl/geometry/shape/cylinder.h, line 40 at r1 (raw file):

#ifndef FCL_SHAPE_CYLINDER_H
#define FCL_SHAPE_CYLINDER_H

BTW, #include <iostream> ?

---

include/fcl/geometry/shape/cylinder.h, line 81 at r1 (raw file):

  friend
  std::ostream& operator<<(std::ostream& out, const Cylinder& cylinder) {
    out << "Capsule(r: " << cylinder.radius << ", lz: " << cylinder.lz << ")";

Nit, out << "Cylinder not Capsule.

---

include/fcl/geometry/shape/ellipsoid.h, line 40 at r1 (raw file):

#ifndef FCL_SHAPE_ELLIPSOID_H
#define FCL_SHAPE_ELLIPSOID_H

BTW, #include <iostream> ?

---

include/fcl/geometry/shape/halfspace.h, line 40 at r1 (raw file):

#ifndef FCL_SHAPE_HALFSPACE_H
#define FCL_SHAPE_HALFSPACE_H

BTW, #include <iostream> ?

---

include/fcl/geometry/shape/plane.h, line 40 at r1 (raw file):

#ifndef FCL_SHAPE_PLANE_H
#define FCL_SHAPE_PLANE_H

BTW, #include <iostream> ?

---

include/fcl/geometry/shape/triangle_p.h, line 40 at r1 (raw file):

#ifndef FCL_SHAPE_TRIANGLE_P_H
#define FCL_SHAPE_TRIANGLE_P_H

BTW, #include <iostream> ?

---

include/fcl/narrowphase/detail/gjk_solver_indep.h, line 40 at r1 (raw file):

#ifndef FCL_NARROWPHASE_GJKSOLVERINDEP_H
#define FCL_NARROWPHASE_GJKSOLVERINDEP_H

BTW, #include <iostream> ?

---

include/fcl/narrowphase/detail/gjk_solver_libccd.h, line 40 at r1 (raw file):

#ifndef FCL_NARROWPHASE_GJKSOLVERLIBCCD_H
#define FCL_NARROWPHASE_GJKSOLVERLIBCCD_H

BTW, #include <iostream> ?

---

include/fcl/narrowphase/detail/unexpected_configuration_exception.h, line 37 at r1 (raw file):

/** @author Sean Curtis (sean@tri.global) */

#ifndef FCL_CONFIGURATION_EXCEPTION_H

BTW, would FCL_UNEXPECTED_CONFIGURATION_EXCEPTION_H have been too long?
Another note, I'm just curious whether we should consider #pragma once.

---

include/fcl/narrowphase/detail/unexpected_configuration_exception.h, line 76 at r1 (raw file):

 * @param message  The error message itself.
 * @param file     The name of the file associated with the exception.
 * @param func     The name of the invoking function.

BTW, the function signature has const char* func before const char* file. Perhaps we want the documentation to go in that order too?

---

include/fcl/narrowphase/detail/unexpected_configuration_exception.h, line 103 at r1 (raw file):

     << "\n  Original error message: " << e.what()
     << "\n  Shape 1: " << s1
     << "\n  X_FS1\n" << X_FS1.matrix().transpose()

BTW, I understand vector.transpose() make sense because it will print in one line. However, I'm not sure how matrix().transpose() help? It still prints in multiple lines? I may not understand Eigen enough.

---

include/fcl/narrowphase/detail/unexpected_configuration_exception.h, line 105 at r1 (raw file):

     << "\n  X_FS1\n" << X_FS1.matrix().transpose()
     << "\n  Shape 2: " << s2
     << "\n  X_FS2\n" << X_FS2.matrix().transpose()

BTW, transpose() or not?

---

include/fcl/narrowphase/detail/unexpected_configuration_exception.h, line 118 at r1 (raw file):

// TODO(SeanCurtis-TRI): Add some helper functions to transform UCException into
// std::logic_error.

BTW, just to confirm my understanding. ThrowDetailedConfiguration throws std::logic_error, and ThrowUnexpectedConfigurationException throws UnexpectedConfigurationException. The TODO is about converting the latter to std::logic_error with perhaps additional info?

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1202 at r1 (raw file):

 * @pre The face `f` is visible from `query_point`.
 * @pre Output parameters are non-null.
 * TODO(hongkai.dai@tri.global) Replace patch computation with patch deletion

BTW, should we add something like this to the documentation?

@throws UnexpectedConfigurationException if we fail to compute a visible patch.

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1258 at r1 (raw file):

 * @throws UnexpectedConfigurationException if expanding is meaningless either
 * because 1) the nearest feature is a vertex, 2) the new vertex lies on
 * an edge of the current polytope, or 3) the polytope has zero-area triangles.

BTW, I see 1) at line 1283 and 2) at line 1300. However, I do not see 3) the polytope has zero-area triangles.

---

test/expect_throws_message.h, line 9 at r1 (raw file):

#define FCL_EXPECT_THROWS_MESSAGE_HELPER(expression, exception, regexp, \
                                         must_throw, fatal_failure) \

BTW, it would be nice if we have a short documentation about must_throw and fatal_failure. I thought I understood, but then I got confused at line 38 below.

---

test/expect_throws_message.h, line 38 at r1 (raw file):

#ifdef NDEBUG
// Throwing the expected message is optional in this case.

BTW, it made me pause and think about {debug, non-debug}x{EXPECT, ASSERT}. Conceptually, I think it was like:

_EXPECT_THROW_MESSAGE_IF_DEBUG =
    non-debug ? _EXPECT_THROWS_MESSAGE_HELPER(...,optional, non-fatal) : _EXPECT_THROWS_MESSAGE(...,)
_ASSERT_THROW_MESSAGE_IF_DEBUG =
    non-debug ? _EXPECT_THROWS_MESSAGE_HELPER(...,optional, fatal) : _ASSERT_THROWS_MESSAGE(...)

---

test/expect_throws_message.h, line 42 at r1 (raw file):

#define FCL_EXPECT_THROWS_MESSAGE_IF_DEBUG(expression, exception, regexp) \
  FCL_EXPECT_THROWS_MESSAGE_HELPER(expression, exception, regexp, \
                                   false /*optional*/, false /*non-fatal*/)

BTW, when I first saw false /*optional*/, I thought optional = false, so it's mandatory. Then, I realized it's "must_throw = false", so it's really optional.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp, line 50 at r1 (raw file):

#include "fcl/narrowphase/detail/convexity_based_algorithm/polytope.h"
#include "expect_throws_message.h"

BTW, would #include "test/expect_throws_message.h" work too? Or the build system knew its location already.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp, line 144 at r1 (raw file):

}

// Produces an equilateral tetrahedron, but moves the top vertex to be one

Nit, to be one => to be on

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp, line 288 at r1 (raw file):

}

// Tests the error condition for this operation -- i.e., a degenerate triangle.

BTW, degenerate triangle => degenerate triangle in a tetrahedron ?

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp, line 294 at r1 (raw file):

  // Degenerate triangle (in this case, co-linear vertices) in polytope.
  // By construction, face 1 is the triangle that has been made degenerate.
  FCL_EXPECT_THROWS_MESSAGE_IF_DEBUG(

BTW, the suffix "_IF_DEBUG" made me think the statement is no-op in non-debug mode. Is that true?

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp, line 774 at r1 (raw file):

// Tests that the sanity check causes `ComputeVisiblePatch()` to throw in
// debug builds.

BTW, I am confused about how this function works. When I saw the documentation above said "to throw in debug builds", I thought we will use FCL_EXPECT_THROWS_MESSAGE_IF_DEBUG. However, I only saw EXPECT_TRUE and EXPECT_FALSE.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp, line 782 at r1 (raw file):

  std::unordered_set<ccd_pt_edge_t*> internal_edges;

  //   Top view labels of vertices, edges and faces.

BTW, the picture helps a lot. Thanks!

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp, line 793 at r1 (raw file):
Nit, change

v0    e0    v1  v0

to

v0      e0      v1

[SeanCurtis-TRI]

Good eye, @DamrongGuoy . I've made changes for your comments as well as test failures.

Reviewable status: 5 of 20 files reviewed, all discussions resolved (waiting on @DamrongGuoy)

---

include/fcl/narrowphase/detail/unexpected_configuration_exception.h, line 37 at r1 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, would FCL_UNEXPECTED_CONFIGURATION_EXCEPTION_H have been too long?
Another note, I'm just curious whether we should consider #pragma once.

Done It was me changing the name of the exception/file without changing the name of the guard. And, in this case, fcl doesn't use #pragma once, so I'm following suit.

---

include/fcl/narrowphase/detail/unexpected_configuration_exception.h, line 76 at r1 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, the function signature has const char* func before const char* file. Perhaps we want the documentation to go in that order too?

Done

---

include/fcl/narrowphase/detail/unexpected_configuration_exception.h, line 103 at r1 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, I understand vector.transpose() make sense because it will print in one line. However, I'm not sure how matrix().transpose() help? It still prints in multiple lines? I may not understand Eigen enough.

Done Copy pasta.

---

include/fcl/narrowphase/detail/unexpected_configuration_exception.h, line 118 at r1 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, just to confirm my understanding. ThrowDetailedConfiguration throws std::logic_error, and ThrowUnexpectedConfigurationException throws UnexpectedConfigurationException. The TODO is about converting the latter to std::logic_error with perhaps additional info?

Not just "perhaps" additional info, it's whole purpose in life is to collect and format that info in a consistent manner. But otherwise, yes, your understanding is correct. I've modified the class documentation but do you have further suggestions for making it clearer?

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1202 at r1 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, should we add something like this to the documentation?

@throws UnexpectedConfigurationException if we fail to compute a visible patch.

Done

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1258 at r1 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, I see 1) at line 1283 and 2) at line 1300. However, I do not see 3) the polytope has zero-area triangles.

That is embedded in the calls to isOutsidePolytopeFace. I've also clarified the comment to align better with the code.

---

test/expect_throws_message.h, line 9 at r1 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, it would be nice if we have a short documentation about must_throw and fatal_failure. I thought I understood, but then I got confused at line 38 below.

Done Documentation added.

---

test/expect_throws_message.h, line 38 at r1 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, it made me pause and think about {debug, non-debug}x{EXPECT, ASSERT}. Conceptually, I think it was like:

_EXPECT_THROW_MESSAGE_IF_DEBUG =
    non-debug ? _EXPECT_THROWS_MESSAGE_HELPER(...,optional, non-fatal) : _EXPECT_THROWS_MESSAGE(...,)
_ASSERT_THROW_MESSAGE_IF_DEBUG =
    non-debug ? _EXPECT_THROWS_MESSAGE_HELPER(...,optional, fatal) : _ASSERT_THROWS_MESSAGE(...)

OK I agree; the double negative macro is always painful. Fortunately, I copied and pasted from drake (documentation to that end has been added).

---

test/expect_throws_message.h, line 42 at r1 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, when I first saw false /*optional*/, I thought optional = false, so it's mandatory. Then, I realized it's "must_throw = false", so it's really optional.

OK Hopefully, bringing over the docs help.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp, line 50 at r1 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, would #include "test/expect_throws_message.h" work too? Or the build system knew its location already.

OK It doesn't. The CMake system is not configured that way. Go figure.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp, line 288 at r1 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, degenerate triangle => degenerate triangle in a tetrahedron ?

Done

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp, line 294 at r1 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, the suffix "_IF_DEBUG" made me think the statement is no-op in non-debug mode. Is that true?

OK Hopefully the augmented documentation will help.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp, line 774 at r1 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, I am confused about how this function works. When I saw the documentation above said "to throw in debug builds", I thought we will use FCL_EXPECT_THROWS_MESSAGE_IF_DEBUG. However, I only saw EXPECT_TRUE and EXPECT_FALSE.

OK see newly added documentation.

[SeanCurtis-TRI]

@sherm1 feel free to assign yourself if you also want to take a pass on this.

Reviewable status: 5 of 20 files reviewed, all discussions resolved (waiting on @DamrongGuoy)

[sherm1]

+@sherm1
OK, I did a vaguely platform-review-ish pass (didn't look at everything it detail). Some comments, PTAL.

Looking at the usages, I'm wondering if the exception might be misnamed? It doesn't really seem that the configuration is unexpected (any configuration is allowed) but that something unexpected happened at that configuration. Maybe something more like FailedAtThisConfiguration ?

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

---

include/fcl/geometry/shape/box.h, line 85 at r2 (raw file):

  friend
  std::ostream& operator<<(std::ostream& out, const Box& box) {

I have a few thoughts about this (META):

• it doesn't seem to need to be a friend since it only refers to a public member
• this is not declared inline, so won't it be multiply-defined if this header is included in multiple compilation units that are linked together?

On the latter point I'm not certain but I doubt that friend provides the same ODR loophole that inline does.

---

include/fcl/narrowphase/detail/unexpected_configuration_exception.h, line 70 at r2 (raw file):

      : std::exception(), message_(message) {}

  const char* what() const noexcept override { return message_.c_str(); }

BTW consider final here for emphasis.

---

include/fcl/narrowphase/detail/unexpected_configuration_exception.h, line 109 at r2 (raw file):

     << "\n  Original error message: " << e.what()
     << "\n  Shape 1: " << s1
     << "\n  X_FS1\n" << X_FS1.matrix()

BTW the default precision for output is only about 6 digits I think. That might be inadequate for reproducing failures, which likely depend on very narrow circumstances. Since this is a debugging utility, consider always outputting full precision (20 digits guarantees every bit can be reproduced).

---

include/fcl/narrowphase/detail/unexpected_configuration_exception.h, line 121 at r2 (raw file):

#define FCL_THROW_UNEXPECTED_CONFIGURATION_EXCEPTION(message, func)   \
  ::fcl::detail::ThrowUnexpectedConfigurationException(message, func, \
                                                       __FILE__, __LINE__)

BTW why not capture __func__ automatically here also? (That is the C++ standard-approved way to grab function names.)

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp, line 714 at r2 (raw file):

  //    / ╱e3    e4╲ \       / ╱   f1   ╲ \                             .
  //   /╱____________╲\     /╱____________╲\                            .
  //  v0      e0       v1                                               .

BTW wow!

[DamrongGuoy]

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

---

include/fcl/narrowphase/detail/unexpected_configuration_exception.h, line 118 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Not just "perhaps" additional info, it's whole purpose in life is to collect and format that info in a consistent manner. But otherwise, yes, your understanding is correct. I've modified the class documentation but do you have further suggestions for making it clearer?

LGTM. It's clearer now. The class summary at the beginning of UnexpectedConfigurationException is very helpful.

---

test/expect_throws_message.h, line 9 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Done Documentation added.

OK. Thanks! It's very useful.

---

test/expect_throws_message.h, line 42 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

OK Hopefully, bringing over the docs help.

OK. The docs help indeed!

---

test/expect_throws_message.h, line 77 at r2 (raw file):

a throw in Release builds. However, if the Release build does throw it must
throw the right message. More precisely, the thrown message is required
whenever `FCL_ENABLE_ASSERTS` is defined, which Debug builds do be default.

BTW, s/be default/by default

[DamrongGuoy]

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

[SeanCurtis-TRI]

I like it. That's why I have you look at things. I was never particularly inspired by the name originally given, so I just threw something at the wall. I like yours more.

Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @sherm1)

---

include/fcl/geometry/shape/box.h, line 85 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

I have a few thoughts about this (META):

• it doesn't seem to need to be a friend since it only refers to a public member
• this is not declared inline, so won't it be multiply-defined if this header is included in multiple compilation units that are linked together?

On the latter point I'm not certain but I doubt that friend provides the same ODR loophole that inline does.

1. One of the "shapes" (Convex) required a friend streaming operator so I copied and pasted for convenience. Particularly if, in the future, we push these classes to match the drake styleguide.
2. cppreference feels that this is inlined (see the second paragraph).

That said, if you feel friend is misleading and opaque, I can change it to inline for the non-Convex types.

---

include/fcl/narrowphase/detail/unexpected_configuration_exception.h, line 70 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider final here for emphasis.

Done (does it seem odd that a class declared final allows anything but final overrides?)

---

include/fcl/narrowphase/detail/unexpected_configuration_exception.h, line 109 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW the default precision for output is only about 6 digits I think. That might be inadequate for reproducing failures, which likely depend on very narrow circumstances. Since this is a debugging utility, consider always outputting full precision (20 digits guarantees every bit can be reproduced).

Done Humorously, I'd included <iomanip> with that in mind but failed to finish that out. :-/

---

include/fcl/narrowphase/detail/unexpected_configuration_exception.h, line 121 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW why not capture __func__ automatically here also? (That is the C++ standard-approved way to grab function names.)

Done It was inspired by the vague idea of being able to report a calling function different from the the actual call site; but a) I don't do it in practice and b) that would conflict with line and file anyways.

---

test/expect_throws_message.h, line 77 at r2 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, s/be default/by default

Done

[sherm1]

, thanks Sean.

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

---

include/fcl/geometry/shape/box.h, line 85 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

1. One of the "shapes" (Convex) required a friend streaming operator so I copied and pasted for convenience. Particularly if, in the future, we push these classes to match the drake styleguide.
2. cppreference feels that this is inlined (see the second paragraph).

That said, if you feel friend is misleading and opaque, I can change it to inline for the non-Convex types.

Per f2f, TIL that any method defined within a class gets an ODR loophole! That makes me feel much better about this and I withdraw my objection.

---

include/fcl/narrowphase/detail/unexpected_configuration_exception.h, line 70 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Done (does it seem odd that a class declared final allows anything but final overrides?)

Yes.

[DamrongGuoy]

Reviewable status: complete! all files reviewed, all discussions resolved

[SeanCurtis-TRI]

Prior to this most recent revision, the PR had passed all tests. The newest revision fixes some inconsistent doxygen formatting and should have no bearing on the correctness.

Reviewable status: 19 of 20 files reviewed, all discussions resolved (waiting on @sherm1)

[sherm1]

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