Skip to content
Snippets Groups Projects
Select Git revision
  • 17eb75e371ba2ccff7a75ff0af47fcfca8dc4709
  • master default protected
  • noetic/main
  • feature/chem-feature-integration
  • feature/tests
5 results

grasp_util.cpp

Blame
  • 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);