MoveIt plan fails after attaching an object, but not from Rviz or after restarting requesting node

I am implementing a tool change procedure by attaching a collision object to the robot's gripper. However, after attaching the object, the planner fails to sample any valid states for the goal tree. More strangely, if I leave the scene untouched, restart the node without re-attaching the object, and execute the same planning requests, they succeed. Launching the planning requests from Rviz works, and goToNamedPose (see code below) also works, strangely. I figure that means that the move group itself is intact, but something pollutes my code that causes the planning to fail. What is it? What could I do to debug further? Setting the rqt_logger_level to debug did not turn up any useful messages from the move group. I have tried to condense the code to the pertinent parts. I can send a dockerized sandbox to anyone who is interested in checking this out, it just contains other messy code that I wouldn't want to upload needlessly. Any pointers or ideas are appreciated. main() { // ROS_INFO("Testing the tool mounting."); o2as_skill_server.equipTool("b_bot", "tool_m5"); o2as_skill_server.goToNamedPose("home_b", "b_bot"); geometry_msgs::PoseStamped target_ps = makePoseStamped(); target_ps.header.frame_id = "tray_1"; target_ps.pose.position.z = .02; // We ignore the orientation of the (they always point down) and assign an appropriate orientation // TODO: How to find a good pose for the robot automatically? Launch a planning request to MoveIt? target_ps.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, -(110.0/180.0 *M_PI)); // Z-axis pointing up. std::string link_name = "b_bot_tool_m5_tip_link"; std::string robot_name = "b_bot"; moveToCartPosePTP(target_ps, robot_name, true, link_name); o2as_skill_server.unequipTool("b_bot"); ROS_INFO("Done."); } bool equipTool(std::string robot_name, std::string tool_id) {return equipUnequipTool(robot_name, tool_id, "equip");} bool unequipTool(std::string robot_name) {return equipUnequipTool(robot_name, held_tool_, "unequip");} bool equipUnequipTool(std::string robot_name, std::string tool_id, std::string equip_or_unequip) { bool equip = (equip_or_unequip == "equip"); bool unequip = (equip_or_unequip == "unequip"); // Plan & execute motion to in front of tool holder geometry_msgs::PoseStamped ps_approach, ps_tool_holder; ps_approach.header.frame_id = tool_id + "_link"; ps_approach.pose.position.y = .1; ps_approach.pose.position.z = .017; ps_approach.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, -M_PI / 2); ROS_INFO("Moving to tool approach pose PTP."); moveToCartPosePTP(ps_approach, robot_name); if (equip) { openGripper(robot_name); ROS_INFO("Spawning tool. Waiting for a while to be safe."); spawnTool(tool_id); } // Plan & execute linear motion to the tool change position ps_tool_holder = ps_approach; ps_tool_holder.pose.position.y = -0.03; ROS_INFO("Moving to pose in tool holder LIN."); moveToCartPoseLIN(ps_tool_holder, robot_name); // Close gripper, attach the tool object to the gripper in the Planning Scene. if (equip) { closeGripper(robot_name); attachTool(tool_id, robot_name); } else if (unequip) { openGripper(robot_name); detachTool(tool_id, robot_name); } // Plan & execute linear motion away from the tool change position ROS_INFO("Moving back to tool approach pose LIN."); moveToCartPoseLIN(ps_approach, robot_name); if (unequip) despawnTool(tool_id); return true; } bool moveToCartPosePTP(geometry_msgs::PoseStamped pose, std::string robot_name, bool wait, std::string end_effector_link) { moveit::planning_interface::MoveGroupInterface::Plan myplan; moveit::planning_interface::MoveItErrorCode success_plan = moveit_msgs::MoveItErrorCodes::FAILURE, motion_done = moveit_msgs::MoveItErrorCodes::FAILURE; moveit::planning_interface::MoveGroupInterface* group_pointer; group_pointer = robotNameToMoveGroup(robot_name); group_pointer->setStartStateToCurrentState(); group_pointer->setEndEffectorLink(end_effector_link); group_pointer->setPoseTarget(pose); ROS_DEBUG_STREAM("Planning motion for robot " << robot_name << " and EE link " << end_effector_link + "_tip_link."); success_plan = group_pointer->move(myplan); if (success_plan) { if (wait) motion_done = group_pointer->execute(myplan); else motion_done = group_pointer->asyncExecute(myplan); if (motion_done) { return true; } } ROS_WARN("Failed to perform motion."); return false; } bool goToNamedPose(std::string pose_name, std::string robot_name) { moveit::planning_interface::MoveGroupInterface* group_pointer; group_pointer = robotNameToMoveGroup(robot_name); group_pointer->setStartStateToCurrentState(); group_pointer->setNamedTarget(pose_name); moveit::planning_interface::MoveGroupInterface::Plan myplan; moveit::planning_interface::MoveItErrorCode success_plan = moveit_msgs::MoveItErrorCodes::FAILURE, motion_done = moveit_msgs::MoveItErrorCodes::FAILURE; success_plan = group_pointer->move(); return true; } bool spawnTool(std::string tool_id) { std::vector collision_objects; collision_objects.resize(1); moveit_msgs::CollisionObject tool_m5; tool_m5.header.frame_id = "tool_m5_link"; tool_m5.id = "tool_m5"; tool_m5.operation = tool_m5.ADD; tool_m5.primitives.resize(1); tool_m5.primitive_poses.resize(1); tool_m5.primitives[0].type = tool_m5.primitives[0].BOX; tool_m5.primitives[0].dimensions.resize(3); tool_m5.primitives[0].dimensions[0] = 0.026; tool_m5.primitives[0].dimensions[1] = 0.04; tool_m5.primitives[0].dimensions[2] = 0.055; tool_m5.primitive_poses[0].position.x = 0; tool_m5.primitive_poses[0].position.y = -0.009; tool_m5.primitive_poses[0].position.z = 0.0275; if (tool_id == "tool_m5") collision_objects[0] = tool_m5; collision_objects[0].operation = collision_objects[0].ADD; planning_scene_interface_.applyCollisionObjects(collision_objects); return true; } // Remove the tool from the scene so it does not cause unnecessary collision calculations bool despawnTool(std::string tool_id) { std::vector collision_objects; collision_objects.resize(1); collision_objects[0].id = tool_id; collision_objects[0].operation = collision_objects[0].REMOVE; planning_scene_interface_.applyCollisionObjects(collision_objects); return true; } bool attachDetachTool(std::string tool_id, std::string robot_name, std::string attach_or_detach) { moveit_msgs::AttachedCollisionObject att_coll_object; if (tool_id == "tool_m5") att_coll_object.object = tool_m5; else { ROS_WARN_STREAM("No tool specified to " << attach_or_detach); } att_coll_object.link_name = robot_name + "_gripper_tip_link"; if (attach_or_detach == "attach") att_coll_object.object.operation = att_coll_object.object.ADD; else if (attach_or_detach == "detach") att_coll_object.object.operation = att_coll_object.object.REMOVE; ROS_INFO_STREAM(attach_or_detach << "ing tool " << tool_id); planning_scene_interface_.applyAttachedCollisionObject(att_coll_object); return true; }