Skip to content

Commit

Permalink
Readded open_loop_control param with deprecation notice, added migrat…
Browse files Browse the repository at this point in the history
…ion and release notes
  • Loading branch information
Thieso committed Feb 7, 2025
1 parent 4de9cbd commit ab27c4f
Show file tree
Hide file tree
Showing 5 changed files with 26 additions and 2 deletions.
1 change: 1 addition & 0 deletions doc/migration.rst
Original file line number Diff line number Diff line change
Expand Up @@ -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 <https://github.com/ros-controls/ros2_controllers/pull/796>`_).
* The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 <https://github.com/ros-controls/ros2_controllers/pull/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 <https://github.com/ros-controls/ros2_controllers/pull/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 <https://github.com/ros-controls/ros2_controllers/pull/1525>`_).
1 change: 1 addition & 0 deletions doc/release_notes.rst
Original file line number Diff line number Diff line change
Expand Up @@ -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 <https://github.com/ros-controls/ros2_controllers/pull/1231>`_).
* Parameter ``open_loop_control`` is replaced by ``interpolate_from_desired_state`` and setting the feedback gains to zero (`#1525 <https://github.com/ros-controls/ros2_controllers/pull/1525>`_).

mecanum_drive_controller
************************
Expand Down
16 changes: 14 additions & 2 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_);
Expand Down Expand Up @@ -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();

Expand Down Expand Up @@ -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_)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down

0 comments on commit ab27c4f

Please sign in to comment.