make height of pre-grasp pose configurable

src/grasping/grasp_util.cpp

@@ -38,7 +38,7 @@ 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) {
+                                tf2::Quaternion orientation_gripper, double height) {
 
     geometry_msgs::Pose target_pose;
 
@@ -58,7 +58,7 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, geomet
     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 + 0.1;
+    target_pose.position.z = object_to_pick_pose.position.z + 0.5 * dimensions.z + height;
 
     return target_pose;
 }

src/grasping/grasp_util.h

@@ -42,11 +42,12 @@ public:
      * @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 height specifies how much the pose is above the object
      * @return the "pre-pick"-pose
      */
     geometry_msgs::Pose
     getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Vector3 dimensions,
-                         tf2::Quaternion orientation_gripper);
+                         tf2::Quaternion orientation_gripper, double height=0.1);
 
     /**
      * Util method to pick an object from above.