Skip to content
Snippets Groups Projects
Commit 1784f0d5 authored by Sebastian Ebert's avatar Sebastian Ebert
Browse files

adjusted grasp spot, fixed tests

parent bff34589
No related branches found
No related tags found
1 merge request!2Feature/public grasp side
...@@ -72,7 +72,7 @@ public: ...@@ -72,7 +72,7 @@ public:
* @return * @return
*/ */
geometry_msgs::Pose geometry_msgs::Pose
getPickFromBack(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2); getPickFromBack(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
/** /**
* *
...@@ -83,7 +83,7 @@ public: ...@@ -83,7 +83,7 @@ public:
* @return * @return
*/ */
geometry_msgs::Pose geometry_msgs::Pose
getPickFromFront(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2); getPickFromFront(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
/** /**
* *
* @param object_pose_msg * @param object_pose_msg
...@@ -93,7 +93,7 @@ public: ...@@ -93,7 +93,7 @@ public:
* @return * @return
*/ */
geometry_msgs::Pose geometry_msgs::Pose
getPickFromLeft(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2); getPickFromLeft(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
/** /**
* *
...@@ -104,7 +104,7 @@ public: ...@@ -104,7 +104,7 @@ public:
* @return * @return
*/ */
geometry_msgs::Pose geometry_msgs::Pose
getPickFromRight(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2); getPickFromRight(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
/** /**
* Util method to pick an object from above. * Util method to pick an object from above.
...@@ -182,7 +182,7 @@ private: ...@@ -182,7 +182,7 @@ private:
* @return * @return
*/ */
geometry_msgs::Pose geometry_msgs::Pose
getPickFromSidePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Quaternion gripper_orientation, getPickFromSidePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation,
tf2::Quaternion approach_orientation, float y_distance_diff, float x_distance_diff, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2); tf2::Quaternion approach_orientation, float y_distance_diff, float x_distance_diff, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
}; };
......
...@@ -31,7 +31,7 @@ void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory &posture, do ...@@ -31,7 +31,7 @@ void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory &posture, do
} }
geometry_msgs::Pose geometry_msgs::Pose
GraspUtil::getPickFromBack(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach){ GraspUtil::getPickFromBack(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach){
double approach_roll, approach_pitch, approach_yaw; double approach_roll, approach_pitch, approach_yaw;
approach_roll = -M_PI_2; approach_roll = -M_PI_2;
...@@ -40,12 +40,12 @@ GraspUtil::getPickFromBack(geometry_msgs::Pose &object_pose_msg, geometry_msgs:: ...@@ -40,12 +40,12 @@ GraspUtil::getPickFromBack(geometry_msgs::Pose &object_pose_msg, geometry_msgs::
tf2::Quaternion approach_orientation; tf2::Quaternion approach_orientation;
approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw); approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
return getPickFromSidePose(object_pose_msg, gripper_orientation_msg, return getPickFromSidePose(object_pose_msg, dimensions, gripper_orientation_msg,
approach_orientation, 0, -0.1, reach); approach_orientation, 0, -0.1, reach);
} }
geometry_msgs::Pose geometry_msgs::Pose
GraspUtil::getPickFromFront(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach){ GraspUtil::getPickFromFront(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach){
double approach_roll, approach_pitch, approach_yaw; double approach_roll, approach_pitch, approach_yaw;
approach_roll = M_PI_2; approach_roll = M_PI_2;
...@@ -54,12 +54,12 @@ GraspUtil::getPickFromFront(geometry_msgs::Pose &object_pose_msg, geometry_msgs: ...@@ -54,12 +54,12 @@ GraspUtil::getPickFromFront(geometry_msgs::Pose &object_pose_msg, geometry_msgs:
tf2::Quaternion approach_orientation; tf2::Quaternion approach_orientation;
approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw); approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
return getPickFromSidePose(object_pose_msg, gripper_orientation_msg, return getPickFromSidePose(object_pose_msg, dimensions, gripper_orientation_msg,
approach_orientation, 0, 0.1, reach); approach_orientation, 0, 0.1, reach);
} }
geometry_msgs::Pose geometry_msgs::Pose
GraspUtil::getPickFromLeft(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach){ GraspUtil::getPickFromLeft(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach){
double approach_roll, approach_pitch, approach_yaw; double approach_roll, approach_pitch, approach_yaw;
approach_roll = M_PI_2; approach_roll = M_PI_2;
...@@ -68,12 +68,12 @@ GraspUtil::getPickFromLeft(geometry_msgs::Pose &object_pose_msg, geometry_msgs:: ...@@ -68,12 +68,12 @@ GraspUtil::getPickFromLeft(geometry_msgs::Pose &object_pose_msg, geometry_msgs::
tf2::Quaternion approach_orientation; tf2::Quaternion approach_orientation;
approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw); approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
return getPickFromSidePose(object_pose_msg, gripper_orientation_msg, return getPickFromSidePose(object_pose_msg, dimensions, gripper_orientation_msg,
approach_orientation, -0.1, 0, reach); approach_orientation, -0.1, 0, reach);
} }
geometry_msgs::Pose geometry_msgs::Pose
GraspUtil::getPickFromRight(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach){ GraspUtil::getPickFromRight(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach){
double approach_roll, approach_pitch, approach_yaw; double approach_roll, approach_pitch, approach_yaw;
approach_roll = -M_PI_2; approach_roll = -M_PI_2;
...@@ -82,12 +82,12 @@ GraspUtil::getPickFromRight(geometry_msgs::Pose &object_pose_msg, geometry_msgs: ...@@ -82,12 +82,12 @@ GraspUtil::getPickFromRight(geometry_msgs::Pose &object_pose_msg, geometry_msgs:
tf2::Quaternion approach_orientation; tf2::Quaternion approach_orientation;
approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw); approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
return getPickFromSidePose(object_pose_msg, gripper_orientation_msg, return getPickFromSidePose(object_pose_msg, dimensions, gripper_orientation_msg,
approach_orientation, 0.1, 0, reach); approach_orientation, 0.1, 0, reach);
} }
geometry_msgs::Pose geometry_msgs::Pose
GraspUtil::getPickFromSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, GraspUtil::getPickFromSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg,
tf2::Quaternion approach_orientation, float y_distance_diff, float x_distance_diff, double reach){ tf2::Quaternion approach_orientation, float y_distance_diff, float x_distance_diff, double reach){
// object orientation // object orientation
...@@ -103,6 +103,23 @@ GraspUtil::getPickFromSidePose(geometry_msgs::Pose &object_pose_msg, geometry_ms ...@@ -103,6 +103,23 @@ GraspUtil::getPickFromSidePose(geometry_msgs::Pose &object_pose_msg, geometry_ms
target_pose.orientation = tf2::toMsg(orientation); target_pose.orientation = tf2::toMsg(orientation);
target_pose.position.x = object_pose_msg.position.x + x_distance_diff; target_pose.position.x = object_pose_msg.position.x + x_distance_diff;
target_pose.position.y = object_pose_msg.position.y + y_distance_diff; target_pose.position.y = object_pose_msg.position.y + y_distance_diff;
if(x_distance_diff > 0){
target_pose.position.x = object_pose_msg.position.x + (0.5 * dimensions.x) + FRANKA_GRIPPER_DISTANCE - reach;
}
if(x_distance_diff < 0){
target_pose.position.x = object_pose_msg.position.x - (0.5 * dimensions.x) - FRANKA_GRIPPER_DISTANCE + reach;
}
if(y_distance_diff > 0){
target_pose.position.y = object_pose_msg.position.y + (0.5 * dimensions.y) + FRANKA_GRIPPER_DISTANCE - reach;
}
if(y_distance_diff < 0){
target_pose.position.y = object_pose_msg.position.y - (0.5 * dimensions.y) - FRANKA_GRIPPER_DISTANCE + reach;
}
target_pose.position.z = object_pose_msg.position.z; target_pose.position.z = object_pose_msg.position.z;
return target_pose; return target_pose;
......
...@@ -27,6 +27,8 @@ protected: ...@@ -27,6 +27,8 @@ protected:
tf2::Quaternion gripper_orientation; tf2::Quaternion gripper_orientation;
tf2::Quaternion place_orientation; tf2::Quaternion place_orientation;
geometry_msgs::Vector3 dimensions;
void SetUp() override { void SetUp() override {
ros::NodeHandle n("panda_grasping"); ros::NodeHandle n("panda_grasping");
...@@ -34,6 +36,9 @@ protected: ...@@ -34,6 +36,9 @@ protected:
gripper_orientation.setRPY(0, 0, -M_PI_4); gripper_orientation.setRPY(0, 0, -M_PI_4);
place_pose.position.z = 0.4 + 0.05; // table height + object height / 2 + buffer place_pose.position.z = 0.4 + 0.05; // table height + object height / 2 + buffer
place_orientation.setRPY(0, 0, M_PI / 2); place_orientation.setRPY(0, 0, M_PI / 2);
dimensions.x = 0.05;
dimensions.y = 0.05;
dimensions.z = 0.1;
CleanupScene(); CleanupScene();
} }
...@@ -90,9 +95,9 @@ protected: ...@@ -90,9 +95,9 @@ protected:
collision_objects[2].primitives.resize(1); collision_objects[2].primitives.resize(1);
collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].BOX; collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].BOX;
collision_objects[2].primitives[0].dimensions.resize(3); collision_objects[2].primitives[0].dimensions.resize(3);
collision_objects[2].primitives[0].dimensions[0] = 0.05; collision_objects[2].primitives[0].dimensions[0] = dimensions.x;
collision_objects[2].primitives[0].dimensions[1] = 0.05; collision_objects[2].primitives[0].dimensions[1] = dimensions.y;
collision_objects[2].primitives[0].dimensions[2] = 0.1; collision_objects[2].primitives[0].dimensions[2] = dimensions.z;
collision_objects[2].primitive_poses.resize(1); collision_objects[2].primitive_poses.resize(1);
collision_objects[2].primitive_poses[0] = obj_pose; collision_objects[2].primitive_poses[0] = obj_pose;
collision_objects[2].operation = collision_objects[2].ADD; collision_objects[2].operation = collision_objects[2].ADD;
...@@ -108,8 +113,8 @@ protected: ...@@ -108,8 +113,8 @@ protected:
shape_msgs::SolidPrimitive object; shape_msgs::SolidPrimitive object;
object.type = shape_msgs::SolidPrimitive::CYLINDER; object.type = shape_msgs::SolidPrimitive::CYLINDER;
object.dimensions.resize(2); object.dimensions.resize(2);
object.dimensions[0] = 0.2; object.dimensions[0] = dimensions.x;
object.dimensions[1] = 0.008; object.dimensions[1] = dimensions.y;
GraspUtil grasp_util; GraspUtil grasp_util;
...@@ -152,7 +157,7 @@ TEST_F(Grasping, SIDE_GRASPING_1) { ...@@ -152,7 +157,7 @@ TEST_F(Grasping, SIDE_GRASPING_1) {
geometry_msgs::Vector3 approach_direction; geometry_msgs::Vector3 approach_direction;
approach_direction.x = 1; approach_direction.x = 1;
geometry_msgs::Pose pick_pose = grasp_util.getPickFromBack(obj_pose, tf2::toMsg(gripper_orientation)); geometry_msgs::Pose pick_pose = grasp_util.getPickFromBack(obj_pose, dimensions, tf2::toMsg(gripper_orientation));
doTest(pick_pose, pick_table_pose, place_table_pose, obj_pose, place_pose, approach_direction); doTest(pick_pose, pick_table_pose, place_table_pose, obj_pose, place_pose, approach_direction);
} }
...@@ -183,7 +188,7 @@ TEST_F(Grasping, SIDE_GRASPING_2) { ...@@ -183,7 +188,7 @@ TEST_F(Grasping, SIDE_GRASPING_2) {
geometry_msgs::Vector3 approach_direction; geometry_msgs::Vector3 approach_direction;
approach_direction.x = -1; approach_direction.x = -1;
geometry_msgs::Pose pick_pose = grasp_util.getPickFromFront(obj_pose, tf2::toMsg(gripper_orientation)); geometry_msgs::Pose pick_pose = grasp_util.getPickFromFront(obj_pose, dimensions, tf2::toMsg(gripper_orientation));
doTest(pick_pose, pick_table_pose, place_table_pose, obj_pose, place_pose, approach_direction); doTest(pick_pose, pick_table_pose, place_table_pose, obj_pose, place_pose, approach_direction);
} }
...@@ -214,7 +219,7 @@ TEST_F(Grasping, SIDE_GRASPING_3) { ...@@ -214,7 +219,7 @@ TEST_F(Grasping, SIDE_GRASPING_3) {
geometry_msgs::Vector3 approach_direction; geometry_msgs::Vector3 approach_direction;
approach_direction.y = +1; approach_direction.y = +1;
geometry_msgs::Pose pick_pose = grasp_util.getPickFromLeft(obj_pose, tf2::toMsg(gripper_orientation)); geometry_msgs::Pose pick_pose = grasp_util.getPickFromLeft(obj_pose, dimensions, tf2::toMsg(gripper_orientation));
doTest(pick_pose, pick_table_pose, place_table_pose, obj_pose, place_pose, approach_direction); doTest(pick_pose, pick_table_pose, place_table_pose, obj_pose, place_pose, approach_direction);
} }
...@@ -245,7 +250,7 @@ TEST_F(Grasping, SIDE_GRASPING_4) { ...@@ -245,7 +250,7 @@ TEST_F(Grasping, SIDE_GRASPING_4) {
geometry_msgs::Vector3 approach_direction; geometry_msgs::Vector3 approach_direction;
approach_direction.y = -1; approach_direction.y = -1;
geometry_msgs::Pose pick_pose = grasp_util.getPickFromRight(obj_pose, tf2::toMsg(gripper_orientation)); geometry_msgs::Pose pick_pose = grasp_util.getPickFromRight(obj_pose, dimensions, tf2::toMsg(gripper_orientation));
doTest(pick_pose, pick_table_pose, place_table_pose, obj_pose, place_pose, approach_direction); doTest(pick_pose, pick_table_pose, place_table_pose, obj_pose, place_pose, approach_direction);
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment