draw_x.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, R, P, Y]*/
static const std::vector<double> CORNER1_POSE = { 0.1, -0.65, 0.15, 3.14, 0, -3.14};
static const std::vector<double> CORNER2_POSE = {-0.1, -0.45, 0.15, 3.14, 0, -3.14};
static const std::vector<double> CORNER3_POSE = {-0.1, -0.65, 0.15, 3.14, 0, -3.14};
static const std::vector<double> CORNER4_POSE = { 0.1, -0.45, 0.15, 3.14, 0, -3.14};

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

  // Move to the first corner
  robot->moveToTcpPose(CORNER1_POSE[0], CORNER1_POSE[1], CORNER1_POSE[2],
                       CORNER1_POSE[3], CORNER1_POSE[4], CORNER1_POSE[5], 1.05, 1.4);

  robot->moveToTcpPose(CORNER1_POSE[0], CORNER1_POSE[1], CORNER1_POSE[2] - 0.05,
                       CORNER1_POSE[3], CORNER1_POSE[4], CORNER1_POSE[5], 1.05, 1.4);

  // Move to the second corner
  robot->moveToTcpPose(CORNER2_POSE[0], CORNER2_POSE[1], CORNER2_POSE[2] - 0.05,
                       CORNER2_POSE[3], CORNER2_POSE[4], CORNER2_POSE[5], 1.05, 1.4);

  robot->moveToTcpPose(CORNER2_POSE[0], CORNER2_POSE[1], CORNER2_POSE[2],
                       CORNER2_POSE[3], CORNER2_POSE[4], CORNER2_POSE[5], 1.05, 1.4);

  // Move to the third corner
  robot->moveToTcpPose(CORNER3_POSE[0], CORNER3_POSE[1], CORNER3_POSE[2],
                       CORNER3_POSE[3], CORNER3_POSE[4], CORNER3_POSE[5], 1.05, 1.4);

  robot->moveToTcpPose(CORNER3_POSE[0], CORNER3_POSE[1], CORNER3_POSE[2] - 0.05,
                       CORNER3_POSE[3], CORNER3_POSE[4], CORNER3_POSE[5], 1.05, 1.4);

  // Move to the fourth corner
  robot->moveToTcpPose(CORNER4_POSE[0], CORNER4_POSE[1], CORNER4_POSE[2] - 0.05,
                       CORNER4_POSE[3], CORNER4_POSE[4], CORNER4_POSE[5], 1.05, 1.4);

  robot->moveToTcpPose(CORNER4_POSE[0], CORNER4_POSE[1], CORNER4_POSE[2],
                       CORNER4_POSE[3], CORNER4_POSE[4], CORNER4_POSE[5], 1.05, 1.4);

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

  rclcpp::shutdown();
  return 0;

}