diff --git a/include/grasp_util.h b/include/grasp_util.h
index 33793a25d87a0123a1a6975ad8d866cd05b5b8ff..325a826306b0463eb56c2cda7adad2491e62a7e9 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 e1973ec3801e12d4ed22f10ec5d284d76b59f2a0..7d9924561a137645e096641c6a4dbb3cb198b6fe 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 03f44e0262e37777a442a0b6df31c8fb12ff49ad..2fcda0fd336d537a7e11abe3f9d9517676ef09f2 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 6b017a8a9a22d76eaa69c0b505ed9f7e5e594ccd..fe9ae97168a8ee0567a074d1da256cfe4ecf8987 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" />