diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp index 77f3dff1cde73a72be2ad4f2d9b0349d72180643..da0d5cff9ac1616ebba480281e054870a5081683 100644 --- a/src/grasping/grasp_util.cpp +++ b/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; } diff --git a/src/grasping/grasp_util.h b/src/grasping/grasp_util.h index e2eff0019dc286257cea719b93e3c6557f4347cd..34a6fefb4dfc34c265faaeaec4758cd9fecb9b2e 100644 --- a/src/grasping/grasp_util.h +++ b/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.