Rosanswers logo

Hi everyone,

I am doing research about safety issue about human and robot cooperation. I am really interested in using collision detection functionality provided by moveit. As far as I know, I can get the distance between world object and any part of pr2 link through moveit API of collision detection. I am wondering if it is possible to get the coordinates information about the point on pr2 link which is closest to the world object? And is it possible to get the joint vector (standard vector on z-axis of a joint) information?

Thanks for any help in advance!


For FCL distance check called in moveit, can I simply create a new fcl::DistanceRequest object with the public attribute "enable_nearest_points" set to true and pass the fcl::DistanceRequest object to "double d = fcl::distance(o1, o2, fcl::DistanceRequest(), dist_result);" ? If I do so, can I get nearest points between o1 and o2 from dist_result?


I modified the code in FCL distance check in following way:

fcl::DistanceResult dist_result;
dist_result.update(cdata->res_->distance, NULL, NULL, fcl::DistanceResult::NONE, fcl::DistanceResult::NONE); // can be faster
double d = fcl::distance(o1, o2, fcl::DistanceRequest(true), dist_result);
printf("\n print nearest point 1 !!!!!!!!!!!!!!!!!!!!!!!!!!!!!! \n  %f %f %f  \n",dist_result.nearest_points[0].data.vs[0], dist_result.nearest_points[0].data.vs[1], dist_result.nearest_points[0].data.vs[2]);
printf("\n print nearest point 2 !!!!!!!!!!!!!!!!!!!!!!!!!!!!!! \n  %f %f %f  \n",dist_result.nearest_points[1].data.vs[0], dist_result.nearest_points[1].data.vs[1], dist_result.nearest_points[1].data.vs[2]);

But I don't know why I got (0,0,0) for each nearest point.


Originally posted by AdrianPeng on ROS Answers with karma: 441 on 2013-08-02

Post score: 4

Original comments

Comment by Adolfo Rodriguez T on 2013-09-03:
I +1 a feature request for allowing to query the closest distance vector, or equivalently the two witness points associated to the closest distance.


2 Answers 2


Rosanswers logo

MoveIt uses FCL for Collision Detection. This library definitely supports returning the closest-point coordinates you're interested in. See fcl::distance() and fcl::DistanceResult. I've used this library independently from MoveIt to retrieve closest-point pairs by scanning through different combinations of STL mesh data. It works great, and is really fast!

However, looking through the MoveIt code, I don't think you can get distance pairs "out of the box" with the current design. You can get collision contact points and a scalar value for the minimum distance between non-contacting parts, but not the closest-distance points you're looking for.

See the MoveIt wiki for info on how to Retrieve Contact Info. That example checks for self-collisions, but a similar call is used for robot/world collision checks. The result of this call is a CollisionResult, which contains a scalar distance field and a list of Contact data that specifies collision points. If we dig into the MoveIt collision detection code, we see that the FCL distance check is called here using a default fcl::DistanceRequest object. This default request does not include the nearest-point calculations (only the scalar distance). And MoveIt only saves the closest distance value, as seen here.

So, it looks to me like you're going to need to call the FCL library directly to perform the distance checks you're looking for. That being said, the MoveIt developers are fairly responsive, and may be able to add the functionality you're looking for. Your best bet will be to send an email to the MoveIt Developer's List, as they follow that list closer than this website.

Originally posted by Jeremy Zoss with karma: 4976 on 2013-08-06

This answer was ACCEPTED on the original site

Post score: 6

Original comments

Comment by AdrianPeng on 2013-09-03:
Hi Jeremy, I really appreciate your patient reply! I have another question about FCL distance check called in Moveit (update in question). I am wondering if I can simply modify the code in moveit to get the nearest point information.

Comment by Jeremy Zoss on 2013-09-03:
Sure. There's no reason why you can't. But if you do fork the MoveIt code like that, you'll need to manually keep your local version updated with any new updates released by the MoveIt team. And your code won't be portable to other users (unless they also make the same MoveIt updates). Once you develop a patch, you might consider submitting it to the MoveIt developers, so they can integrate it into the official distribution.

Comment by AdrianPeng on 2013-09-04:
Hi Jeremy, I tried to Modify the code in moveit to get the nearest points, but I meet the question in update 2. Do you mind having a look on that if you have time? Thanks a lot!

Comment by dsolomon on 2013-10-18:
Adrian, I will be examining this problem deeper in the next few weeks, but I have a brief suggestion to check: try changing printf to "std::cout << "Nearest point 1: " << dist_result.nearest_points[0] << std::endl;" as fcl::Vec3f type has a built-in ostream operator that may show values correctly

Comment by dsolomon on 2013-10-18:
Also, you may want to additionally check that your objects are not currently in collision, as the distance check may default to 0,0,0 in that case


Rosanswers logo

Adrian, I got it to work by excluding the update step. I can see both distance and closest points on each object.

fcl::DistanceResult dist_result
double d = fcl::distance(o1, o2, fcl::DistanceRequest(true), dist_result);
std::cout << "d: " << d << std::endl <<
             "p1: " << dist_result.nearest_points[0] << std::endl <<
             "p2: " << dist_result.nearest_points[1] << std::endl;

This will return something similar to

d: 0.33571
p1: (1.21739 6.7476e-16 1.22103)
p2: (0.908283 6.76542e-16 1.352)

I am not 100% sure yet what frame the points are given in, but given the values it looks like the current world frame. Remember that each CollisionObject that is getting passed into this function contains both geometry and transform data, and my current assumption is that the transform of each object is in the current world frame that MoveIt is using.

Update Oct29-2013:
The points are given in the planning frame. This should be the base frame of the robot, but it can be changed by adding a virtual frame in the robot srdf that is used by MoveIt to create the planning scene.

Originally posted by dsolomon with karma: 253 on 2013-10-19

This answer was NOT ACCEPTED on the original site

Post score: 3

Original comments

Comment by AdrianPeng on 2013-10-19:
Hi dsolomon. Does it work before you exclude the update step? I found it only works with mesh object. Do you use mesh object or primitives?

Comment by dsolomon on 2013-10-19:
This is mesh to mesh. Are you calling distanceCallback() directly through MoveIt, and if so, how did you configure that? I just copied the three lines above into collisionCallback so I could quickly test it with the existing ROS-Industrial irb_2400_moveit_config package

Comment by AdrianPeng on 2013-10-19:
I simply stored coordinates of nearest points which has least distance in ROS parameter server in distanceCallback(). Then I just grab the nearest points coordinates from ROS parameter server where I want to use them. .

Comment by Karsten on 2013-10-28:
I can confirm that nearest point computation is working perfectly on mesh-to-mesh. However, as soon as there is one Primitive Shape involved, it returns a Zero-Vector. This is done through exclusively FCL and no MoveIt! Any idea how to tackle this option also for Primitives?

Comment by dsolomon on 2013-10-29:
I haven't tried with primitives, so I can't fully answer. There are helper functions in MoveIt geometric_shapes library https://github.com/ros-planning/geometric_shapes/blob/hydro-devel/src/mesh_operations.cpp#L365 that could convert a primitive to a mesh if you are truly stuck.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.