Reference frame of nearest point

[Macxou]

Hi,
I'm currently using fcl 0.5 and i would like to compute the distance between 2 objects.
Both of them are placed in the world frame, so I can display them correctly (with VTK)

The blue and red sphere are corresponding to the closest points given by the distance method.
I want to compute the distance between the tool and the join 4 which is the part on the left

But when I try to compute the distance between them, I get a wrong value and the closest points are not even the closest one.
I guess that this is the closest one but they are given in the wrong frame.
I 've seen the post :
#171 and #288

I would like to know when FCL 0.6 will be released or how i can fix manually this issue.

Regards

[SeanCurtis-TRI]

The cited issues address the frame if you use the fcl::distance() query. The answers should be in the world frame (as suggested by the issue and PR).

The most obvious point for confusion here would come from the poses of the two geometries. Have they been reported in the world frame? If the poses (called via CollisionObject::setTransform()) are not both in the same frame, the response you get will be gibberish.

Do you have code that illustrates erroneous query results that we can run?

[Macxou]

I set both objects in the world frame. I am sure of that because I use the same 4x4 matrix for VTK and for FCL. So I can display what i give to FCL.
As I have multiples objects, I use 2 std::vector
std::vector<std::shared_ptr<fcl::CollisionGeometry>> tool_ptr = std::vector<std::shared_ptr<fcl::CollisionGeometry>>(); std::vector<fcl::Transform3f > tranformVect = std::vector<fcl::Transform3f >();
I don't use a manager because when I do an auto-collision test, i don't want to test the collision for 2 consecutive parts.
Then I call the method to do the auto-collision
`
bool CollisionFcl::Manager::isAutoColliding(int index1, int index2)
{
fcl::CollisionObject toolCollision1(tool_ptr.at(index1), tranformVect.at(index1));
fcl::CollisionObject toolCollision2(tool_ptr.at(index2), tranformVect.at(index2));
fcl::DistanceData cdata;
cdata.request.gjk_solver_type = fcl::GST_INDEP;
cdata.request.enable_nearest_points = true;

bool collide = defaultDistanceFunctionMine(&toolCollision1, &toolCollision2, &cdata, minDist);
    return collide;

}`

where
bool defaultDistanceFunctionMine(CollisionObject* o1, CollisionObject* o2, void* cdata_, FCL_REAL& distmin) { DistanceData* cdata = static_cast<DistanceData*>(cdata_); const DistanceRequest& request = cdata->request; DistanceResult& result = cdata->result; FCL_REAL dist; FCL_REAL res; if (cdata->done) { dist = result.min_distance; return true; } res=distance(o1, o2, request, result); if (res == -1) return true; dist = result.min_distance; if (dist <= distmin) return true; // in collision or in touch return cdata->done; }

So, to set the position of each part, i set them in the std::vector to use it later.
The poses are set using the geometrical model of the robot. I have the pose of the last part, and I multiply matrix to get the position of the previous one, and I do that for each of them.
As you can see, in the previous picture, there are the tool, the join 6, the join 5 and the join 4.

[SeanCurtis-TRI]

I note that you're using fcl::GST_INDEP. Try using fcl::GST_LIBCCD. We've been working on improving the answers from the libccd solver but haven't tackled the indep solver. My past experience suggests that they can be quite different.

I can't promise that it'll solve your problem, but, at the very least, it'll put it into the domain with which I'm more familiar.

[Macxou]

I have changed from fcl::GST_INDEP to fcl::GST_LIBCCD and I have the same results...
And I have a point that is ok because the object is placed directly on the world frame. The matrix is Identity.
for the red point, the pose it set by using the geometrical model of the robot

[SeanCurtis-TRI]

Several more questions:

1. The geometry used for querying: are they the meshes you show above? And those meshes are non-convex?
2. It's not immediately obvious in the original image which meshes are unique geometries. So, I'm not sure which geometries the red and blue dots are supposed to correspond to. Can you clarify that?
3. Your defaultDistanceFunctionMine definition is a bit confusing (due to formatting errors). I've copied-and-pasted it here below:

bool defaultDistanceFunctionMine(CollisionObject* o1, CollisionObject* o2, void* cdata_,
                                 FCL_REAL& distmin) {
  DistanceData* cdata = static_cast<DistanceData*>(cdata_);
  const DistanceRequest& request = cdata->request;
  DistanceResult& result = cdata->result;
  FCL_REAL dist;
  FCL_REAL res;
  if (cdata->done) {  // This will never become true.
    dist = result.min_distance;  // this assignment has no affect.
    return true;  // if cdata->done were ever true, you'd consider that a collision.
  }
  res = distance(o1, o2, request, result);
  if (res == -1) return true;
  dist = result.min_distance;
  if (dist <= distmin) return true; // in collision or in touch return cdata->done;
}

I now have the following thoughts on this function.

1. You test against cdata->done but you never write to cdata->done. It will always be false.
   1. This is good, because the body of that conditional doesn't do what you want (see comments in code).
2. It seems like you're defining collision with some padding (distmin). So, if two objects aren't separated by at least mindist, they are considered to be in collision, correct?

[Macxou]

1-The objects I use are meshes sometimes non convex meshes.
2-About the dot, the blue one correspond to the tool on the right and the red one must but on the join 4 on the left.

3- I'm sorry about the formatting, on my preview it was ok...
3-1- Well this is false if I use the function only once. But if I chain, it might be true. I see it as bool = bool 1 || bool2
3-2- Yes. I am considering some kind of safety space between these objects to avoid the colliding

[SeanCurtis-TRI]

1. You may be getting into trouble with non-convex meshes.
2. You've only indicated two "tools". Are there more than two tools in that image? More particularly, are those two tools in contact. If they are, distance should report it accordingly.
3. In the code snippet you have shown, cdata->done is created inside the for loop, before the call to your distance function and destroyed on the way out. How does any other code set the value? Also, what about the issues of what you do when it is set to true?

I strongly suspect that your assumptions of what you're passing in may not be as correct as you hope. I strongly suggest you exercise your code with simple primitives (spheres, for example). They are easy to reason about and it is trivial to determine what the correct answer is. This will allow you to validate the code that is exercising FCL.

Unfortunately, I can't offer much more hlep because I have no idea how to reproduce your problem locally. We can continue playing 20 questions in order to try and help find where the bug is but it'll be a very long process -- it's up to you if you want to do that. I definitely want to get this to work for you -- particularly if it reveals a bug in FCL, but my view into your problem is too limited to help in any significant degree.