CollisionObjects don't collide with environment after attaching to robot

[IvoD1998]

Hi community

I am attempting to use the addCollisionObject function to incorporate a tool-changing functionality, mostly following the MoveIt!-tutorial (https://ros-planning.github.io/moveit_tutorials/doc/move_group_interface/move_group_interface_tutorial.html). Loading the mesh of the tool or primitives is not an issue as they appear in the planning scene as expected. When attaching the object of the tool to the robot to use it for collision checking with the environment however, collision with the tool-object is not considered by MoveIt!.

code:

    //Setup movegroup interface
    moveit::planning_interface::MoveGroupInterface move_group("manipulator");

    //Setup planning scene interface to add and remove collision objects in the scene
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    //Collision object
    moveit_msgs::CollisionObject gripper;
    gripper.header.frame_id = "tool0";
    moveit_msgs::CollisionObject block;
    block.header.frame_id = "map";
    //Identifier
    gripper.id = "gripper";
    block.id = "block";
    //Define object 
    shapes::Mesh* mesh = shapes::createMeshFromResource("FILEPATH", {0.001, 0.001, 0.001});
    shape_msgs::Mesh gripper_mesh;
    shapes::ShapeMsg mesh_msg;
    shapes::constructMsgFromShape(mesh, mesh_msg);
    gripper_mesh = boost::get<shape_msgs::Mesh>(mesh_msg);

    shape_msgs::SolidPrimitive primitive;
    primitive.type = primitive.BOX;
    primitive.dimensions.resize(3);
    primitive.dimensions[primitive.BOX_X] = 0.1;
    primitive.dimensions[primitive.BOX_Y] = 1.5;
    primitive.dimensions[primitive.BOX_Z] = 0.5;

    //Give a pose
    geometry_msgs::Pose gripper_pose;
    gripper_pose.position.x = 0.0;
    gripper_pose.position.y = 0.0;
    gripper_pose.position.z = 0.1;
    gripper_pose.orientation.x = 0.7071068;
    gripper_pose.orientation.y = 0.0;
    gripper_pose.orientation.z = 0.0;
    gripper_pose.orientation.w = 0.7071068;

    geometry_msgs::Pose box_pose;
    box_pose.orientation.w = 1.0;
    box_pose.position.x = 0.5;
    box_pose.position.y = 0.0;
    box_pose.position.z = 0.25;

    //Put in collision object
    block.primitives.push_back(primitive);
    block.primitive_poses.push_back(box_pose);
    gripper.meshes.push_back(gripper_mesh);
    gripper.mesh_poses.push_back(gripper_pose);
    gripper.operation = gripper.ADD;
    block.operation = block.ADD;

    std::vector<moveit_msgs::CollisionObject> collision_objects;
    collision_objects.push_back(gripper);
    collision_objects.push_back(block);

    //Add it to the scene
    planning_scene_interface.applyCollisionObjects(collision_objects);
    //Attach to robot
    move_group.attachObject(gripper.id, "tool0");

Does anyone know if I am missing something or what I might be doing wrong?
Thanks!
Ivo

[felixvd]

Can you check if this is related to moveit/moveit#2676 ?

[IvoD1998]

hi @felixvd

I don't think this is related as I am not using Pilz, but OMPL PRM.
However, Pilz was giving me a lot of issues when trying to launch my planning_execution, so I uncommented the Pilz related launch segments. I don't see how that could effect this, but maybe there is something there?

Thanks!
Ivo

[IvoD1998]

hi @felixvd

The issues appears to be a simple mistake on my end. I assumed that by attaching the the object to link_6 of the robot, the collision between these two would be ignored, which turns out is not the case. After adding a small offset so the tool and link_6 are no longer "touching" everything works fine. Although the collision of the tool is not highlighted in red like with other parts of the robot, it is now recognised and taken into account.

thank you!
Ivo