diff --git a/include/grasp_util.h b/include/grasp_util.h index 8953a48c703085c5d9ea45eba269487a3b7bccb5..ff343430432a336e9d8a13888f3fc1c5aa4ae98b 100644 --- a/include/grasp_util.h +++ b/include/grasp_util.h @@ -76,7 +76,7 @@ public: */ 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, - 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. @@ -88,7 +88,19 @@ public: * @param object_to_place_id ID of the object to place */ 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); }; diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp index e8d90c28627ad6140f7255b79374ebeafc7d1585..a51d2e5def48b26358f6bbb4e6d32f62f703d06b 100644 --- a/src/grasping/grasp_util.cpp +++ b/src/grasping/grasp_util.cpp @@ -72,10 +72,82 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m 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 GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose, 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; grasps.resize(1); @@ -113,19 +185,25 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr 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) { - return true; + while(!success && tries < max_planning_retries){ + 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); - return false; + ROS_ERROR_STREAM("Error while executing pick task."); + return success; } bool 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; place_location.resize(1); @@ -149,12 +227,17 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g setupOpenGripper(place_location[0].post_place_posture, open_amount); 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) { - return true; + while(!success && tries < max_planning_retries){ + 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); - return false; + ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode"); + return success; }