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

changes from merge-request

parent 50381920
No related branches found
No related tags found
1 merge request!2Feature/public grasp side
Show whitespace changes
Inline Side-by-side
include/grasp_util.h
+ 21
− 22
View file @ 6885f92f
......@@ -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
src/grasping/grasp_util.cpp
+ 10
− 14
View file @ 6885f92f
......@@ -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
......
test/grasp_from_side_test.cpp
+ 32
− 4
View file @ 6885f92f
......@@ -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);
}
......
test/side_grasping.test
+ 2
− 2
View file @ 6885f92f
<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" />
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment