Scaled jtc

joint_trajectory_controller/doc/speed_scaling.rst

@@ -0,0 +1,75 @@
+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>`_.
+
+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 (neglectling acceleration and deceleration influcences 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.
+
+.. todo:: Describe method behind this scaling approach.
+
+
+On-Controller speed scaling
+~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+Conceptionally, 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 by the ``setSpeedScaling``
+service directly.
+
+.. note::
+   The current implementation only works for position-based interfaces.
+
+
+Goal time tolerances
+--------------------
+
+.. todo::
+   What happens to goal time tolerances if we scale down a trajectory?

joint_trajectory_controller/doc/userdoc.rst

@@ -206,6 +206,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/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -23,6 +23,7 @@
 
 #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"
@@ -53,6 +54,14 @@ using namespace std::chrono_literals;  // NOLINT
 namespace joint_trajectory_controller
 {
 
+struct TimeData
+{
+  TimeData() : time(0.0), period(rclcpp::Duration::from_nanoseconds(0.0)), uptime(0.0) {}
+  rclcpp::Time time;
+  rclcpp::Duration period;
+  rclcpp::Time uptime;
+};
+
 class JointTrajectoryController : public controller_interface::ControllerInterface
 {
 public:
@@ -162,6 +171,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
+  double scaling_factor_{1.0};
+  TimeData time_data_;
+
   // Timeout to consider commands old
   double cmd_timeout_;
   // True if holding position or repeating last trajectory point in case of success
@@ -302,8 +315,14 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   void resize_joint_trajectory_point_command(
     trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);
 
+  bool set_scaling_factor(const double scaling_factor);
+
   urdf::Model model_;
 
+  realtime_tools::RealtimeBuffer<double> scaling_factor_rt_buff_;
+  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/src/joint_trajectory_controller.cpp

@@ -16,6 +16,7 @@
 
 #include <chrono>
 #include <functional>
+#include <iostream>
 #include <memory>
 
 #include <string>
@@ -100,12 +101,17 @@ JointTrajectoryController::command_interface_configuration() const
       conf.names.push_back(joint_name + "/" + interface_type);
     }
   }
+  if (!params_.speed_scaling_command_interface_name.empty())
+  {
+    conf.names.push_back(params_.speed_scaling_command_interface_name);
+  }
   return conf;
 }
 
 controller_interface::InterfaceConfiguration
 JointTrajectoryController::state_interface_configuration() const
 {
+  const auto logger = get_node()->get_logger();
   controller_interface::InterfaceConfiguration conf;
   conf.type = controller_interface::interface_configuration_type::INDIVIDUAL;
   conf.names.reserve(dof_ * params_.state_interfaces.size());
@@ -116,12 +122,37 @@ JointTrajectoryController::state_interface_configuration() const
       conf.names.push_back(joint_name + "/" + interface_type);
     }
   }
+  if (!params_.speed_scaling_state_interface_name.empty())
+  {
+    RCLCPP_INFO(
+      logger, "Using scaling state from the hardware from interface %s.",
+      params_.speed_scaling_state_interface_name.c_str());
+    conf.names.push_back(params_.speed_scaling_state_interface_name);
+  }
   return conf;
 }
 
 controller_interface::return_type JointTrajectoryController::update(
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
+  if (params_.speed_scaling_state_interface_name.empty())
+  {
+    scaling_factor_ = *(scaling_factor_rt_buff_.readFromRT());
+  }
+  else
+  {
+    if (state_interfaces_.back().get_name() == params_.speed_scaling_state_interface_name)
+    {
+      scaling_factor_ = state_interfaces_.back().get_value();
+    }
+    else
+    {
+      RCLCPP_ERROR(
+        get_node()->get_logger(), "Speed scaling interface (%s) not found in hardware interface.",
+        params_.speed_scaling_state_interface_name.c_str());
+    }
+  }
+
   if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
     return controller_interface::return_type::OK;
@@ -164,6 +195,15 @@ controller_interface::return_type JointTrajectoryController::update(
   // currently carrying out a trajectory
   if (has_active_trajectory())
   {
+    // Adjust time with scaling factor
+    TimeData time_data;
+    time_data.time = time;
+    rcl_duration_value_t t_period = (time_data.time - time_data_.time).nanoseconds();
+    time_data.period = rclcpp::Duration::from_nanoseconds(t_period) * scaling_factor_;
+    time_data.uptime = time_data_.uptime + time_data.period;
+    rclcpp::Time traj_time = time_data_.uptime + rclcpp::Duration::from_nanoseconds(t_period);
+    time_data_ = time_data;
+
     bool first_sample = false;
     // if sampling the first time, set the point before you sample
     if (!traj_external_point_ptr_->is_sampled_already())
@@ -172,19 +212,19 @@ controller_interface::return_type JointTrajectoryController::update(
       if (params_.open_loop_control)
       {
         traj_external_point_ptr_->set_point_before_trajectory_msg(
-          time, last_commanded_state_, joints_angle_wraparound_);
+          traj_time, last_commanded_state_, joints_angle_wraparound_);
       }
       else
       {
         traj_external_point_ptr_->set_point_before_trajectory_msg(
-          time, state_current_, joints_angle_wraparound_);
+          traj_time, state_current_, joints_angle_wraparound_);
       }
     }
 
     // find segment for current timestamp
     TrajectoryPointConstIter start_segment_itr, end_segment_itr;
     const bool valid_point = traj_external_point_ptr_->sample(
-      time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
+      traj_time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
 
     if (valid_point)
     {
@@ -196,7 +236,7 @@ controller_interface::return_type JointTrajectoryController::update(
       // time_difference is
       // - negative until first point is reached
       // - counting from zero to time_from_start of next point
-      double time_difference = time.seconds() - segment_time_from_start.seconds();
+      const double time_difference = time_data.uptime.seconds() - segment_time_from_start.seconds();
       bool tolerance_violated_while_moving = false;
       bool outside_goal_tolerance = false;
       bool within_goal_time = true;
@@ -866,10 +906,33 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
   resize_joint_trajectory_point(state_error_, dof_);
   resize_joint_trajectory_point(last_commanded_state_, dof_);
 
+  // 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_)
+  {
+    scaling_factor_sub_ = get_node()->create_subscription<SpeedScalingMsg>(
+      "~/speed_scaling_input", rclcpp::SystemDefaultsQoS(),
+      [&](const SpeedScalingMsg & msg) { set_scaling_factor(msg.factor); });
+    RCLCPP_INFO(
+      logger, "Setting initial scaling factor to %2f", params_.scaling_factor_initial_default);
+    scaling_factor_rt_buff_.writeFromNonRT(params_.scaling_factor_initial_default);
+  }
+  else
+  {
+    RCLCPP_WARN(
+      logger,
+      "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_rt_buff_.writeFromNonRT(1.0);
+  }
+
+  // set scaling factor to low value default
+
   return CallbackReturn::SUCCESS;
 }
 
@@ -886,6 +949,11 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
 
   // parse remaining parameters
   default_tolerances_ = get_segment_tolerances(logger, params_);
+  // Setup time_data buffer used for scaling
+  TimeData time_data;
+  time_data_.time = get_node()->now();
+  time_data_.period = rclcpp::Duration::from_nanoseconds(0);
+  time_data_.uptime = get_node()->now();
 
   // order all joints in the storage
   for (const auto & interface : params_.command_interfaces)
@@ -1088,6 +1156,7 @@ void JointTrajectoryController::publish_state(
     {
       state_publisher_->msg_.output = command_current_;
     }
+    state_publisher_->msg_.speed_scaling_factor = *(scaling_factor_rt_buff_.readFromRT());
 
     state_publisher_->unlockAndPublish();
   }
@@ -1563,6 +1632,45 @@ void JointTrajectoryController::resize_joint_trajectory_point_command(
   }
 }
 
+bool JointTrajectoryController::set_scaling_factor(const 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;
+  }
+
+  RCLCPP_INFO(get_node()->get_logger(), "New scaling factor will be %f", scaling_factor);
+  if (params_.speed_scaling_command_interface_name.empty())
+  {
+    if (!params_.speed_scaling_state_interface_name.empty())
+    {
+      RCLCPP_WARN(
+        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");
+    }
+    scaling_factor_rt_buff_.writeFromNonRT(scaling_factor);
+  }
+  else
+  {
+    if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+    {
+      for (auto & interface : command_interfaces_)
+      {
+        if (interface.get_name() == params_.speed_scaling_command_interface_name)
+        {
+          interface.set_value(static_cast<double>(scaling_factor));
+        }
+      }
+    }
+  }
+  return true;
+}
+
 bool JointTrajectoryController::has_active_trajectory() const
 {
   return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg();

joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

@@ -39,6 +39,26 @@ joint_trajectory_controller:
       "joint_trajectory_controller::state_interface_type_combinations": null,
     }
   }
+  scaling_factor_initial_default: {
+    type: double,
+    default_value: 1.0,
+    description: "The initial value of the scaling factor if not exchange with hardware takes place.",
+    validation: {
+      gt_eq<>: 0.0,
+    }
+  }
+  speed_scaling_state_interface_name: {
+    type: string,
+    default_value: "",
+    read_only: true,
+    description: "Fully qualified name of the speed scaling state interface name"
+  }
+  speed_scaling_command_interface_name: {
+    type: string,
+    default_value: "",
+    read_only: true,
+    description: "Command interface name used for setting the speed scaling factor on the hardware."
+  }
   allow_partial_joints_goal: {
     type: bool,
     default_value: false,