Skip to content
Snippets Groups Projects
Select Git revision
  • ccnc-demo
  • noetic/main default
  • noetic/feature/robotics-festival
  • ga-demo
  • noetic/feature/tags
  • noetic/realrobot
  • noetic/simulation protected
7 results

.gitmodules

Blame
  • Forked from an inaccessible project.
    grasp_util.h 4.39 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);
    
        /**
         * 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, int max_planning_retries = 1);
    
        /**
         * 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, int max_planning_retries = 1);
    
        /**
         * TODO: TEST / FIX
         * @param move_group an initialized MoveGroupInterface instance
         * @param object_pose_msg pose of the object to pick
         * @param open_amount total opening amount (space between "forks")
         * @param dimensions of the object
         * @return true on success
         */
        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);
    
    };
    
    #endif //PANDA_GRASPING_GRASP_UTIL_H