fixed/testes grasping from side, added approach vector as param

include/grasp_util.h

@@ -122,7 +122,7 @@ public:
      * @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,
+    getPickFromSidePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Quaternion gripper_orientation, tf2::Quaternion approach_orientation, float y_distance_diff, float 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.
@@ -138,18 +138,30 @@ public:
                        geometry_msgs::Vector3 dimensions, double open_amount, std::string supporting_surface_id,
                        std::string object_to_pick_id, bool plan_only = false);
 
+    /**
+     *
+     * @param move_group
+     * @param pick_pose
+     * @param object_to_pick
+     * @param open_amount
+     * @param supporting_surface_id
+     * @param object_to_pick_id
+     * @param plan_only
+     * @return
+     */
     bool pickFromSide(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
-                            shape_msgs::SolidPrimitive &object_to_pick,
+                            shape_msgs::SolidPrimitive &object_to_pick, geometry_msgs::Vector3 &approach_direction,
                             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
-     * @param place_pose target place pose for the object
-     * @param close_amount total close amount (space between "forks")
-     * @param open_amount total opening amount (space between "forks")
-     * @param supporting_surface_id ID of surface where object is placed on (e.g. the table)
-     * @param object_to_place_id ID of the object to place
+     *
+     * @param move_group
+     * @param place_pose
+     * @param open_amount
+     * @param supporting_surface_id
+     * @param object_to_place_id
+     * @param plan_only
+     * @return
      */
     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);

src/grasping/grasp_util.cpp

@@ -35,7 +35,7 @@ GraspUtil::getPickFromMinusXSidePose(geometry_msgs::Pose &object_pose_msg, geome
     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;
+    approach_yaw =   -M_PI_4;
     tf2::Quaternion approach_orientation;
     approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
 
@@ -90,7 +90,7 @@ GraspUtil::getPickFromPlusYSidePose(geometry_msgs::Pose &object_pose_msg, geomet
 
 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,
+                    tf2::Quaternion approach_orientation, float y_distance_diff, float x_distance_diff, tf2::Quaternion object_orientation,
                     double reach){
 
     // object orientation
@@ -156,7 +156,7 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m
 
 bool
 GraspUtil::pickFromSide(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
-                        shape_msgs::SolidPrimitive &object_to_pick,
+                        shape_msgs::SolidPrimitive &object_to_pick, geometry_msgs::Vector3 &approach_direction,
                          double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only) {
 
     std::vector<moveit_msgs::Grasp> grasps;
@@ -164,9 +164,12 @@ GraspUtil::pickFromSide(moveit::planning_interface::MoveGroupInterface &move_gro
     grasps[0].grasp_pose.header.frame_id = "panda_link0";
     grasps[0].grasp_pose.pose = pick_pose;
 
+    geometry_msgs::Vector3Stamped stamped_approach_dir;
+    stamped_approach_dir.vector = approach_direction;
+
     // 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.direction = stamped_approach_dir;
     grasps[0].pre_grasp_approach.min_distance = 0.095;
     grasps[0].pre_grasp_approach.desired_distance = 0.115;
 
@@ -274,6 +277,7 @@ GraspUtil::placeFromSide(moveit::planning_interface::MoveGroupInterface &move_gr
     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].place_pose.pose.position.z = place_location[0].place_pose.pose.position.z;
 
     place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0";
     place_location[0].pre_place_approach.direction.vector.z = -1.0;
@@ -281,7 +285,7 @@ GraspUtil::placeFromSide(moveit::planning_interface::MoveGroupInterface &move_gr
     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.direction.vector.z = 1.0;
     place_location[0].post_place_retreat.min_distance = 0.1;
     place_location[0].post_place_retreat.desired_distance = 0.25;