Bugfix/orientation (2)

src/grasping/environment_util.cpp

@@ -5,7 +5,8 @@
 
 #include "environment_util.h"
 
-void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension, double y_dimension) {
+void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension,
+                                     double y_dimension) {
 
     moveit_msgs::CollisionObject plate;
 
@@ -55,7 +56,8 @@ void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject>
     collision_objects.push_back(pick_support);
 }
 
-moveit_msgs::CollisionObject EnvironmentUtil::constructObjectToPick(std::vector<moveit_msgs::CollisionObject> &collision_objects,
+moveit_msgs::CollisionObject
+EnvironmentUtil::constructObjectToPick(std::vector<moveit_msgs::CollisionObject> &collision_objects,
                                        std::string id, geometry_msgs::Pose &pose, geometry_msgs::Vector3 &dimensions) {
 
     moveit_msgs::CollisionObject object_to_pick;

src/grasping/grasp_service.cpp

@@ -25,39 +25,6 @@ namespace grasping_state {
  */
 tf2_ros::Buffer tfBuffer;
 
-
-/**
- * Move the robot to a pose between different pick/place-actions.
- * Solves currently the problem of the wrong eef-to-object orientation.
- * @param group a initialized MoveGroupInterface instance
- * @return true on success
- */
-bool movetoInitPose(moveit::planning_interface::MoveGroupInterface &group) {
-
-    geometry_msgs::Pose another_pose;
-    another_pose.orientation.w = 0;
-    another_pose.orientation.x = 0.981;
-    another_pose.orientation.y = 0.196;
-    another_pose.orientation.z = 0;
-
-    another_pose.position.x = 0.26;
-    another_pose.position.y = 0;
-    another_pose.position.z = 0.87;
-    group.setPoseTarget(another_pose);
-
-    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
-
-    group.plan(my_plan);
-    moveit_msgs::MoveItErrorCodes miec = group.move();
-
-    if (miec.val == miec.SUCCESS) {
-        return true;
-    }
-
-    ROS_ERROR_STREAM("Error while moving to initial pose. MoveItErrorCode: " << miec.val);
-    return false;
-}
-
 /**
  * Picks a specified object
  * @param req a message containing the object specification
@@ -98,22 +65,12 @@ bool pickObject(panda_grasping::PickObject::Request &req,
             return false;
         }
 
-        ROS_INFO("Moving to initial pose.");
-        if (!movetoInitPose(group)) {
-            res.success = false;
-            return false;
-        }
-
-        ROS_INFO("Retrieving current eef pose.");
-        geometry_msgs::TransformStamped transformStamped = tfBuffer.lookupTransform("world", "panda_hand",
-                                                                                    ros::Time(0));
-        tf2::Quaternion orientation_gripper{transformStamped.transform.rotation.x,
-                                            transformStamped.transform.rotation.y,
-                                            transformStamped.transform.rotation.z,
-                                            transformStamped.transform.rotation.w};
+        ROS_INFO("Retrieving the orientation of the gripper wrt. to the hand.");
+        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

@@ -36,44 +36,46 @@ void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory &posture, do
     posture.points[0].time_from_start = ros::Duration(0.5);
 }
 
-double inline correctHeightError(double height) {
-    return (-0.5 * height) + 0.1;
-}
-
 geometry_msgs::Pose
-GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Vector3 dimensions,
-                                tf2::Quaternion orientation_gripper) {
-
-    geometry_msgs::Pose target_pose;
-
-    tf2::Quaternion orientation_object(object_to_pick_pose.orientation.x, object_to_pick_pose.orientation.y,
-                                       object_to_pick_pose.orientation.z, object_to_pick_pose.orientation.w);
-
-    tf2::Matrix3x3 m_gripper(orientation_gripper);
-    double eef_roll, eef_pitch, eef_yaw;
-    m_gripper.getRPY(eef_roll, eef_pitch, eef_yaw);
-
-    tf2::Matrix3x3 o_gripper(orientation_object);
-    double o_roll, o_pitch, o_yaw;
-    o_gripper.getRPY(o_roll, o_pitch, o_yaw);
-
-    tf2::Quaternion orientation;
-
-    if (dimensions.x <= 0.08) {
-        orientation.setRPY(M_PI, 0, (o_yaw - (2 * eef_yaw)));
-    } else if (dimensions.x > 0.08 && dimensions.y <= 0.08) {
-        orientation.setRPY(M_PI, 0, (o_yaw - (2 * eef_yaw) + M_PI_2));
-    } else {
-        orientation.setRPY(M_PI, 0, 0);
+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;
-
-    // works only with straight placed objects currently
-    double baseZ = object_to_pick_pose.position.z + dimensions.z;
-    target_pose.position.z = baseZ + correctHeightError(dimensions.z);
+    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,12 +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
+     * @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);
+                         geometry_msgs::Quaternion gripper_orientation,
+                         double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
 
     /**
      * Util method to pick an object from above.