diff --git a/include/grasp_util.h b/include/grasp_util.h index 9f714b36997fa96f6174f807c2c79439b331cb72..4e778c010060038aae0a2ac6b8825b80237e3ab2 100644 --- a/include/grasp_util.h +++ b/include/grasp_util.h @@ -64,44 +64,45 @@ public: 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 + * Generate a pick-pose for grasping an object from its back. + * @param object_pose_msg the object pose + * @param gripper_orientation_msg the target gripper orientation -> use a yaw of -M_PI_4 for an orientation orthogonal to the object + * @param object_orientation the objects orientation -> use (0,0,0,1) for axisymmetric orientation + * @param reach specifies how far the gripper reaches around the object (0 means the center of the finger tip is at the object's corner) + * @return the pick pose */ geometry_msgs::Pose getPickFromBack(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2); /** * - * @param object_pose_msg - * @param gripper_orientation_msg - * @param object_orientation - * @param reach - * @return + * Generate a pick-pose for grasping an object from its front. + * @param object_pose_msg the object pose + * @param gripper_orientation_msg the target gripper orientation -> use a yaw of -M_PI_4 for an orientation orthogonal to the object + * @param object_orientation the objects orientation -> use (0,0,0,1) for axisymmetric orientation + * @param reach specifies how far the gripper reaches around the object (0 means the center of the finger tip is at the object's corner) + * @return the pick pose */ geometry_msgs::Pose getPickFromFront(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2); /** - * - * @param object_pose_msg - * @param gripper_orientation_msg - * @param object_orientation - * @param reach - * @return + * Generate a pick-pose for grasping an object from left. + * @param object_pose_msg the object pose + * @param gripper_orientation_msg the target gripper orientation -> use a yaw of -M_PI_4 for an orientation orthogonal to the object + * @param object_orientation the objects orientation -> use (0,0,0,1) for axisymmetric orientation + * @param reach specifies how far the gripper reaches around the object (0 means the center of the finger tip is at the object's corner) + * @return the pick pose */ geometry_msgs::Pose getPickFromLeft(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2); /** - * - * @param object_pose_msg - * @param gripper_orientation_msg - * @param object_orientation - * @param reach - * @return + * Generate a pick-pose for grasping an object from right. + * @param object_pose_msg the object pose + * @param gripper_orientation_msg the target gripper orientation -> use a yaw of -M_PI_4 for an orientation orthogonal to the object + * @param object_orientation the objects orientation -> use (0,0,0,1) for axisymmetric orientation + * @param reach specifies how far the gripper reaches around the object (0 means the center of the finger tip is at the object's corner) + * @return the pick pose */ geometry_msgs::Pose getPickFromRight(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2); @@ -121,42 +122,42 @@ public: std::string object_to_pick_id, bool plan_only = false); /** - * - * @param move_group - * @param pick_pose - * @param object_to_pick - * @param open_amount - * @param supporting_surface_id - * @param object_to_pick_id + * Pick an object from the side based on pick-pose. + * @param move_group an initialized MoveGroupInterface instance + * @param pick_pose pick pose for the approach + * @param object_to_pick geometric description of the object + * @param open_amount intial opening amount of the gripper + * @param supporting_surface_id ID of surface where object is placed on (e.g. the table) + * @param object_to_pick_id ID of the object to place * @param plan_only - * @return + * @return true on success */ bool pickFromSide(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose, shape_msgs::SolidPrimitive &object_to_pick, geometry_msgs::Vector3 &approach_direction, double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only = false); /** - * - * @param move_group - * @param place_pose - * @param open_amount - * @param supporting_surface_id - * @param object_to_place_id + * Place an object from above. + * @param move_group an initialized MoveGroupInterface instance + * @param place_pose a pre place pose + * @param open_amount intial opening amount of the gripper + * @param supporting_surface_id ID of surface where object is place on (e.g. the table) + * @param object_to_pick_id ID of the object to place * @param plan_only - * @return + * @return true on success */ 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 + * Place an object from side. + * @param move_group an initialized MoveGroupInterface instance + * @param place_pose a pre-place pose + * @param open_amount intial opening amount of the gripper + * @param supporting_surface_id ID of surface where object is place on (e.g. the table) + * @param object_to_pick_id ID of the object to place * @param plan_only - * @return + * @return true on success */ 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);