Skip to content
Snippets Groups Projects
Select Git revision
  • f1d7dea4a60b9eebc3131c1297895d4cff32f4dd
  • master default protected
2 results

CMakeLists.txt

Blame
    • grasp_util.cpp 7.56 KiB 
    //
// Created by sebastian on 19.05.20.
//
#include "grasp_util.h"
#include <tf2/transform_datatypes.h>

// Actionlib
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/simple_client_goal_state.h>

// Franka
// if this should be required, the dependency to franka_gripper must also be declared
#include <franka_gripper/MoveAction.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;
    target_pose.position.y = object_pose_msg.position.y;
    target_pose.position.z = object_pose_msg.position.z + (0.5 * dimensions.z) + FRANKA_GRIPPER_DISTANCE - reach;

    return target_pose;
}

bool
GraspUtil::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) {

    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.z = -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);

    if (dimensions.x <= 0.08) {
        setupClosedGripper(grasps[0].grasp_posture, dimensions.x);
    }

    if (dimensions.x > 0.08 && dimensions.y <= 0.08) {
        setupClosedGripper(grasps[0].grasp_posture, dimensions.y);
    }

    if (dimensions.x > 0.08 && dimensions.y > 0.08) {
        ROS_ERROR("Could not grasp: Object to big (in any dimension).");
        return false;
    }

    move_group.setSupportSurfaceName(supporting_surface_id);
    moveit_msgs::MoveItErrorCodes miec = move_group.pick(object_to_pick_id, grasps);

    if (miec.val == miec.SUCCESS) {
        return true;
    }

    ROS_ERROR_STREAM("Error while executing pick task. MoveItErrorCode: " << miec.val);
    return false;
}

bool
GraspUtil::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) {

    std::vector<moveit_msgs::PlaceLocation> place_location;
    place_location.resize(1);

    place_location[0].place_pose.header.frame_id = "panda_link0";
        //  tf2::Quaternion orientation;
    //  orientation.setRPY(0, 0, M_PI / 2);
    place_location[0].place_pose.pose.orientation.w = 1;// = tf2::toMsg(orientation);
    place_location[0].place_pose.pose.position = place_pose.position;

    place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0";
    place_location[0].pre_place_approach.direction.vector.z = -1.0;
    place_location[0].pre_place_approach.min_distance = 0.095;
    place_location[0].pre_place_approach.desired_distance = 0.115;

    place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0";
    place_location[0].post_place_retreat.direction.vector.z = 1.0;
    place_location[0].post_place_retreat.min_distance = 0.1;
    place_location[0].post_place_retreat.desired_distance = 0.25;

    setupOpenGripper(place_location[0].post_place_posture, open_amount);
    move_group.setSupportSurfaceName(supporting_surface_id);
    moveit_msgs::MoveItErrorCodes miec = move_group.place(object_to_place_id, place_location);

    if (miec.val == miec.SUCCESS) {
        return true;
    }

    ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode: " << miec.val);
    return false;
}

void GraspUtil::resetGripperForNextAction() {

    actionlib::SimpleActionClient<franka_gripper::MoveAction> ac("franka_gripper/move", true);

    ROS_INFO("Waiting for reset action server to start.");
    ac.waitForServer();

    ROS_INFO("Reset action server started, sending goal.");
    franka_gripper::MoveGoal goal;
    goal.speed = 0.08;
    goal.width = 0.0;
    ac.sendGoal(goal);

    //wait for the action to return
    bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));

    if (finished_before_timeout) {
        actionlib::SimpleClientGoalState state = ac.getState();
        ROS_INFO("Reset action finished: %s", state.toString().c_str());
    } else {
        ROS_INFO("Reset action did not finish before the time out.");
    }
}