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: ...@@ -72,8 +72,7 @@ public:
* @return * @return
*/ */
geometry_msgs::Pose geometry_msgs::Pose
getPickFromMinusXSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, getPickFromBack(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
tf2::Quaternion object_orientation = {0, 0, 0, 1}, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
/** /**
* *
...@@ -84,8 +83,7 @@ public: ...@@ -84,8 +83,7 @@ public:
* @return * @return
*/ */
geometry_msgs::Pose geometry_msgs::Pose
getPickFromPlusXSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, getPickFromFront(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
tf2::Quaternion object_orientation = {0, 0, 0, 1}, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
/** /**
* *
* @param object_pose_msg * @param object_pose_msg
...@@ -95,8 +93,7 @@ public: ...@@ -95,8 +93,7 @@ public:
* @return * @return
*/ */
geometry_msgs::Pose geometry_msgs::Pose
getPickFromMinusYSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, getPickFromLeft(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
tf2::Quaternion object_orientation = {0, 0, 0, 1}, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
/** /**
* *
...@@ -107,23 +104,8 @@ public: ...@@ -107,23 +104,8 @@ public:
* @return * @return
*/ */
geometry_msgs::Pose geometry_msgs::Pose
getPickFromPlusYSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, getPickFromRight(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
tf2::Quaternion object_orientation = {0, 0, 0, 1}, 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. * Util method to pick an object from above.
* @param move_group an initialized MoveGroupInterface instance * @param move_group an initialized MoveGroupInterface instance
...@@ -178,6 +160,23 @@ public: ...@@ -178,6 +160,23 @@ public:
*/ */
bool placeFromSide(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose, 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); 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 #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 ...@@ -29,8 +29,7 @@ void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory &posture, do
} }
geometry_msgs::Pose geometry_msgs::Pose
GraspUtil::getPickFromMinusXSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, GraspUtil::getPickFromBack(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach){
tf2::Quaternion object_orientation, 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 +39,11 @@ GraspUtil::getPickFromMinusXSidePose(geometry_msgs::Pose &object_pose_msg, geome ...@@ -40,12 +39,11 @@ GraspUtil::getPickFromMinusXSidePose(geometry_msgs::Pose &object_pose_msg, geome
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, gripper_orientation_msg,
approach_orientation, 0, -0.1, object_orientation, reach); approach_orientation, 0, -0.1, reach);
} }
geometry_msgs::Pose geometry_msgs::Pose
GraspUtil::getPickFromPlusXSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, GraspUtil::getPickFromFront(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach){
tf2::Quaternion object_orientation, 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;
...@@ -55,12 +53,11 @@ GraspUtil::getPickFromPlusXSidePose(geometry_msgs::Pose &object_pose_msg, geomet ...@@ -55,12 +53,11 @@ GraspUtil::getPickFromPlusXSidePose(geometry_msgs::Pose &object_pose_msg, geomet
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, gripper_orientation_msg,
approach_orientation, 0, 0.1, object_orientation, reach); approach_orientation, 0, 0.1, reach);
} }
geometry_msgs::Pose geometry_msgs::Pose
GraspUtil::getPickFromMinusYSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, GraspUtil::getPickFromLeft(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach){
tf2::Quaternion object_orientation, 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;
...@@ -70,12 +67,11 @@ GraspUtil::getPickFromMinusYSidePose(geometry_msgs::Pose &object_pose_msg, geome ...@@ -70,12 +67,11 @@ GraspUtil::getPickFromMinusYSidePose(geometry_msgs::Pose &object_pose_msg, geome
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, gripper_orientation_msg,
approach_orientation, -0.1, 0, object_orientation, reach); approach_orientation, -0.1, 0, reach);
} }
geometry_msgs::Pose geometry_msgs::Pose
GraspUtil::getPickFromPlusYSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, GraspUtil::getPickFromRight(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, double reach){
tf2::Quaternion object_orientation, 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;
...@@ -85,15 +81,15 @@ GraspUtil::getPickFromPlusYSidePose(geometry_msgs::Pose &object_pose_msg, geomet ...@@ -85,15 +81,15 @@ GraspUtil::getPickFromPlusYSidePose(geometry_msgs::Pose &object_pose_msg, geomet
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, gripper_orientation_msg,
approach_orientation, 0.1, 0, object_orientation, 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::Quaternion gripper_orientation_msg,
tf2::Quaternion approach_orientation, float y_distance_diff, float x_distance_diff, tf2::Quaternion object_orientation, tf2::Quaternion approach_orientation, float y_distance_diff, float x_distance_diff, double reach){
double reach){
// object orientation // object orientation
tf2::Quaternion object_orientation;
tf2::fromMsg(object_pose_msg.orientation, object_orientation); tf2::fromMsg(object_pose_msg.orientation, object_orientation);
// gripper orientation // gripper orientation
......
test/grasp_from_side_test.cpp
+ 32
− 4
View file @ 6885f92f
...@@ -138,9 +138,16 @@ TEST_F(Grasping, SIDE_GRASPING_1) { ...@@ -138,9 +138,16 @@ TEST_F(Grasping, SIDE_GRASPING_1) {
place_table_pose.position.z = 0.2; place_table_pose.position.z = 0.2;
place_table_pose.orientation.w = 1.0; place_table_pose.orientation.w = 1.0;
/*
obj_pose.position.x = 0.5; obj_pose.position.x = 0.5;
obj_pose.position.y = -0.5; obj_pose.position.y = -0.5;
obj_pose.position.z = 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; obj_pose.orientation.w = 1.0;
place_pose.orientation = tf2::toMsg(place_orientation); place_pose.orientation = tf2::toMsg(place_orientation);
...@@ -150,7 +157,7 @@ TEST_F(Grasping, SIDE_GRASPING_1) { ...@@ -150,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.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); 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) { ...@@ -169,9 +176,16 @@ TEST_F(Grasping, SIDE_GRASPING_2) {
place_table_pose.position.z = 0.2; place_table_pose.position.z = 0.2;
place_table_pose.orientation.w = 1.0; place_table_pose.orientation.w = 1.0;
/*
obj_pose.position.x = -0.5; obj_pose.position.x = -0.5;
obj_pose.position.y = -0.5; obj_pose.position.y = -0.5;
obj_pose.position.z = 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; obj_pose.orientation.w = 1.0;
place_pose.orientation = tf2::toMsg(place_orientation); place_pose.orientation = tf2::toMsg(place_orientation);
...@@ -181,7 +195,7 @@ TEST_F(Grasping, SIDE_GRASPING_2) { ...@@ -181,7 +195,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.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); 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) { ...@@ -200,9 +214,16 @@ TEST_F(Grasping, SIDE_GRASPING_3) {
place_table_pose.position.z = 0.2; place_table_pose.position.z = 0.2;
place_table_pose.orientation.w = 1.0; place_table_pose.orientation.w = 1.0;
/*
obj_pose.position.x = 0.5; obj_pose.position.x = 0.5;
obj_pose.position.y = 0.5; obj_pose.position.y = 0.5;
obj_pose.position.z = 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; obj_pose.orientation.w = 1.0;
place_pose.orientation = tf2::toMsg(place_orientation); place_pose.orientation = tf2::toMsg(place_orientation);
...@@ -212,7 +233,7 @@ TEST_F(Grasping, SIDE_GRASPING_3) { ...@@ -212,7 +233,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.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); 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) { ...@@ -231,9 +252,16 @@ TEST_F(Grasping, SIDE_GRASPING_4) {
place_table_pose.position.z = 0.2; place_table_pose.position.z = 0.2;
place_table_pose.orientation.w = 1.0; place_table_pose.orientation.w = 1.0;
/*
obj_pose.position.x = 0.5; obj_pose.position.x = 0.5;
obj_pose.position.y = -0.5; obj_pose.position.y = -0.5;
obj_pose.position.z = 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; obj_pose.orientation.w = 1.0;
place_pose.orientation = tf2::toMsg(place_orientation); place_pose.orientation = tf2::toMsg(place_orientation);
...@@ -243,7 +271,7 @@ TEST_F(Grasping, SIDE_GRASPING_4) { ...@@ -243,7 +271,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.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); 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> <launch>
<include file="$(find panda_simulation)/launch/simulation.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"> args="-d $(find panda_grasping)/test/plan_visualizer.rviz" output="screen">
<rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/> <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
</node>--> </node>
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" /> <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