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/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..f29e72c1ed8382b0eb1d97ff3e39bb929469256c --- /dev/null +++ b/src/pick_place_tutorial.cpp @@ -0,0 +1,349 @@ +/********************************************************************* +* 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> + +// Zone Spawner +#include <GazeboZoneSpawner.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| + // 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); + // 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); + // 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. |br| + // |br| + // Ideally, you would create a vector of place locations to be attempted although in this example, we only create + // a 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); + + /* 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); + // 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; + collision_objects[0].primitive_poses[0].orientation.w = 1.0; + + shape_msgs::SolidPrimitive table1; + table1.type = shape_msgs::SolidPrimitive::BOX; + table1.dimensions = collision_objects[0].primitives[0].dimensions; + + 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; + + // the orientation is always the default + table1pose.orientation.w = 1; + + GazeboZoneSpawner::spawnPrimitive("table1", table1, table1pose, 20, true); + // 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; + collision_objects[1].primitive_poses[0].orientation.w = 1.0; + + shape_msgs::SolidPrimitive table2; + table2.type = shape_msgs::SolidPrimitive::BOX; + table2.dimensions = collision_objects[1].primitives[0].dimensions; + + 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; + + // the orientation is always the default + table2pose.orientation.w = 1; + + GazeboZoneSpawner::spawnPrimitive("table2", table2, table2pose, 20, true); + // 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; + collision_objects[2].primitive_poses[0].orientation.w = 1.0; + + shape_msgs::SolidPrimitive object; + object.type = shape_msgs::SolidPrimitive::BOX; + object.dimensions = collision_objects[2].primitives[0].dimensions; + + 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; + + // the orientation is always the default + objectpose.orientation.w = 1; + + GazeboZoneSpawner::spawnPrimitive("pickObject", object, objectpose, 1); + // 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); + + ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<"); + ros::Duration(5.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; +}