Error signed distance between box and cylinder

[lucarossini-iit]

Dear all,

I am having some issues while calculating the signed distance between different shape primitives. In particular, good results are obtained when computing it between boxes and spheres but problems arises when a cylinder collides with any kind of other shape primitive (distance = 0). Looking into some old issues I saw that the signed distance should work fine between simple shape primitives but this is not the case. Did you experience this problem somewhere else? I am currently using fcl on the tag v0.6.1.

Thanks for the amazing work you carry on! Best, Luca

[SeanCurtis-TRI]

Parr of this may depend on the solver you're using. Which one?

In fact, it might be simplest if you could give us a code snippet creating a couple of collision objects (and their relative configuration) such that the distance result is unexpected.

[lucarossini-iit]

Thanks for the fast reply! At each iteration, we generate a moveit_msgs::PlanningSceneWorld msg containing all the CollisionObjects we are going to check. Object poses can be online modified using InteractiveMarkers and we can check their relative position directly on rviz. From this, we create a set of shared pointer of a fcl shapes using:

fcl_shape = std::make_shared<fcl::Boxd>(shape.dimensions[0], shape.dimensions[1], shape.dimensions[2]);
fcl_shape = std::make_shared<fcl::Sphered>(shape.dimensions[0]);
fcl_shape = std::make_shared<fcl::Cylinderd>(shape.dimensions[0], shape.dimensions[1]);

where shape is a shape_msgs::SolidPrimitive that defines the type of the fcl_shape. at the same way, we add the robot links to the scene converting each of them from the urdf in a fcl_shape whose type is defined by the <collision> tag (shperes, boxes or capsules).

Then, we check distances between each possible combination of fcl::CollisionObjectd> created by each fcl_shape:

fcl::DistanceRequestd request;
request.gjk_solver_type = fcl::GST_INDEP; // fcl::GST_LIBCCD;
request.enable_nearest_points = true;
request.enable_signed_distance = true;

fcl::DistanceResultd result;
// perform distance test
fcl::distance(coll_A, coll_B, request, result);

Now, if I create two obstacles of type Box or Sphere, the result is the expected and if I move the obstacle using the interactive markers, I see the distance getting more and more negative the more I interpenetrate the two obstacles. If one of the two CollisionObject is a cylinder, the returned distance is always 0 when the two objects interpenetrate.

Do you see anything wrong? I am available to add any further information. Thanks again