grasp_util.cpp

//
// Created by sebastian on 19.05.20.
//
#include "grasp_util.h"
#include <tf2/transform_datatypes.h>
#include <franka_gripper/HomingAction.h>
#include <franka_gripper/HomingGoal.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_pose_msg, geometry_msgs::Vector3 dimensions,
                                geometry_msgs::Quaternion gripper_orientation_msg, double reach) {

    // object orientation
    tf2::Quaternion object_orientation;
    tf2::fromMsg(object_pose_msg.orientation, object_orientation);

    // gripper orientation
    tf2::Quaternion gripper_orientation;
    tf2::fromMsg(gripper_orientation_msg, gripper_orientation);

    // approach orientation
    double approach_roll, approach_pitch, approach_yaw;
    approach_roll = M_PI;  // turn upside down
    approach_pitch = 0;
    approach_yaw = 0;
    tf2::Quaternion approach_orientation;
    approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);

    tf2::Quaternion orientation = object_orientation * gripper_orientation * approach_orientation;

    // Actually, the maximum reach should be FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE/2, but half a
    // finger length is considered a safety distance.
    if (reach > FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE) {
        ROS_WARN_STREAM("Reach around object reduced from " << reach << " to " << FRANKA_GRIPPER_FINGER_LENGTH -
                                                                                  FRANKA_GRIPPER_FINGERTIP_SIZE << ".");
        reach = FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE;
    } else if (reach < -FRANKA_GRIPPER_FINGERTIP_SIZE / 2) {
        ROS_WARN_STREAM("Reach around object increased from " << reach << " to 0." << ". Minimum allowed reach is "
                                                              << (-FRANKA_GRIPPER_FINGERTIP_SIZE / 2) << ".");
        reach = 0;
    }

    // The pose is 10 cm straight above the object to pick. This only works on upright boxes.
    geometry_msgs::Pose target_pose;
    target_pose.orientation = tf2::toMsg(orientation);
    target_pose.position.x = object_pose_msg.position.x;