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

pick_and_place_sample.cpp

Blame
  • pick_and_place_sample.cpp 12.37 KiB
    // ROS
    #include <ros/ros.h>
    
    // MoveIt!
    #include <moveit/planning_scene_interface/planning_scene_interface.h>
    #include <moveit/move_group_interface/move_group_interface.h>
    
    // TF2
    #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
    
    geometry_msgs::Quaternion q_pick_object;
    
    geometry_msgs::Pose getTargetPickPose(){
    
        std::vector<double> co_dim{0.02, 0.02, 0.2};
        std::vector<double> co_pose{0.5, 0.0, 0.2};
    
        geometry_msgs::Pose target_pose;
    
        tf2::Quaternion orientation_gripper(-0.924, 0.383, 0, 0);
        tf2::Quaternion orientation_object(q_pick_object.x, q_pick_object.y, q_pick_object.z, q_pick_object.w);
    
        tf2::Quaternion orientation = orientation_gripper * orientation_object;
        orientation.setZ(orientation.getZ() * -1);
        orientation.setY(orientation.getY() * -1);
        orientation.setX(orientation.getX() * -1);
    
        target_pose.orientation  = tf2::toMsg(orientation);
        target_pose.position.x = co_pose[0];
        target_pose.position.y = co_pose[1];
        target_pose.position.z = co_pose[2] + 0.2;
    
        return target_pose;
    }
    
    void openGripper(trajectory_msgs::JointTrajectory& posture)
    {
        // BEGIN_SUB_TUTORIAL open_gripper
        /* Add both finger joints of panda robot. */
        posture.joint_names.resize(2);
        posture.joint_names[0] = "panda_finger_joint1";
        posture.joint_names[1] = "panda_finger_joint2";
    
        /* Set them as open, wide enough for the object to fit. */
        posture.points.resize(1);
        posture.points[0].positions.resize(2);
        posture.points[0].positions[0] = 0.04;
        posture.points[0].positions[1] = 0.04;
        posture.points[0].time_from_start = ros::Duration(0.5);
        // END_SUB_TUTORIAL
    }
    
    void closedGripper(trajectory_msgs::JointTrajectory& posture)
    {
        // BEGIN_SUB_TUTORIAL closed_gripper
        /* Add both finger joints of panda robot. */
        posture.joint_names.resize(2);
        posture.joint_names[0] = "panda_finger_joint1";
        posture.joint_names[1] = "panda_finger_joint2";
    
        /* Set them as closed. */
        posture.points.resize(1);
        posture.points[0].positions.resize(2);
        posture.points[0].positions[0] = 0.00;
        posture.points[0].positions[1] = 0.00;
        posture.points[0].time_from_start = ros::Duration(0.5);
        // END_SUB_TUTORIAL
    }
    
    void pick(moveit::planning_interface::MoveGroupInterface& move_group)