text: #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;
  
  }