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/MoveAction.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;
  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, bool plan_only)
{
  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, plan_only);

  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, bool plan_only)
{
  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, plan_only);

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

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

bool GraspUtil::homeGripper()
{
  actionlib::SimpleActionClient<franka_gripper::MoveAction> moveActionClient("franka_gripper/move", true);
  if (moveActionClient.waitForServer(ros::Duration(1.0)))
  {
    moveActionClient.cancelAllGoals();
  }

  actionlib::SimpleActionClient<franka_gripper::HomingAction> ac("/franka_gripper/homing", true);
  if (!ac.waitForServer(ros::Duration(10.0)))
  {
    ROS_ERROR("Gripper homing failed (action server not available).");
    return false;
  }

  ac.cancelAllGoals();
  franka_gripper::HomingGoal goal;
  ac.sendGoal(goal);
  if (ac.waitForResult(ros::Duration(15.0)))
  {
    actionlib::SimpleClientGoalState state = ac.getState();
    ROS_INFO_STREAM("Homing finished: " << state.toString());
    return state == actionlib::SimpleClientGoalState::SUCCEEDED;
  }
  else
  {
    ROS_ERROR("Gripper homing failed (with timeout).");
    return false;
  }
}