Merge branch 'improvement/grasping' into 'master'

Improvement/grasping

See merge request ceti/ros-internal/sample_applications!2

CMakeLists.txt

@@ -24,7 +24,8 @@ find_package(catkin REQUIRED
         moveit_ros_planning
         moveit_ros_planning_interface
         controller_interface
-        hardware_interface)
+        hardware_interface
+        simulation_util)
 
 # System dependencies are found with CMake's conventions
 find_package(PkgConfig REQUIRED)
@@ -40,6 +41,7 @@ catkin_package(CATKIN_DEPENDS
         controller_interface
         hardware_interface
         pluginlib
+        simulation_util
         DEPENDS
         # system_lib
         )
@@ -55,9 +57,13 @@ add_executable(SampleConstraintPlanner src/SampleConstraintPlanner.cpp)
 add_executable(SampleTimedCartesianPlanner src/SampleTimedCartesianPlanner.cpp)
 add_executable(SampleSimpleMotion src/SampleSimpleMotion.cpp)
 add_executable(MinimalSimpleMotion src/MinimalSimpleMotion.cpp)
+add_executable(${PROJECT_NAME}_pick_place src/pick_place_tutorial.cpp)
+
+set_target_properties(${PROJECT_NAME}_pick_place PROPERTIES OUTPUT_NAME pick_place PREFIX "")
 
 # Specify libraries to link a library or executable target against
 target_link_libraries(SampleConstraintPlanner ${catkin_LIBRARIES})
 target_link_libraries(SampleTimedCartesianPlanner ${catkin_LIBRARIES})
 target_link_libraries(SampleSimpleMotion ${catkin_LIBRARIES})
 target_link_libraries(MinimalSimpleMotion ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME}_pick_place ${catkin_LIBRARIES})

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.

launch/pick_place_simulation.launch

+<launch>
+
+    <include file="$(find panda_simulation)/launch/simulation.launch"/>
+
+<!--    <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />-->
+
+<!--    <rosparam file="$(find sample_applications)/config/config.yaml" command="load" />-->
+<!--    <node name="safety_zone_spawner" pkg="sample_applications" type="safety_zone_spawner" respawn="false" output="screen"/>-->
+
+    <node pkg="sample_applications" type="pick_place" name="pick_place" />
+<!--    <node name="rqt_console" pkg="rqt_console" type="rqt_console" />-->
+
+</launch>

package.xml

@@ -40,6 +40,7 @@
   <depend>moveit_visual_tools</depend>
   <depend>controller_interface</depend>
   <depend>hardware_interface</depend>
+  <depend>simulation_util</depend>
 
   <exec_depend>pluginlib</exec_depend>
 

src/pick_place_tutorial.cpp

