Revision history [back]

In a classic example of rubber duck debugging, I figured it out after finishing the question. After picking up the tool, the pose goal is assigned to a different end effector link, but the previous goal for the default link remains. The planning request thus asks for the end effector to be in two different places at once. The solution is simply clearing the goals.

The call to "clearPoseTargets()" is the only thing I had to change above.

  [...]
  moveit::planning_interface::MoveGroupInterface* group_pointer;
  group_pointer = robotNameToMoveGroup(robot_name);

  group_pointer->clearPoseTargets();
  group_pointer->setStartStateToCurrentState();
  group_pointer->setEndEffectorLink(end_effector_link);
  group_pointer->setPoseTarget(pose);
  [...]

I appreciate that this behavior exists for multi-arm setups, but I wish there had been a more helpful message somewhere (maybe "X pose goals set for links Y" in the debug log).