Update scaled JTC (#1753)

urfeex · web-flow · commit 4683ee695d8a · 2026-04-09T10:26:03.000+02:00
This commits updates the scaled JTC to come closer to what we have
available from Jazzy on.

Added tests for ScaledJointTrajectoryController
diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt
@@ -184,6 +184,31 @@ if(BUILD_TESTING)
     controller_manager
     ros2_control_test_assets
   )
+  ament_add_gmock(test_load_scaled_joint_trajectory_controller
+    test/test_load_scaled_joint_trajectory_controller.cpp
+  )
+  target_link_libraries(test_load_scaled_joint_trajectory_controller
+    ${PROJECT_NAME}
+  )
+  ament_target_dependencies(test_load_scaled_joint_trajectory_controller
+    controller_manager
+    ros2_control_test_assets
+  )
+  ament_add_gmock(test_scaled_joint_trajectory_controller_sampling
+    test/test_scaled_joint_trajectory_controller_sampling.cpp
+  )
+  target_link_libraries(test_scaled_joint_trajectory_controller_sampling
+    ${PROJECT_NAME}
+  )
+  ament_target_dependencies(test_scaled_joint_trajectory_controller_sampling
+    controller_interface
+    hardware_interface
+    joint_trajectory_controller
+    lifecycle_msgs
+    rclcpp
+    rclcpp_lifecycle
+    trajectory_msgs
+  )
   ament_add_gmock(test_load_friction_model_controller
     test/test_load_friction_model_controller.cpp
   )
diff --git a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp
@@ -57,26 +57,23 @@ class ScaledJointTrajectoryController : public joint_trajectory_controller::Join
 
   controller_interface::InterfaceConfiguration state_interface_configuration() const override;
 
+  controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& state) override;
+
   controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& state) override;
 
   controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
 
   CallbackReturn on_init() override;
 
-protected:
-  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;
-  };
-
 private:
   double scaling_factor_{ 1.0 };
-  realtime_tools::RealtimeBuffer<TimeData> time_data_;
+  void update_pids();
+  rclcpp::Duration update_period_{ 0, 0 };
+
+  trajectory_msgs::msg::JointTrajectoryPoint command_next_;
+
+  rclcpp::Time last_commanded_time_;
+  rclcpp::Time traj_time_;
 
   std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> scaling_state_interface_ =
       std::nullopt;
diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp
@@ -72,14 +72,15 @@ controller_interface::InterfaceConfiguration ScaledJointTrajectoryController::st
   return conf;
 }
 
-controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activate(const rclcpp_lifecycle::State& state)
+controller_interface::CallbackReturn ScaledJointTrajectoryController::on_configure(const rclcpp_lifecycle::State& state)
 {
-  TimeData time_data;
-  time_data.time = get_node()->now();
-  time_data.period = rclcpp::Duration::from_nanoseconds(0);
-  time_data.uptime = get_node()->now();
-  time_data_.initRT(time_data);
+  update_period_ = rclcpp::Duration(0.0, static_cast<uint32_t>(1.0e9 / static_cast<double>(get_update_rate())));
+
+  return JointTrajectoryController::on_configure(state);
+}
 
+controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activate(const rclcpp_lifecycle::State& state)
+{
   // Set scaling interfaces
   if (!scaled_params_.speed_scaling_interface_name.empty()) {
     auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), [&](auto& interface) {
@@ -91,6 +92,7 @@ controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activat
       RCLCPP_ERROR(get_node()->get_logger(), "Did not find speed scaling interface in state interfaces.");
     }
   }
+  last_commanded_time_ = rclcpp::Time();
 
   return JointTrajectoryController::on_activate(state);
 }
@@ -101,7 +103,6 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
   if (scaling_state_interface_.has_value()) {
     scaling_factor_ = scaling_state_interface_->get().get_value();
   }
-
   if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
     return controller_interface::return_type::OK;
   }
@@ -110,9 +111,14 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
   if (param_listener_->is_old(params_)) {
     params_ = param_listener_->get_params();
     default_tolerances_ = get_segment_tolerances(logger, params_);
+    // update the PID gains
+    // variable use_closed_loop_pid_adapter_ is updated in on_configure only
+    if (use_closed_loop_pid_adapter_) {
+      update_pids();
+    }
   }
 
