diff --git a/src/grasping/grasp_service.cpp b/src/grasping/grasp_service.cpp index 4b3c78bfd1a644d3957abddcc27bc32ed8e3e4ce..5da6b4a8c711ba5941b3877b897ed4ccf8b621a4 100644 --- a/src/grasping/grasp_service.cpp +++ b/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 diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp index da0d5cff9ac1616ebba480281e054870a5081683..d3058621b0d8d115d4de59a8e959494dd94221b3 100644 --- a/src/grasping/grasp_util.cpp +++ b/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; } diff --git a/src/grasping/grasp_util.h b/src/grasping/grasp_util.h index 3ff588e1801668e4c58c74cc864beb9ec2921407..effadabd3ae6186285b0c10c33222ec562fd786c 100644 --- a/src/grasping/grasp_util.h +++ b/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.