basic grasping from the side, helper methods to grasp from 4 sides

include/grasp_util.h

@@ -61,9 +61,69 @@ public:
      */
     geometry_msgs::Pose
     getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Vector3 dimensions,
-                         geometry_msgs::Quaternion gripper_orientation,
-                         double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
+                         geometry_msgs::Quaternion gripper_orientation, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
 
+    /**
+     *
+     * @param object_pose_msg
+     * @param gripper_orientation_msg
+     * @param object_orientation
+     * @param reach
+     * @return
+     */
+    geometry_msgs::Pose
+    getPickFromMinusXSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg,
+                                         tf2::Quaternion object_orientation = {0, 0, 0, 1}, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
+
+    /**
+     *
+     * @param object_pose_msg
+     * @param gripper_orientation_msg
+     * @param object_orientation
+     * @param reach
+     * @return
+     */
+    geometry_msgs::Pose
+    getPickFromPlusXSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg,
+                                        tf2::Quaternion object_orientation = {0, 0, 0, 1}, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
+    /**
+     *
+     * @param object_pose_msg
+     * @param gripper_orientation_msg
+     * @param object_orientation
+     * @param reach
+     * @return
+     */
+    geometry_msgs::Pose
+    getPickFromMinusYSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg,
+                                         tf2::Quaternion object_orientation = {0, 0, 0, 1}, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
+
+    /**
+     *
+     * @param object_pose_msg
+     * @param gripper_orientation_msg
+     * @param object_orientation
+     * @param reach
+     * @return
+     */
+    geometry_msgs::Pose
+    getPickFromPlusYSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg,
+                                        tf2::Quaternion object_orientation = {0, 0, 0, 1}, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
+
+    /**
+     *
+     * @param object_to_pick_pose
+     * @param gripper_orientation
+     * @param approach_orientation
+     * @param y_distance_diff
+     * @param x_distance_diff
+     * @param object_orientation
+     * @param reach
+     * @return
+     */
+    geometry_msgs::Pose
+    getPickFromSidePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Quaternion gripper_orientation, tf2::Quaternion approach_orientation, int y_distance_diff, int x_distance_diff,
+                         tf2::Quaternion object_orientation = {0, 0, 0, 1}, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
     /**
      * Util method to pick an object from above.
      * @param move_group an initialized MoveGroupInterface instance
@@ -78,6 +138,10 @@ public:
                        geometry_msgs::Vector3 dimensions, double open_amount, std::string supporting_surface_id,
                        std::string object_to_pick_id, bool plan_only = false);
 
+    bool pickFromSide(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
+                            shape_msgs::SolidPrimitive &object_to_pick,
+                            double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only = false);
+
     /**
      * Util method to place an object from above.
      * @param move_group an initialized MoveGroupInterface instance
@@ -90,6 +154,18 @@ public:
     bool placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose,
                         double open_amount, std::string supporting_surface_id, std::string object_to_place_id, bool plan_only = false);
 
+    /**
+     *
+     * @param move_group
+     * @param place_pose
+     * @param open_amount
+     * @param supporting_surface_id
+     * @param object_to_place_id
+     * @param plan_only
+     * @return
+     */
+    bool placeFromSide(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose,
+                        double open_amount, std::string supporting_surface_id, std::string object_to_place_id, bool plan_only = false);
 };
 
 #endif //PANDA_GRASPING_GRASP_UTIL_H

src/grasping/grasp_util.cpp

@@ -28,6 +28,88 @@ void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory &posture, do
     posture.points[0].time_from_start = ros::Duration(0.5);
 }
 
