Attaching Object to Robot and then use with move_arm package

edit

Hello all,

I am using the move_arm package to plan and execute trajectories for the arm of the KUKA youbot robot. Everything is working fine.

In some cases, I want to pick up a cube and place it on the back of the youbot. So when I pick the cube, I am adding it as an attached object on the robot using the code from the Attaching Virtual Objects to the Robot tutorial, in order to avoid collisions when I am going to place it on the back of the youbot. However, when I call the move_arm action server afterwards to move the arm to the place position the attached object is not taken into consideration for the trajectory plan.

I call the get_planning_scene service with the attached object and then I call the set_planning_scene_diff service with the resulted planning scene. This is not suppose to forward the updated scene to all components?

arm_navigation_msgs::GetPlanningScene planning_scene;
planning_scene.request.planning_scene_diff.attached_collision_objects.push_back(att_object);
if(!get_planning_scene_client.call(planning_scene))
{
    ROS_WARN("Can't get planning scene");
    return false;
}

arm_navigation_msgs::SetPlanningSceneDiff srv;
srv.request.planning_scene_diff = planning_scene.response.planning_scene;
if(!set_planning_scene_client.call(srv))
{
    ROS_WARN("Can't set planning scene");
    return false;
}

PS. The planning scene from the response contains the object. I validate it using the collision models and visualise the attached collision objects markers.