From 82a37aacbd500d31fdcd3b4b63a904cf523c57df Mon Sep 17 00:00:00 2001 From: Thies Oelerich Date: Tue, 4 Feb 2025 16:02:00 +0100 Subject: [PATCH 1/8] Renamed open_loop_control parameter --- .../doc/parameters_context.yaml | 5 +++-- joint_trajectory_controller/doc/userdoc.rst | 2 +- .../src/joint_trajectory_controller.cpp | 5 ++--- ...oint_trajectory_controller_parameters.yaml | 8 ++++---- .../test/test_trajectory_controller.cpp | 19 +++++++++---------- .../test/test_trajectory_controller_utils.hpp | 2 -- 6 files changed, 19 insertions(+), 22 deletions(-) diff --git a/joint_trajectory_controller/doc/parameters_context.yaml b/joint_trajectory_controller/doc/parameters_context.yaml index 4e4dacf202..c77ff8edd0 100644 --- a/joint_trajectory_controller/doc/parameters_context.yaml +++ b/joint_trajectory_controller/doc/parameters_context.yaml @@ -2,8 +2,6 @@ constraints: Default values for tolerances if no explicit values are set in the ``JointTrajectory`` message. gains: | - Only relevant, if ``open_loop_control`` is not set. - If ``velocity`` is the only command interface for all joints or an ``effort`` command interface is configured, PID controllers are used for every joint. This structure contains the controller gains for every joint with the control law @@ -16,3 +14,6 @@ gains: | i.e., the shortest rotation to the target position is the desired motion. Otherwise :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured position :math:`s` from the state interface. + + If you want to turn of the PIDs (open loop control), set the gains to zero. + diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 4dcb71a064..b4b64ed179 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -97,7 +97,7 @@ A yaml file for using it could be: action_monitor_rate: 20.0 allow_partial_joints_goal: false - open_loop_control: true + interpolate_from_desired_state: true constraints: stopped_velocity_tolerance: 0.01 goal_time: 0.0 diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index a4ad5210be..d5e89be89f 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -192,7 +192,7 @@ controller_interface::return_type JointTrajectoryController::update( if (!traj_external_point_ptr_->is_sampled_already()) { first_sample = true; - if (params_.open_loop_control) + if (params_.interpolate_from_desired_state) { traj_external_point_ptr_->set_point_before_trajectory_msg( time, last_commanded_state_, joints_angle_wraparound_); @@ -719,8 +719,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_; if (use_closed_loop_pid_adapter_) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 14b71f0711..609fca4548 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -44,15 +44,15 @@ joint_trajectory_controller: default_value: false, description: "Allow joint goals defining trajectory for only some joints.", } - open_loop_control: { + interpolate_from_desired_state: { type: bool, - default_value: false, - description: "Use controller in open-loop control mode + default_value: true, + description: "Interpolate from the current desired state when receiving a new trajectory. \n\n * The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation.\n - * It deactivates the feedback control, see the ``gains`` structure. \n\n This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). + Furthermore, it is necessary if you have a reference trajectory that you send over multiple messages (e.g. for MPC-style trajectory planning). \n\n If this flag is set, the controller tries to read the values from the command interfaces on activation. If they have real numeric values, those will be used instead of state interfaces. diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 21d0277541..7130eb4fdc 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1005,8 +1005,7 @@ TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid) (traj_controller_->has_velocity_command_interface() && !traj_controller_->has_position_command_interface() && !traj_controller_->has_effort_command_interface() && - !traj_controller_->has_acceleration_command_interface() && - !traj_controller_->is_open_loop()) || + !traj_controller_->has_acceleration_command_interface()) || traj_controller_->has_effort_command_interface()) { EXPECT_TRUE(traj_controller_->use_closed_loop_pid_adapter()); @@ -1685,8 +1684,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro { rclcpp::executors::SingleThreadedExecutor executor; // default if false so it will not be actually set parameter - rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + rclcpp::Parameter interp_desired_state_parameter("interpolate_from_desired_state", false); + SetUpAndActivateTrajectoryController(executor, {interp_desired_state_parameter}, true); if (traj_controller_->has_position_command_interface() == false) { @@ -1790,10 +1789,10 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e { rclcpp::executors::SingleThreadedExecutor executor; // set open loop to true, this should change behavior from above - rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + rclcpp::Parameter interp_desired_state_parameter("interpolate_from_desired_state", true); rclcpp::Parameter update_rate_param("update_rate", 100); SetUpAndActivateTrajectoryController( - executor, {is_open_loop_parameters, update_rate_param}, true); + executor, {interp_desired_state_parameter, update_rate_param}, true); if (traj_controller_->has_position_command_interface() == false) { @@ -1893,7 +1892,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_controller_start) { rclcpp::executors::SingleThreadedExecutor executor; - rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + rclcpp::Parameter interp_desired_state_parameter("interpolate_from_desired_state", true); // set command values to NaN std::vector initial_pos_cmd{3, std::numeric_limits::quiet_NaN()}; @@ -1901,7 +1900,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co std::vector initial_acc_cmd{3, std::numeric_limits::quiet_NaN()}; SetUpAndActivateTrajectoryController( - executor, {is_open_loop_parameters}, true, 0., 1., initial_pos_cmd, initial_vel_cmd, + executor, {interp_desired_state_parameter}, true, 0., 1., initial_pos_cmd, initial_vel_cmd, initial_acc_cmd); // no call of update method, so the values should be read from state interfaces @@ -1934,7 +1933,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_controller_start) { rclcpp::executors::SingleThreadedExecutor executor; - rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + rclcpp::Parameter interp_desired_state_parameter("interpolate_from_desired_state", true); // set command values to arbitrary values std::vector initial_pos_cmd, initial_vel_cmd, initial_acc_cmd; @@ -1945,7 +1944,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co initial_acc_cmd.push_back(0.02 + static_cast(i) / 10.0); } SetUpAndActivateTrajectoryController( - executor, {is_open_loop_parameters}, true, 0., 1., initial_pos_cmd, initial_vel_cmd, + executor, {interp_desired_state_parameter}, true, 0., 1., initial_pos_cmd, initial_vel_cmd, initial_acc_cmd); // no call of update method, so the values should be read from command interfaces diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index b0e66394d1..0729d774ce 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -167,8 +167,6 @@ class TestableJointTrajectoryController bool use_closed_loop_pid_adapter() const { return use_closed_loop_pid_adapter_; } - bool is_open_loop() const { return params_.open_loop_control; } - joint_trajectory_controller::SegmentTolerances get_active_tolerances() { return *(active_tolerances_.readFromRT()); From 8514909d24219e92b59cea71519d533a28d3c7c2 Mon Sep 17 00:00:00 2001 From: Thies Oelerich Date: Fri, 7 Feb 2025 23:03:29 +0100 Subject: [PATCH 2/8] Update joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- .../src/joint_trajectory_controller_parameters.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 609fca4548..e8029584f4 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -46,7 +46,7 @@ joint_trajectory_controller: } interpolate_from_desired_state: { type: bool, - default_value: true, + default_value: false, description: "Interpolate from the current desired state when receiving a new trajectory. \n\n * The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation.\n From 4de9cbd5845a014875923bd7d2a19f05e0273448 Mon Sep 17 00:00:00 2001 From: Thies Oelerich Date: Fri, 7 Feb 2025 23:03:34 +0100 Subject: [PATCH 3/8] Update joint_trajectory_controller/doc/parameters_context.yaml MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- joint_trajectory_controller/doc/parameters_context.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/doc/parameters_context.yaml b/joint_trajectory_controller/doc/parameters_context.yaml index c77ff8edd0..b652e14555 100644 --- a/joint_trajectory_controller/doc/parameters_context.yaml +++ b/joint_trajectory_controller/doc/parameters_context.yaml @@ -15,5 +15,5 @@ gains: | Otherwise :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured position :math:`s` from the state interface. - If you want to turn of the PIDs (open loop control), set the gains to zero. + If you want to turn off the PIDs (open loop control), set the feedback gains to zero. From ab27c4f52a17f057d06928b847a8e43291458f3b Mon Sep 17 00:00:00 2001 From: Thies Oelerich Date: Fri, 7 Feb 2025 23:29:47 +0100 Subject: [PATCH 4/8] Readded open_loop_control param with deprecation notice, added migration and release notes --- doc/migration.rst | 1 + doc/release_notes.rst | 1 + .../src/joint_trajectory_controller.cpp | 16 ++++++++++++++-- .../joint_trajectory_controller_parameters.yaml | 6 ++++++ .../test/test_trajectory_controller_utils.hpp | 4 ++++ 5 files changed, 26 insertions(+), 2 deletions(-) diff --git a/doc/migration.rst b/doc/migration.rst index 4f603880bb..fb7c314f6f 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -20,3 +20,4 @@ joint_trajectory_controller * Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 `_). * The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). Remove the ``angle_wraparound`` parameter from the configuration and set continuous joint type in the URDF of the respective joint. * Tolerances sent with the action goal were not used before, but are now processed and used for the upcoming action. (`#716 `_). Adaptions to the action goal might be necessary. +* Parameter ``open_loop_control`` is replaced by ``interpolate_from_desired_state`` and setting the feedback gains to zero (`#1525 `_). diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 74cd0402f1..56c3e9a503 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -52,6 +52,7 @@ joint_trajectory_controller allowed to move without restriction. * Add the boolean parameter ``set_last_command_interface_value_as_state_on_activation``. When set to ``true``, the last command interface value is used as both the current state and the last commanded state upon activation. When set to ``false``, the current state is used for both (`#1231 `_). +* Parameter ``open_loop_control`` is replaced by ``interpolate_from_desired_state`` and setting the feedback gains to zero (`#1525 `_). mecanum_drive_controller ************************ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index d5e89be89f..4f2b86b61b 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -192,7 +192,7 @@ controller_interface::return_type JointTrajectoryController::update( if (!traj_external_point_ptr_->is_sampled_already()) { first_sample = true; - if (params_.interpolate_from_desired_state) + if (params_.interpolate_from_desired_state || params_.open_loop_control) { traj_external_point_ptr_->set_point_before_trajectory_msg( time, last_commanded_state_, joints_angle_wraparound_); @@ -661,6 +661,17 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( return controller_interface::CallbackReturn::ERROR; } + // 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" + "NAN"); + + } + // END DEPRECATE + // update the dynamic map parameters param_listener_->refresh_dynamic_parameters(); @@ -719,7 +730,8 @@ 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) || + (has_velocity_command_interface_ && params_.command_interfaces.size() == 1 && + !params_.open_loop_control) || has_effort_command_interface_; if (use_closed_loop_pid_adapter_) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index e8029584f4..82c9539b15 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -44,6 +44,12 @@ 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: "depcrecated: 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 0729d774ce..ab7f6e447e 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -167,6 +167,10 @@ 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 415cd67e80ebe795fc01968bc0a536f333bae183 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 11 Feb 2025 20:03:05 +0000 Subject: [PATCH 5/8] Fix pre-commit --- joint_trajectory_controller/doc/parameters_context.yaml | 1 - .../src/joint_trajectory_controller.cpp | 5 ++--- .../src/joint_trajectory_controller_parameters.yaml | 2 +- 3 files changed, 3 insertions(+), 5 deletions(-) diff --git a/joint_trajectory_controller/doc/parameters_context.yaml b/joint_trajectory_controller/doc/parameters_context.yaml index b652e14555..9623899245 100644 --- a/joint_trajectory_controller/doc/parameters_context.yaml +++ b/joint_trajectory_controller/doc/parameters_context.yaml @@ -16,4 +16,3 @@ gains: | position :math:`s` from the state interface. If you want to turn off the PIDs (open loop control), set the feedback gains to zero. - diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 4f2b86b61b..b4b696319b 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -666,9 +666,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( { RCLCPP_WARN( logger, - "[deprecated] open_loop_control parameter is deprecated, instead set the feedback gains to zero and use interpolate_from_desired_state parameter" - "NAN"); - + "[deprecated] 'open_loop_control' parameter is deprecated. Instead, set the feedback gains " + "to zero and use 'interpolate_from_desired_state' parameter"); } // END DEPRECATE diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 82c9539b15..9de8ca51aa 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -47,7 +47,7 @@ joint_trajectory_controller: open_loop_control: { type: bool, default_value: false, - description: "depcrecated: use interpolate_from_desired_state and set feedback gains to zero instead", + description: "deprecated: use interpolate_from_desired_state and set feedback gains to zero instead", read_only: true, } interpolate_from_desired_state: { From 5ec4197e43d59b87e96797ea80c59ae25158ee80 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 11 Feb 2025 20:26:36 +0000 Subject: [PATCH 6/8] Fix rst syntax in yaml --- .../src/joint_trajectory_controller_parameters.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 9de8ca51aa..8e4f2cdb16 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -55,7 +55,7 @@ joint_trajectory_controller: default_value: false, description: "Interpolate from the current desired state when receiving a new trajectory. \n\n - * The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation.\n + The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation. \n\n This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). Furthermore, it is necessary if you have a reference trajectory that you send over multiple messages (e.g. for MPC-style trajectory planning). From 70b59303535d7bc65901d144c74d0e61afb7fa5c Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 14 Feb 2025 20:48:59 +0000 Subject: [PATCH 7/8] Format rst syntax in yaml --- .../src/joint_trajectory_controller_parameters.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 8e4f2cdb16..6830846d99 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -47,7 +47,7 @@ joint_trajectory_controller: open_loop_control: { type: bool, default_value: false, - description: "deprecated: use interpolate_from_desired_state and set feedback gains to zero instead", + description: "deprecated: use ``interpolate_from_desired_state`` and set feedback gains to zero instead", read_only: true, } interpolate_from_desired_state: { @@ -78,7 +78,7 @@ joint_trajectory_controller: action_monitor_rate: { type: double, default_value: 20.0, - description: "Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory)", + description: "Rate to monitor status changes when the controller is executing action (``control_msgs::action::FollowJointTrajectory``)", read_only: true, validation: { gt_eq: [0.1] From 6d7a77d9c26c4f3f5f5037d8363d96e977e25889 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 14 Feb 2025 20:51:32 +0000 Subject: [PATCH 8/8] Update comments --- .../test/test_trajectory_controller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index cb39ca92d6..957e1bb252 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1649,7 +1649,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) { rclcpp::executors::SingleThreadedExecutor executor; - // default if false so it will not be actually set parameter + // default is false so it will not be actually set parameter rclcpp::Parameter interp_desired_state_parameter("interpolate_from_desired_state", false); SetUpAndActivateTrajectoryController(executor, {interp_desired_state_parameter}, true); @@ -1680,7 +1680,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro traj_controller_->wait_for_trajectory(executor); updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); - // JTC is executing trajectory in open-loop therefore: + // JTC is executing trajectory with interpolate_from_desired_state, therefore: // - internal state does not have to be updated (in this test-case it shouldn't) // - internal command is updated EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); @@ -1754,7 +1754,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) { rclcpp::executors::SingleThreadedExecutor executor; - // set open loop to true, this should change behavior from above + // set interpolate_from_desired_state to true, this should change behavior from above rclcpp::Parameter interp_desired_state_parameter("interpolate_from_desired_state", true); rclcpp::Parameter update_rate_param("update_rate", 100); SetUpAndActivateTrajectoryController( @@ -1784,7 +1784,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e traj_controller_->wait_for_trajectory(executor); updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); - // JTC is executing trajectory in open-loop therefore: + // JTC is executing trajectory with interpolate_from_desired_state therefore: // - internal state does not have to be updated (in this test-case it shouldn't) // - internal command is updated EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD);