fixed_position_pick.cpp

#include <geometry_msgs/msg/pose_stamped.hpp>
#include <rclcpp/rclcpp.hpp>
#include <robot_interface/control_ur.hpp>

/* pose in joint values*/
static const std::vector<double> HOME = {0.87, -1.44, 1.68, -1.81, -1.56, 0};
/* pose in [x, y, z, qx, qy, qz, qw]*/
static const std::vector<double> PICK_POSE = { -0.157402, -0.679509, 0.094437, 0.190600, 0.948295, 0.239947, 0.082662};
static const std::vector<double> PLACE_POSE = {-0.350, -0.296, 0.145, -0.311507, 0.950216, -0.004305, 0.005879};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);

  // init robot control
  auto robot = std::make_shared<URControl>("robot_control",
       rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));
  robot->parseArgs();
  robot->startLoop();
  rclcpp::sleep_for(2s);

  // Move to home
  robot->moveToJointValues(HOME, 1.05, 1.4);

  // Pick
  geometry_msgs::msg::PoseStamped pose_pick;
  pose_pick.header.frame_id = "base";
  pose_pick.header.stamp = robot->now();
  pose_pick.pose.position.x = PICK_POSE[0];
  pose_pick.pose.position.y = PICK_POSE[1];
  pose_pick.pose.position.z = PICK_POSE[2];
  pose_pick.pose.orientation.x = PICK_POSE[3];
  pose_pick.pose.orientation.y = PICK_POSE[4];
  pose_pick.pose.orientation.z = PICK_POSE[5];
  pose_pick.pose.orientation.w = PICK_POSE[6];

  robot->pick(pose_pick, 1.05, 1.4, 0.5, 0.1);

  // Place
  geometry_msgs::msg::PoseStamped pose_place;
  pose_place.header.frame_id = "base";
  pose_place.header.stamp = robot->now();
  pose_place.pose.position.x = PLACE_POSE[0];
  pose_place.pose.position.y = PLACE_POSE[1];
  pose_place.pose.position.z = PLACE_POSE[2];
  pose_place.pose.orientation.x = PLACE_POSE[3];
  pose_place.pose.orientation.y = PLACE_POSE[4];
  pose_place.pose.orientation.z = PLACE_POSE[5];
  pose_place.pose.orientation.w = PLACE_POSE[6];

  robot->place(pose_place, 1.05, 1.4, 0.5, 0.1);

  // Move back to home
  robot->moveToJointValues(HOME, 1.05, 1.4);

  rclcpp::shutdown();
  return 0;

}