add goal tolerance examples

doc/move_group_interface/move_group_interface_tutorial.rst

@@ -33,13 +33,16 @@ See the `YouTube video <https://youtu.be/_5siHkFQPBQ>`_ at the top of this tutor
  2. The robot moves its arm to the joint goal at its side.
  3. The robot moves its arm back to a new pose goal while maintaining the end-effector level.
  4. The robot moves its arm along the desired Cartesian path (a triangle down, right, up+left).
- 5. A box object is added into the environment to the right of the arm.
+ - A box object is added into the environment to the right of the arm.
     |B|
-
+ 5. The robot moves its arm to a new pose goal.
  6. The robot moves its arm to the pose goal, avoiding collision with the box.
- 7. The object is attached to the wrist (its color will change to purple/orange/green).
- 8. The object is detached from the wrist (its color will change back to green).
- 9. The object is removed from the environment.
+ - The object is attached to the wrist (its color will change to purple/orange/green).
+ 7. The robot moves its arm to the pose goal, avoiding collision between the cylinder and the box box.
+ - The object is detached from the wrist (its color will change back to green).
+ 8. The robot moves its arm to a pose goal with orientation tolerance, avoiding collision with the box.
+ 9. The robot moves its arm to a pose goal with position tolerance, avoiding collision with the box.
+ - The objects are removed from the environment.
 
 .. |B| image:: ./move_group_interface_tutorial_robot_with_box.png
 

doc/move_group_interface/src/move_group_interface_tutorial.cpp

@@ -452,7 +452,8 @@ int main(int argc, char** argv)
   // .. image:: ./move_group_interface_tutorial_attached_object.gif
   //    :alt: animation showing the arm moving differently once the object is attached
   //
-  // Detaching and Removing Objects
+
+  // Detaching Objects
   // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
   //
   // Now, let's detach the cylinder from the robot's gripper.
@@ -467,6 +468,58 @@ int main(int argc, char** argv)
   /* Wait for MoveGroup to receive and process the attached collision object message */
   visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot");
 
+  // Moving to a goal pose with orientation tolerances
+  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+  //
+  // Show text in RViz of status
+  visual_tools.deleteAllMarkers();
+  visual_tools.publishText(text_pose, "Goal with orientation tolerance", rvt::WHITE, rvt::XLARGE);
+  visual_tools.trigger();
+  // Let's plan to a goal colliding on the top of the collision object.
+  move_group_interface.setStartState(*move_group_interface.getCurrentState());
+  geometry_msgs::Pose pose_above_collision_object;
+  pose_above_collision_object.orientation.x = 1.0;
+  pose_above_collision_object.position.x = 0.45;
+  pose_above_collision_object.position.y = 0.0;
+  pose_above_collision_object.position.z = 0.59;
+  move_group_interface.setPoseTarget(pose_above_collision_object);
+
+  std::vector<double> goal_orientation_tolerance_xyz = { 1e-3, M_PI / 2, M_PI / 4 };
+  move_group_interface.setGoalOrientationToleranceXYZ(goal_orientation_tolerance_xyz);
+
+  success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+  ROS_INFO_NAMED("tutorial", "Visualizing plan 8 (Pose goal with orientation tolerances avoiding collision) %s",
+                 success ? "" : "FAILED");
+  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
+  visual_tools.trigger();
+  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete");
+
+  // Moving to a goal pose with position tolerances
+  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+  //
+  // Show text in RViz of status
+  visual_tools.deleteAllMarkers();
+  visual_tools.publishText(text_pose, "Goal with position tolerance", rvt::WHITE, rvt::XLARGE);
+  visual_tools.trigger();
+  // Let's plan to a goal colliding on the top of the collision object.
+  move_group_interface.setStartState(*move_group_interface.getCurrentState());
+  move_group_interface.setPoseTarget(pose_above_collision_object);
+
+  goal_orientation_tolerance_xyz = { 1e-3, 1e-3, 1e-3 };
+  move_group_interface.setGoalOrientationToleranceXYZ(goal_orientation_tolerance_xyz);
+  std::vector<double> goal_position_tolerance_xyz = { 1e-4, 1e-4, 0.2 };
+  move_group_interface.setGoalPositionToleranceXYZ(goal_position_tolerance_xyz);
+
+  success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+  ROS_INFO_NAMED("tutorial", "Visualizing plan 9 (Pose goal with position tolerances avoiding collision) %s",
+                 success ? "" : "FAILED");
+  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
+  visual_tools.trigger();
+  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete");
+
+  // Removing Objects
+  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+  //
   // Now, let's remove the objects from the world.
   ROS_INFO_NAMED("tutorial", "Remove the objects from the world");
   std::vector<std::string> object_ids;
@@ -475,11 +528,12 @@ int main(int argc, char** argv)
   planning_scene_interface.removeCollisionObjects(object_ids);
 
   // Show text in RViz of status
+  visual_tools.deleteAllMarkers();
   visual_tools.publishText(text_pose, "Objects removed", rvt::WHITE, rvt::XLARGE);
   visual_tools.trigger();
 
   /* Wait for MoveGroup to receive and process the attached collision object message */
-  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears");
+  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to shutdown the demo node");
 
   // END_TUTORIAL