/********************************************************************* * 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; }