Select Git revision
PickPlacePlanner.cpp
Forked from
CeTI / ROS / ROS Packages / sample_applications
3 commits behind the upstream repository.

Johannes Mey authored
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;
}