Select Git revision
grasp_util.cpp
grasp_util.cpp 15.61 KiB
//
// 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::getPickFromBack(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach){
double approach_roll, approach_pitch, approach_yaw;
approach_roll = -M_PI_2;
approach_pitch = -M_PI_4;
approach_yaw = -M_PI_4;
tf2::Quaternion approach_orientation;
approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
return getPickFromSidePose(object_pose_msg, dimensions, gripper_orientation_msg,
approach_orientation, 0, -0.1, reach);
}
geometry_msgs::Pose
GraspUtil::getPickFromFront(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach){
double approach_roll, approach_pitch, approach_yaw;
approach_roll = M_PI_2;
approach_pitch = -M_PI_4 - M_PI_2;
approach_yaw = -M_PI_4;
tf2::Quaternion approach_orientation;
approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
return getPickFromSidePose(object_pose_msg, dimensions, gripper_orientation_msg,
approach_orientation, 0, 0.1, reach);
}
geometry_msgs::Pose
GraspUtil::getPickFromLeft(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach){
double approach_roll, approach_pitch, approach_yaw;
approach_roll = M_PI_2;
approach_pitch = -M_PI_4 - M_PI_2;
approach_yaw = -M_PI_4 - M_PI_2;
tf2::Quaternion approach_orientation;
approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);