Attached collision objects ignore environment

edit

Hello fellow ROS users,

I've never had problems using collision objects (unattached); collision checking always works perfectly with Moveit or Descartes. The issue arise when I try to attach a collision object (in this example, a tool) to the robot and use the move_group api to move the robot into collision (another collision object, which is a table). I can see visually that the collision object is attached (orange to purple in rViz and I can move the robot with the tool), but collision checking is ignored between the attached tool and the environment when using the move_group api.

Using the moveit motion planning directly in rViz, collision between the attached tool and the table is detected correctly. This is the following message I get when I plan to move the attached tool into it :

[ INFO] [1568823161.942566491]: Found a contact between 'table' (type 'Object') and 'tool' (type 'Robot attached'), which constitutes a collision. Contact information is not stored.

Planning failed like it was supposed too. I can't seem to find why it doesn't fail when using the move_group api. I've tried using different link_name (since tool0 has no geometry), but to no avail. I've noticed that there doesn't seem to be any attached collision objects (or even collision objects) in the planning scene when I use getAttachedCollisionObjectMsgs() or getCollisionObjectMsgs(), is the planning scene not updated? Yet, it fails if any robot link (BUT the tool) touches the table, so the table is in the planning scene.

A similar question: #q324672.

*EDIT : I've noticed that in rViz, the looping animation doesn't have the attached tool while using the move_group api, but does, when using motion planning plugin on rViz directly. By looking at the topic move_group/display_planned_path, I can see that there is no attached collision object when using the move_group api. *

Here's my code :

int main(int argc, char** argv)
{
// Ros init
ros::init(argc, argv, "moveit_node");
ros::NodeHandle nh;

// Async spinner
ros::AsyncSpinner async_spinner(1);
async_spinner.start();

// Planning scene interface for adding collision objects
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

/*/ Create table collision object /*/

// Collisions objects and colors vector
std::vector<moveit_msgs::CollisionObject> collision_objects;
std::vector<moveit_msgs::ObjectColor> object_colors;

// Create collision object (table)
moveit_msgs::CollisionObject table;
table.header.frame_id = "world";
table.id = "table";

shape_msgs::SolidPrimitive table_primitive;
table_primitive.type = table_primitive.BOX;
table_primitive.dimensions.resize(3);
table_primitive.dimensions[0] = 2;
table_primitive.dimensions[1] = 2;
table_primitive.dimensions[2] = 0.01;

geometry_msgs::Pose table_pose;
table_pose.orientation.w = 1.0;
table_pose.position.x = 0;
table_pose.position.y = 0;
table_pose.position.z = -0.01;

table.primitives.push_back(table_primitive);
table.primitive_poses.push_back(table_pose);
table.operation = table.ADD;

collision_objects.push_back(table);

moveit_msgs::ObjectColor table_color;
table_color.id = "table";
table_color.color.a = 1;
table_color.color.r = 0.7;
table_color.color.g = 0.7;
table_color.color.b = 0.8;

object_colors.push_back(table_color);

// Create collision object (tool)
moveit_msgs::CollisionObject tool_object;
tool_object.id = "tool";
tool_object.header.frame_id = "tool0";

shape_msgs::SolidPrimitive tool_primitive;
tool_primitive.type = table_primitive.BOX;
tool_primitive.dimensions.resize(3);
tool_primitive.dimensions[0] = 0.05;
tool_primitive.dimensions ...