Questions about Moveit collision detection

asked 2013-08-02 08:37:41 -0500

AdrianPeng
441 ●50 ●57 ●66

updated 2013-11-14 10:40:43 -0500

tfoote

58457 ●128 ●550 ●526 http://www.ros.org/

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!

update1:

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?

update2:

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.

Quan

edit retag flag offensive close merge delete

Comments

I +1 a feature request for allowing to query the closest distance vector, or equivalently the two witness points associated to the closest distance.

Adolfo Rodriguez T ( 2013-09-03 21:27:46 -0500 )edit

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2013-08-06 18:15:08 -0500

Jeremy Zoss

4976 ●48 ●48 ●73 http://rosindustrial.org/

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.

edit flag offensive delete link

Comments

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.

AdrianPeng ( 2013-09-03 10:50:40 -0500 )edit

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.

Jeremy Zoss ( 2013-09-03 10:55:07 -0500 )edit

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!

AdrianPeng ( 2013-09-04 09:04:11 -0500 )edit

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

dsolomon ( 2013-10-18 06:11:30 -0500 )edit

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

dsolomon ( 2013-10-18 06:12:22 -0500 )edit

add a comment

answered 2013-10-19 06:13:53 -0500

dsolomon
253 ●7 ●11 ●21

updated 2013-10-29 10:56:07 -0500

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.

edit flag offensive delete link

Comments

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?

AdrianPeng ( 2013-10-19 08:42:51 -0500 )edit

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

dsolomon ( 2013-10-19 10:33:01 -0500 )edit

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. .

AdrianPeng ( 2013-10-19 10:39:01 -0500 )edit

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?

Karsten ( 2013-10-28 04:51:45 -0500 )edit

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.

dsolomon ( 2013-10-29 11:01:41 -0500 )edit

add a comment

Questions about Moveit collision detection

Comments

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Questions about Moveit collision detection edit

Comments

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Questions about Moveit collision detection