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 9435b7141b..7de5eb5f74 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/doc/parameters_context.yaml b/joint_trajectory_controller/doc/parameters_context.yaml index 4e4dacf202..9623899245 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,5 @@ 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 off the PIDs (open loop control), set the feedback 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 b823fe33b0..6a1bc71a33 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -188,7 +188,7 @@ controller_interface::return_type JointTrajectoryController::update( if (!current_trajectory_->is_sampled_already()) { first_sample = true; - if (params_.open_loop_control) + if (params_.interpolate_from_desired_state || params_.open_loop_control) { if (std::abs(last_commanded_time_.seconds()) < std::numeric_limits::epsilon()) { @@ -655,6 +655,16 @@ 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(); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 14b71f0711..6830846d99 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -47,12 +47,18 @@ joint_trajectory_controller: open_loop_control: { type: bool, default_value: false, - description: "Use controller in open-loop control mode + 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, + 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. + 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). \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. @@ -72,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] diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index e110e244b7..d2ec38d827 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -971,8 +971,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()); @@ -1650,9 +1649,9 @@ 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 - rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + // 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); if (traj_controller_->has_position_command_interface() == false) { @@ -1681,7 +1680,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro traj_controller_->wait_for_trajectory(executor); auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); - // JTC is NOT executing trajectory in open-loop therefore: + // JTC is NOT 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); @@ -1755,11 +1754,11 @@ 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 - rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + // 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( - 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) { @@ -1785,7 +1784,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e traj_controller_->wait_for_trajectory(executor); auto end_time = 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); @@ -1859,7 +1858,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()}; @@ -1867,7 +1866,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 @@ -1900,7 +1899,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; @@ -1911,7 +1910,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 0180a1831f..a5e37f4f78 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -167,7 +167,9 @@ 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() {