Collision checking for attached object during planning

[Simon Jansen]

Hello,

I'm building a simple pick-and-place simulation in moveit by adding a collision object to the world, moving to robot arm to its location, attaching the object and then moving the arm to a drop-off location.

All is working well, except that the OMPL planner does not seem to check for collisions between the attached object and the world. Am I right in assuming that OMPL only checks for self-collision and collisions between the robot and the world and not for collisions between (attached) object and the world (octomap and other objects)?

If this is so, is there a way to make the OMPL planner aware of the attached object or some other way to check for collisions between the attached object and the world?

with regards,

Simon Jansen
Alten Mechatronics

[Ioan Sucan]

Hello Simon,

How do you attach the object? In theory, collision checking should work as expected -- collisions should be avoided between the attached object and the environment.

Ioan

[Simon Jansen]

Hello Ioan,

It is good news that collision checking for attached objects should work!

I attached it by publishing the object on the /attached_collision_object topic of the move_group node.

I checked the visualization in Rviz and the planning scene using the service /get_planning_scene and according to both, the object is attached.

regards,

Simon

[Ioan Sucan]

Hey Simon,

After attaching the object, did you immediately trigger a planning request? Could it be synchronization issues?

Ioan

[Simon Jansen]

Hello,

I checked for timing and this was not the issue (with enough time before the planning request, the collision for the attached object was still not checked).

I think I found the problem though: The attached object was present in the planning_scene, but not in the RobotState associated with my MoveGroup instance.
I assume this RobotState does not update its attached objects when I publish them externally using the /attached_collision_object topic?

I tried creating a new RobotState with the attached object and setting that as StartState for my MoveGroup instance to try and tell the planner to include the object, but this does not seem to work. I assume by setting a StartState, I'm only updating the joint values and not the attached objects?

How can I tell my MoveGroup instance that the RobotState has changed (with an attached object)? Or how can I tell my MoveGroup instance directly that an object is attached?

regards,
Simon

[Ioan Sucan]

Hello Simon,

Indeed the MoveGroup object does not monitor attached objects, so if you send any updates to the start state in the request, the attached object could be lost. But the behaviour you see sounds like a bug to me. If you could send me the code that exhibits the problem, that would make things much easier for me to debug.

Ioan

[Simon Jansen]

Hey,

I tried to simplify the code that reproduces the behavior into a single block:

/*The MoveGroup node was launched separately*/

ros::NodeHandle nodehandle;

//Create the MoveGroup interface
moveit::planning_interface::MoveGroup *group= new moveit::planning_interface::MoveGroup("arm");

//Publish a collision object
ros::Publisher object_pub = nodehandle.advertise<moveit_msgs::CollisionObject>("/collision_object",10);
ros::Duration(0.5).sleep();
moveit_msgs::CollisionObject object;
 geometry_msgs::Pose object_pose;
object_pose.position.x = 1;
object_pose.position.y = 1;
object_pose.position.z = 0.5;
object_pose.orientation.w = 1;
object_pose.orientation.x = 0;
object_pose.orientation.y = 0;
object_pose.orientation.z = 0;
shape_msgs::SolidPrimitive shape;
shape.type = shape.BOX;
shape.dimensions.push_back(0.1); //x dimension
shape.dimensions.push_back(0.1); //y dimension
shape.dimensions.push_back(0.1); //z dimension
object.header.frame_id = "/world";
object.header.stamp = ros::Time::now();
object.primitives.push_back(shape);             //shape is of type shape_msgs::SolidPrimitive
object.primitive_poses.push_back(object_pose);  //object_pose is of type geometry_msgs::Pose
object.operation = object.ADD;
object.id = "object1";
object_pub.publish(object);

//Sleep to make sure the object is received within the MoveGroup node
ros::Duration(0.5).sleep();

//Attach the object
ros::Publisher aco_pub = nodehandle.advertise<moveit_msgs::AttachedCollisionObject>("/attached_collision_object",10);
ros::Duration(0.5).sleep();
moveit_msgs::AttachedCollisionObject aco;
aco.object.id = object.id;
aco.link_name = "link_eef";
aco.touch_links.push_back(aco.link_name);
aco.object.operation = moveit_msgs::CollisionObject::ADD;
aco_pub.publish(aco);

//Sleep again
ros::Duration(0.5).sleep();

