diff --git a/include/grasp_util.h b/include/grasp_util.h index 8953a48c703085c5d9ea45eba269487a3b7bccb5..411ca4b9fa58710476521ca5f044fabe4e54e2f2 100644 --- a/include/grasp_util.h +++ b/include/grasp_util.h @@ -61,9 +61,69 @@ public: */ geometry_msgs::Pose getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Vector3 dimensions, - geometry_msgs::Quaternion gripper_orientation, - double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2); + geometry_msgs::Quaternion gripper_orientation, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2); + /** + * + * @param object_pose_msg + * @param gripper_orientation_msg + * @param object_orientation + * @param reach + * @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); + + /** + * + * @param object_pose_msg + * @param gripper_orientation_msg + * @param object_orientation + * @param reach + * @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); + /** + * + * @param object_pose_msg + * @param gripper_orientation_msg + * @param object_orientation + * @param reach + * @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); + + /** + * + * @param object_pose_msg + * @param gripper_orientation_msg + * @param object_orientation + * @param reach + * @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); + + /** + * + * @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, int y_distance_diff, int 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 @@ -78,6 +138,10 @@ public: geometry_msgs::Vector3 dimensions, double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only = false); + bool pickFromSide(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose, + shape_msgs::SolidPrimitive &object_to_pick, + double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only = false); + /** * Util method to place an object from above. * @param move_group an initialized MoveGroupInterface instance @@ -90,6 +154,18 @@ public: 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); + /** + * + * @param move_group + * @param place_pose + * @param open_amount + * @param supporting_surface_id + * @param object_to_place_id + * @param plan_only + * @return + */ + 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); }; #endif //PANDA_GRASPING_GRASP_UTIL_H diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp index e8d90c28627ad6140f7255b79374ebeafc7d1585..195823f18343ca11d794530683924957be5fa0c7 100644 --- a/src/grasping/grasp_util.cpp +++ b/src/grasping/grasp_util.cpp @@ -28,6 +28,88 @@ void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory &posture, do posture.points[0].time_from_start = ros::Duration(0.5); } +geometry_msgs::Pose +GraspUtil::getPickFromMinusXSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, + tf2::Quaternion object_orientation, double reach){ + + double approach_roll, approach_pitch, approach_yaw; + approach_roll = -M_PI_2; + approach_pitch = -M_PI_4; + approach_yaw = -M_PI_4 - M_PI_2; + tf2::Quaternion approach_orientation; + 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); +} + +geometry_msgs::Pose +GraspUtil::getPickFromPlusXSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, + tf2::Quaternion object_orientation, double reach){ + + double approach_roll, approach_pitch, approach_yaw; + approach_roll = M_PI_2; + approach_pitch = -M_PI_4 - M_PI_2; + approach_yaw = -M_PI_4; + tf2::Quaternion approach_orientation; + 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); +} + +geometry_msgs::Pose +GraspUtil::getPickFromMinusYSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, + tf2::Quaternion object_orientation, double reach){ + + double approach_roll, approach_pitch, approach_yaw; + approach_roll = M_PI_2; + approach_pitch = -M_PI_4 - M_PI_2; + approach_yaw = -M_PI_4 - M_PI_2; + tf2::Quaternion approach_orientation; + 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); +} + +geometry_msgs::Pose +GraspUtil::getPickFromPlusYSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, + tf2::Quaternion object_orientation, double reach){ + + double approach_roll, approach_pitch, approach_yaw; + approach_roll = -M_PI_2; + approach_pitch = -M_PI_4; + approach_yaw = -M_PI_4 - M_PI_2; + tf2::Quaternion approach_orientation; + 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); +} + +geometry_msgs::Pose +GraspUtil::getPickFromSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, + tf2::Quaternion approach_orientation, int y_distance_diff, int x_distance_diff, tf2::Quaternion object_orientation, + double reach){ + + // object orientation + tf2::fromMsg(object_pose_msg.orientation, object_orientation); + + // gripper orientation + tf2::Quaternion gripper_orientation; + tf2::fromMsg(gripper_orientation_msg, gripper_orientation); + tf2::Quaternion orientation = object_orientation * gripper_orientation * approach_orientation; + + geometry_msgs::Pose target_pose; + target_pose.orientation = tf2::toMsg(orientation); + 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.z = object_pose_msg.position.z; + + return target_pose; +} + geometry_msgs::Pose GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach) { @@ -72,6 +154,66 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m return target_pose; } +bool +GraspUtil::pickFromSide(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose, + shape_msgs::SolidPrimitive &object_to_pick, + double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only) { + + std::vector<moveit_msgs::Grasp> grasps; + grasps.resize(1); + grasps[0].grasp_pose.header.frame_id = "panda_link0"; + grasps[0].grasp_pose.pose = pick_pose; + + // Setup pre-grasp approach + grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0"; + grasps[0].pre_grasp_approach.direction.vector.z = -1.0; + grasps[0].pre_grasp_approach.min_distance = 0.095; + grasps[0].pre_grasp_approach.desired_distance = 0.115; + + // Setup post-grasp retreat + 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.25; + + setupOpenGripper(grasps[0].pre_grasp_posture, open_amount); + + if(object_to_pick.type == shape_msgs::SolidPrimitive::CYLINDER){ + + if(object_to_pick.dimensions[0] <= 0.08){ + ROS_ERROR("Could not grasp: Cylinder object to big."); + return false; + } + + setupClosedGripper(grasps[0].grasp_posture, object_to_pick.dimensions[1]); + }else{ + if (object_to_pick.dimensions[0] <= 0.08) { + setupClosedGripper(grasps[0].grasp_posture, object_to_pick.dimensions[0]); + } + + if (object_to_pick.dimensions[0] > 0.08 && object_to_pick.dimensions[1] <= 0.08) { + setupClosedGripper(grasps[0].grasp_posture, object_to_pick.dimensions[1]); + } + + if (object_to_pick.dimensions[0] > 0.08 && object_to_pick.dimensions[1] > 0.08) { + ROS_ERROR("Could not grasp: Object to big (in any dimension)."); + return false; + } + } + + move_group.setSupportSurfaceName(supporting_surface_id); + + moveit_msgs::MoveItErrorCodes miec = move_group.pick(object_to_pick_id, grasps, plan_only); + + if (miec.val == miec.SUCCESS) { + return true; + } + + ROS_ERROR_STREAM("Error while executing pick task. MoveItErrorCode: " << miec.val); + + return false; +} + bool GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose, geometry_msgs::Vector3 dimensions, @@ -122,6 +264,39 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr ROS_ERROR_STREAM("Error while executing pick task. MoveItErrorCode: " << miec.val); return false; } +bool +GraspUtil::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){ + + std::vector<moveit_msgs::PlaceLocation> place_location; + place_location.resize(1); + + place_location[0].place_pose.header.frame_id = "panda_link0"; + place_location[0].place_pose.pose.orientation.w = 1; + place_location[0].place_pose.pose.position = place_pose.position; + + place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0"; + place_location[0].pre_place_approach.direction.vector.z = -1.0; + place_location[0].pre_place_approach.min_distance = 0.095; + place_location[0].pre_place_approach.desired_distance = 0.115; + + place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0"; + place_location[0].post_place_retreat.direction.vector.y = -1.0; + place_location[0].post_place_retreat.min_distance = 0.1; + place_location[0].post_place_retreat.desired_distance = 0.25; + + 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); + + if (miec.val == miec.SUCCESS) { + return true; + } + + ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode: " << miec.val); + return false; +} bool GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose,