Wrong closest points between shapes (box, sphere, cylinder) in distance computations

[cmas1]

I have been playing around with FCL to compute the distance between geometric shapes (e.g. boxes, cylinders and spheres) to test some simple collision avoidance methods.

For this purpose I am using the method.
FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const DistanceRequest& request, DistanceResult& result)

The method returns not only the distance between the two collision objects but also the nearest points, one per object. While the distance is correct the nearest points appear to be wrong.
More specifically, it seems that the nearest point is correct only for sphere objects, but not for the other shape geometries.

Here is a quick example to reproduce this error. In this example I am only checking the distance between a sphere of radius 1 placed in (0.0, 0.0, 0.0) and a box of dimensions (10, 2, 10) and placed in (0.0, 5.0, 3.0).

#include <iostream>
#include <fcl/collision_object.h>
#include <fcl/shape/geometric_shapes.h>
#include <fcl/collision_data.h>
#include <fcl/distance.h>

using namespace std;
using namespace fcl;

int main(int argc, char** argv){

    DistanceResult result;
    FCL_REAL distance;


    Matrix3f rotSphere(1.0, 0.0, 0.0,
                    0.0, 1.0, 0.0,
                    0.0, 0.0, 1.0);

    Vec3f trSphere(0.0, 0.0, 0.0);

    Matrix3f rotBox(1.0, 0.0, 0.0,
                    0.0, 1.0, 0.0,
                    0.0, 0.0, 1.0);

    Vec3f trBox(0.0, 5.0, 3.0);


    Sphere sphere(1);
    Box box(10, 2, 10);

    boost::shared_ptr< CollisionGeometry > cgeomSphere_(&sphere);
    boost::shared_ptr< CollisionGeometry > cgeomBox_(&box);

    CollisionObject *objSphere = new CollisionObject(cgeomSphere_, rotSphere, trSphere);
    CollisionObject *objBox = new CollisionObject(cgeomBox_, rotBox, trBox);

    result.clear();
    DistanceRequest request(true);
    distance = fcl::distance(objSphere, objBox, request, result) ;


    cout << "distance = " << distance << endl;
    cout << " point on sphere: x = " << result.nearest_points[0].data[0] << " y = " << result.nearest_points[0].data[1] << " z = " << result.nearest_points[0].data[2] << endl;
    cout << " point on box: x = " << result.nearest_points[1].data[0] << " y = " << result.nearest_points[1].data[1] << " z = " << result.nearest_points[1].data[2] << endl;

    return 0;
}

The output on screen is
distance = 3
point on sphere: x = 1.58946e-07 y = 1 z = 0
point on box: x = -5 y = -1 z = 0

The distance and the point on the sphere are correct, but the point on the box in local coordinates should be x =0, y=-1, z = -2.

As I already mentioned, this same behaviour happens also with the other geometric primitives.

So, is this behaviour a bug in the library or am I doing something wrong? If it is a bug, is there any way to bypass the problem and retrieve the nearest points?

[scpeters]

Should the z coordinate of the point on the box in local coordinates be -3?

[cmas1]

Yes, you are correct, that was a typo. In any case, the point returned from the function makes no sense.

I noticed another thing. For a generic box fcl::Box box1(dimx, dimy, dimz) the point returned on the box is always a triplet [x, y, z] where:

• x can be -0.5 * dimx, 0, 0.5 * dimx
• y can be -0.5 * dimy, 0, 0.5 * dimy
• z can be -0.5 * dimz, 0, 0.5 * dimz

[avalenzu]

This should be resolved by the changes in #215.

[KevinKDufour]

Hi, I am doing some distance calculations using ROS and MoveIt with the package ros-indigo-fcl however the nearest points are still wrong. Will the ros-indigo package be updated to the changes ?

[balakumar-s]

Has this been fixed? When using box primitive, nearest point is the closest vertex of the box which is not accurate.

[sherm1]

/cc @SeanCurtis-TRI Do you know if this is fixed?

[SeanCurtis-TRI]

Sadly, I haven't spelunked into the distance query yet, so I have no certain knowledge one way or the other.