grasp_util.h

//
// Created by sebastian on 27.05.20.
//

#ifndef PANDA_GRASPING_GRASP_UTIL_H
#define PANDA_GRASPING_GRASP_UTIL_H

#include <ros/ros.h>

#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <trajectory_msgs/JointTrajectory.h>
#include <geometry_msgs/Pose.h>

/**
 * Util for grasping.
 */
class GraspUtil {

public:

    /**
     * distance of the center of the hand to the center of the finger tip
     */
    static constexpr tf2Scalar FRANKA_GRIPPER_DISTANCE{0.1034};

    /**
     * length of a finger
     */
    static constexpr tf2Scalar FRANKA_GRIPPER_FINGER_LENGTH{0.054};

    /**
     * size of the finger tip
     */
    static constexpr tf2Scalar FRANKA_GRIPPER_FINGERTIP_SIZE{0.018};

    /**
     * Sets up how far the gripper should be opened for the next action.
     * @param posture empty posture object defining grasp
     * @param amount total opening amount (space between "forks")
     */
    void setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount);

    /**
    * Sets up how much the gripper should be closed for the next action.
    * @param posture empty posture object defining grasp
    * @param amount total close amount (space between "forks")
    */
    void setupClosedGripper(trajectory_msgs::JointTrajectory &posture, double amount);

    /**
     * Compute the "pre-pick"-pose
     * @param object_to_pick_pose pose of the object to pick (currently only vertically oriented objects are supported)
     * @param dimensions of the object
     * @param gripper_orientation current gripper orientation relative to the link it is attached to
     * @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 "pre-pick"-pose
     */
    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);

    /**
     *
     * @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
     * @param pick_pose via getPickFromAbovePose-method computed pre-pick-pose
     * @param close_amount total close amount (space between "forks")
     * @param open_amount total opening amount (space between "forks")
     * @param supporting_surface_id ID of surface where object is picked from (e.g. the table)
     * @param object_to_pick_id ID of the object to pick
     * @return true if successfully picked
     */
    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);

    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
     * @param place_pose target place pose for the object
     * @param close_amount total close amount (space between "forks")
     * @param open_amount total opening amount (space between "forks")
     * @param supporting_surface_id ID of surface where object is placed on (e.g. the table)
     * @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);

    /**
     *
     * @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