Adjust lite6 params

Dockerfile

@@ -75,7 +75,7 @@ RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
     rm -rf ${WS_LOG_DIR}
 
 # Copy example svg images
-COPY ./svg svg
+COPY ./svg test-images
 
 ### Add workspace to the ROS entrypoint
 ### Source ROS workspace inside `~/.bashrc` to enable autocompletion

README.md

@@ -34,13 +34,21 @@ If active changes are being made, run:
 bash .docker/devel.bash
 ```
 This will mount the host `drawing-robot-ros2` directory in the container at `src/drawing-robot-ros2`.
-If using podman instead of docker using the following will allow the container to access `/dev/` which is needed by the axidraw robot. 
+
+Optionally you can pass a directory to the container with
+``` sh
+bash .docker/run.bash -v PATH_TO_SVG:/ws/PATH_IN_CONTAINER:ro
+```
+
+#### Podman issues
+If using podman instead of docker, using the following will allow the container to access `/dev/` which is needed by the axidraw robot. 
 ``` sh
 sudo bash .docker/build.bash
 ```
 ``` sh
 sudo bash .docker/devel.bash
 ```
+Adding sudo may cause gazebo not to work, so it is recommended to use docker instead of podman.
 
 ## TODO Building locally
 

src/axidraw_controller/src/py/axidraw_serial.py

@@ -66,6 +66,8 @@ class AxidrawSerial(Node):
         self.ad.options.accel = 100          # 100% speed
         self.ad.options.pen_rate_lower = 100 # 100% speed
         self.ad.options.pen_rate_raise = 100 # 100% speed
+        #self.ad.options.pen_delay_down = 50
+        #self.ad.options.pen_delay_up = 50
         self.ad.update()                     # Process changes to options
         self.status["serial"] = "ready"
         self.status["motion"] = "ready"

src/custom_xarm_moveit_config/config/lite6/cartesian_limits.yaml

@@ -1,5 +1,5 @@
 cartesian_limits:
-  max_trans_vel: 2.0
-  max_trans_acc: 3.00
+  max_trans_vel: 0.4
+  max_trans_acc: 4.00
   max_trans_dec: -5.0
   max_rot_vel: 1.57

src/custom_xarm_moveit_config/config/lite6/joint_limits.yaml

@@ -4,31 +4,31 @@
 joint_limits:
   joint1:
     has_velocity_limits: true
-    max_velocity: 10.14
+    max_velocity: 4.14
     has_acceleration_limits: true
-    max_acceleration: 20.0
+    max_acceleration: 30.0
   joint2:
     has_velocity_limits: true
-    max_velocity: 10.14
+    max_velocity: 4.14
     has_acceleration_limits: true
-    max_acceleration: 20.0
+    max_acceleration: 30.0
   joint3:
     has_velocity_limits: true
-    max_velocity: 10.14
+    max_velocity: 4.14
     has_acceleration_limits: true
-    max_acceleration: 20.0
+    max_acceleration: 30.0
   joint4:
     has_velocity_limits: true
-    max_velocity: 10.14
+    max_velocity: 4.14
     has_acceleration_limits: true
-    max_acceleration: 20.0
+    max_acceleration: 30.0
   joint5:
     has_velocity_limits: true
-    max_velocity: 10.14
+    max_velocity: 4.14
     has_acceleration_limits: true
-    max_acceleration: 20.0
+    max_acceleration: 30.0
   joint6:
     has_velocity_limits: true
-    max_velocity: 10.14
+    max_velocity: 4.14
     has_acceleration_limits: true
-    max_acceleration: 20.0
+    max_acceleration: 30.0

src/drawing_controller/drawing_controller/svg_path_parser.py

@@ -115,7 +115,7 @@ class SVGPathParser():
             maxval = np.amax(np.absolute(control_points))
             #print('maxxv', maxval)
             #control_points = control_points / maxval #normalize values
-            n = 500
+            n = 100
             curve = cf.bezier(control_points)
             lin = np.linspace(curve.start(0), curve.end(0), n)
             coordinates = curve(lin)
@@ -140,7 +140,7 @@ class SVGPathParser():
             nonlocal x
             nonlocal y
             control_points = np.array(control_points)
-            n = 500
+            n = 100
             curve = cf.bezier(control_points, quadratic=True)
             lin = np.linspace(curve.start(0), curve.end(0), n)
             coordinates = curve(lin)

src/drawing_controller/drawing_controller/svg_processor.py

@@ -197,8 +197,7 @@ class SVGProcessor():
         Simplify line with https://pypi.org/project/simplification/
         """
         # For RDP, Try an epsilon of 1.0 to start with. Other sensible values include 0.01, 0.001
-        #epsilon = 0.009
-        epsilon = 0.0005
+        epsilon =  0.00005
 
         tmp = []
         out = []

src/lite6_controller/launch/lite6_gazebo.launch.py

@@ -382,15 +382,8 @@ def launch_setup(context, *args, **kwargs):
         'publish_geometry_updates': True,
         'publish_state_updates': True,
         'publish_transforms_updates': True,
-        # "planning_scene_monitor_options": {
-        #     "name": "planning_scene_monitor",
-        #     "robot_description": "robot_description",
-        #     "joint_state_topic": "/joint_states",
-        #     "attached_collision_object_topic": "/move_group/planning_scene_monitor",
-        #     "publish_planning_scene_topic": "/move_group/publish_planning_scene",
-        #     "monitored_planning_scene_topic": "/move_group/monitored_planning_scene",
-        #     "wait_for_initial_state_timeout": 10.0,
-        # },
+        'publish_robot_description': True,
+        'publish_robot_description_semantic' :True,
     }
 
     # Start the actual move_group node/action server

src/lite6_controller/launch/lite6_real.launch.py

@@ -227,15 +227,8 @@ def launch_setup(context, *args, **kwargs):
         'publish_geometry_updates': True,
         'publish_state_updates': True,
         'publish_transforms_updates': True,
-        # "planning_scene_monitor_options": {
-        #     "name": "planning_scene_monitor",
-        #     "robot_description": "robot_description",
-        #     "joint_state_topic": "/joint_states",
-        #     "attached_collision_object_topic": "/move_group/planning_scene_monitor",
-        #     "publish_planning_scene_topic": "/move_group/publish_planning_scene",
-        #     "monitored_planning_scene_topic": "/move_group/monitored_planning_scene",
-        #     "wait_for_initial_state_timeout": 10.0,
-        # },
+        'publish_robot_description': True,
+        'publish_robot_description_semantic' :True,
     }
 
 

src/lite6_controller/launch/lite6_real_no_gui.launch.py

@@ -233,15 +233,8 @@ def launch_setup(context, *args, **kwargs):
         'publish_geometry_updates': True,
         'publish_state_updates': True,
         'publish_transforms_updates': True,
-        # "planning_scene_monitor_options": {
-        #     "name": "planning_scene_monitor",
-        #     "robot_description": "robot_description",
-        #     "joint_state_topic": "/joint_states",
-        #     "attached_collision_object_topic": "/move_group/planning_scene_monitor",
-        #     "publish_planning_scene_topic": "/move_group/publish_planning_scene",
-        #     "monitored_planning_scene_topic": "/move_group/monitored_planning_scene",
-        #     "wait_for_initial_state_timeout": 10.0,
-        # },
+        'publish_robot_description': True,
+        'publish_robot_description_semantic' :True,
     }
 
     # Start the actual move_group node/action server

src/lite6_controller/src/lite6_calibration.cpp

@@ -5,6 +5,15 @@
 #include <iostream>
 #include <string>
 
+// MoveIt
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_state/robot_state.h>
+
+
+const std::string MOVE_GROUP = "lite6";
+
+std::shared_ptr<rclcpp::Node> node;
 
 void exit_sig_handler(int signum)
 {
@@ -16,24 +25,31 @@ void wait()
 {
  do
  {
-   std::cout << '\n' << "Press a key to continue...";
+   std::cout << "Press a key to continue...";
  } while (std::cin.get() != '\n');
 }
 
-void println(std::string s) {
+void println(std::string s)
+{
   std::cout << s << std::endl;
 }
 
-void print_joints(std::vector<float> jnts) {
+void print_joints(std::vector<float> jnts)
+{
+    std::string out = "";
+    out = out + "{ ";
     for (auto i : jnts)
-      std::cout << i << std::endl;
+      out = out + std::to_string(i) + ", ";
+    out = out.substr(0, out.size()-2); //remove trailing comma
+    out = out + " }";
+    std::cout << out << std::endl;
 }
 
 int main(int argc, char **argv)
 {
     rclcpp::init(argc, argv);
     std::string hw_ns = "ufactory";
-    std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("lite6_calibration");
+    node = rclcpp::Node::make_shared("lite6_calibration");
     int ret;
 
     signal(SIGINT, exit_sig_handler);
@@ -55,52 +71,61 @@ int main(int argc, char **argv)
     client.set_state(0);
     rclcpp::sleep_for(std::chrono::seconds(2));
 
-    std::vector<float> jnt_drawing_pose = {-0.813972, -0.0103697, 0.554617, 3.09979, -0.627334, -0.397301, 0};
+    //std::vector<float> jnt_drawing_pose = { -0.975004, 0.0734182, 0.443928, 3.14102, -0.370552, -0.85577, 0 };
+    std::vector<float> jnt_drawing_pose = { -0.975008, 0.00889134, 0.453255, 3.1414, -0.444295, -0.85692, 0 } ;
 
     std::vector<float> jnt_pose = { 0, 0, 0, 0, 0, 0, 0 };
     client.get_servo_angle(jnt_pose);
-    println("Moving to start drawing pose");
-//XArmROSClient::set_servo_angle(const std::vector<fp32>& angles, fp32 speed, fp32 acc, fp32 mvtime, bool wait, fp32 timeout, fp32 radius)
-    client.set_servo_angle(jnt_drawing_pose, 1, 1, 1, true, 100, -1);
+
+    print_joints(jnt_pose);
+
+
+
     //client.set_servo_angle_j(jnt_drawing_pose, 1, 1, 100);
     //rclcpp::sleep_for(std::chrono::seconds(5));
 
 
+    // Compute position of pen from joint state
+    robot_model_loader::RobotModelLoader robot_model_loader(node);
+    const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel();
+    moveit::core::RobotStatePtr kinematic_state(new moveit::core::RobotState(kinematic_model));
+    const moveit::core::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup(MOVE_GROUP);
+    std::string ee_link = "pen_link";
+
+    std::vector<double> jnts;
+    for (auto j : jnt_pose)
+        jnts.push_back(j);
+
+    jnts.resize(joint_model_group->getVariableNames().size());
+    kinematic_state->setJointGroupPositions(joint_model_group, jnts);
+
+    // from tutorial https://ros-planning.github.io/moveit_tutorials/doc/robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html
+    // https://github.com/ros-planning/moveit_tutorials/blob/master/doc/robot_model_and_robot_state/src/robot_model_and_robot_state_tutorial.cpp
+    //robot_model_loader::RobotModelLoader robot_model_loader(node, "robot_description");
+
+    kinematic_state->setJointGroupPositions(joint_model_group, jnts);
+    // https://mycourses.aalto.fi/pluginfile.php/1433193/mod_resource/content/2/Intro_to_ROS_Eigen.pdf
+    const Eigen::Isometry3d& end_effector_state = kinematic_state->getGlobalLinkTransform(ee_link);
+
+    auto x = std::to_string(end_effector_state.translation().x());
+    auto y = std::to_string(end_effector_state.translation().y());
+    auto z = std::to_string(end_effector_state.translation().z());
+    //println("x for '" + ee_link + "': " + x);
+    //println("y for '" + ee_link + "': " + y);
+    println("z-offset value for '" + ee_link + "' (use this value for the current pen): " + z);
+
+
+    println("Moving to start drawing pose");
+    wait();
+    client.set_servo_angle(jnt_drawing_pose, 1, 1, 1, true, 100, -1);
+
 
     client.set_mode(0);
     client.set_state(0);
-    return 0;
 
-    client.set_mode(4);
-    client.set_state(0);
-    std::vector<float> jnt_v = { 1, 0, 0, 0, 0, 0, 0 };
-    ret = client.vc_set_joint_velocity(jnt_v);
-    RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_joint: %d", ret);
-    rclcpp::sleep_for(std::chrono::seconds(2));
-    jnt_v[0] = -1;
-    ret = client.vc_set_joint_velocity(jnt_v);
-    RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_joint: %d", ret);
-    rclcpp::sleep_for(std::chrono::seconds(2));
-    // stop
-    jnt_v[0] = 0;
-    ret = client.vc_set_joint_velocity(jnt_v);
-    RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_joint: %d", ret);
+    rclcpp::shutdown();
 
-    std::vector<float> line_v = { 1, 0, 0, 0, 0, 0};
-    client.set_mode(5);
-	client.set_state(0);
-    ret = client.vc_set_cartesian_velocity(line_v);
-    RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_line: %d", ret);
-    rclcpp::sleep_for(std::chrono::seconds(2));
-    line_v[0] = -1;
-    ret = client.vc_set_cartesian_velocity(line_v);
-    RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_line: %d", ret);
-    rclcpp::sleep_for(std::chrono::seconds(2));
-    // stop
-    line_v[0] = 0;
-    ret = client.vc_set_cartesian_velocity(line_v);
-    RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_line: %d", ret);
-
-    RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "test_xarm_velo_move over");
+    println("Done, ignore any errors after this");
+    wait();
     return 0;
 }

src/lite6_controller/src/lite6_controller.cpp

@@ -47,15 +47,6 @@ const std::string MOVE_GROUP = "lite6";
 
 using namespace std::chrono_literals;
 
-// MOTION PLANNING API
-// https://github.com/ros-planning/moveit2_tutorials/blob/humble/doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp
-// https://moveit.picknik.ai/humble/doc/examples/motion_planning_api/motion_planning_api_tutorial.html
-// https://github.com/ros-planning/moveit2/blob/main/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp
-//
-//
-// USE
-// https://industrial-training-master.readthedocs.io/en/foxy/_source/session4/ros2/0-Motion-Planning-CPP.html
-
 /**
  * Controller for xArm Lite6, implementing RobotController
  */
@@ -89,20 +80,10 @@ public:
   float xlim_upper = 0.305;
   float ylim_lower = -0.1475;
   float ylim_upper = 0.1475;
-  float zlim_lower = 0.1945;
-  float zlim_upper = 0.200;
+  //float zlim_lower = 0.1945;
+  float zlim_lower = 0.207493;
+  float zlim_upper = zlim_lower + 0.01;
 
-  //bool moved = false;
-  //
-  // TODO get pilz working
-  //std::unique_ptr<pilz_industrial_motion_planner::CommandListManager> command_list_manager_;
-  // command_list_manager_ = std::make_unique<pilz_industrial_motion_planner::CommandListManager>(this->move_group.getNodeHandle(), this->move_group.getRobotModel());
-
-  /**
-   * CommandListManager, used to plan MotionSequenceRequest
-   */
-  //pilz_industrial_motion_planner::CommandListManager command_list_manager;
-  //pilz_industrial_motion_planner::CommandListManager command_list_manager(*std::shared_ptr<rclcpp::Node>(std::move(this)), this->move_group.getRobotModel());
 
   /**
    * Constructor
@@ -110,24 +91,11 @@ public:
   Lite6Controller(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
     : RobotController(options),
       move_group(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
-      //moveit_cpp_(std::shared_ptr<rclcpp::Node>(std::move(this))),
-      //planning_component_(MOVE_GROUP, moveit_cpp_),
-      //command_list_manager(std::shared_ptr<rclcpp::Node>(std::move(this)), this->move_group.getRobotModel())
   {
-
-    //command_list_manager = new pilz_industrial_motion_planner::CommandListManager(this->move_group.getNodeHandle(), this->move_group.getRobotModel());
-    //command_list_manager = new pilz_industrial_motion_planner::CommandListManager(std::shared_ptr<rclcpp::Node>(std::move(this)), this->move_group.getRobotModel());
-    // Use upper joint velocity and acceleration limits
     this->move_group.setMaxAccelerationScalingFactor(1.0);
-
-    //setting this lower seems to improve overall time and prevents robot from moving too fast
     this->move_group.setMaxVelocityScalingFactor(1.0);
     //this->move_group.setMaxVelocityScalingFactor(0.8);
 
-    // Subscribe to target pose
-    //target_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1));
-
-    //
     trajectory_timer_ = this->create_wall_timer(
       10ms, std::bind(&Lite6Controller::executeTrajectoryFromQueue, this));
 
@@ -135,18 +103,10 @@ public:
 
   void setup()
   {
-    //moveit_cpp_ = std::make_shared<moveit_cpp::MoveItCpp>(move_group.getNodeHandle());
     try
     {
-      //moveit_cpp_ = std::make_shared<moveit_cpp::MoveItCpp>(this->shared_from_this());
-      //moveit_cpp_ = std::make_shared<moveit_cpp::MoveItCpp>(move_group.getNodeHandle());
-      //planning_component_ = std::make_shared<moveit_cpp::PlanningComponent>(MOVE_GROUP, moveit_cpp_);
-      //
-
       planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(this->shared_from_this(),
                                                                                                "robot_description");
-      //RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Getting initial robot state");
-      //move_group.startStateMonitor(5);
 
       this->sequence_client_ =
         this->create_client<moveit_msgs::srv::GetMotionSequence>("plan_sequence_path");
@@ -217,11 +177,12 @@ public:
   /**
    *
    */
-  moveit_msgs::msg::MotionSequenceRequest createMotionSequenceRequest(const std::vector<geometry_msgs::msg::PoseStamped> *path)
+  moveit_msgs::msg::MotionSequenceRequest createMotionSequenceRequest(const std::vector<geometry_msgs::msg::PoseStamped> *path, std::string planner_id)
   {
     moveit_msgs::msg::MotionSequenceRequest msr = moveit_msgs::msg::MotionSequenceRequest();
     //waypoints.push_back(move_group.getCurrentPose().pose);
-    std::string ee_link = move_group.getLinkNames().back();
+    //std::string ee_link = move_group.getLinkNames().back();
+    std::string ee_link = "pen_link";
     RCLCPP_INFO(this->get_logger(), "Got ee_link: %s", ee_link.c_str());
     geometry_msgs::msg::Point previous_point;
     //previous_point.point.x = -1.0;
@@ -233,15 +194,16 @@ public:
       moveit_msgs::msg::MotionSequenceItem msi = moveit_msgs::msg::MotionSequenceItem();
 
       planning_interface::MotionPlanRequest mpr = planning_interface::MotionPlanRequest();
+      mpr.planner_id = planner_id;
       //mpr.planner_id = "PTP";
-      mpr.planner_id = "LIN";
+      //mpr.planner_id = "LIN";
       mpr.group_name = move_group.getName();
       //mpr.max_velocity_scaling_factor = 1.0;
-      mpr.max_velocity_scaling_factor = 0.3;
-      mpr.max_acceleration_scaling_factor = 0.4;
+      mpr.max_velocity_scaling_factor = 1.0;
+      mpr.max_acceleration_scaling_factor = 0.8;
       //mpr.max_acceleration_scaling_factor = 0.1;
-      mpr.allowed_planning_time = 10;
-      mpr.max_cartesian_speed = 3; // m/s
+      mpr.allowed_planning_time = 20;
+      //mpr.max_cartesian_speed = 2; // m/s
       //mpr.goal_constraints.position_constraints.header.frame_id = "world";
 
       // A tolerance of 0.01 m is specified in position
@@ -299,6 +261,11 @@ public:
     return msr;
   }
 
+  moveit_msgs::msg::MotionSequenceRequest createMotionSequenceRequest(const std::vector<geometry_msgs::msg::PoseStamped> *path)
+  {
+    return createMotionSequenceRequest(path, "LIN");
+  }
+
   /**
    *
    */
@@ -402,8 +369,17 @@ public:
 
   void executeTrajectoryFromQueue()
   {
-    if (busy || trajectory_queue.empty())
+    if (trajectory_queue.empty())
+    {
+      if (busy)
+        return;
+
+      busy = true;
+      //RCLCPP_INFO(this->get_logger(), "Getting robot state");
+      //move_group_state = move_group.getCurrentState(10);
+      busy = false;
       return;
+    }
     busy = true;
     RCLCPP_INFO(this->get_logger(), "Executing next trajectory from queue");
     move_group.execute(trajectory_queue.front());
@@ -411,6 +387,103 @@ public:
     busy = false;
   }
 
+  moveit_msgs::msg::RobotTrajectory sendRequest(moveit_msgs::msg::MotionSequenceRequest msr)
+  {
+    auto req = std::make_shared<moveit_msgs::srv::GetMotionSequence::Request>();
+    req->request = msr;
+    RCLCPP_INFO(this->get_logger(), "Sending request to sequence service");
+    auto res = sequence_client_->async_send_request(req);
+
+    auto ts = res.get()->response.planned_trajectories;
+
+    // Set move_group_state to the last state of planned trajectory (planning of next trajectory starts there)
+
+    if (ts.empty())
+    {
+      moveit_msgs::msg::RobotTrajectory t;
+      return t;
+    }
+
+
+    return ts[0];
+  }
+
+  void setMoveGroupState(moveit_msgs::msg::RobotTrajectory t)
+  {
+
+    if (t.joint_trajectory.points.empty())
+    {
+      RCLCPP_ERROR(this->get_logger(), "TRAJECTORY IS EMPTY");
+      return;
+    }
+
+    robot_trajectory::RobotTrajectory rt(move_group.getRobotModel());
+    rt.setRobotTrajectoryMsg(*move_group_state, t);
+    move_group_state = rt.getLastWayPointPtr();
+  }
+
+  void pathToTrajectory(const std::vector<geometry_msgs::msg::PoseStamped> *path, std::vector<moveit_msgs::msg::RobotTrajectory> *ts)
+  {
+    std::vector<geometry_msgs::msg::PoseStamped> penup = {};
+    std::vector<geometry_msgs::msg::PoseStamped> tail = {};
+    bool up = true;
+    if (!path)
+    {
+      RCLCPP_ERROR(this->get_logger(), "Received null pointer for path");
+      return;
+    }
+    for (auto p : *path)
+    {
+      if (p.pose.position.z > 0)
+        up = false;
+      if (up)
+        //penup->push_back(p);
+        penup.push_back(p);
+      else
+      {
+        up = false;
+        //tail->push_back(p);
+        tail.push_back(p);
+      }
+    }
+
+    if (!penup.empty())
+    {
+      auto msr = createMotionSequenceRequest(&penup, "PTP");
+      RCLCPP_ERROR(this->get_logger(), "Created MSR for penup");
+      if (msr.items.empty())
+      {
+        RCLCPP_ERROR(this->get_logger(), "MSR EMPTY");
+        return;
+      }
+      ts->push_back(sendRequest(msr));
+      //planAndLogOffset(&penup);
+      setMoveGroupState(ts->back());
+
+    }
+    else
+    {
+      RCLCPP_ERROR(this->get_logger(), "Penup path is empty, all motions will be LIN");
+    }
+    if (!tail.empty())
+    {
+      auto msr = createMotionSequenceRequest(&tail, "LIN");
+      RCLCPP_ERROR(this->get_logger(), "Created MSR for tail");
+      if (msr.items.empty())
+      {
+        RCLCPP_ERROR(this->get_logger(), "MSR EMPTY");
+        return;
+      }
+      ts->push_back(sendRequest(msr));
+      //planAndLogOffset(&tail);
+      setMoveGroupState(ts->back());
+    }
+    else
+    {
+      RCLCPP_ERROR(this->get_logger(), "Path was empty, no trajectories created");
+    }
+  }
+
   /**
    * Callback that executes path on robot
    */
@@ -423,37 +496,33 @@ public:
     auto feedback = std::make_shared<robot_interfaces::action::ExecuteMotion::Feedback>();
     auto result = std::make_shared<robot_interfaces::action::ExecuteMotion::Result>();
 
-    auto msr = createMotionSequenceRequest(&goal->motion.path);
-    RCLCPP_INFO(this->get_logger(), "Created MSR");
+    std::vector<moveit_msgs::msg::RobotTrajectory> ts;
+    pathToTrajectory(&goal->motion.path, &ts);
 
-    //planAndLogOffset(&goal->motion.path);
+    long unsigned int n = 0;
+
+    RCLCPP_INFO(this->get_logger(), "Got %ld trajectories", ts.size());
+    for (auto t : ts)
+    {
+      RCLCPP_INFO(this->get_logger(), "Adding result to motion queue");
+      if (t.joint_trajectory.points.empty())
+      {
+        RCLCPP_ERROR(this->get_logger(), "TRAJECTORY IS EMPTY");
+        continue;
+      }
+      trajectory_queue.push(t);
+      n++;
+    }
 
-    RCLCPP_INFO(this->get_logger(), "Creating req");
-    auto req = std::make_shared<moveit_msgs::srv::GetMotionSequence::Request>();
-    RCLCPP_INFO(this->get_logger(), "Setting msr request");
-    req->request = msr;
-    RCLCPP_INFO(this->get_logger(), "Sending request to sequence service");
-    auto res = sequence_client_->async_send_request(req);
-    RCLCPP_INFO(this->get_logger(), "Waiting for result");
-    auto ts = res.get()->response.planned_trajectories;
     std::string status = "";
-    if (ts.size() > 0)
+    if (n == ts.size())
     {
       status = "success";
-      RCLCPP_INFO(this->get_logger(), "Got %ld trajectories", ts.size());
-      RCLCPP_INFO(this->get_logger(), "Adding result to motion queue");
-
-      trajectory_queue.push(ts[0]);
-
-      // Set move_group_state to the last state of planned trajectory (planning of next trajectory starts there)
-      robot_trajectory::RobotTrajectory rt(move_group.getRobotModel());
-      rt.setRobotTrajectoryMsg(*move_group_state, ts[0]);
-      move_group_state = rt.getLastWayPointPtr();
-
       status = status + "," + pointsToString(&goal->motion.path,0,0,0);
       appendLineToFile("OUTPUT.csv", status);
 
       result->result = "success";
+
       goal_handle->succeed(result);
       RCLCPP_INFO(this->get_logger(), "Goal succeeded");
       return;