-  auto compute_error_for_joint = [&](JointTrajectoryPoint& error, int index, const JointTrajectoryPoint& current,
+  auto compute_error_for_joint = [&](JointTrajectoryPoint& error, size_t index, const JointTrajectoryPoint& current,
                                      const JointTrajectoryPoint& desired) {
     // error defined as the difference between current and desired
     if (joints_angle_wraparound_[index]) {
@@ -153,38 +159,36 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
   };
 
   // current state update
-  state_current_.time_from_start.set__sec(0);
+  state_current_.time_from_start.sec = 0;
+  state_current_.time_from_start.nanosec = 0;
   read_state_from_state_interfaces(state_current_);
 
   // currently carrying out a trajectory
   if (has_active_trajectory()) {
-    // Main Speed scaling difference...
-    // Adjust time with scaling factor
-    TimeData time_data;
-    time_data.time = time;
-    rcl_duration_value_t t_period = (time_data.time - time_data_.readFromRT()->time).nanoseconds();
-    time_data.period = rclcpp::Duration::from_nanoseconds(scaling_factor_ * t_period);
-    time_data.uptime = time_data_.readFromRT()->uptime + time_data.period;
-    rclcpp::Time traj_time = time_data_.readFromRT()->uptime + rclcpp::Duration::from_nanoseconds(t_period);
-    time_data_.reset();
-    time_data_.initRT(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()) {
       first_sample = true;
       if (params_.open_loop_control) {
-        traj_external_point_ptr_->set_point_before_trajectory_msg(traj_time, last_commanded_state_,
+        traj_external_point_ptr_->set_point_before_trajectory_msg(last_commanded_time_, last_commanded_state_,
                                                                   joints_angle_wraparound_);
       } else {
-        traj_external_point_ptr_->set_point_before_trajectory_msg(traj_time, state_current_, joints_angle_wraparound_);
+        traj_external_point_ptr_->set_point_before_trajectory_msg(time, state_current_, joints_angle_wraparound_);
       }
+      traj_time_ = time;
+    } else {
+      traj_time_ += period * scaling_factor_;
     }
+    joint_trajectory_controller::TrajectoryPointConstIter start_segment_itr, end_segment_itr;
+    // Sample expected state from the trajectory
+    traj_external_point_ptr_->sample(traj_time_, interpolation_method_, state_desired_, start_segment_itr,
+                                     end_segment_itr);
+    state_desired_.time_from_start = traj_time_ - traj_external_point_ptr_->time_from_start();
 
     // find segment for current timestamp
-    joint_trajectory_controller::TrajectoryPointConstIter start_segment_itr, end_segment_itr;
-    const bool valid_point = traj_external_point_ptr_->sample(traj_time, interpolation_method_, state_desired_,
-                                                              start_segment_itr, end_segment_itr);
+    const bool valid_point = traj_external_point_ptr_->sample(traj_time_ + update_period_, interpolation_method_,
+                                                              command_next_, start_segment_itr, end_segment_itr);
+    state_current_.time_from_start = time - traj_external_point_ptr_->time_from_start();
 
     if (valid_point) {
       const rclcpp::Time traj_start = traj_external_point_ptr_->time_from_start();
@@ -195,7 +199,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
       // time_difference is
       // - negative until first point is reached
       // - counting from zero to time_from_start of next point
-      const double time_difference = time_data.uptime.seconds() - segment_time_from_start.seconds();
+      double time_difference = traj_time_.seconds() - segment_time_from_start.seconds();
       bool tolerance_violated_while_moving = false;
       bool outside_goal_tolerance = false;
       bool within_goal_time = true;
@@ -246,32 +250,33 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
         if (use_closed_loop_pid_adapter_) {
           // Update PIDs
           for (auto i = 0ul; i < dof_; ++i) {
-            tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) +
+            tmp_command_[i] = (command_next_.velocities[i] * ff_velocity_scale_[i]) +
                               pids_[i]->computeCommand(state_error_.positions[i], state_error_.velocities[i],
                                                        (uint64_t)period.nanoseconds());
           }
         }
 
         // set values for next hardware write()
         if (has_position_command_interface_) {
-          assign_interface_from_point(joint_command_interface_[0], state_desired_.positions);
+          assign_interface_from_point(joint_command_interface_[0], command_next_.positions);
         }
         if (has_velocity_command_interface_) {
           if (use_closed_loop_pid_adapter_) {
             assign_interface_from_point(joint_command_interface_[1], tmp_command_);
           } else {
-            assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities);
+            assign_interface_from_point(joint_command_interface_[1], command_next_.velocities);
           }
         }
         if (has_acceleration_command_interface_) {
-          assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations);
+          assign_interface_from_point(joint_command_interface_[2], command_next_.accelerations);
         }
         if (has_effort_command_interface_) {
           assign_interface_from_point(joint_command_interface_[3], tmp_command_);
         }
 
         // store the previous command. Used in open-loop control mode
-        last_commanded_state_ = state_desired_;
+        last_commanded_state_ = command_next_;
+        last_commanded_time_ = time;
       }
 
       if (active_goal) {
@@ -329,7 +334,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
             rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
             rt_has_pending_goal_ = false;
 
-            RCLCPP_WARN(logger, error_string.c_str());
+            RCLCPP_WARN(logger, "%s", error_string.c_str());
 
             traj_msg_external_point_ptr_.reset();
             traj_msg_external_point_ptr_.initRT(set_hold_position());
@@ -357,6 +362,30 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
   return controller_interface::return_type::OK;
 }
 
+void ScaledJointTrajectoryController::update_pids()
+{
+  for (size_t i = 0; i < dof_; ++i) {
+    const auto& gains = params_.gains.joints_map.at(params_.joints[i]);
+    if (pids_[i]) {
+      // update PIDs with gains from ROS parameters
+      pids_[i]->setGains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp);
+    } else {
+      // Init PIDs with gains from ROS parameters
+      pids_[i] = std::make_shared<control_toolbox::Pid>(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp);
+    }
+    // Check deprecated style for "ff_velocity_scale" parameter definition.
+    if (gains.ff_velocity_scale == 0.0) {
+      RCLCPP_WARN(get_node()->get_logger(), "'ff_velocity_scale' parameters is not defined under 'gains.<joint_name>.' "
+                                            "structure. "
+                                            "Maybe you are using deprecated format 'ff_velocity_scale/<joint_name>'!");
+
+      ff_velocity_scale_[i] = auto_declare<double>("ff_velocity_scale/" + params_.joints[i], 0.0);
+    } else {
+      ff_velocity_scale_[i] = gains.ff_velocity_scale;
+    }
+  }
+}
+
 }  // namespace ur_controllers
 
 #include "pluginlib/class_list_macros.hpp"
