Add speed scaling support to JTC (#1191)

Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>
Co-authored-by: Manuel M <mamueluth@gmail.com>
Co-authored-by: Lennart Nachtigall <mail@firesurfer.de>
Co-authored-by: Dr. Denis <denis@stoglrobotics.de>
Co-authored-by: Sai Kishor Kothakota <saisastra3@gmail.com>
(cherry picked from commit 4a42b1f)

# Conflicts:
#	doc/release_notes.rst

Author: fmauch

doc/release_notes.rst

@@ -72,6 +72,7 @@ joint_trajectory_controller
 * Feed-forward effort trajectories are supported now (`#1200 <https://github.com/ros-controls/ros2_controllers/pull/1200>`_).
 * Parameter ``open_loop_control`` is replaced by ``interpolate_from_desired_state`` and setting the feedback gains to zero (`#1525 <https://github.com/ros-controls/ros2_controllers/pull/1525>`_).
 * The controller now supports the new anti-windup strategy of the PID class, which allows for more flexible control of the anti-windup behavior. (`#1759 <https://github.com/ros-controls/ros2_controllers/pull/1759>`__).
+<<<<<<< HEAD
 * Fill in 0 velocities and accelerations into point before trajectories if the state interfaces
   don't contain velocity / acceleration information, but the trajectory does. This way, the segment
   up to the first waypoint will use the same interpolation as the rest of the trajectory. (`#2043
@@ -88,6 +89,12 @@ The ``parallel_gripper_action_controller/GripperActionController`` was added. `(
 omni_wheel_drive_controller
 *********************************
 * 🚀 The omni_wheel_drive_controller was added 🎉 (`#1535 <https://github.com/ros-controls/ros2_controllers/pull/1535>`_).
+=======
+* Scaling support was added in `#1191
+  <https://github.com/ros-controls/ros2_controllers/pull/1191>`__. With this the controller
+  "stretches the time" with which it progresses in the trajectory. Scaling can either be set
+  manually or it can be synchronized with the hardware. See :ref:`jtc_speed_scaling` for details.
+>>>>>>> 4a42b1f (Add speed scaling support to JTC (#1191))
 
 pid_controller
 ************************

joint_trajectory_controller/doc/speed_scaling.rst

@@ -0,0 +1,110 @@
+.. _jtc_speed_scaling:
+
+Speed scaling
+=============
+
+The ``joint_trajectory_controller`` (JTC) supports dynamically scaling its trajectory execution speed.
+That means, when specifying a scaling factor :math:`{f}` of less than 1, execution will proceed only
+:math:`{f \cdot \Delta_t}` per control step where :math:`{\Delta_t}` is the controller's cycle time.
+
+Methods of speed scaling
+------------------------
+
+Generally, the speed scaling feature has two separate scaling approaches in mind: On-Robot scaling
+and On-Controller scaling. They are both conceptually different and to correctly configure speed
+scaling it is important to understand the differences.
+
+On-Robot speed scaling
+~~~~~~~~~~~~~~~~~~~~~~
+
+This scaling method is intended for robots that provide a scaling feature directly on the robot's
+teach pendant and / or through a safety feature. One example of such robots are the `Universal
+Robots manipulators <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver>`_.
+
+The hardware interface needs to report the speed scaling through a state interface so it can be
+read by the controller. Optionally, a command interface for setting the speed scaling value on the
+hardware can be provided (if applicable) in order to set speed scaling through a ROS topic.
+
+For the scope of this documentation a user-defined scaling and a safety-limited scaling will be
+treated the same resulting in a "hardware scaling factor".
+
+In this setup, the hardware will treat the command sent from the ROS controller (e.g. Reach joint
+configuration :math:`{\theta}` within :math:`{\Delta_t}` seconds.). This effectively means that the
+robot will only make half of the way towards the target configuration when a scaling factor of 0.5
+is given (neglecting acceleration and deceleration influences during this time period).
+
+The following plot shows trajectory execution (for one joint) with a hardware-scaled execution and
+a controller that is **not** aware of speed scaling:
+
+.. image:: traj_without_speed_scaling.png
+   :alt: Trajectory with a hardware-scaled-down execution with a non-scaled controller
+
+The graph shows a trajectory with one joint being moved to a target point and back to its starting
+point. As the joint's speed is limited to a very low setting on the teach pendant, speed scaling
+(black line) activates and limits the joint speed (green line). As a result, the target trajectory
+(light blue) doesn't get executed by the robot, but instead the pink trajectory is executed. The
+vertical distance between the light blue line and the pink line is the path error in each control
+cycle. We can see that the path deviation gets above 300 degrees at some point and the target point
+at -6 radians never gets reached.
+
+With the scaled version of the trajectory controller the example motion shown in the previous diagram becomes:
+
+.. image:: traj_with_speed_scaling.png
+   :alt: Trajectory with a hardware-scaled-down execution with a scaled controller
+
+The deviation between trajectory interpolation on the ROS side and actual robot execution stays
+minimal and the robot reaches the intermediate setpoint instead of returning "too early" as in the
+example above.
+
+Scaling is done in such a way, that the time in the trajectory is virtually scaled. For example, if
+a controller runs with a cycle time of 100 Hz, each control cycle is 10 ms long. A speed scaling of
+0.5 means that in each time step the trajectory is moved forward by 5 ms instead.
+So, the beginning of the 3rd timestep is 15 ms instead of 30 ms in the trajectory.
+
+Command sampling is performed as in the unscaled case, with the timestep's start plus the **full**
+cycle time of 10 ms. The robot will scale down the motion command by 50% resulting in only half of
+the distance being executed, which is why the next control cycle will be started at the current
+start plus half of the step time.
+
+
+On-Controller speed scaling
+~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+Conceptually, with this scaling the robot hardware isn't aware of any scaling happening. The JTC
+generates commands to be sent to the robot that are already scaled down accordingly, so they can be
+directly executed by the robot.
+
+Since the hardware isn't aware of speed scaling, the speed-scaling related command and state
+interfaces should not be specified and the scaling factor will be set through the
+``~/speed_scaling_input`` topic directly:
+
+.. code:: console
+
+   $ ros2 topic pub --qos-durability transient_local --once \
+     /joint_trajectory_controller/speed_scaling_input control_msgs/msg/SpeedScalingFactor "{factor: 0.5}"
+
+
+.. note::
+   The ``~/speed_scaling_input`` topic uses the QoS durability profile ``transient_local``. This
+   means you can restart the controller while still having a publisher on that topic active.
+
+.. note::
+   The current implementation only works for position-based interfaces.
+
+
+Effect on tolerances
+--------------------
+
+When speed scaling is used while executing a trajectory, the tolerances configured for execution
+will be scaled, as well.
+
+Since commands are generated from the scaled trajectory time, **path errors** will also be compared
+to the scaled trajectory.
+
+The **goal time tolerance** also uses the virtual trajectory time. This means that a trajectory
+being executed with a constant scaling factor of 0.5 will take twice as long for execution than the
+``time_from_start`` value of the last trajectory point specifies. As long as the robot doesn't take
+longer than that the goal time tolerance is considered to be met.
+
+If an application relies on the actual execution time as set in the ``time_from_start`` fields, an
+external monitoring has to be wrapped around the trajectory execution action.

joint_trajectory_controller/doc/traj_with_speed_scaling.png

@@ -209,6 +209,7 @@ Further information
    :titlesonly:
 
    Trajectory Representation <trajectory.rst>
+   Speed scaling <speed_scaling.rst>
    joint_trajectory_controller Parameters <parameters.rst>
    rqt_joint_trajectory_controller <../../rqt_joint_trajectory_controller/doc/userdoc.rst>
 

joint_trajectory_controller/doc/traj_without_speed_scaling.png

@@ -23,9 +23,11 @@
 
 #include "control_msgs/action/follow_joint_trajectory.hpp"
 #include "control_msgs/msg/joint_trajectory_controller_state.hpp"
+#include "control_msgs/msg/speed_scaling_factor.hpp"
 #include "control_msgs/srv/query_trajectory_state.hpp"
 #include "control_toolbox/pid.hpp"
 #include "controller_interface/controller_interface.hpp"
+#include "hardware_interface/loaned_command_interface.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "joint_trajectory_controller/interpolation_methods.hpp"
 #include "joint_trajectory_controller/tolerances.hpp"
@@ -123,6 +125,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
 
   InterfaceReferences<hardware_interface::LoanedCommandInterface> joint_command_interface_;
   InterfaceReferences<hardware_interface::LoanedStateInterface> joint_state_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
+    scaling_state_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    scaling_command_interface_;
 
   bool has_position_state_interface_ = false;
   bool has_velocity_state_interface_ = false;
@@ -144,6 +150,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   // reserved storage for result of the command when closed loop pid adapter is used
   std::vector<double> tmp_command_;
 
+  // Things around speed scaling
+  std::atomic<double> scaling_factor_{1.0};
+  std::atomic<double> scaling_factor_cmd_{1.0};
+
   // Timeout to consider commands old
   double cmd_timeout_;
   // True if holding position or repeating last trajectory point in case of success
@@ -269,6 +279,27 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   void resize_joint_trajectory_point_command(
     trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double value = 0.0);
 
+  /**
+   * @brief Set scaling factor used for speed scaling trajectory execution
+   *
+   * If the hardware supports and has configured setting speed scaling, that will be sent to the
+   * hardware's command interface.
+   *
+   * If the hardware doesn't support a command interface for speed scaling, but a state interface
+   * for reading speed scaling, calling this function will have no effect, as the factor will be
+   * overwritten by the state interface.
+   *
+   * @param scaling_factor has to be >= 0
+   *
+   * @return True if the value was valid and set, false if the value is < 0
+   * interval
+   *
+   */
+  bool set_scaling_factor(double scaling_factor);
+
+  using SpeedScalingMsg = control_msgs::msg::SpeedScalingFactor;
+  rclcpp::Subscription<SpeedScalingMsg>::SharedPtr scaling_factor_sub_;
+
   /**
    * @brief Assigns the values from a trajectory point interface to a joint interface.
    *

joint_trajectory_controller/doc/userdoc.rst

@@ -113,6 +113,10 @@ JointTrajectoryController::command_interface_configuration() const
       conf.names.push_back(joint_name + "/" + interface_type);
     }
   }
+  if (!params_.speed_scaling.command_interface.empty())
+  {
+    conf.names.push_back(params_.speed_scaling.command_interface);
+  }
   return conf;
 }
 
@@ -129,12 +133,30 @@ JointTrajectoryController::state_interface_configuration() const
       conf.names.push_back(joint_name + "/" + interface_type);
     }
   }
+  if (!params_.speed_scaling.state_interface.empty())
+  {
+    conf.names.push_back(params_.speed_scaling.state_interface);
+  }
   return conf;
 }
 
 controller_interface::return_type JointTrajectoryController::update(
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
+  if (scaling_state_interface_.has_value())
+  {
+    scaling_factor_ = scaling_state_interface_->get().get_value();
+  }
+
+  if (scaling_command_interface_.has_value())
+  {
+    if (!scaling_command_interface_->get().set_value(scaling_factor_cmd_.load()))
+    {
+      RCLCPP_ERROR(
+        get_node()->get_logger(), "Could not set speed scaling factor through command interfaces.");
+    }
+  }
+
   auto logger = this->get_node()->get_logger();
   // update dynamic parameters
   if (param_listener_->is_old(params_))
@@ -197,7 +219,7 @@ controller_interface::return_type JointTrajectoryController::update(
     }
     else
     {
-      traj_time_ += period;
+      traj_time_ += period * scaling_factor_.load();
     }
 
     // Sample expected state from the trajectory
@@ -999,10 +1021,47 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
   resize_joint_trajectory_point(
     last_commanded_state_, dof_, std::numeric_limits<double>::quiet_NaN());
 
+  // create services
   query_state_srv_ = get_node()->create_service<control_msgs::srv::QueryTrajectoryState>(
     std::string(get_node()->get_name()) + "/query_state",
     std::bind(&JointTrajectoryController::query_state_service, this, _1, _2));
 
+  if (
+    !has_velocity_command_interface_ && !has_acceleration_command_interface_ &&
+    !has_effort_command_interface_)
+  {
+    auto qos = rclcpp::SystemDefaultsQoS();
+    qos.transient_local();
+    scaling_factor_sub_ = get_node()->create_subscription<SpeedScalingMsg>(
+      "~/speed_scaling_input", qos,
+      [&](const SpeedScalingMsg & msg) { set_scaling_factor(msg.factor); });
+    RCLCPP_INFO(
+      logger, "Setting initial scaling factor to %2f",
+      params_.speed_scaling.initial_scaling_factor);
+    scaling_factor_ = params_.speed_scaling.initial_scaling_factor;
+  }
+  else
+  {
+    RCLCPP_WARN_EXPRESSION(
+      logger, params_.speed_scaling.initial_scaling_factor != 1.0,
+      "Speed scaling is currently only supported for position interfaces. If you want to make use "
+      "of speed scaling, please only use a position interface when configuring this controller.");
+    scaling_factor_ = 1.0;
+  }
+  if (!params_.speed_scaling.state_interface.empty())
+  {
+    RCLCPP_INFO(
+      logger, "Using scaling state from the hardware from interface %s.",
+      params_.speed_scaling.state_interface.c_str());
+  }
+  else
+  {
+    RCLCPP_INFO(
+      get_node()->get_logger(),
+      "No scaling interface set. This controller will not read speed scaling from the hardware.");
+  }
+  scaling_factor_cmd_.store(scaling_factor_.load());
+
   if (get_update_rate() == 0)
   {
     throw std::runtime_error("Controller's update rate is set to 0. This should not happen!");
@@ -1027,6 +1086,42 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
   // parse remaining parameters
   default_tolerances_ = get_segment_tolerances(logger, params_);
 
+  // Set scaling interfaces
+  if (!params_.speed_scaling.state_interface.empty())
+  {
+    auto it = std::find_if(
+      state_interfaces_.begin(), state_interfaces_.end(), [&](auto & interface)
+      { return (interface.get_name() == params_.speed_scaling.state_interface); });
+    if (it != state_interfaces_.end())
+    {
+      scaling_state_interface_ = *it;
+    }
+    else
+    {
+      RCLCPP_ERROR(
+        logger, "Did not find speed scaling interface '%s' in state interfaces.",
+        params_.speed_scaling.state_interface.c_str());
+      return CallbackReturn::ERROR;
+    }
+  }
+  if (!params_.speed_scaling.command_interface.empty())
+  {
+    auto it = std::find_if(
+      command_interfaces_.begin(), command_interfaces_.end(), [&](auto & interface)
+      { return (interface.get_name() == params_.speed_scaling.command_interface); });
+    if (it != command_interfaces_.end())
+    {
+      scaling_command_interface_ = *it;
+    }
+    else
+    {
+      RCLCPP_ERROR(
+        logger, "Did not find speed scaling interface '%s' in command interfaces.",
+        params_.speed_scaling.command_interface.c_str());
+      return CallbackReturn::ERROR;
+    }
+  }
+
   // order all joints in the storage
   for (const auto & interface : params_.command_interfaces)
   {
@@ -1242,6 +1337,7 @@ void JointTrajectoryController::publish_state(
     {
       state_msg_.output = command_current_;
     }
+    state_publisher_->msg_.speed_scaling_factor = scaling_factor_;
 
     state_publisher_->try_publish(state_msg_);
   }
@@ -1757,6 +1853,40 @@ void JointTrajectoryController::resize_joint_trajectory_point_command(
   }
 }
 
+bool JointTrajectoryController::set_scaling_factor(double scaling_factor)
+{
+  if (scaling_factor < 0)
+  {
+    RCLCPP_WARN(
+      get_node()->get_logger(),
+      "Scaling factor has to be greater or equal to 0.0 - Ignoring input!");
+    return false;
+  }
+
+  if (scaling_factor != scaling_factor_.load())
+  {
+    RCLCPP_INFO(
+      get_node()->get_logger().get_child("speed_scaling"), "New scaling factor will be %f",
+      scaling_factor);
+  }
+  scaling_factor_.store(scaling_factor);
+  if (
+    params_.speed_scaling.command_interface.empty() &&
+    !params_.speed_scaling.state_interface.empty())
+  {
+    RCLCPP_WARN_ONCE(
+      get_node()->get_logger(),
+      "Setting the scaling factor while only one-way communication with the hardware is setup. "
+      "This will likely get overwritten by the hardware again. If available, please also setup "
+      "the speed_scaling_command_interface_name");
+  }
+  else
+  {
+    scaling_factor_cmd_.store(scaling_factor);
+  }
+  return true;
+}
+
 bool JointTrajectoryController::has_active_trajectory() const
 {
   return current_trajectory_ != nullptr && current_trajectory_->has_trajectory_msg();