From ded8d273aaedd51b0a296d0b6fa75335b5b0cebc Mon Sep 17 00:00:00 2001 From: Thies Oelerich Date: Wed, 19 Mar 2025 16:01:36 +0100 Subject: [PATCH 1/2] Removed deprecated code --- .../src/joint_trajectory_controller.cpp | 15 ++------------- .../joint_trajectory_controller_parameters.yaml | 6 ------ .../test/test_trajectory_controller_utils.hpp | 4 ---- 3 files changed, 2 insertions(+), 23 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 6890cea55e..2e37c8572f 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -179,7 +179,7 @@ controller_interface::return_type JointTrajectoryController::update( if (!current_trajectory_->is_sampled_already()) { first_sample = true; - if (params_.interpolate_from_desired_state || params_.open_loop_control) + if (params_.interpolate_from_desired_state) { if (std::abs(last_commanded_time_.seconds()) < std::numeric_limits::epsilon()) { @@ -693,16 +693,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( { auto logger = get_node()->get_logger(); - // START DEPRECATE - if (params_.open_loop_control) - { - RCLCPP_WARN( - logger, - "[deprecated] 'open_loop_control' parameter is deprecated. Instead, set the feedback gains " - "to zero and use 'interpolate_from_desired_state' parameter"); - } - // END DEPRECATE - // update the dynamic map parameters param_listener_->refresh_dynamic_parameters(); @@ -783,8 +773,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // if there is only velocity or if there is effort command interface // then use also PID adapter use_closed_loop_pid_adapter_ = - (has_velocity_command_interface_ && params_.command_interfaces.size() == 1 && - !params_.open_loop_control) || + (has_velocity_command_interface_ && params_.command_interfaces.size() == 1) || (has_effort_command_interface_ && params_.command_interfaces.size() == 1); tmp_command_.resize(dof_, 0.0); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 926366cde6..2a4c5b343f 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -51,12 +51,6 @@ joint_trajectory_controller: default_value: false, description: "Allow joint goals defining trajectory for only some joints.", } - open_loop_control: { - type: bool, - default_value: false, - description: "deprecated: use ``interpolate_from_desired_state`` and set feedback gains to zero instead", - read_only: true, - } interpolate_from_desired_state: { type: bool, default_value: false, diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 1d4302fbc6..c777fb5b18 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -168,10 +168,6 @@ class TestableJointTrajectoryController bool use_closed_loop_pid_adapter() const { return use_closed_loop_pid_adapter_; } - // START DEPRECATE - bool is_open_loop() const { return params_.open_loop_control; } - // END DEPRECATE - joint_trajectory_controller::SegmentTolerances get_active_tolerances() { return *(active_tolerances_.readFromRT()); From 0a5020aeda0c3444bf486c96c6ac769653a49cf0 Mon Sep 17 00:00:00 2001 From: Thies Oelerich Date: Wed, 19 Mar 2025 20:38:45 +0100 Subject: [PATCH 2/2] Fixed comment --- .../src/joint_trajectory_controller.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 2e37c8572f..65f133d6ff 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1007,8 +1007,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( resize_joint_trajectory_point(state, dof_); // read from cmd joints only if all joints have command interface // otherwise it leaves the entries of joints without command interface NaN. - // if no open_loop control, state_current_ is then used for `set_point_before_trajectory_msg` and - // future trajectory sampling will always give NaN for these joints + // if no interpolate_from_desired_state, state_current_ is then used for + // `set_point_before_trajectory_msg` and future trajectory sampling will always give NaN for these + // joints if ( params_.set_last_command_interface_value_as_state_on_activation && dof_ == num_cmd_joints_ && read_state_from_command_interfaces(state))