diff --git a/src/grasping/environment_util.cpp b/src/grasping/environment_util.cpp
index b838534bca48859e7f9dd0e05d4d5def842af94d..54766834b5e4fb2a625ddabc44a907940babeea2 100644
--- a/src/grasping/environment_util.cpp
+++ b/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;
@@ -30,8 +31,8 @@ void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject> &
}
void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject> &collision_objects, std::string id,
- double object_x_dimension, double object_y_dimension, double object_z_dimension,
- geometry_msgs::Pose &object_to_pick_pose) {
+ double object_x_dimension, double object_y_dimension, double object_z_dimension,
+ geometry_msgs::Pose &object_to_pick_pose) {
moveit_msgs::CollisionObject pick_support;
@@ -55,8 +56,9 @@ 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,
- std::string id, geometry_msgs::Pose &pose, geometry_msgs::Vector3 &dimensions){
+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;
diff --git a/src/grasping/grasp_service.cpp b/src/grasping/grasp_service.cpp
index bca8656e91b67a8821567598793dfa80e8c5b40e..5da6b4a8c711ba5941b3877b897ed4ccf8b621a4 100644
--- a/src/grasping/grasp_service.cpp
+++ b/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
@@ -164,7 +121,7 @@ placeObject(panda_grasping::PlaceObject::Request &req, panda_grasping::PlaceObje
}
ROS_INFO("Placing object.");
- if(!grasp_util.placeFromAbove(group, req.pose, 0.08, "place_support", "pick_object")){
+ if (!grasp_util.placeFromAbove(group, req.pose, 0.08, "place_support", "pick_object")) {
res.success = false;
return false;
}
diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp
index 7544aa4ab11de26295bdd4d763be118be3dff275..d3058621b0d8d115d4de59a8e959494dd94221b3 100644
--- a/src/grasping/grasp_util.cpp
+++ b/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;
}
@@ -185,4 +187,4 @@ void GraspUtil::resetGripperForNextAction() {
} else {
ROS_INFO("Reset action did not finish before the time out.");
}
-}
\ No newline at end of file
+}
diff --git a/src/grasping/grasp_util.h b/src/grasping/grasp_util.h
index e2eff0019dc286257cea719b93e3c6557f4347cd..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,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.