bodies::Body::containsPoint() has non-consistent behavior for "surface" points

[peci1]

Sphere excludes surface points.

Cylinder includes top and bottom surfaces, but excludes the sides.

Box includes all surface points.

Mesh includes all surface points.

I think being consistent would be great. What's bad is that there is no documentation that would help determining what's the desired result.

I created a test suite in https://github.com/ros-planning/geometric_shapes/pull/90 . I defined a macro

#define EXPECT_SURF EXPECT_TRUE
//#define EXPECT_SURF EXPECT_FALSE

which you can change to see what happens if you define that surface points should be included or excluded. So far, the test suite fails in both cases.

With #define EXPECT_SURF EXPECT_FALSE:

With #define EXPECT_SURF EXPECT_TRUE:

What to do next? Apparently, there are 3 options:

1. Define that surface points should be considered inside (my preferred option).
2. Define that surface points should be considered outside.
3. Do nothing and claim the current status as desired (wouldn't break existing code; but Wiki says C++ API is UNREVIEWED and UNSTABLE, so it shouldn't matter).

What are your opinions? When there's a consensus about this choice, I can implement the fix.

[rhaschke]

I think the main issue here, is that spheres and cylinders are eventually approximated by triangle meshes. Hence, there is only a minimal chance to exactly hit a vertex point on the surface of a cylinder / sphere surface - which then will be considered inside. I'm afraid this won't fix.

[peci1]

In what use-case are they approximated? In visuals? But that's no problem... we're talking about collision shapes here, and these guys are pretty often composed of these basic shapes to speed up computations... Or am I missing some point?

[rhaschke]

geometric_shapes is used by MoveIt for collision shapes, yes. But as MoveIt uses FCL for collision checking, everything is approximated using triangle meshes.

[peci1]

Hmm, that's interesting, I thought FCL is more "clever"... But anyways, the code of collision_shapes is used at some places anyways, regardless of whether it continues to FCL or not. And in these places, I still think consistency would be good, wouldn't it?

[peci1]

At least in narrowphase it looks like at least some collision checks are done precisely.

[rhaschke]

@guihomework I remember you mentioned FCL being slower with geometric collision checks. Do you remember the reference?

[guihomework]

@rhaschke , the paper tested it, and converting geometric shapes to meshes led to similar or faster results, unfortunately approximating the cylinders (problem we faced at some point but not anymore apparently, maybe they changed the meshing) I put here the table they have in the paper http://gamma.cs.unc.edu/FCL/fcl_docs/webpage/pdfs/fcl_icra2012.pdf

object	ODE	FCL
Boxes	0.780889	0.700815
Spheres	0.487176	0.570329
Cylinders	0.236988	0.264515
Triangle Meshes	2.340178	0.240228

[peci1]

Thanks for the interesting paper reference.

But back to the original question - I still don't understand why should Body::containsPoint() be related to how FCL processes collisions.

I searched for usage in the ros-planning org, and these are the places I found: https://github.com/ros-planning/moveit/blob/melodic-devel/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp#L169 https://github.com/ros-planning/moveit_advanced/blob/indigo-devel/collision_detection_distance_field/src/static_distance_field.cpp#L62 https://github.com/ros-planning/moveit/blob/melodic-devel/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp#L444 https://github.com/ros-planning/moveit_advanced/blob/indigo-devel/robot_sphere_representation/src/sphere_calc.cpp#L1822 https://github.com/ros-planning/moveit_core/blob/kinetic-devel/distance_field/src/find_internal_points.cpp#L56

Well, I haven't noticed anywhere in the code that there would be a situation when you first call containsPoint() and subsequently call FCL (at least not directly). Also, most of the methods using containsPoint() are called getInsidePoints or similar, so it seems that "users" of containsPoint() automatically assumed that it returns interior points.

If you'd like to keep 1:1 compatibility with FCL results, then this method would have to be rewritten using FCL, because I don't think it'd be wise to duplicate their code. Anyways, I say I'd be quite surprised to call a Spehere::containsPoint() method and get the results valid for its bounding icosahedron...

[rhaschke]

@guihomework Looks like your information stems from the original paper only (comparing FCL to ODE). Since then, FCL has added geometry-based collisions as well. We need to find more recent results on this!

@peci1 I can understand your point of view. Actually, I would prefer to switch to geometry-based collisions for FCL as well. However, this is probably a lot of work... Any volunteers?

[peci1]

@peci1 I can understand your point of view. Actually, I would prefer to switch to geometry-based collisions for FCL as well. However, this is probably a lot of work... Any volunteers?

Uhm, not me, sorry for that... This would be way more work than I can do for Moveit now...

[simonschmeisser]

@rhaschke I cannot follow completely. Do you know where exactly this conversion from primitive (eg sphere) to mesh happens on it's way to fcl? Looking here https://github.com/ros-planning/moveit/blob/abc4f4070e300352c851f84fb53fa6d06c9eacbb/moveit_core/collision_detection_fcl/src/collision_common.cpp#L784 I can see that a fcl::Sphered is created

[rhaschke]

@simonschmeisser Thanks for looking deeper. Indeed, MoveIt seems to have used basic shapes for ages and only FCL itself improved representation of basic shapes such that the issue https://github.com/flexible-collision-library/fcl/issues/122 I remembered is solved now. Hence, may I ask @peci1 to fix Cylinder::containsPoint() to consider all surface points inside. Please feel free to directly add to #90.

[simonschmeisser]

@rhaschke yes, they added several special cases (primitive-primitive collisions instead of generic mesh ones) recently and also seem to have added quite some tests for them

[peci1]

Nice findings. Okay, I'll do the cylinder - but then I think sphere should also be fixed to include the surface points, shouldn't it?

[rhaschke]

I think sphere should also be fixed to include the surface points, shouldn't it?

Yes, of course. I didn't read the initial comment carefully enough. I had the impression, spheres were handled correctly already.

[peci1]

I updated #91 with the fix for this issue.

[BryceStevenWilley]

Given that #97 (which succeeds #90) has been merged, I'm closing this issue.