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

working on integration of new features

parent c47afd31
No related branches found
No related tags found
No related merge requests found
...@@ -76,7 +76,7 @@ public: ...@@ -76,7 +76,7 @@ public:
*/ */
bool pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose, bool pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
geometry_msgs::Vector3 dimensions, double open_amount, std::string supporting_surface_id, geometry_msgs::Vector3 dimensions, double open_amount, std::string supporting_surface_id,
std::string object_to_pick_id, bool plan_only = false); std::string object_to_pick_id, bool plan_only = false, int max_planning_retries = 1);
/** /**
* Util method to place an object from above. * Util method to place an object from above.
...@@ -88,7 +88,19 @@ public: ...@@ -88,7 +88,19 @@ public:
* @param object_to_place_id ID of the object to place * @param object_to_place_id ID of the object to place
*/ */
bool placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose, bool placeFromAbove(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, int max_planning_retries = 1);
/**
* TODO: TEST
* @param move_group
* @param object_pose_msg
* @param open_amount
* @param dimensions
* @return
*/
bool pickObjectOnRobotfrontFromSide(moveit::planning_interface::MoveGroupInterface &move_group, std::string object_to_pick_id,
geometry_msgs::Pose &object_pose_msg, double open_amount, geometry_msgs::Vector3 dimensions, int max_planning_retries = 1);
}; };
......
...@@ -72,10 +72,82 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m ...@@ -72,10 +72,82 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m
return target_pose; return target_pose;
} }
bool GraspUtil::pickObjectOnRobotfrontFromSide(moveit::planning_interface::MoveGroupInterface &move_group, std::string object_to_pick_id,
geometry_msgs::Pose &object_pose_msg, double open_amount, geometry_msgs::Vector3 dimensions, int max_planning_retries) {
std::vector<moveit_msgs::Grasp> grasps;
grasps.resize(1);
grasps[0].grasp_pose.header.frame_id = "panda_link0";
tf2::Quaternion object_orientation;
tf2::fromMsg(object_pose_msg.orientation, object_orientation);
tf2::Quaternion gripper_orientation;
// We know the gripper is rotated -45 degrees from the urdf model
gripper_orientation.setRPY(0, 0, -M_PI_4);
// approach orientation
double approach_roll, approach_pitch, approach_yaw;
approach_roll = -M_PI_2; // The neutral rotation has the gripper "looking down",
approach_pitch = -M_PI_4; // with these transformations, it grasps from one side.
approach_yaw = -M_PI_4; // This results in (x=-0.5,y=-0.5,z=0,w=0.7071068)
tf2::Quaternion approach_orientation;
approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
tf2::Quaternion orientation = object_orientation * gripper_orientation * approach_orientation;
grasps[0].grasp_pose.pose.orientation = tf2::toMsg(orientation);
grasps[0].grasp_pose.pose.position.x = object_pose_msg.position.x - 0.1;
grasps[0].grasp_pose.pose.position.y = object_pose_msg.position.y;
grasps[0].grasp_pose.pose.position.z = object_pose_msg.position.z + 0.03;
grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";
grasps[0].pre_grasp_approach.direction.vector.x = 1.0;
grasps[0].pre_grasp_approach.min_distance = 0.095;
grasps[0].pre_grasp_approach.desired_distance = 0.115;
grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";
grasps[0].post_grasp_retreat.direction.vector.z = 1.0;
grasps[0].post_grasp_retreat.min_distance = 0.1;
grasps[0].post_grasp_retreat.desired_distance = 0.15;
setupOpenGripper(grasps[0].pre_grasp_posture, open_amount);
if (dimensions.x <= 0.08) {
setupClosedGripper(grasps[0].pre_grasp_posture, dimensions.x);
}
if (dimensions.x > 0.08 && dimensions.y <= 0.08) {
setupClosedGripper(grasps[0].pre_grasp_posture, dimensions.y);
}
if (dimensions.x > 0.08 && dimensions.y > 0.08) {
ROS_ERROR("Could not grasp: Object to big (in any dimension).");
return false;
}
int tries = 0;
bool success = false;
while(!success && tries < max_planning_retries){
success = move_group.pick(object_to_pick_id, grasps) == moveit_msgs::MoveItErrorCodes::SUCCESS;
if(!success) {
ROS_ERROR("[CC][Cobot1] Retrying pick");
}
tries++;
}
return success;
}
bool bool
GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose, GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
geometry_msgs::Vector3 dimensions, geometry_msgs::Vector3 dimensions,
double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only) { double open_amount, std::string supporting_surface_id,
std::string object_to_pick_id, bool plan_only, int max_planning_retries) {
std::vector<moveit_msgs::Grasp> grasps; std::vector<moveit_msgs::Grasp> grasps;
grasps.resize(1); grasps.resize(1);
...@@ -113,19 +185,25 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr ...@@ -113,19 +185,25 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr
move_group.setSupportSurfaceName(supporting_surface_id); move_group.setSupportSurfaceName(supporting_surface_id);
moveit_msgs::MoveItErrorCodes miec = move_group.pick(object_to_pick_id, grasps, plan_only); int tries = 0;
bool success = false;
if (miec.val == miec.SUCCESS) { while(!success && tries < max_planning_retries){
return true; success = move_group.pick(object_to_pick_id, grasps, plan_only) == moveit_msgs::MoveItErrorCodes::SUCCESS;
if(!success) {
ROS_ERROR("[CC][Cobot1] Retrying pick");
}
tries++;
} }
ROS_ERROR_STREAM("Error while executing pick task. MoveItErrorCode: " << miec.val); ROS_ERROR_STREAM("Error while executing pick task.");
return false; return success;
} }
bool bool
GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose, GraspUtil::placeFromAbove(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) { double open_amount, std::string supporting_surface_id,
std::string object_to_place_id, bool plan_only, int max_planning_retries) {
std::vector<moveit_msgs::PlaceLocation> place_location; std::vector<moveit_msgs::PlaceLocation> place_location;
place_location.resize(1); place_location.resize(1);
...@@ -149,12 +227,17 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g ...@@ -149,12 +227,17 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g
setupOpenGripper(place_location[0].post_place_posture, open_amount); setupOpenGripper(place_location[0].post_place_posture, open_amount);
move_group.setSupportSurfaceName(supporting_surface_id); move_group.setSupportSurfaceName(supporting_surface_id);
moveit_msgs::MoveItErrorCodes miec = move_group.place(object_to_place_id, place_location, plan_only); int tries = 0;
bool success = false;
if (miec.val == miec.SUCCESS) { while(!success && tries < max_planning_retries){
return true; success = move_group.place(object_to_place_id, place_location, plan_only) == moveit_msgs::MoveItErrorCodes::SUCCESS;
if(!success) {
ROS_ERROR("[CC][Cobot1] Retry bottle pick");
}
tries++;
} }
ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode: " << miec.val); ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode");
return false; return success;
} }
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment