Ask Your Question
1

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

asked 2018-08-08 00:08:16 -0500

fvd gravatar image

updated 2018-08-08 00:10:40 -0500

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-08-08 00:39:31 -0500

fvd gravatar image

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).

edit flag offensive delete link more

Comments

1

Thank you. I have problem too

huthamcau gravatar imagehuthamcau ( 2018-09-04 02:06:17 -0500 )edit

Yes, this is confusing indeed. Please provide a patch if possible (or an issue at least with a proposal how to fix it)!

v4hn gravatar imagev4hn ( 2018-09-04 08:03:51 -0500 )edit

I dumped it as an issue for now so that I have it on my list.

fvd gravatar imagefvd ( 2018-09-04 09:55:58 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2018-08-08 00:08:16 -0500

Seen: 76 times

Last updated: Aug 08 '18