Add command queue to servo to account for latency

moveit_ros/moveit_servo/config/panda_simulated_config.yaml

@@ -3,10 +3,12 @@
 ###############################################
 
 # Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling)
-# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]
+# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0], 0.0 disables it
+# override_acceleration_scaling_factor = 0.0 # valid range [0.0:1.0], 0.0 disables it
 
 ## Properties of outgoing commands
-publish_period: 0.034  # 1/Nominal publish rate [seconds]
+publish_period: 0.01  # 1/Nominal publish rate [seconds]
+max_expected_latency: 0.1  # delay between sending a command and the robot executing it [seconds]
 
 command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
 scale:

moveit_ros/moveit_servo/config/servo_parameters.yaml

@@ -16,13 +16,24 @@ servo:
   publish_period: {
     type: double,
     read_only: true,
-    default_value: 0.034,
+    default_value: 0.01,
     description: " 1 / (Nominal publish rate) [seconds]",
     validation: {
       gt<>: 0.0
     }
   }
 
+  max_expected_latency: {
+    type: double,
+    read_only: true,
+    default_value: 0.1,
+    description: "Maximum expected latency between generating a servo \
+                  command and the controller receiving it [seconds].",
+    validation: {
+      gt<>: 0.0
+    }
+  }
+
   move_group_name: {
     type: string,
     read_only: true,
@@ -318,5 +329,8 @@ servo:
   override_velocity_scaling_factor: {
     type: double,
     default_value: 0.0,
-    description: "Scaling factor when servo overrides the velocity (eg: near singularities)"
+    description: "Scaling factor when servo overrides the velocity (eg: near singularities)",
+    validation: {
+      bounds<>: [0.0, 1.0]
+    }
   }

moveit_ros/moveit_servo/config/test_config_panda.yaml

@@ -24,7 +24,7 @@ command_out_type: trajectory_msgs/JointTrajectory
 # What to publish? Can save some bandwidth as most robots only require positions or velocities
 publish_joint_positions: true
 publish_joint_velocities: true
-publish_joint_accelerations: false
+publish_joint_accelerations: true
 
 ## Plugins for smoothing outgoing commands
 use_smoothing: false

moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp

@@ -75,10 +75,16 @@ int main(int argc, char* argv[])
   // This is just for convenience, should not be used for sync in real application.
   std::this_thread::sleep_for(std::chrono::seconds(3));
 
+  // Get the robot state and joint model group info.
+  auto robot_state = planning_scene_monitor->getStateMonitor()->getCurrentState();
+  const moveit::core::JointModelGroup* joint_model_group =
+      robot_state->getJointModelGroup(servo_params.move_group_name);
+
   // Set the command type for servo.
   servo.setCommandType(CommandType::JOINT_JOG);
-  // JointJog command that moves only the 7th joint at +1.0 rad/s
-  JointJogCommand joint_jog{ { "panda_joint7" }, { 1.0 } };
+
+  // JointJog command that moves only the 7th joint at +0.4 rad/s
+  JointJogCommand joint_jog{ { "panda_joint7" }, { 0.4 } };
 
   // Frequency at which commands will be sent to the robot controller.
   rclcpp::WallRate rate(1.0 / servo_params.publish_period);
@@ -87,10 +93,13 @@ int main(int argc, char* argv[])
   std::chrono::seconds time_elapsed(0);
   auto start_time = std::chrono::steady_clock::now();
 
+  // create command queue to build trajectory message
+  std::deque<KinematicState> joint_cmd_rolling_window;
+
   RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage());
   while (rclcpp::ok())
   {
-    const KinematicState joint_state = servo.getNextJointState(joint_jog);
+    KinematicState joint_state = servo.getNextJointState(robot_state, joint_jog);
     const StatusCode status = servo.getStatus();
 
     auto current_time = std::chrono::steady_clock::now();
@@ -102,7 +111,16 @@ int main(int argc, char* argv[])
     }
     else if (status != StatusCode::INVALID)
     {
-      trajectory_outgoing_cmd_pub->publish(composeTrajectoryMessage(servo_params, joint_state));
+      updateSlidingWindow(joint_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now());
+      if (const auto msg = composeTrajectoryMessage(servo_params, joint_cmd_rolling_window))
+      {
+        trajectory_outgoing_cmd_pub->publish(msg.value());
+      }
+      if (!joint_cmd_rolling_window.empty())
+      {
+        robot_state->setJointGroupPositions(joint_model_group, joint_cmd_rolling_window.back().positions);
+        robot_state->setJointGroupVelocities(joint_model_group, joint_cmd_rolling_window.back().velocities);
+      }
     }
     rate.sleep();
   }

moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp

@@ -81,87 +81,91 @@ int main(int argc, char* argv[])
   Servo servo = Servo(demo_node, servo_param_listener, planning_scene_monitor);
 
   // Helper function to get the current pose of a specified frame.
-  const auto get_current_pose = [planning_scene_monitor](const std::string& target_frame) {
-    return planning_scene_monitor->getStateMonitor()->getCurrentState()->getGlobalLinkTransform(target_frame);
+  const auto get_current_pose = [](const std::string& target_frame, const moveit::core::RobotStatePtr& robot_state) {
+    return robot_state->getGlobalLinkTransform(target_frame);
   };
 
   // Wait for some time, so that the planning scene is loaded in rviz.
   // This is just for convenience, should not be used for sync in real application.
   std::this_thread::sleep_for(std::chrono::seconds(3));
 
-  // For syncing pose tracking thread and main thread.
-  std::mutex pose_guard;
-  std::atomic<bool> stop_tracking = false;
+  // Get the robot state and joint model group info.
+  auto robot_state = planning_scene_monitor->getStateMonitor()->getCurrentState();
+  const moveit::core::JointModelGroup* joint_model_group =
+      robot_state->getJointModelGroup(servo_params.move_group_name);
 
   // Set the command type for servo.
   servo.setCommandType(CommandType::POSE);
 
   // The dynamically updated target pose.
   PoseCommand target_pose;
   target_pose.frame_id = K_BASE_FRAME;
-  // Initializing the target pose as end effector pose, this can be any pose.
-  target_pose.pose = get_current_pose(K_TIP_FRAME);
-
-  // The pose tracking lambda that will be run in a separate thread.
-  auto pose_tracker = [&]() {
-    KinematicState joint_state;
-    rclcpp::WallRate tracking_rate(1 / servo_params.publish_period);
-    while (!stop_tracking && rclcpp::ok())
-    {
-      {
-        std::lock_guard<std::mutex> pguard(pose_guard);
-        joint_state = servo.getNextJointState(target_pose);
-      }
-      StatusCode status = servo.getStatus();
-      if (status != StatusCode::INVALID)
-        trajectory_outgoing_cmd_pub->publish(composeTrajectoryMessage(servo_params, joint_state));
 
-      tracking_rate.sleep();
-    }
-  };
+  // Initializing the target pose as end effector pose, this can be any pose.
+  target_pose.pose = get_current_pose(K_TIP_FRAME, robot_state);
 
-  // Pose tracking thread will exit upon reaching this pose.
+  // servo loop will exit upon reaching this pose.
   Eigen::Isometry3d terminal_pose = target_pose.pose;
   terminal_pose.rotate(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ()));
   terminal_pose.translate(Eigen::Vector3d(0.0, 0.0, -0.1));
 
-  std::thread tracker_thread(pose_tracker);
-  tracker_thread.detach();
-
   // The target pose (frame being tracked) moves by this step size each iteration.
-  Eigen::Vector3d linear_step_size{ 0.0, 0.0, -0.002 };
+  Eigen::Vector3d linear_step_size{ 0.0, 0.0, -0.001 };
   Eigen::AngleAxisd angular_step_size(0.01, Eigen::Vector3d::UnitZ());
 
-  // Frequency at which commands will be sent to the robot controller.
-  rclcpp::WallRate command_rate(50);
   RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage());
 