+geometry_msgs::Pose
+GraspUtil::getPickFromMinusXSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg,
+        tf2::Quaternion object_orientation, double reach){
+
+    double approach_roll, approach_pitch, approach_yaw;
+    approach_roll =  -M_PI_2;
+    approach_pitch = -M_PI_4;
+    approach_yaw =   -M_PI_4 - M_PI_2;
+    tf2::Quaternion approach_orientation;
+    approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
+
+    return getPickFromSidePose(object_pose_msg, gripper_orientation_msg,
+            approach_orientation, 0, -0.1, object_orientation, reach);
+}
+
+geometry_msgs::Pose
+GraspUtil::getPickFromPlusXSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg,
+                                    tf2::Quaternion object_orientation, double reach){
+
+    double approach_roll, approach_pitch, approach_yaw;
+    approach_roll =  M_PI_2;
+    approach_pitch = -M_PI_4 - M_PI_2;
+    approach_yaw =   -M_PI_4;
+    tf2::Quaternion approach_orientation;
+    approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
+
+    return getPickFromSidePose(object_pose_msg, gripper_orientation_msg,
+                               approach_orientation, 0, 0.1, object_orientation, reach);
+}
+
+geometry_msgs::Pose
+GraspUtil::getPickFromMinusYSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg,
+                                     tf2::Quaternion object_orientation, double reach){
+
+    double approach_roll, approach_pitch, approach_yaw;
+    approach_roll =  M_PI_2;
+    approach_pitch = -M_PI_4 - M_PI_2;
+    approach_yaw =   -M_PI_4 - M_PI_2;
+    tf2::Quaternion approach_orientation;
+    approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
+
+    return getPickFromSidePose(object_pose_msg, gripper_orientation_msg,
+                               approach_orientation, -0.1, 0, object_orientation, reach);
+}
+
+geometry_msgs::Pose
+GraspUtil::getPickFromPlusYSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg,
+                                    tf2::Quaternion object_orientation, double reach){
+
+    double approach_roll, approach_pitch, approach_yaw;
+    approach_roll =  -M_PI_2;
+    approach_pitch = -M_PI_4;
+    approach_yaw =   -M_PI_4 - M_PI_2;
+    tf2::Quaternion approach_orientation;
+    approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
+
+    return getPickFromSidePose(object_pose_msg, gripper_orientation_msg,
+                               approach_orientation, 0.1, 0, object_orientation, reach);
+}
+
+geometry_msgs::Pose
+GraspUtil::getPickFromSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg,
+                    tf2::Quaternion approach_orientation, int y_distance_diff, int x_distance_diff, tf2::Quaternion object_orientation,
+                    double reach){
+
+    // object orientation
+    tf2::fromMsg(object_pose_msg.orientation, object_orientation);
+
+    // gripper orientation
+    tf2::Quaternion gripper_orientation;
+    tf2::fromMsg(gripper_orientation_msg, gripper_orientation);
+    tf2::Quaternion orientation = object_orientation * gripper_orientation * approach_orientation;
+
+    geometry_msgs::Pose target_pose;
+    target_pose.orientation = tf2::toMsg(orientation);
+    target_pose.position.x = object_pose_msg.position.x + x_distance_diff;
+    target_pose.position.y = object_pose_msg.position.y + y_distance_diff;
+    target_pose.position.z = object_pose_msg.position.z;
+
+    return target_pose;
+}
+
 geometry_msgs::Pose
 GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions,
                                 geometry_msgs::Quaternion gripper_orientation_msg, double reach) {
@@ -72,6 +154,66 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m
     return target_pose;
 }
 
+bool
+GraspUtil::pickFromSide(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
+                        shape_msgs::SolidPrimitive &object_to_pick,
+                         double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only) {
+
+    std::vector<moveit_msgs::Grasp> grasps;
+    grasps.resize(1);
+    grasps[0].grasp_pose.header.frame_id = "panda_link0";
+    grasps[0].grasp_pose.pose = pick_pose;
+
+    // Setup pre-grasp approach
+    grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";
+    grasps[0].pre_grasp_approach.direction.vector.z = -1.0;
+    grasps[0].pre_grasp_approach.min_distance = 0.095;
+    grasps[0].pre_grasp_approach.desired_distance = 0.115;
+
+    // Setup post-grasp retreat
+    grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";
+    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;
+
+    setupOpenGripper(grasps[0].pre_grasp_posture, open_amount);
+
+    if(object_to_pick.type == shape_msgs::SolidPrimitive::CYLINDER){
+
+        if(object_to_pick.dimensions[0] <= 0.08){
+            ROS_ERROR("Could not grasp: Cylinder object to big.");
+            return false;
+        }
+
+        setupClosedGripper(grasps[0].grasp_posture, object_to_pick.dimensions[1]);
+    }else{
+        if (object_to_pick.dimensions[0] <= 0.08) {
+            setupClosedGripper(grasps[0].grasp_posture, object_to_pick.dimensions[0]);
+        }
+
+        if (object_to_pick.dimensions[0] > 0.08 && object_to_pick.dimensions[1] <= 0.08) {
+            setupClosedGripper(grasps[0].grasp_posture, object_to_pick.dimensions[1]);
+        }
+
+        if (object_to_pick.dimensions[0] > 0.08 && object_to_pick.dimensions[1] > 0.08) {
+            ROS_ERROR("Could not grasp: Object to big (in any dimension).");
+            return false;
+        }
+    }
+
+    move_group.setSupportSurfaceName(supporting_surface_id);
+
+    moveit_msgs::MoveItErrorCodes miec = move_group.pick(object_to_pick_id, grasps, plan_only);
+
+    if (miec.val == miec.SUCCESS) {
+        return true;
+    }
+
+    ROS_ERROR_STREAM("Error while executing pick task. MoveItErrorCode: " << miec.val);
+
+    return false;
+}
+
 bool
 GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
                          geometry_msgs::Vector3 dimensions,
@@ -122,6 +264,39 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr
     ROS_ERROR_STREAM("Error while executing pick task. MoveItErrorCode: " << miec.val);
     return false;
 }
+bool
+GraspUtil::placeFromSide(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose,
+                   double open_amount, std::string supporting_surface_id, std::string object_to_place_id, bool plan_only){
+
+    std::vector<moveit_msgs::PlaceLocation> place_location;
+    place_location.resize(1);
+
+    place_location[0].place_pose.header.frame_id = "panda_link0";
+    place_location[0].place_pose.pose.orientation.w = 1;
+    place_location[0].place_pose.pose.position = place_pose.position;
+
+    place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0";
+    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;
+
+    place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0";
+    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;
+
+    setupOpenGripper(place_location[0].post_place_posture, open_amount);
+    move_group.setSupportSurfaceName(supporting_surface_id);
+
+    moveit_msgs::MoveItErrorCodes miec = move_group.place(object_to_place_id, place_location, plan_only);
+
+    if (miec.val == miec.SUCCESS) {
+        return true;
+    }
+
+    ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode: " << miec.val);
+    return false;
+}
 
 bool
 GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose,