Skip to content
Snippets Groups Projects
Select Git revision
  • 24f09b77df74158392bf4d13a615645ee480f375
  • master default protected
  • ci
3 results

gradlew

Blame
  • grasp_util.h 6.38 KiB
    //
    // 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
        getPickFromBack(geometry_msgs::Pose &object_pose_msg, 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
         */
        geometry_msgs::Pose
        getPickFromFront(geometry_msgs::Pose &object_pose_msg, 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
         */
        geometry_msgs::Pose
        getPickFromLeft(geometry_msgs::Pose &object_pose_msg, 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
         */
        geometry_msgs::Pose
        getPickFromRight(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, 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);
    
        /**
         *
         * @param move_group
         * @param pick_pose
         * @param object_to_pick
         * @param open_amount
         * @param supporting_surface_id
         * @param object_to_pick_id
         * @param plan_only
         * @return
         */
        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
         * @param plan_only
         * @return
         */
        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);
    
    private:
    
        /**
         *
         * @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, float y_distance_diff, float x_distance_diff, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
    };
    
    #endif //PANDA_GRASPING_GRASP_UTIL_H