+  KinematicState joint_state;
+  KinematicState current_state = servo.getCurrentRobotState();
+  rclcpp::WallRate servo_rate(1 / servo_params.publish_period);
+
+  // create command queue to build trajectory message
+  std::deque<KinematicState> joint_cmd_rolling_window;
+
+  bool satisfies_linear_tolerance = false;
+  bool satisfies_angular_tolerance = false;
+  bool stop_tracking = false;
   while (!stop_tracking && rclcpp::ok())
   {
+    // check if goal reached
+    target_pose.pose = get_current_pose(K_TIP_FRAME, robot_state);
+    satisfies_linear_tolerance |= target_pose.pose.translation().isApprox(terminal_pose.translation(),
+                                                                          servo_params.pose_tracking.linear_tolerance);
+    satisfies_angular_tolerance |=
+        target_pose.pose.rotation().isApprox(terminal_pose.rotation(), servo_params.pose_tracking.angular_tolerance);
+    stop_tracking = satisfies_linear_tolerance && satisfies_angular_tolerance;
+
+    // Dynamically update the target pose.
+    if (!satisfies_linear_tolerance)
     {
-      std::lock_guard<std::mutex> pguard(pose_guard);
-      target_pose.pose = get_current_pose(K_TIP_FRAME);
-      const bool satisfies_linear_tolerance = target_pose.pose.translation().isApprox(
-          terminal_pose.translation(), servo_params.pose_tracking.linear_tolerance);
-      const bool satisfies_angular_tolerance =
-          target_pose.pose.rotation().isApprox(terminal_pose.rotation(), servo_params.pose_tracking.angular_tolerance);
-      stop_tracking = satisfies_linear_tolerance && satisfies_angular_tolerance;
-      // Dynamically update the target pose.
-      if (!satisfies_linear_tolerance)
-        target_pose.pose.translate(linear_step_size);
-      if (!satisfies_angular_tolerance)
-        target_pose.pose.rotate(angular_step_size);
+      target_pose.pose.translate(linear_step_size);
+    }
+    if (!satisfies_angular_tolerance)
+    {
+      target_pose.pose.rotate(angular_step_size);
     }
 
-    command_rate.sleep();
+    // get next servo command
+    joint_state = servo.getNextJointState(robot_state, target_pose);
+    StatusCode status = servo.getStatus();
+    if (status != StatusCode::INVALID)
+    {
+      updateSlidingWindow(joint_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now());
+      if (const auto msg = composeTrajectoryMessage(servo_params, joint_cmd_rolling_window))
+      {
+        trajectory_outgoing_cmd_pub->publish(msg.value());
+      }
+      if (!joint_cmd_rolling_window.empty())
+      {
+        robot_state->setJointGroupPositions(joint_model_group, joint_cmd_rolling_window.back().positions);
+        robot_state->setJointGroupVelocities(joint_model_group, joint_cmd_rolling_window.back().velocities);
+      }
+    }
+
+    servo_rate.sleep();
   }
 
   RCLCPP_INFO_STREAM(demo_node->get_logger(), "REACHED : " << stop_tracking);
-  stop_tracking = true;
-
-  if (tracker_thread.joinable())
-    tracker_thread.join();
-
   RCLCPP_INFO(demo_node->get_logger(), "Exiting demo.");
   rclcpp::shutdown();
 }

moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp

@@ -77,24 +77,32 @@ int main(int argc, char* argv[])
   // This is just for convenience, should not be used for sync in real application.
   std::this_thread::sleep_for(std::chrono::seconds(3));
 
+  // Get the robot state and joint model group info.
+  auto robot_state = planning_scene_monitor->getStateMonitor()->getCurrentState();
+  const moveit::core::JointModelGroup* joint_model_group =
+      robot_state->getJointModelGroup(servo_params.move_group_name);
+
   // Set the command type for servo.
   servo.setCommandType(CommandType::TWIST);
 
-  // Move end effector in the +z direction at 10 cm/s
-  // while turning around z axis in the +ve direction at 0.5 rad/s
-  TwistCommand target_twist{ "panda_link0", { 0.0, 0.0, 0.1, 0.0, 0.0, 0.5 } };
+  // Move end effector in the +z direction at 5 cm/s
+  // while turning around z axis in the +ve direction at 0.4 rad/s
+  TwistCommand target_twist{ "panda_link0", { 0.0, 0.0, 0.05, 0.0, 0.0, 0.4 } };
 
   // Frequency at which commands will be sent to the robot controller.
   rclcpp::WallRate rate(1.0 / servo_params.publish_period);
 
-  std::chrono::seconds timeout_duration(5);
+  std::chrono::seconds timeout_duration(4);
   std::chrono::seconds time_elapsed(0);
   auto start_time = std::chrono::steady_clock::now();
 
