improved documentation

README.md

@@ -34,7 +34,7 @@ This is the same example as the first one, but this time, additional tooling is
     - The motion does **not** start automatically, instead it must be triggered by pressing *next* in the RvizVisualToolsGUI within RViz
 
 
-### Execution of a Simple Motion Costrained by a Blocking Object
+### Execution of a Simple Motion Constrained by a Blocking Object
 
 In this example, an object can be observed in the RViz planning window. This object constrains the motion of the robot, such that the robot moves around it.
 
@@ -54,6 +54,23 @@ In this example, an object can be observed in the RViz planning window. This obj
     - An RViz window to see the plan and trigger the motion open.
     - The motion does **not** start automatically, instead it must be triggered by pressing *next* in the RvizVisualToolsGUI within RViz
 
+### Execution of a Pick-and-Place Task
+
+- Command: `roslaunch sample_applications pick_place_simulation.launch`
+- Expected results:
+    - A Gazebo window to observe the executed robot motion opens
+    - Two tables an a "stick" on top of one of them are spawned in Gazebo
+    - The Robot positions itself behind the stick and grasps it from the side.
+    - It then moves to the other table and places it on top of it.
+- Remarks and potential issues:
+    - The grasping in *not* done using the physical properties of the object (which would require extensive tweaking
+      of the surface properties of it), but using a Gazebo plugin [grasp fix](https://github.com/JenniferBuehler/gazebo-pkgs/wiki/The-Gazebo-grasp-fix-plugin).
+    - The process of this tutorial is a bit fragile. It may happen that the gripper fails to grab the object, that it
+      slips out of the gripper during the motion or that is is not placed correctly on the second table (and thus falls
+      down). We believe that these problems might be reduced by tweaking of some properties, but in the end, all of the
+      problems can also happen during an actual execution, so we have to live with them.
+    - If a problem like this occurs, be thankful that it was just a simulation and simply restart.
+
 ### Planning and Simulation based on RViz
 
 This is an example to use RViz and Gazebo interactively. Motions can be planned and executed from the RViz GUI.

src/pick_place_tutorial.cpp

@@ -34,6 +34,17 @@
 
 /* Author: Ioan Sucan, Ridhwan Luthra*/
 
+/*********************************************************************
+* This version of the pick and place tutorial was modified and adapted
+* to use the object spawner provided by the simulation_util package.
+* Furthermore, additional comments have been added.
+*
+* The original code can be found at
+* https://github.com/ros-planning/moveit_tutorials/blob/master/doc/pick_place/src/pick_place_tutorial.cpp
+*
+* Author: Johannes Mey
+*********************************************************************/
+
 // ROS
 #include <ros/ros.h>
 
@@ -44,9 +55,13 @@
 // TF2
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
-// Zone Spawner
+// Zone Spawner to add the collision objects to Gazebo
 #include <GazeboZoneSpawner.h>
 
+/**
+ * Sets the posture argument to an open state for the two gripper fingers (8cm apart).
+ * @param posture An empty posture that is filled with a posture for two fingers
+ */
 void openGripper(trajectory_msgs::JointTrajectory& posture)
 {
   // BEGIN_SUB_TUTORIAL open_gripper
@@ -64,9 +79,12 @@ void openGripper(trajectory_msgs::JointTrajectory& posture)
   // END_SUB_TUTORIAL
 }
 
+/**
+ * Sets the posture argument to a closed state for the two gripper fingers
+ * @param posture An empty posture that is filled with a posture for two fingers
+ */
 void closedGripper(trajectory_msgs::JointTrajectory& posture)
 {
-  // BEGIN_SUB_TUTORIAL closed_gripper
   /* Add both finger joints of panda robot. */
   posture.joint_names.resize(2);
   posture.joint_names[0] = "panda_finger_joint1";
@@ -78,12 +96,14 @@ void closedGripper(trajectory_msgs::JointTrajectory& posture)
   posture.points[0].positions[0] = 0.00;
   posture.points[0].positions[1] = 0.00;
   posture.points[0].time_from_start = ros::Duration(0.5);
-  // END_SUB_TUTORIAL
 }
 
+/**
+ * Pick an object from a fixed position.
+ * @param move_group The move group the pick operation is performed on
+ */
 void pick(moveit::planning_interface::MoveGroupInterface& move_group)
 {
-  // BEGIN_SUB_TUTORIAL pick1
   // Create a vector of grasps to be attempted, currently only creating single grasp.
   // This is essentially useful when using a grasp generator to generate and test multiple grasps.
   std::vector<moveit_msgs::Grasp> grasps;
@@ -124,30 +144,23 @@ void pick(moveit::planning_interface::MoveGroupInterface& move_group)
   // Setting posture of eef before grasp
   // +++++++++++++++++++++++++++++++++++
   openGripper(grasps[0].pre_grasp_posture);
-  // END_SUB_TUTORIAL
 
-  // BEGIN_SUB_TUTORIAL pick2
   // Setting posture of eef during grasp
   // +++++++++++++++++++++++++++++++++++
   closedGripper(grasps[0].grasp_posture);
-  // END_SUB_TUTORIAL
 
-  // BEGIN_SUB_TUTORIAL pick3
   // Set support surface as table1.
   move_group.setSupportSurfaceName("table1");
   // Call pick to pick up the object using the grasps given
   move_group.pick("object", grasps);
-  // END_SUB_TUTORIAL
 }
 
+/**
+ * Place an object at a fixed position on top of table2.
+ * @param group
+ */
 void place(moveit::planning_interface::MoveGroupInterface& group)
 {
-  // BEGIN_SUB_TUTORIAL place
-  // TODO(@ridhwanluthra) - Calling place function may lead to "All supplied place locations failed. Retrying last
-  // location in verbose mode." This is a known issue. |br|
-  // |br|
-  // Ideally, you would create a vector of place locations to be attempted although in this example, we only create
-  // a single place location.
   std::vector<moveit_msgs::PlaceLocation> place_location;
   place_location.resize(1);
 
@@ -190,20 +203,26 @@ void place(moveit::planning_interface::MoveGroupInterface& group)
   group.setSupportSurfaceName("table2");
   // Call place to place the object using the place locations given.
   group.place("object", place_location);
-  // END_SUB_TUTORIAL
 }
 
+/**
+ * Add the collision objects to the planning scene and to gazebo.
+ * @param planning_scene_interface The interface to access the planning scene.
+ */
 void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface& planning_scene_interface)
 {
-  // BEGIN_SUB_TUTORIAL table1
-  //
-  // Creating Environment
-  // ^^^^^^^^^^^^^^^^^^^^
+  // Besides the robot, our scene comprises three objects. Two tables, table1 and table2, and the object to be picked
+  // and placed. Thus, three collision objects have to be added to the planning scene and to Gazebo.
+
   // Create vector to hold 3 collision objects.
   std::vector<moveit_msgs::CollisionObject> collision_objects;
   collision_objects.resize(3);
 
-  // Add the first table where the cube will originally be kept.
+  /********************************************************************************
+   * table1 (the one on which the pick object is positioned before the operation)
+   ********************************************************************************/
+
+  // part 1: add the object to the scene
   collision_objects[0].id = "table1";
   collision_objects[0].header.frame_id = "panda_link0";
 
@@ -222,25 +241,29 @@ void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface& pla
   collision_objects[0].primitive_poses[0].position.z = 0.2;
   collision_objects[0].primitive_poses[0].orientation.w = 1.0;
 
+  collision_objects[0].operation = collision_objects[0].ADD;
+
+  // part 2: add the object to gazebo
+  /* Define the primitive and its dimensions *again*. */
   shape_msgs::SolidPrimitive table1;
   table1.type = shape_msgs::SolidPrimitive::BOX;
   table1.dimensions = collision_objects[0].primitives[0].dimensions;
 
+  /* Define the pose of the table *again*. */
   geometry_msgs::Pose table1pose;
   table1pose.position.x = collision_objects[0].primitive_poses[0].position.x;
   table1pose.position.y = collision_objects[0].primitive_poses[0].position.y;
   table1pose.position.z = collision_objects[0].primitive_poses[0].position.z;
+  table1pose.orientation.w = 1; // the orientation is always the default
 
-  // the orientation is always the default
-  table1pose.orientation.w = 1;
-
+  /* spawn the table in Gazebo, weighing 20 kg, but without physics enabled to increase stability */
   GazeboZoneSpawner::spawnPrimitive("table1", table1, table1pose, std_msgs::ColorRGBA(), 20, true);
-  // END_SUB_TUTORIAL
 
-  collision_objects[0].operation = collision_objects[0].ADD;
+  /********************************************************************************
+   * table2 (the one to which the pick object is move to by the operation)
+   ********************************************************************************/
 
-  // BEGIN_SUB_TUTORIAL table2
-  // Add the second table where we will be placing the cube.
+  // part 1: add the object to the scene
   collision_objects[1].id = "table2";
   collision_objects[1].header.frame_id = "panda_link0";
 
@@ -259,24 +282,27 @@ void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface& pla
   collision_objects[1].primitive_poses[0].position.z = 0.2;
   collision_objects[1].primitive_poses[0].orientation.w = 1.0;
 
+  collision_objects[1].operation = collision_objects[1].ADD;
+
+  // part 2: add the object to gazebo
+  /* Define the primitive and its dimensions *again*. */
   shape_msgs::SolidPrimitive table2;
   table2.type = shape_msgs::SolidPrimitive::BOX;
   table2.dimensions = collision_objects[1].primitives[0].dimensions;
 
+  /* Define the pose of the table *again*. */
   geometry_msgs::Pose table2pose;
   table2pose.position.x = collision_objects[1].primitive_poses[0].position.x;
   table2pose.position.y = collision_objects[1].primitive_poses[0].position.y;
   table2pose.position.z = collision_objects[1].primitive_poses[0].position.z;
+  table2pose.orientation.w = 1; // the orientation is always the default
 
-  // the orientation is always the default
-  table2pose.orientation.w = 1;
-
+  /* spawn the table in Gazebo, weighing 20 kg, but without physics enabled to increase stability */
   GazeboZoneSpawner::spawnPrimitive("table2", table2, table2pose, std_msgs::ColorRGBA(), 20, true);
-  // END_SUB_TUTORIAL
-
-  collision_objects[1].operation = collision_objects[1].ADD;
 
-  // BEGIN_SUB_TUTORIAL object
+  /********************************************************************************
+   * object (the object to be picked and placed)
+   ********************************************************************************/
   // Define the object that we will be manipulating
   collision_objects[2].header.frame_id = "panda_link0";
   collision_objects[2].id = "object";
@@ -296,23 +322,25 @@ void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface& pla
   collision_objects[2].primitive_poses[0].position.z = 0.5;
   collision_objects[2].primitive_poses[0].orientation.w = 1.0;
 
+  collision_objects[2].operation = collision_objects[2].ADD;
+
+  // part 2: add the object to gazebo
+  /* Define the primitive and its dimensions *again*. */
   shape_msgs::SolidPrimitive object;
   object.type = shape_msgs::SolidPrimitive::BOX;
   object.dimensions = collision_objects[2].primitives[0].dimensions;
 
+  /* Define the pose of the object *again*. */
   geometry_msgs::Pose objectpose;
   objectpose.position.x = collision_objects[2].primitive_poses[0].position.x;
   objectpose.position.y = collision_objects[2].primitive_poses[0].position.y;
   objectpose.position.z = collision_objects[2].primitive_poses[0].position.z;
+  objectpose.orientation.w = 1; // the orientation is always the default
 
-  // the orientation is always the default
-  objectpose.orientation.w = 1;
-
-  GazeboZoneSpawner::spawnPrimitive("pickObject", object, objectpose, std_msgs::ColorRGBA(), 1);
-  // END_SUB_TUTORIAL
-
-  collision_objects[2].operation = collision_objects[2].ADD;
+  /* spawn the object in Gazebo, weighing 1 kg, with physics enabled */
+  GazeboZoneSpawner::spawnPrimitive("pickObject", object, objectpose, std_msgs::ColorRGBA(), 1, false);
 
+  // now that we have defined all three objects, we add them to the planning scene
   planning_scene_interface.applyCollisionObjects(collision_objects);
 }
 
@@ -322,8 +350,8 @@ int main(int argc, char** argv)
   ros::NodeHandle nh;
   ros::AsyncSpinner spinner(1);
 
-  ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
-  ros::Duration(5.0).sleep();
+  ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<");
+  ros::Duration(3.0).sleep();
   ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
 
   spinner.start();