Select Git revision
grasp_util.h

Sebastian Ebert authored
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