Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[JTC] Rename open_loop_control parameter and slightly change its scope #1525

Open
wants to merge 14 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
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
4 changes: 2 additions & 2 deletions joint_trajectory_controller/doc/parameters_context.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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.
2 changes: 1 addition & 1 deletion joint_trajectory_controller/doc/userdoc.rst
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
12 changes: 11 additions & 1 deletion joint_trajectory_controller/src/joint_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<float>::epsilon())
{
Expand Down Expand Up @@ -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();

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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]
Expand Down
27 changes: 13 additions & 14 deletions joint_trajectory_controller/test/test_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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)
{
Expand All @@ -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);
Expand Down Expand Up @@ -1859,15 +1858,15 @@ 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<double> initial_pos_cmd{3, std::numeric_limits<double>::quiet_NaN()};
std::vector<double> initial_vel_cmd{3, std::numeric_limits<double>::quiet_NaN()};
std::vector<double> initial_acc_cmd{3, std::numeric_limits<double>::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
Expand Down Expand Up @@ -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<double> initial_pos_cmd, initial_vel_cmd, initial_acc_cmd;
Expand All @@ -1911,7 +1910,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co
initial_acc_cmd.push_back(0.02 + static_cast<double>(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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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()
{
Expand Down