+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2012, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* 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>
+
+// MoveIt
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+
+// TF2
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+// 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
+  /* Add both finger joints of panda robot. */
+  posture.joint_names.resize(2);
+  posture.joint_names[0] = "panda_finger_joint1";
+  posture.joint_names[1] = "panda_finger_joint2";
+
+  /* Set them as open, wide enough for the object to fit. */
+  posture.points.resize(1);
+  posture.points[0].positions.resize(2);
+  posture.points[0].positions[0] = 0.04;
+  posture.points[0].positions[1] = 0.04;
+  posture.points[0].time_from_start = ros::Duration(0.5);
+  // 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)
+{
+  /* Add both finger joints of panda robot. */
+  posture.joint_names.resize(2);
+  posture.joint_names[0] = "panda_finger_joint1";
+  posture.joint_names[1] = "panda_finger_joint2";
+
+  /* Set them as closed. */
+  posture.points.resize(1);
+  posture.points[0].positions.resize(2);
+  posture.points[0].positions[0] = 0.00;
+  posture.points[0].positions[1] = 0.00;
+  posture.points[0].time_from_start = ros::Duration(0.5);
+}
+
+/**
+ * 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)
+{
+  // 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;
+  grasps.resize(1);
+
+  // Setting grasp pose
+  // ++++++++++++++++++++++
+  // This is the pose of panda_link8. |br|
+  // Make sure that when you set the grasp_pose, you are setting it to be the pose of the last link in
+  // your manipulator which in this case would be `"panda_link8"` You will have to compensate for the
+  // transform from `"panda_link8"` to the palm of the end effector.
+  grasps[0].grasp_pose.header.frame_id = "panda_link0";
+  tf2::Quaternion orientation;
+  orientation.setRPY(-M_PI / 2, -M_PI / 4, -M_PI / 2);
+  grasps[0].grasp_pose.pose.orientation = tf2::toMsg(orientation);
+  grasps[0].grasp_pose.pose.position.x = 0.4;
+  grasps[0].grasp_pose.pose.position.y = 0;
+  grasps[0].grasp_pose.pose.position.z = 0.5;
+
+  // Setting pre-grasp approach
+  // ++++++++++++++++++++++++++
+  /* Defined with respect to frame_id */
+  grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";
+  /* Direction is set as positive x axis */
+  grasps[0].pre_grasp_approach.direction.vector.x = 1.0;
+  grasps[0].pre_grasp_approach.min_distance = 0.095;
+  grasps[0].pre_grasp_approach.desired_distance = 0.115;
+
+  // Setting post-grasp retreat
+  // ++++++++++++++++++++++++++
+  /* Defined with respect to frame_id */
+  grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";
+  /* Direction is set as positive z axis */
+  grasps[0].post_grasp_retreat.direction.vector.z = 1.0;
+  grasps[0].post_grasp_retreat.min_distance = 0.1;
+  grasps[0].post_grasp_retreat.desired_distance = 0.25;
+
+  // Setting posture of eef before grasp
+  // +++++++++++++++++++++++++++++++++++
+  openGripper(grasps[0].pre_grasp_posture);
+
+  // Setting posture of eef during grasp
+  // +++++++++++++++++++++++++++++++++++
+  closedGripper(grasps[0].grasp_posture);
+
+  // 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);
+}
+
+/**
+ * Place an object at a fixed position on top of table2.
+ * @param group
+ */
+void place(moveit::planning_interface::MoveGroupInterface& group)
+{
+  std::vector<moveit_msgs::PlaceLocation> place_location;
+  place_location.resize(1);
+
+  // Setting place location pose
+  // +++++++++++++++++++++++++++
+  place_location[0].place_pose.header.frame_id = "panda_link0";
+  tf2::Quaternion orientation;
+  orientation.setRPY(0, 0, M_PI / 2);
+  place_location[0].place_pose.pose.orientation = tf2::toMsg(orientation);
+
+  /* For place location, we set the value to the exact location of the center of the object. */
+  place_location[0].place_pose.pose.position.x = 0;
+  place_location[0].place_pose.pose.position.y = 0.5;
+  place_location[0].place_pose.pose.position.z = 0.5;
+
+  // Setting pre-place approach
+  // ++++++++++++++++++++++++++
+  /* Defined with respect to frame_id */
+  place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0";
+  /* Direction is set as negative z axis */
+  place_location[0].pre_place_approach.direction.vector.z = -1.0;
+  place_location[0].pre_place_approach.min_distance = 0.095;
+  place_location[0].pre_place_approach.desired_distance = 0.115;
+
+  // Setting post-grasp retreat
+  // ++++++++++++++++++++++++++
+  /* Defined with respect to frame_id */
+  place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0";
+  /* Direction is set as negative y axis */
+  place_location[0].post_place_retreat.direction.vector.y = -1.0;
+  place_location[0].post_place_retreat.min_distance = 0.1;
+  place_location[0].post_place_retreat.desired_distance = 0.25;
+
+  // Setting posture of eef after placing object
+  // +++++++++++++++++++++++++++++++++++++++++++
+  /* Similar to the pick case */
+  openGripper(place_location[0].post_place_posture);
+
+  // Set support surface as table2.
+  group.setSupportSurfaceName("table2");
+  // Call place to place the object using the place locations given.
+  group.place("object", place_location);
+}
+
+/**
+ * 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)
+{
+  // 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);
+
+  /********************************************************************************
+   * 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";
+
+  /* Define the primitive and its dimensions. */
+  collision_objects[0].primitives.resize(1);
+  collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX;
+  collision_objects[0].primitives[0].dimensions.resize(3);
+  collision_objects[0].primitives[0].dimensions[0] = 0.2;
+  collision_objects[0].primitives[0].dimensions[1] = 0.4;
+  collision_objects[0].primitives[0].dimensions[2] = 0.4;
+
+  /* Define the pose of the table. */
+  collision_objects[0].primitive_poses.resize(1);
+  collision_objects[0].primitive_poses[0].position.x = 0.5;
+  collision_objects[0].primitive_poses[0].position.y = 0;
+  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
+
+  /* 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);
+
+  /********************************************************************************
+   * table2 (the one to which the pick object is move to by the operation)
+   ********************************************************************************/
+
+  // part 1: add the object to the scene
+  collision_objects[1].id = "table2";
+  collision_objects[1].header.frame_id = "panda_link0";
+
+  /* Define the primitive and its dimensions. */
+  collision_objects[1].primitives.resize(1);
+  collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX;
+  collision_objects[1].primitives[0].dimensions.resize(3);
+  collision_objects[1].primitives[0].dimensions[0] = 0.4;
+  collision_objects[1].primitives[0].dimensions[1] = 0.2;
+  collision_objects[1].primitives[0].dimensions[2] = 0.4;
+
+  /* Define the pose of the table. */
+  collision_objects[1].primitive_poses.resize(1);
+  collision_objects[1].primitive_poses[0].position.x = 0;
+  collision_objects[1].primitive_poses[0].position.y = 0.5;
+  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
+
+  /* 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);
+
+  /********************************************************************************
+   * 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";
+
+  /* Define the primitive and its dimensions. */
+  collision_objects[2].primitives.resize(1);
+  collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].BOX;
+  collision_objects[2].primitives[0].dimensions.resize(3);
+  collision_objects[2].primitives[0].dimensions[0] = 0.02;
+  collision_objects[2].primitives[0].dimensions[1] = 0.02;
+  collision_objects[2].primitives[0].dimensions[2] = 0.2;
+
+  /* Define the pose of the object. */
+  collision_objects[2].primitive_poses.resize(1);
+  collision_objects[2].primitive_poses[0].position.x = 0.5;
+  collision_objects[2].primitive_poses[0].position.y = 0;
+  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
+
+  /* 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);
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "panda_arm_pick_place");
+  ros::NodeHandle nh;
+  ros::AsyncSpinner spinner(1);
+
+  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();
+
+  ros::WallDuration(1.0).sleep();
+  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+  moveit::planning_interface::MoveGroupInterface group("panda_arm");
+  group.setPlanningTime(45.0);
+
+  addCollisionObjects(planning_scene_interface);
+
+  // Wait a bit for ROS things to initialize
+  ros::WallDuration(1.0).sleep();
+
+  pick(group);
+
+  ros::WallDuration(1.0).sleep();
+
+  place(group);
+
+  ros::waitForShutdown();
+  return 0;
+}