fix misconception of height parameter, add constants from franka manual

src/grasping/grasp_service.cpp

@@ -66,15 +66,11 @@ bool pickObject(panda_grasping::PickObject::Request &req,
         }
 
         ROS_INFO("Retrieving the orientation of the gripper wrt. to the hand.");
-        geometry_msgs::TransformStamped transformStamped = tfBuffer.lookupTransform("panda_hand", "panda_link8",
-                                                                                    ros::Time(0));
-        tf2::Quaternion orientation_gripper{transformStamped.transform.rotation.x,
-                                            transformStamped.transform.rotation.y,
-                                            transformStamped.transform.rotation.z,
-                                            transformStamped.transform.rotation.w};
+        geometry_msgs::Transform gripper_transform = tfBuffer.lookupTransform("panda_hand", "panda_link8",
+                                                                                     ros::Time(0)).transform;
 
         ROS_INFO("Computing pre-grasp-pose.");
-        geometry_msgs::Pose pick_pose = grasp_util.getPickFromAbovePose(req.pose, req.dimensions, orientation_gripper);
+        geometry_msgs::Pose pick_pose = grasp_util.getPickFromAbovePose(req.pose, req.dimensions, gripper_transform.rotation);
 
         ROS_INFO("Picking up ...");
         // 0.08 = 8cm = maximum gripper opening

src/grasping/grasp_util.cpp

@@ -37,28 +37,45 @@ void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory &posture, do
 }
 
 geometry_msgs::Pose
-GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Vector3 dimensions,
-                                tf2::Quaternion orientation_gripper, double height) {
-
-    geometry_msgs::Pose target_pose;
-
-    tf2::Quaternion orientation_object;
-    tf2::fromMsg(object_to_pick_pose.orientation, orientation_object);
-
-    double eef_roll, eef_pitch, eef_yaw;
-    tf2::Matrix3x3(orientation_gripper).getRPY(eef_roll, eef_pitch, eef_yaw);
-
-    double o_roll, o_pitch, o_yaw;
-    tf2::Matrix3x3(orientation_object).getRPY(o_roll, o_pitch, o_yaw);
-
-    tf2::Quaternion orientation;
-    orientation.setRPY(M_PI, 0, o_yaw + eef_yaw);
+GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions,
+                                geometry_msgs::Quaternion gripper_orientation_msg, double reach) {
+
+    // object orientation
+    tf2::Quaternion object_orientation;
+    tf2::fromMsg(object_pose_msg.orientation, object_orientation);
+
+    // gripper orientation
+    tf2::Quaternion gripper_orientation;
+    tf2::fromMsg(gripper_orientation_msg, gripper_orientation);
+
+    // approach orientation
+    double approach_roll, approach_pitch, approach_yaw;
+    approach_roll = M_PI;  // turn upside down
+    approach_pitch = 0;
+    approach_yaw = 0;
+    tf2::Quaternion approach_orientation;
+    approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
+
+    tf2::Quaternion orientation = object_orientation * gripper_orientation * approach_orientation;
+
+    // Actually, the maximum reach should be FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE/2, but half a
+    // finger length is considered a safety distance.
+    if (reach > FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE) {
+        ROS_WARN_STREAM("Reach around object reduced from " << reach << " to " << FRANKA_GRIPPER_FINGER_LENGTH -
+                                                                                  FRANKA_GRIPPER_FINGERTIP_SIZE << ".");
+        reach = FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE;
+    } else if (reach < -FRANKA_GRIPPER_FINGERTIP_SIZE / 2) {
+        ROS_WARN_STREAM("Reach around object increased from " << reach << " to 0." << ". Minimum allowed reach is "
+                                                              << (-FRANKA_GRIPPER_FINGERTIP_SIZE / 2) << ".");
+        reach = 0;
+    }
 
     // The pose is 10 cm straight above the object to pick. This only works on upright boxes.
+    geometry_msgs::Pose target_pose;
     target_pose.orientation = tf2::toMsg(orientation);
-    target_pose.position.x = object_to_pick_pose.position.x;
-    target_pose.position.y = object_to_pick_pose.position.y;
-    target_pose.position.z = object_to_pick_pose.position.z + 0.5 * dimensions.z + height;
+    target_pose.position.x = object_pose_msg.position.x;
+    target_pose.position.y = object_pose_msg.position.y;
+    target_pose.position.z = object_pose_msg.position.z + (0.5 * dimensions.z) + FRANKA_GRIPPER_DISTANCE - reach;
 
     return target_pose;
 }

src/grasping/grasp_util.h

@@ -11,7 +11,6 @@
 #include <moveit/move_group_interface/move_group_interface.h>
 
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-//#include <tf2_eigen/tf2_eigen.h>
 
 #include <trajectory_msgs/JointTrajectory.h>
 #include <geometry_msgs/Pose.h>
@@ -23,6 +22,21 @@ class GraspUtil {
 
 public:
 
+    /**
+     * distance of the center of the hand to the center of the finger tip
+     */
+    static constexpr tf2Scalar FRANKA_GRIPPER_DISTANCE{0.1034};
+
+    /**
+     * length of a finger
+     */
+    static constexpr tf2Scalar FRANKA_GRIPPER_FINGER_LENGTH{0.054};
+
+    /**
+     * size of the finger tip
+     */
+    static constexpr tf2Scalar FRANKA_GRIPPER_FINGERTIP_SIZE{0.018};
+
     /**
      * Sets up how far the gripper should be opened for the next action.
      * @param posture empty posture object defining grasp
@@ -41,13 +55,14 @@ public:
      * Compute the "pre-pick"-pose
      * @param object_to_pick_pose pose of the object to pick (currently only vertically oriented objects are supported)
      * @param dimensions of the object
-     * @param orientation_gripper current gripper orientation relative to the link it is attached to
-     * @param height specifies how much the pose is above the object
+     * @param gripper_orientation current gripper orientation relative to the link it is attached to
+     * @param reach specifies how far the gripper reaches around the object (0 means the center of the finger tip is at the object's corner)
      * @return the "pre-pick"-pose
      */
     geometry_msgs::Pose
     getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Vector3 dimensions,
-                         tf2::Quaternion orientation_gripper, double height=0.1);
+                         geometry_msgs::Quaternion gripper_orientation,
+                         double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
 
     /**
      * Util method to pick an object from above.