//Check if the object is now attached using the /get_planning_scene service
ros::ServiceClient client_get_scene = nodehandle.serviceClient<moveit_msgs::GetPlanningScene>("/get_planning_scene");
moveit_msgs::GetPlanningScene scene_srv;
scene_srv.request.components.components = scene_srv.request.components.ROBOT_STATE_ATTACHED_OBJECTS;
 if (!client_get_scene.call(scene_srv)){
    ROS_WARN("Failed to call service /get_planning_scene");
} else {
    ROS_INFO_STREAM("Number of attached bodies according to /get_planning_scene: " << scene_srv.response.scene.robot_state.attached_collision_objects.size());
}

//Check if the object is now attached by getting the current state of the robot via the MoveGroup interface
robot_state::RobotStatePtr RS = group->getCurrentState();
std::vector<const robot_state::AttachedBody*> bodies;
RS->getAttachedBodies(bodies);
ROS_INFO_STREAM("Number of attached bodies according to MoveGroup interface: " << bodies.size());

ros::waitForShutdown();
return 0;

Running this code gives me the following result:

[ INFO] [1379318006.970417558, 298.023000000]: Number of attached bodies according to /get_planning_scene: 1

[ INFO] [1379318006.970601829, 298.024000000]: Number of attached bodies according to MoveGroup interface: 0

I left out the actual planning, because I assume the planner will not consider the attached object unless it is present in the RobotState associated with the MoveGroup interface.

Hope this helps.

Simon

[Simon Jansen]

Hello,

Any news about this? Is this indeed a bug or did I make some mistake?

We would really like to include the attached object in collision checking for our application.

regards,

Simon

[Ioan Sucan]

Hello Simon,

Your code helped me find a few issues with how attached bodies were handled, thank you! I believe the code you e-mailed works as I expect it. However, there is a caveat: the current state as monitored by the MoveGroupInterface does not monitor attached objects; this means that the get_planning_scene service should indeed return 1, and the current state you mention is going to always return 0. That said,
if you attach an object, it will be considered in planning (previously it got cleared immediately) even though it is not in the MoveGroupInterface. So I think your usecase will work. Can you try things again?

Ioan

[Simon Jansen]

When you say 'previously it got cleared immediately', do you mean I need the latest version of MoveIt?

Currently, I just have the standard ROS groovy debian package installed.

[Ioan Sucan]

By previously, I meant the recent bugs in ROS Hydro (that were fixed). The same behaviour should work in Groovy, but if it is possible for you, I recommend switching to Hydro as soon as moveit_ros_* 0.5.6+ debs come out.

Ioan

[Simon Jansen]

Hello Ioan,

I finally got time to test this problem again, but it's still not working for me.

I tried generating a plan where the attached object would collide with the robot itself, and there was no error. After the robot reached its target (where the attached object really did collide with the robot model), I detached the object and tried planning, but this time the planner failed (correctly).

It appears attached objects are completely ignored by the planner. The only thing I can think of, is that I somehow call the planner incorrectly. What I do now basically:

group->attachObject(object);

ros::Duration(1.0).sleep();

group = new moveit::planning_interface::MoveGroup("arm");

group->setPoseTarget(target_pose);

group->setStartStateToCurrentState();

group->plan(planA);

I'm now using version 0.5.8 of moveit under Hydro.

Any help is much appreciated.

Regards,

Simon

[Tobias Fromm]

Hi Simon,

we had the same problem recently. It turned out that sleeping until the
object is actually attached is mandatory, but the necessary time seems
to vary. My colleague implemented a method to wait until the object was
actually present in the scene, not only one second like in your example.

Unfortunately, I don't have access to the code at the moment. However,
specifying a larger waiting time and optimally double-checking that the
object actually has been attached should help.

Cheers,
Tobi

--
Tobias Fromm
Robotics Group
Jacobs University

Research I, Room 38 +49-421-200-3106
Campus Ring 1 t.f...@jacobs-university.de
28759 Bremen, Germany http://robotics.jacobs-university.de

