diff --git a/README.md b/README.md index 788e53dfce4cc9d5f373cfd79b0162da482cddf1..851da8b52ea381bbe605ac08469e797a8193ef58 100644 --- a/README.md +++ b/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. diff --git a/src/pick_place_tutorial.cpp b/src/pick_place_tutorial.cpp index 41503c40628ffc5190069b28933014e52a631c95..4f236a0e5e653790409bdcd798638c2b17adedf0 100644 --- a/src/pick_place_tutorial.cpp +++ b/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();