+  // create command queue to build trajectory message
+  std::deque<KinematicState> joint_cmd_rolling_window;
+
   RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage());
   while (rclcpp::ok())
   {
-    const KinematicState joint_state = servo.getNextJointState(target_twist);
+    KinematicState joint_state = servo.getNextJointState(robot_state, target_twist);
     const StatusCode status = servo.getStatus();
 
     auto current_time = std::chrono::steady_clock::now();
@@ -106,7 +114,16 @@ int main(int argc, char* argv[])
     }
     else if (status != StatusCode::INVALID)
     {
-      trajectory_outgoing_cmd_pub->publish(composeTrajectoryMessage(servo_params, joint_state));
+      updateSlidingWindow(joint_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now());
+      if (const auto msg = composeTrajectoryMessage(servo_params, joint_cmd_rolling_window))
+      {
+        trajectory_outgoing_cmd_pub->publish(msg.value());
+      }
+      if (!joint_cmd_rolling_window.empty())
+      {
+        robot_state->setJointGroupPositions(joint_model_group, joint_cmd_rolling_window.back().positions);
+        robot_state->setJointGroupVelocities(joint_model_group, joint_cmd_rolling_window.back().velocities);
+      }
     }
     rate.sleep();
   }

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

@@ -54,6 +54,7 @@
 #include <tf2_ros/transform_listener.h>
 #include <variant>
 #include <rclcpp/logger.hpp>
+#include <queue>
 
 namespace moveit_servo
 {
@@ -74,10 +75,11 @@ class Servo
 
   /**
    * \brief Computes the joint state required to follow the given command.
+   * @param robot_state RobotStatePtr instance used for calculating the next joint state.
    * @param command The command to follow, std::variant type, can handle JointJog, Twist and Pose.
    * @return The required joint state.
    */
-  KinematicState getNextJointState(const ServoInput& command);
+  KinematicState getNextJointState(const moveit::core::RobotStatePtr& robot_state, const ServoInput& command);
 
   /**
    * \brief Set the type of incoming servo command.
@@ -122,10 +124,12 @@ class Servo
 
   /**
    * \brief Smoothly halt at a commanded state when command goes stale.
-   * @param The last commanded joint states.
+   * @param robot_state A RobotStatePtr instance.
+   * @param halt_state The desired stop state.
    * @return The next state stepping towards the required halting state.
    */
-  std::pair<bool, KinematicState> smoothHalt(const KinematicState& halt_state);
+  std::pair<bool, KinematicState> smoothHalt(const moveit::core::RobotStatePtr& robot_state,
+                                             const KinematicState& halt_state);
 
   /**
    * \brief Applies smoothing to an input state, if a smoothing plugin is set.
@@ -175,15 +179,11 @@ class Servo
   /**
    * \brief Compute the change in joint position required to follow the received command.
    * @param command The incoming servo command.
+   * @param robot_state RobotStatePtr instance used for calculating the command.
    * @return The joint position change required (delta).
    */
   Eigen::VectorXd jointDeltaFromCommand(const ServoInput& command, const moveit::core::RobotStatePtr& robot_state);
 
-  /**
-   * \brief Updates data depending on joint model group
-   */
-  void updateJointModelGroup();
-
   /**
    * \brief Validate the servo parameters
    * @param servo_params The servo parameters

moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp

@@ -99,9 +99,9 @@ class ServoNode
   void twistCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg);
   void poseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg);
 
-  std::optional<KinematicState> processJointJogCommand();
-  std::optional<KinematicState> processTwistCommand();
-  std::optional<KinematicState> processPoseCommand();
+  std::optional<KinematicState> processJointJogCommand(const moveit::core::RobotStatePtr& robot_state);
+  std::optional<KinematicState> processTwistCommand(const moveit::core::RobotStatePtr& robot_state);
+  std::optional<KinematicState> processPoseCommand(const moveit::core::RobotStatePtr& robot_state);
 
   // Variables
 
@@ -132,6 +132,9 @@ class ServoNode
 
   // Threads used by ServoNode
   std::thread servo_loop_thread_;
+
+  // rolling window of joint commands
+  std::deque<KinematicState> joint_cmd_rolling_window_;
 };
 
 }  // namespace moveit_servo