From 6885f92f6c92f1c8ae263ef2be46aed49b286440 Mon Sep 17 00:00:00 2001
From: SebastianEbert <sebastian.ebert@tu-dresden.de>
Date: Tue, 12 Apr 2022 10:34:25 +0200
Subject: [PATCH] changes from merge-request
---
include/grasp_util.h | 43 +++++++++++++++++------------------
src/grasping/grasp_util.cpp | 24 ++++++++-----------
test/grasp_from_side_test.cpp | 36 +++++++++++++++++++++++++----
test/side_grasping.test | 4 ++--
4 files changed, 65 insertions(+), 42 deletions(-)
diff --git a/include/grasp_util.h b/include/grasp_util.h
index 33793a2..325a826 100644
--- a/include/grasp_util.h
+++ b/include/grasp_util.h
@@ -72,8 +72,7 @@ public:
* @return
*/
geometry_msgs::Pose
- getPickFromMinusXSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg,
- tf2::Quaternion object_orientation = {0, 0, 0, 1}, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
+ getPickFromBack(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
/**
*
@@ -84,8 +83,7 @@ public:
* @return
*/
geometry_msgs::Pose
- getPickFromPlusXSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg,
- tf2::Quaternion object_orientation = {0, 0, 0, 1}, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
+ getPickFromFront(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
/**
*
* @param object_pose_msg
@@ -95,8 +93,7 @@ public:
* @return
*/
geometry_msgs::Pose
- getPickFromMinusYSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg,
- tf2::Quaternion object_orientation = {0, 0, 0, 1}, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
+ getPickFromLeft(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
/**
*
@@ -107,23 +104,8 @@ public:
* @return
*/
geometry_msgs::Pose
- getPickFromPlusYSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg,
- tf2::Quaternion object_orientation = {0, 0, 0, 1}, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
+ getPickFromRight(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
- /**
- *
- * @param object_to_pick_pose
- * @param gripper_orientation
- * @param approach_orientation
- * @param y_distance_diff
- * @param x_distance_diff
- * @param object_orientation
- * @param reach
- * @return
- */
- geometry_msgs::Pose
- getPickFromSidePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Quaternion gripper_orientation, tf2::Quaternion approach_orientation, float y_distance_diff, float x_distance_diff,
- tf2::Quaternion object_orientation = {0, 0, 0, 1}, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
/**
* Util method to pick an object from above.
* @param move_group an initialized MoveGroupInterface instance
@@ -178,6 +160,23 @@ public:
*/
bool placeFromSide(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose,
double open_amount, std::string supporting_surface_id, std::string object_to_place_id, bool plan_only = false);
+
+private:
+
+ /**
+ *
+ * @param object_to_pick_pose
+ * @param gripper_orientation
+ * @param approach_orientation
+ * @param y_distance_diff
+ * @param x_distance_diff
+ * @param object_orientation
+ * @param reach
+ * @return
+ */
+ geometry_msgs::Pose
+ getPickFromSidePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Quaternion gripper_orientation,
+ tf2::Quaternion approach_orientation, float y_distance_diff, float x_distance_diff, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
};
#endif //PANDA_GRASPING_GRASP_UTIL_H
diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp
index e1973ec..7d99245 100644
--- a/src/grasping/grasp_util.cpp
+++ b/src/grasping/grasp_util.cpp
@@ -29,8 +29,7 @@ void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory &posture, do
}
geometry_msgs::Pose
-GraspUtil::getPickFromMinusXSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg,
- tf2::Quaternion object_orientation, double reach){
+GraspUtil::getPickFromBack(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach){
double approach_roll, approach_pitch, approach_yaw;
approach_roll = -M_PI_2;
@@ -40,12 +39,11 @@ GraspUtil::getPickFromMinusXSidePose(geometry_msgs::Pose &object_pose_msg, geome
approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
return getPickFromSidePose(object_pose_msg, gripper_orientation_msg,
- approach_orientation, 0, -0.1, object_orientation, reach);
+ approach_orientation, 0, -0.1, reach);
}
geometry_msgs::Pose
-GraspUtil::getPickFromPlusXSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg,
- tf2::Quaternion object_orientation, double reach){
+GraspUtil::getPickFromFront(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach){
double approach_roll, approach_pitch, approach_yaw;
approach_roll = M_PI_2;
@@ -55,12 +53,11 @@ GraspUtil::getPickFromPlusXSidePose(geometry_msgs::Pose &object_pose_msg, geomet
approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
return getPickFromSidePose(object_pose_msg, gripper_orientation_msg,
- approach_orientation, 0, 0.1, object_orientation, reach);
+ approach_orientation, 0, 0.1, reach);
}
geometry_msgs::Pose
-GraspUtil::getPickFromMinusYSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg,
- tf2::Quaternion object_orientation, double reach){
+GraspUtil::getPickFromLeft(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach){
double approach_roll, approach_pitch, approach_yaw;
approach_roll = M_PI_2;
@@ -70,12 +67,11 @@ GraspUtil::getPickFromMinusYSidePose(geometry_msgs::Pose &object_pose_msg, geome
approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
return getPickFromSidePose(object_pose_msg, gripper_orientation_msg,
- approach_orientation, -0.1, 0, object_orientation, reach);
+ approach_orientation, -0.1, 0, reach);
}
geometry_msgs::Pose
-GraspUtil::getPickFromPlusYSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg,
- tf2::Quaternion object_orientation, double reach){
+GraspUtil::getPickFromRight(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach){
double approach_roll, approach_pitch, approach_yaw;
approach_roll = -M_PI_2;
@@ -85,15 +81,15 @@ GraspUtil::getPickFromPlusYSidePose(geometry_msgs::Pose &object_pose_msg, geomet
approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
return getPickFromSidePose(object_pose_msg, gripper_orientation_msg,
- approach_orientation, 0.1, 0, object_orientation, reach);
+ approach_orientation, 0.1, 0, reach);
}
geometry_msgs::Pose
GraspUtil::getPickFromSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg,
- tf2::Quaternion approach_orientation, float y_distance_diff, float x_distance_diff, tf2::Quaternion object_orientation,
- double reach){
+ tf2::Quaternion approach_orientation, float y_distance_diff, float x_distance_diff, double reach){
// object orientation
+ tf2::Quaternion object_orientation;
tf2::fromMsg(object_pose_msg.orientation, object_orientation);
// gripper orientation
diff --git a/test/grasp_from_side_test.cpp b/test/grasp_from_side_test.cpp
index 03f44e0..2fcda0f 100644
--- a/test/grasp_from_side_test.cpp
+++ b/test/grasp_from_side_test.cpp
@@ -138,9 +138,16 @@ TEST_F(Grasping, SIDE_GRASPING_1) {
place_table_pose.position.z = 0.2;
place_table_pose.orientation.w = 1.0;
+ /*
obj_pose.position.x = 0.5;
obj_pose.position.y = -0.5;
obj_pose.position.z = 0.5;
+ obj_pose.orientation.w = 1.0;
+ */
+
+ obj_pose.position.x = 0;
+ obj_pose.position.y = 0;
+ obj_pose.position.z = 0;
obj_pose.orientation.w = 1.0;
place_pose.orientation = tf2::toMsg(place_orientation);
@@ -150,7 +157,7 @@ TEST_F(Grasping, SIDE_GRASPING_1) {
geometry_msgs::Vector3 approach_direction;
approach_direction.x = 1;
- geometry_msgs::Pose pick_pose = grasp_util.getPickFromMinusXSidePose(obj_pose, tf2::toMsg(gripper_orientation));
+ geometry_msgs::Pose pick_pose = grasp_util.getPickFromBack(obj_pose, tf2::toMsg(gripper_orientation));
doTest(pick_pose, pick_table_pose, place_table_pose, obj_pose, place_pose, approach_direction);
}
@@ -169,9 +176,16 @@ TEST_F(Grasping, SIDE_GRASPING_2) {
place_table_pose.position.z = 0.2;
place_table_pose.orientation.w = 1.0;
+ /*
obj_pose.position.x = -0.5;
obj_pose.position.y = -0.5;
obj_pose.position.z = 0.5;
+ obj_pose.orientation.w = 1.0;
+ */
+
+ obj_pose.position.x = 0;
+ obj_pose.position.y = 0;
+ obj_pose.position.z = 0;
obj_pose.orientation.w = 1.0;
place_pose.orientation = tf2::toMsg(place_orientation);
@@ -181,7 +195,7 @@ TEST_F(Grasping, SIDE_GRASPING_2) {
geometry_msgs::Vector3 approach_direction;
approach_direction.x = -1;
- geometry_msgs::Pose pick_pose = grasp_util.getPickFromPlusXSidePose(obj_pose, tf2::toMsg(gripper_orientation));
+ geometry_msgs::Pose pick_pose = grasp_util.getPickFromFront(obj_pose, tf2::toMsg(gripper_orientation));
doTest(pick_pose, pick_table_pose, place_table_pose, obj_pose, place_pose, approach_direction);
}
@@ -200,9 +214,16 @@ TEST_F(Grasping, SIDE_GRASPING_3) {
place_table_pose.position.z = 0.2;
place_table_pose.orientation.w = 1.0;
+ /*
obj_pose.position.x = 0.5;
obj_pose.position.y = 0.5;
obj_pose.position.z = 0.5;
+ obj_pose.orientation.w = 1.0;
+ */
+
+ obj_pose.position.x = 0;
+ obj_pose.position.y = 0;
+ obj_pose.position.z = 0;
obj_pose.orientation.w = 1.0;
place_pose.orientation = tf2::toMsg(place_orientation);
@@ -212,7 +233,7 @@ TEST_F(Grasping, SIDE_GRASPING_3) {
geometry_msgs::Vector3 approach_direction;
approach_direction.y = +1;
- geometry_msgs::Pose pick_pose = grasp_util.getPickFromMinusYSidePose(obj_pose, tf2::toMsg(gripper_orientation));
+ geometry_msgs::Pose pick_pose = grasp_util.getPickFromLeft(obj_pose, tf2::toMsg(gripper_orientation));
doTest(pick_pose, pick_table_pose, place_table_pose, obj_pose, place_pose, approach_direction);
}
@@ -231,9 +252,16 @@ TEST_F(Grasping, SIDE_GRASPING_4) {
place_table_pose.position.z = 0.2;
place_table_pose.orientation.w = 1.0;
+ /*
obj_pose.position.x = 0.5;
obj_pose.position.y = -0.5;
obj_pose.position.z = 0.5;
+ obj_pose.orientation.w = 1.0;
+ */
+
+ obj_pose.position.x = 0;
+ obj_pose.position.y = 0;
+ obj_pose.position.z = 0;
obj_pose.orientation.w = 1.0;
place_pose.orientation = tf2::toMsg(place_orientation);
@@ -243,7 +271,7 @@ TEST_F(Grasping, SIDE_GRASPING_4) {
geometry_msgs::Vector3 approach_direction;
approach_direction.y = -1;
- geometry_msgs::Pose pick_pose = grasp_util.getPickFromPlusYSidePose(obj_pose, tf2::toMsg(gripper_orientation));
+ geometry_msgs::Pose pick_pose = grasp_util.getPickFromRight(obj_pose, tf2::toMsg(gripper_orientation));
doTest(pick_pose, pick_table_pose, place_table_pose, obj_pose, place_pose, approach_direction);
}
diff --git a/test/side_grasping.test b/test/side_grasping.test
index 6b017a8..fe9ae97 100644
--- a/test/side_grasping.test
+++ b/test/side_grasping.test
@@ -1,10 +1,10 @@
<launch>
<include file="$(find panda_simulation)/launch/simulation.launch"/>
- <!--<node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"
+ <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"
args="-d $(find panda_grasping)/test/plan_visualizer.rviz" output="screen">
<rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
- </node>-->
+ </node>
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
--
GitLab