[Brian O'Neil]

I recently noticed this as well. In my application, I'm using a tool changer so I attach/detach the tools. Because collisions between the attached tool and the environment are rare, I didn't notice it at first. I notice this even when just using the rviz plugin.

To recreate:
From the rviz plugin:
-Import an object from the Scene Objects tab.
-Use interactive eef controls to move the robot wrist near the object.
-Click "Plan and Execute" in the planning tab
-Click the checkbox in the scene objects tab to attach the object to the wrist link.
-Publish the scene from the context tab. The attached object now appears on the orange goal robot.
-Use the interactive controls to move the attached object into collision with the environment. The object in the environment will turn red indicating collision is detected, but the attached object will NOT turn red.
-Click "Plan and Execute" in the planning tab. MoveIt will plan and execute the motion (I would rather it not)

Additional info:
moveit_core version 0.5.4
moveit_ros version 0.5.8

[Jorge Nicho]

Hello,
I've also experience difficulties doing collision avoidance successfully with a payload attached to the robot.  The attached object (a box) just goes right through the octomap as it moves to the drop off location.  I tried attaching the object in multiple ways such as publishing to the 'attached_collision_object'
 or 'planning_scene' topics.  I also made sure that the object already existed in the world before attaching it to the arm.

In my node, I define my attached object message as follows:

// building geometric primitive for attached object
shape_msgs::SolidPrimitive shape;
shape.type = shape_msgs::SolidPrimitive::BOX;
shape.dimensions.resize(3);
shape.dimensions[0] = BOX_SIZE.getX();
shape.dimensions[1] = BOX_SIZE.getY();
shape.dimensions[2] = BOX_SIZE.getZ();

// creating pose of object relative to tcp
geometry_msgs::Pose pose;
pose.position.x = 0; pose.position.y = 0;
pose.position.z = 0.5f* BOX_SIZE.getZ();
pose.orientation.x = pose.orientation.y = pose.orientation.z = 0;
pose.orientation.w = 1;

// creating underlying collision object
moveit_msgs::CollisionObject cobj; cobj.primitives.clear();
cobj.primitives.push_back(shape);
cobj.primitive_poses.clear();
cobj.primitive_poses.push_back(pose);
cobj.header.frame_id = WORLD_FRAME_ID;
cobj.id = ATTACHED_OBJECT_ID;

// creating attached collision object message
ATTACHED_COLLISION_OBJECT.link_name = TCP_LINK_NAME;
ATTACHED_COLLISION_OBJECT.object = cobj;
ATTACHED_COLLISION_OBJECT.object.header.frame_id = TCP_LINK_NAME; ATTACHED_COLLISION_OBJECT.touch_links.push_back("gripper_body");

my code for carrying out the attach operation is as follows:

cfg.ATTACHED_COLLISION_OBJECT.object.operation = moveit_msgs::CollisionObject::ADD;
cfg.MARKER_MESSAGE.action = visualization_msgs::Marker::ADD;

// updating orientation
geometry_msgs::Quaternion q = pose.orientation;
q.x = -q.x;
q.y = -q.y;
q.z = -q.z;
cfg.ATTACHED_COLLISION_OBJECT.object.primitive_poses[0].orientation = q;
cfg.MARKER_MESSAGE.pose.orientation = q;

marker_publisher.publish(cfg.MARKER_MESSAGE);
attach_object_publisher.publish(cfg.ATTACHED_COLLISION_OBJECT);

Alternatively, I tried publishing to the planning scene because publishing to the attached_collision_object didn't work.  That code is similar to the
snippet above but instead it populates the planning scene message as follows:

// set operation
cfg.ATTACHED_COLLISION_OBJECT.object.operation = moveit_msgs::CollisionObject::ADD;
moveit_msgs::CollisionObject remove_obj = cfg.ATTACHED_COLLISION_OBJECT.object;
remove_obj.operation = moveit_msgs::CollisionObject::REMOVE;
cfg.MARKER_MESSAGE.action = visualization_msgs::Marker::ADD;

// updating planning scene message
planning_scene.world.collision_objects.push_back(remove_obj); planning_scene.robot_state.attached_collision_objects.push_back(cfg.ATTACHED_COLLISION_OBJECT);
planning_scene.is_diff = true;

marker_publisher.publish(cfg.MARKER_MESSAGE);
planning_scene_publisher.publish(planning_scene);

The markers for the attached object look correct since the box sticks to the tcp of the robot arm while it's moving. Initially I used groovy and then switched to hydro but the problem persisted; I'm back to groovy again.  In the end, I added a box to the urdf model and modified the allowed collision matrix in order to turn collisions for this box on and off. However, I'd still like to know if collision avoidance with a payload (not one in the urdf) can be done the proper way.  Thank you

Jorge Nicho | Engineer
Southwest Research Institute
Manufacturing Systems Department
Robotics and Automation Engineering Section

http://robotics.swri.org
http://rosindustrial.swri.org/
http://ros.swri.org

[Michael Ferguson]

Something I have noticed, when generating a number of plans in a somewhat simulated environment (and was thus setting the motion_planing_request.start_state.joint_state):

It appears that if you have *anything* set in the start_state of the MotionPlanningRequest (even the robot pose in joint_state), it uses the attached_collision_objects of the start_state. Thus, even if you have published the object as attached, and even though it is typically still visualized by RVIZ as being there, the plan will not take it into account. I found that by simple adding my attached object to the start_state of the request, I got the desired results.

-Fergs

[Martin Günther]

On Thu, 30 Jan 2014 02:06:32 -0800
Michael Ferguson <mfe...@gmail.com> wrote:

> Something I have noticed, when generating a number of plans in a
> somewhat simulated environment (and was thus setting the
> motion_planing_request.start_state.joint_state):
>
> It appears that if you have *anything* set in the start_state of the
> MotionPlanningRequest (even the robot pose in joint_state), it uses
> the attached_collision_objects of the start_state. Thus, even if you
> have published the object as attached, and even though it is
> typically still visualized by RVIZ as being there, the plan will not
> take it into account. I found that by simple adding my attached
> object to the start_state of the request, I got the desired results.
>
> -Fergs

Hi Fergs,

this is interesting. Does this also happen when
`start_state.is_diff = True`? From the comment in the RobotState.msg
file, I'd expect the behaviour you describe exactly when `is_diff =
False`:

# Flag indicating whether this scene is to be interpreted as a diff with respect to some other scene
# This is mostly important for handling the attached bodies (whether or not to clear the attached bodies
# of a moveit::core::RobotState before updating it with this message)
bool is_diff

Cheers,
Martin

[Brian O'Neil]

For what it's worth, I can confirm that if you call the plan_kinematic_path service and explicitly diff the start state with the attached object(s), the returned path does avoid collisions with attached objects. However, if you plan through the MoveGroup class, or through the rviz plugin, collisions with attached objects are ignored.

[Sachin Chitta]

I believe we have this figured out now. There were two issues (and the corresponding fixes):

(1) When plan and execute was called, attached objects were being removed from the robot state with which collision checks were being carried out. This has been fixed: https://github.com/ros-planning/moveit_ros/pull/426

(2) In some cases, collisions between attached objects and the octomap were not being checked for - There is currently a bug which will cause collision checking with attached objects to not be carried out if they are attached to a link without geometry. The user fix for this (for now) is to attach objects to a real link (with geometry) - please let us know if this does not work.

Hope this helps, we will follow up with a proper fix in MoveIt! for (2) that will allow objects to be attached to links without geometry as well (Issue opened on MoveIt!: https://github.com/ros-planning/moveit_core/issues/158).

Best Regards,
Sachin


Sachin Chitta
Associate Director
Robotics Systems and Software
SRI International

[Jorge Nicho]

Sachin,
Thank you for looking into this.  I do in fact have my payload attached to a link with empty geometry, I'll modify my application so that a link with geometry is used instead.  Alternative, would adding a small geometry to the empty link (in the urdf)  trigger a collision check for that link and its attached object?.  Also, is it required to attach the object to a link belonging to the planning group only or it could be any link in the arm? Thanks

Jorge Nicho | Engineer

Southwest Research Institute

Manufacturing Systems Department

Robotics and Automation Engineering Section

 

http://robotics.swri.org

http://rosindustrial.swri.org/

http://ros.swri.org

[Sachin Chitta]

Jorge,

Yes - adding a small collision geometry should trigger a collision check for the attached object - you need to make sure that your srdf is up to date though with the change in geometry (you can edit the srdf manually - especially the allowed collision rules or run the MoveIt! Setup Assistant again).

You can attach the object to a link belonging to the planning group or a link that is distal to the planning group (e.g. a gripper) and moves with the planning group - so any link on the arm should be fine as long as it also moves with the planning group.

Best Regards,
Sachin

[Shaun Edwards]

+1 to Sachin and the SRI team for getting to the bottom of this.

Thanks again,

Shaun

[Jorge Nicho]

Sachin,
I finally got around to running test on my application with the changes discussed.  As I recall, the source of the neglected collision checks for the attached payload was due to the use of empty links (without geometry) for the attach link.  Thus, I added a 1cm diameter sphere to my attach link and then added it to the "touch_links" list  in the Motion plan request in order ignore unnecessary collision checks.  This solution worked, collision free paths were produced for the arm with the attached payload in the presence of obstacle sensor data (octomap). 

There were some strange behaviours which I think are worth mention:
a- The motions plans produced several unnecessary moves before arriving to the target.  This happened ocassionally before but it became more frequent after making the changes above.
b- There are instances when the planner finds a valid path but then it reports a collision inmediately after a path simplification operation. 

Any suggestions for improving or eliminating this behaviour would be very welcome.

I'm thankful for your support and I'd be more than happy to run more rigorous test that may help expose more on this issue.

Jorge

[Brian O'Neil]

I can confirm both of these observations. I figured (a) was part and parcel of the sampling based motion planning. (b) occurs just frequently enough to be irritating, and just infrequently enough to be difficult to debug. I have tried reducing the longest valid segment fraction parameter in the ompl configuration file, but I haven't done enough testing to know if this has made a difference.

--Brian

avia...@gmail.com
Tel: (480) 363-8963

[Simon Jansen]

I too can confirm this. We're testing with an arm in extremely confined spaces (just to test the limits of the planners) and notice that often a path will be unnecessarily long (but I assume this is to be expected from sample-based planners) and that a valid path will become invalid sometimes after simplication.

We assumed both problems were intrinsic to the planners: they are sample-based and cannot be expected to give the right answer every time, especially in confined spaces. Our approach has been to generate multiple paths and compare the succesfull solutions on their length to get an optimal solution.

Our tests consisted basically of planning the arm into a hole. The failure rate (mainly failure due to path simplification) was about 50%.

I would be very interested to know if this is 'normal' behavior of the planner and we just have to make due (which we sort of decided already), or that it can be improved in some manner, or, maybe, that some of this behavior is due to a bug.

Maybe we're getting of the topic of collision checking of attached objects, so we should start a new topic for this...

[Sachin Chitta]

Is there a simulated scenario I can play with that one of you could provide me? Just pointing me to the relevant github repo and launch files I can use would be great.

Thanks,

Sachin

[Sachin Chitta]

BTW, Simon is right - this is off topic for this thread. Whoever replies, start a new thread for this.

Thanks,
Sachin

[Keliang He]

Hi,

I'm running into a similar issue. I've been trying to get a pr2 to plan for paths with an object in hand. The trajectory planned would cause a collision between the attached object and the collision objects in the environment. 

Currently I'm attaching the object by calling 

move_group.attachObject("objectID");

and the objects in the environment are added using 

planning_scene_interface.addCollisionObjects(vector_of_collision_objects);

and planning is done by calling

move_group.plan();

Sacchin mentioned two possible causes for this, the first one being solved, which I checked and made sure that I do have the updated version of the code. The second case I don't think is the issue because I looked in the PR2 urdf files, and the r_wrist_roll_link has both visual and collision geometry.

One thing that I am doing a little different from people is that I'm calling execute on the movegroup to carry out the plan, and then using that as the next start state (without explicitly specifying the start state). would this cause a problem?

Anyone know what might be wrong here? Am I mixing and matching different ways of using MoveIt! that caused the object not actually being added to the scene where planning is actually done? Any help would be greatly appreciated.

Thank you,

Keliang He

Rice University

On Saturday, March 1, 2014 5:45:14 PM UTC-6, Sachin Chitta wrote:

[Alberto Romay]

Hello Keliang,

     I was also running into this issue with the latest version of the moveit .deb packages.

     I manage to solve this in my application by manually setting the start_state.is_diff of my plan request to "true" :

 planning_interface::MotionPlanRequest motion_plan_request;

 motion_plan_request.start_state.is_diff = true;

...

rest of my plan request

...

This was supposedly fixed in #426 and automatically being done, however, I still had to set the start_state.diff to "true" manually in my application.

I don't know why is #426 not working any more in the latest version of moveit.

Unfortunately this is still affecting the ROS Pickup Action so if I want to do goal.pick(); objects are not considered in collision and in this case I can't manually set the start_state.diff =true;  unless I add this to the ROS Pickup Action and recompile the moveit code from source, I think.

Hope the suggestion helps you, and if anyone have any idea why this issue is still there and affecting in particular the Pickup Action (see this example pick_place_tutorial ) will be really appreciated.

Best,

Alberto Romay