From 7536341abdb96a1111f7436c854c1efac338efcc Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Fri, 20 Nov 2020 18:42:11 +0100
Subject: [PATCH] make height of pre-grasp pose configurable
---
src/grasping/grasp_util.cpp | 4 ++--
src/grasping/grasp_util.h | 3 ++-
2 files changed, 4 insertions(+), 3 deletions(-)
diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp
index 77f3dff..da0d5cf 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 e2eff00..34a6fef 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.
--
GitLab