files for grasping sample

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

launch/pick_place_sample.launch

+<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>

src/SamplePickAndPlace.cpp

+/*********************************************************************
+* 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