diff --git a/ur_controllers/test/scaled_joint_trajectory_controller_params.yaml b/ur_controllers/test/scaled_joint_trajectory_controller_params.yaml
@@ -0,0 +1,18 @@
+---
+scaled_joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - joint1
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    state_publish_rate: 50.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    speed_scaling_interface_name: ""
+    constraints:
+      stopped_velocity_tolerance: 0.2
+      goal_time: 0.0
+      joint1: { trajectory: 0.2, goal: 0.1 }
diff --git a/ur_controllers/test/test_load_scaled_joint_trajectory_controller.cpp b/ur_controllers/test/test_load_scaled_joint_trajectory_controller.cpp
@@ -0,0 +1,63 @@
+// Copyright 2026, Universal Robots A/S
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include <gmock/gmock.h>
+#include "controller_manager/controller_manager.hpp"
+#include "rclcpp/executor.hpp"
+#include "rclcpp/executors/single_threaded_executor.hpp"
+#include "rclcpp/utilities.hpp"
+#include "ros2_control_test_assets/descriptions.hpp"
+
+TEST(TestLoadScaledJointTrajectoryController, load_controller)
+{
+  std::shared_ptr<rclcpp::Executor> executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+
+  controller_manager::ControllerManager cm(
+      std::make_unique<hardware_interface::ResourceManager>(ros2_control_test_assets::minimal_robot_urdf), executor,
+      "test_controller_manager");
+
+  const std::string test_file_path = std::string{ TEST_FILES_DIRECTORY } + "/scaled_joint_trajectory_controller_params."
+                                                                           "yaml";
+  cm.set_parameter({ "test_scaled_joint_trajectory_controller.params_file", test_file_path });
+
+  cm.set_parameter({ "test_scaled_joint_trajectory_controller.type", "ur_controllers/"
+                                                                     "ScaledJointTrajectoryController" });
+
+  ASSERT_NE(cm.load_controller("test_scaled_joint_trajectory_controller"), nullptr);
+}
+
+int main(int argc, char* argv[])
+{
+  ::testing::InitGoogleMock(&argc, argv);
+  rclcpp::init(argc, argv);
+
+  int result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+
+  return result;
+}
diff --git a/ur_controllers/test/test_scaled_joint_trajectory_controller_sampling.cpp b/ur_controllers/test/test_scaled_joint_trajectory_controller_sampling.cpp
