Disabling collision in AllowedCollisionMatrix removes attached object from the scene

edit

I am updating the AllowedCollisionMatrix in order to disable collision between an object attached to the robot end-effector and a collision object in the world for a specific robot movement. It works but it also removes the attached object from the scene. So it seems that it is not possible to disable temporary collision for an attached object and enable it afterward.

What would have been useful:

• to be able to update the ACM to disable collision between an attached object and another object in the scene
• use MoveIt to move the end-effector
• update the ACM to enable collision
• the attached object is still present in the scene with its position updated since the robot has moved

Unfortunately, it looks like it just removes the attached object from the scene.

A minimal code looks like this (tested on ROS Kinetic). Run with roslaunch panda_moveit_config demo.launch and rosrun <> <>.

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>

#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "move_group_interface_tutorial");
  ros::NodeHandle node_handle;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  static const std::string PLANNING_GROUP = "panda_arm";
  moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
  robot_model_loader::RobotModelLoaderPtr robot_model_loader(new robot_model_loader::RobotModelLoader("robot_description"));
  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor(new planning_scene_monitor::PlanningSceneMonitor(robot_model_loader));

  planning_scene_monitor->startSceneMonitor();
  planning_scene_monitor->startWorldGeometryMonitor();
  planning_scene_monitor->startStateMonitor();

  //Add collision object
  {
    moveit_msgs::CollisionObject collision_object;
    collision_object.header.frame_id = move_group.getPlanningFrame();
    collision_object.id = "box1";

    shape_msgs::SolidPrimitive primitive;
    primitive.type = primitive.BOX;
    primitive.dimensions.resize(3);
    primitive.dimensions[0] = 0.3;
    primitive.dimensions[1] = 1;
    primitive.dimensions[2] = 0.4;

    geometry_msgs::Pose box_pose;
    box_pose.orientation.w = 1.0;
    box_pose.position.x = 0.3;
    box_pose.position.y = 0;
    box_pose.position.z = 0;

    collision_object.primitives.push_back(primitive);
    collision_object.primitive_poses.push_back(box_pose);
    collision_object.operation = collision_object.ADD;

    std::vector<moveit_msgs::CollisionObject> collision_objects;
    collision_objects.push_back(collision_object);
    planning_scene_interface.applyCollisionObjects(collision_objects);
  }

  //Add collision object and attach it
  {
    moveit_msgs::CollisionObject collision_object;
    collision_object.header.frame_id = move_group.getPlanningFrame();
    collision_object.id = "box_attach";

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

    geometry_msgs::Pose box_pose = move_group.getCurrentPose().pose;
    box_pose.position.z -= 0.175;

    collision_object.primitives.push_back(primitive);
    collision_object.primitive_poses.push_back(box_pose);
    collision_object.operation = collision_object.ADD;

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

    planning_scene_interface.applyCollisionObjects(collision_objects);

    moveit_msgs::AttachedCollisionObject aco;
    aco.object.id = collision_object.id;
    aco.link_name = move_group.getEndEffectorLink();
    aco.touch_links.push_back(move_group.getEndEffectorLink());
    aco.touch_links.push_back("panda_hand");
    aco.touch_links.push_back("panda_leftfinger");
    aco.touch_links.push_back("panda_rightfinger");
    aco.object.operation = moveit_msgs::CollisionObject::ADD;
    planning_scene_interface.applyAttachedCollisionObject(aco);

    std::map<std::string, moveit_msgs::AttachedCollisionObject> attached_co = planning_scene_interface.getAttachedObjects();
    std::cout << "\nAttachedCollisionObject after applyAttachedCollisionObject:" << std::endl;
    for (const auto& kv : attached_co) {
      std::cout << kv.first << " ; " << kv.second.object.id << std::endl;
    }
  }

  //Wait a little
  const int wait_time = 2;
  ros::Duration(wait_time).sleep();

  //Ignore collision between attached object and collision object in the world
  {
    planning_scene_monitor::LockedPlanningSceneRW ls(planning_scene_monitor);
    collision_detection::AllowedCollisionMatrix& acm = ls->getAllowedCollisionMatrixNonConst();
    acm.setEntry("box1", "box_attach", true);
    std::cout << "\nAllowedCollisionMatrix:\n";
    acm.print(std ...