diff --git a/CMakeLists.txt b/CMakeLists.txt index 2b315d3ee71d0bb78402ba2a107db379b880afb0..99f190c9bb3eaa0b49240c4faf513638a7f39cfc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -55,9 +55,11 @@ 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(SamplePickAndPlace src/SamplePickAndPlace.cpp) # 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(SamplePickAndPlace ${catkin_LIBRARIES} ${Franka_LIBRARIES}) \ No newline at end of file diff --git a/launch/pick_place_sample.launch b/launch/pick_place_sample.launch new file mode 100644 index 0000000000000000000000000000000000000000..4873a39f8255a1532eaaa126978dc0a74c22455d --- /dev/null +++ b/launch/pick_place_sample.launch @@ -0,0 +1,11 @@ +<launch> + + <include file="$(find panda_simulation)/launch/simulation.launch"/> + + <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" /> + + <!--<node name="rqt_console" pkg="rqt_console" type="rqt_console" />--> + + <node name="panda_arm_pick_place" pkg="sample_applications" type="SamplePickAndPlace" respawn="false" output="screen"/> + +</launch> diff --git a/launch/simulation.launch b/launch/simulation.launch index adb361e603591121375703bad9bda5073962aaac..2c8041b93aa989de14033a8bd3a21b97fd75976c 100644 --- a/launch/simulation.launch +++ b/launch/simulation.launch @@ -5,5 +5,5 @@ <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" /> <node name="rqt_console" pkg="rqt_console" type="rqt_console" /> - -</launch> + +</launch> \ No newline at end of file diff --git a/src/SamplePickAndPlace.cpp b/src/SamplePickAndPlace.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1d41ac22901bc61d4e7cff3e5c10eb4e9d415bf2 --- /dev/null +++ b/src/SamplePickAndPlace.cpp @@ -0,0 +1,320 @@ +/********************************************************************* +* 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*/ + +// 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> + +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 +} + +void closedGripper(trajectory_msgs::JointTrajectory& posture) +{ + // BEGIN_SUB_TUTORIAL closed_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 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); + // END_SUB_TUTORIAL +} + +void pick(moveit::planning_interface::MoveGroupInterface& move_group) +{ + // BEGIN_SUB_TUTORIAL pick1 + // 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| + // From panda_link8 to the palm of the eef the distance is 0.058, the cube starts 0.01 before 5.0 (half of the length + // of the cube). |br| + // Therefore, the position for panda_link8 = 5 - (length of cube/2 - distance b/w panda_link8 and palm of eef - some + // extra padding) + 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.415; + 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); + // END_SUB_TUTORIAL + + // BEGIN_SUB_TUTORIAL pick2 + // Setting posture of eef during grasp + // +++++++++++++++++++++++++++++++++++ + closedGripper(grasps[0].grasp_posture); + // END_SUB_TUTORIAL + + // BEGIN_SUB_TUTORIAL pick3 + // 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); + move_group.attachObject("object"); + // END_SUB_TUTORIAL +} + +void place(moveit::planning_interface::MoveGroupInterface& group) +{ + // BEGIN_SUB_TUTORIAL place + // TODO(@ridhwanluthra) - Calling place function may lead to "All supplied place locations failed. Retrying last + // location in + // verbose mode." This is a known issue and we are working on fixing it. |br| + // Create a vector of placings to be attempted, currently only creating single place location. + 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); + + /* While placing it is 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); + // END_SUB_TUTORIAL +} + +void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface& planning_scene_interface) +{ + // BEGIN_SUB_TUTORIAL table1 + // + // Creating Environment + // ^^^^^^^^^^^^^^^^^^^^ + // Create vector to hold 3 collision objects. + std::vector<moveit_msgs::CollisionObject> collision_objects; + collision_objects.resize(3); + + // Add the first table where the cube will originally be kept. + 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; + // END_SUB_TUTORIAL + + collision_objects[0].operation = collision_objects[0].ADD; + + // BEGIN_SUB_TUTORIAL table2 + // Add the second table where we will be placing the cube. + 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; + // END_SUB_TUTORIAL + + collision_objects[1].operation = collision_objects[1].ADD; + + // BEGIN_SUB_TUTORIAL object + // 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; + // END_SUB_TUTORIAL + + collision_objects[2].operation = collision_objects[2].ADD; + + 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); + 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); + + std::cout << "TEST TEST TEST TEST TEST TEST TEST TEST TEST TEST" << std::endl; + 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; +} + +// BEGIN_TUTORIAL +// CALL_SUB_TUTORIAL table1 +// CALL_SUB_TUTORIAL table2 +// CALL_SUB_TUTORIAL object +// +// Pick Pipeline +// ^^^^^^^^^^^^^ +// CALL_SUB_TUTORIAL pick1 +// openGripper function +// """""""""""""""""""" +// CALL_SUB_TUTORIAL open_gripper +// CALL_SUB_TUTORIAL pick2 +// closedGripper function +// """""""""""""""""""""" +// CALL_SUB_TUTORIAL closed_gripper +// CALL_SUB_TUTORIAL pick3 +// +// Place Pipeline +// ^^^^^^^^^^^^^^ +// CALL_SUB_TUTORIAL place +// END_TUTORIAL