grasp_util.cpp

//
// Created by sebastian on 19.05.20.
//
#include "grasp_util.h"

void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount) {
    posture.joint_names.resize(2);
    posture.joint_names[0] = "panda_finger_joint1";
    posture.joint_names[1] = "panda_finger_joint2";

    posture.points.resize(1);
    posture.points[0].positions.resize(2);
    posture.points[0].positions[0] = amount / 2;
    posture.points[0].positions[1] = amount / 2;
    posture.points[0].time_from_start = ros::Duration(0.5);
}

void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory &posture, double amount) {
    posture.joint_names.resize(2);
    posture.joint_names[0] = "panda_finger_joint1";
    posture.joint_names[1] = "panda_finger_joint2";

    posture.points.resize(1);
    posture.points[0].positions.resize(2);
    posture.points[0].positions[0] = amount / 2;
    posture.points[0].positions[1] = amount / 2;
    posture.points[0].time_from_start = ros::Duration(0.5);
}

geometry_msgs::Pose GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, double distance_to_eef) {

    geometry_msgs::Pose target_pose;

    tf2::Quaternion orientation_gripper(-0.924, 0.383, 0, 0);
    tf2::Quaternion orientation_object(object_to_pick_pose.orientation.x, object_to_pick_pose.orientation.y,
                                       object_to_pick_pose.orientation.z, object_to_pick_pose.orientation.w);

    tf2::Quaternion orientation = orientation_gripper * orientation_object;
    orientation.setZ(orientation.getZ() * -1);
    orientation.setY(orientation.getY() * -1);
    orientation.setX(orientation.getX() * -1);

    target_pose.orientation = tf2::toMsg(orientation);
    target_pose.position.x = object_to_pick_pose.position.x;
    target_pose.position.y = object_to_pick_pose.position.y;
    target_pose.position.z = object_to_pick_pose.position.z + distance_to_eef;

    return target_pose;
}

void GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
                   double close_amount,
                   double open_amount, std::string supporting_surface_id, std::string object_to_pick_id) {
    std::vector<moveit_msgs::Grasp> grasps;
    grasps.resize(1);
    grasps[0].grasp_pose.header.frame_id = "panda_link0";
    grasps[0].grasp_pose.pose = pick_pose;

    // Setup pre-grasp approach
    grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";
    grasps[0].pre_grasp_approach.direction.vector.x = 1.0;
    grasps[0].pre_grasp_approach.min_distance = 0.095;
    grasps[0].pre_grasp_approach.desired_distance = 0.115;

    // Setup post-grasp retreat
    grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";
    grasps[0].post_grasp_retreat.direction.vector.z = 1.0;
    grasps[0].post_grasp_retreat.min_distance = 0.1;
    grasps[0].post_grasp_retreat.desired_distance = 0.25;

    // Setup the opening / closing - mechanism of the gripper
    setupOpenGripper(grasps[0].pre_grasp_posture, open_amount);
    setupClosedGripper(grasps[0].grasp_posture, close_amount);

    move_group.setSupportSurfaceName(supporting_surface_id);
    move_group.pick(object_to_pick_id, grasps);
}