diff --git a/CMakeLists.txt b/CMakeLists.txt index 2b315d3ee71d0bb78402ba2a107db379b880afb0..5ecece107760633fd9a9060fc7b30b7e99cebd54 100644 --- a/CMakeLists.txt +++ b/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}) 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/launch/pick_place_simulation.launch b/launch/pick_place_simulation.launch new file mode 100644 index 0000000000000000000000000000000000000000..6712abcd9ff9c199bc9fe4ced226344b7e4bb591 --- /dev/null +++ b/launch/pick_place_simulation.launch @@ -0,0 +1,13 @@ +<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> diff --git a/package.xml b/package.xml index 610aabae5c5631e24ae10db34a7b81b5a0ccbcb7..8874bb3bbb2167c7ad1fae18158f56ee771480c3 100644 --- a/package.xml +++ b/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> diff --git a/src/pick_place_tutorial.cpp b/src/pick_place_tutorial.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4f236a0e5e653790409bdcd798638c2b17adedf0 --- /dev/null +++ b/src/pick_place_tutorial.cpp @@ -0,0 +1,377 @@ +/********************************************************************* +* 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; +}