Skip to content
Snippets Groups Projects
Select Git revision
  • d898ba2e844722f6ce8346f52fd2e66dc2abf1d4
  • master default protected
  • grasping_sample
3 results

PickPlacePlanner.cpp

Blame
  • PickPlacePlanner.cpp 15.67 KiB
    /*********************************************************************
    * 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;
    }