working on integration of new features

include/grasp_util.h

@@ -76,7 +76,7 @@ public:
      */
     bool pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
                        geometry_msgs::Vector3 dimensions, double open_amount, std::string supporting_surface_id,
-                       std::string object_to_pick_id, bool plan_only = false);
+                       std::string object_to_pick_id, bool plan_only = false, int max_planning_retries = 1);
 
     /**
      * Util method to place an object from above.
@@ -88,7 +88,19 @@ public:
      * @param object_to_place_id ID of the object to place
      */
     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);
+                        double open_amount, std::string supporting_surface_id, std::string object_to_place_id,
+                        bool plan_only = false, int max_planning_retries = 1);
+
+    /**
+     * TODO: TEST
+     * @param move_group
+     * @param object_pose_msg
+     * @param open_amount
+     * @param dimensions
+     * @return
+     */
+    bool pickObjectOnRobotfrontFromSide(moveit::planning_interface::MoveGroupInterface &move_group, std::string object_to_pick_id,
+            geometry_msgs::Pose &object_pose_msg, double open_amount, geometry_msgs::Vector3 dimensions, int max_planning_retries = 1);
 
 };
 

src/grasping/grasp_util.cpp

@@ -72,10 +72,82 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m
     return target_pose;
 }
 
+bool GraspUtil::pickObjectOnRobotfrontFromSide(moveit::planning_interface::MoveGroupInterface &move_group, std::string object_to_pick_id,
+        geometry_msgs::Pose &object_pose_msg, double open_amount, geometry_msgs::Vector3 dimensions, int max_planning_retries) {
+
+
+    std::vector<moveit_msgs::Grasp> grasps;
+    grasps.resize(1);
+
+    grasps[0].grasp_pose.header.frame_id = "panda_link0";
+
+    tf2::Quaternion object_orientation;
+    tf2::fromMsg(object_pose_msg.orientation, object_orientation);
+
+    tf2::Quaternion gripper_orientation;
+
+    // We know the gripper is rotated -45 degrees from the urdf model
+    gripper_orientation.setRPY(0, 0, -M_PI_4);
+
+    // approach orientation
+    double approach_roll, approach_pitch, approach_yaw;
+    approach_roll = -M_PI_2; // The neutral rotation has the gripper "looking down",
+    approach_pitch = -M_PI_4; // with these transformations, it grasps from one side.
+    approach_yaw = -M_PI_4; // This results in (x=-0.5,y=-0.5,z=0,w=0.7071068)
+    tf2::Quaternion approach_orientation;
+    approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
+
+    tf2::Quaternion orientation = object_orientation * gripper_orientation * approach_orientation;
+
+    grasps[0].grasp_pose.pose.orientation = tf2::toMsg(orientation);
+    grasps[0].grasp_pose.pose.position.x = object_pose_msg.position.x - 0.1;
+    grasps[0].grasp_pose.pose.position.y = object_pose_msg.position.y;
+    grasps[0].grasp_pose.pose.position.z = object_pose_msg.position.z + 0.03;
+
+    grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";
+    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;
+
+    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.15;
+
+    setupOpenGripper(grasps[0].pre_grasp_posture, open_amount);
+
+    if (dimensions.x <= 0.08) {
+        setupClosedGripper(grasps[0].pre_grasp_posture, dimensions.x);
+    }
+
+    if (dimensions.x > 0.08 && dimensions.y <= 0.08) {
+        setupClosedGripper(grasps[0].pre_grasp_posture, dimensions.y);
+    }
+
+    if (dimensions.x > 0.08 && dimensions.y > 0.08) {
+        ROS_ERROR("Could not grasp: Object to big (in any dimension).");
+        return false;
+    }
+
+    int tries = 0;
+    bool success = false;
+
+    while(!success && tries < max_planning_retries){
+        success = move_group.pick(object_to_pick_id, grasps) == moveit_msgs::MoveItErrorCodes::SUCCESS;
+        if(!success) {
+            ROS_ERROR("[CC][Cobot1] Retrying pick");
+        }
+        tries++;
+    }
+
+    return success;
+}
+
 bool
 GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
                          geometry_msgs::Vector3 dimensions,
-                         double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only) {
+                         double open_amount, std::string supporting_surface_id,
+                         std::string object_to_pick_id, bool plan_only, int max_planning_retries) {
 
     std::vector<moveit_msgs::Grasp> grasps;
     grasps.resize(1);
@@ -113,19 +185,25 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr
 
     move_group.setSupportSurfaceName(supporting_surface_id);
 
-    moveit_msgs::MoveItErrorCodes miec = move_group.pick(object_to_pick_id, grasps, plan_only);
+    int tries = 0;
+    bool success = false;
 
-    if (miec.val == miec.SUCCESS) {
-        return true;
+    while(!success && tries < max_planning_retries){
+        success = move_group.pick(object_to_pick_id, grasps, plan_only) == moveit_msgs::MoveItErrorCodes::SUCCESS;
+        if(!success) {
+            ROS_ERROR("[CC][Cobot1] Retrying pick");
+        }
+        tries++;
     }
 
-    ROS_ERROR_STREAM("Error while executing pick task. MoveItErrorCode: " << miec.val);
-    return false;
+    ROS_ERROR_STREAM("Error while executing pick task.");
+    return success;
 }
 
 bool
 GraspUtil::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) {
+                          double open_amount, std::string supporting_surface_id,
+                          std::string object_to_place_id, bool plan_only, int max_planning_retries) {
 
     std::vector<moveit_msgs::PlaceLocation> place_location;
     place_location.resize(1);
@@ -149,12 +227,17 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g
     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);
+    int tries = 0;
+    bool success = false;
 
-    if (miec.val == miec.SUCCESS) {
-        return true;
+    while(!success && tries < max_planning_retries){
+        success = move_group.place(object_to_place_id, place_location, plan_only) == moveit_msgs::MoveItErrorCodes::SUCCESS;
+        if(!success) {
+            ROS_ERROR("[CC][Cobot1] Retry bottle pick");
+        }
+        tries++;
     }
 
-    ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode: " << miec.val);
-    return false;
+    ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode");
+    return success;
 }