Robot link and Robot attached collision in MoveIt!

edit

I'm trying to grasp an object on a table, attach it to the hand, and drop it off somewhere else with the Jaco2 robotic arm. Grasping works fine: I attach the object and close the grippers, which leads me to believe the collision is (correctly!) ignored for gripping the object, but as soon as I want to drop it, I often get the following error.

[INFO] Found a contact between 'cup' [Robot attached] and 'jaco_finger_tip_1' [Robot link] 

[WARN] Start State seems to be in collision with respect to group arm.

This happens in 1 out of 5 times, I would say. The Planner I use is RTTConnectkDefaultConfig.

I am sure, that the object is attached, for multiple reasons:

a. I have a fixed tabletop already in the planning scene. If i don't lower it a cm or so lower than it actually is, I get this error:

[INFO] Found a contact between 'cup' [Robot attached] and 'jaco_finger_tip_1' [Robot link]

b. I check again and again with the move_group interface, if object exists and if it is attached. I get no error messages here

  auto all_objects = planning_scene_interface_.getObjects();
  if (all_objects.find(box.description) == all_objects.end()) {
    ROS_ERROR_STREAM("Object " << box.description << " hasn't been created");
    return;
  }
//  moveit_visuals_.attachObstacle(box);
  auto all_attached_objects = planning_scene_interface_.getAttachedObjects();
  if (all_attached_objects.find(box.description) != all_attached_objects.end()) {
    ROS_WARN_STREAM("Object " << box.description << " is already attached");
    return;
  }

  ROS_STATUS("Attaching obstacle " << box.description);
  move_group_.attachObject(box.description);

Adding objects to the planning scene works fine:

  const auto& pose_goal = goal->pose_goal;
  const auto& box = goal->bounding_box;

  moveit_msgs::CollisionObject collision_object;
  collision_object.header.frame_id = move_group_.getPlanningFrame();
  collision_object.id = box.description;

  shape_msgs::SolidPrimitive primitive;
  primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[0] = box.dimensions.x;
  primitive.dimensions[1] = box.dimensions.y;
  primitive.dimensions[2] = box.dimensions.z;

  geometry_msgs::PointStamped out_pt;
  geometry_msgs::PointStamped in_pt;
  in_pt.header = box.header;
  in_pt.point = box.point;

  // transform point
  try {
    tf_listener_.waitForTransform("root", box.header.frame_id, box.header.stamp, ros::Duration(1));
    tf_listener_.transformPoint("root", in_pt, out_pt);
  }
  catch (tf::TransformException &exception) {
    ROS_INFO_STREAM("Transform failed. Why? - " << exception.what());
  }

  geometry_msgs::Pose pose;
  pose.position.x = out_pt.point.x;
  pose.position.y = out_pt.point.y;
  pose.position.z = out_pt.point.z;
  pose.orientation.w = 1.0;

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

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

  ROS_STATUS("Added Object " << box.description);

detaching/attaching in accordance with tutorials

void JacoManipulationServer::attachObstacle(const jaco_manipulation::BoundingBox &box) {
  ROS_STATUS("Attaching obstacle " << box.description);
  move_group_.attachObject(box.description);
}

void JacoManipulationServer::detachObstacle(const jaco_manipulation::BoundingBox &box) {
  ROS_STATUS("Detaching obstacle " << box.description);
  move_group_.detachObject(box.description);
}

this is my gripping code

 addObstacle(goal);

  bool moved = planAndMove(goal->pose_goal);
  if (!moved) return false;

  attachObstacle(goal->bounding_box);
  closeGripper();

  ROS_STATUS("Gripper closed. Object grasped.");

  return true;

where i add the obstacle (code above), let moveit do planning and moeving towards goal, attach the obstacle, so it can be gripped, and close the gripper.

Similarly, for dropping

 bool moved = planAndMove(goal->pose_goal);
  if (!moved) {
    openGripper();
    detachObstacle(goal->bounding_box);
    return ...