[JTC] Move PID to controller plugin #2

christophfroehlich · 2023-10-14T09:18:43Z

Up to now there were only two possibilities of controlling the system with JTC:

feedforward only
feedback with one single-input-single-output PID per joint.

The aim of this PR is to create an interface for supporting new control strategies, and enable even creating custom controller plugins by users. This is done by plugins of base class TrajectoryControllerBase, loaded by the pluginlib.

Together with the change of #3 it will even be supported to integrate state-space controller where the system output does not have the same size as the system input: See the following swing-up of a cart-pole from this demo, where I implemented a LQR as plugin

swingup_lqr_gh.mp4

Why to change JTC in this way:

Of course this could also be done by a new custom ros2_controller, but I'd really like to use the well-tested interfaces of JTC in my projects as well (and get updates to it).
In principle, JTC could be enhanced by a chained controller. But: Then there would be no direct way to use the trajectory sent to JTC for calculating the control law, nor does it have the information about the start of the sent trajectory to interpolate the gains to the correct time instance.
An alternative would be a JTC base class, with virtual methods for the trajectory controller. The current default JTC is then inheriting this class; users can inherit from the base class with their own methods. Drawback of this would be, that any upstream changes of the JTC base class would force the users to re-build their inherited JTC. This wouldn't be necessary with the proposed plugins.

The API was designed around the following methods:

computeControlLawNonRT: called when a new trajectory is received. This may take some time (e.g., calculating the gains for a LQR by solving Riccati equations). This blocks the execution of the new trajectory by JTC until the calculation is finished (see is_ready()). The user has to implement a realtime-buffer to switch the old control law to the new one, once it is started (see start()). In case of any abort (update trajectory with set_hold_position()), there is a computeControlLawRT which needs to finish in the RT loop.
updateGainsRT: A fast update within the RT loop, e.g., update of the PID gains from dynamic reconfigured ROS parameters.
computeCommands: Evaluate the control law inside the RT loop with earlier calculated/updated gains.

Notes:

w.r.t. the change of [JTC] Accepting different number of number of joints and command_joints parameter #3, the map for the gains should be command_joints. But most of the time this parameter is empty and copied over from joints in JTC. Because it is a static read-only variable, it is not possible to override the parameter for the plugin from JTC -> I had to make it reconfigurable from the generate_parameter_library, but added a callback afterwards to prohibit a later change from the user. If anyone knows a better way to solve this, please let me know.
The naming of the plugin package, the plugin itself, or the base class is open for discussion.

The behavior of existing configurations should not break with the proposed changes.

saikishor

I'll review rest later. So far it looks spot on

saikishor · 2023-10-30T18:15:38Z

joint_trajectory_controller/src/joint_trajectory_controller.cpp

+  if (use_closed_loop_pid_adapter_)
+  {
+    trajectory_msgs::msg::JointTrajectory traj;
+    traj.points.push_back(state_current_);


@christophfroehlich Maybe also adding the state_desired_ would help

computeGains takes a full trajectory, here it is only initialized with the same points as used for set_hold_position

As mentioned in the description, an update for dynamic reconfigure is missing.

But there is also a call missing for receiving a new trajectory, I'll push an update here.

I pushed a new update

saikishor · 2023-11-01T11:58:50Z

joint_trajectory_controller/src/joint_trajectory_controller.cpp

+    lock_cmd_joint_names = get_node()->add_pre_set_parameters_callback(
+      [this](std::vector<rclcpp::Parameter> & parameters)
+      {
+        for (auto & parameter : parameters)
+        {
+          if (parameter.get_name() == "command_joints")
+          {
+            RCLCPP_ERROR(
+              get_node()->get_logger(),
+              "The parameter 'command_joints' is read-only. You can't change it.");
+            parameter = rclcpp::Parameter("command_joints", command_joint_names_);
+          }
+        }
+      });


@christophfroehlich Shouldn't this be set before the set_parameter line?

no because I wouldn't be able to set the parameter then. it should just "lock" the parameter afterwards

…jectory

christophfroehlich changed the title ~~[JTC] controller plugin~~ [JTC] Move PID to controller plugin Oct 23, 2023

christophfroehlich force-pushed the jtc/controller_plugin branch from 0c2f441 to 71b6efb Compare October 23, 2023 08:37

saikishor reviewed Oct 30, 2023

View reviewed changes

saikishor reviewed Nov 1, 2023

View reviewed changes

christophfroehlich force-pushed the jtc/controller_plugin branch from 8b48b19 to d22d5ff Compare November 19, 2023 17:15

christophfroehlich added 25 commits December 6, 2023 12:35

First draft of trajectory controllers plugins

32d93ec

Move pid plugin to own package

202d6ab

Move base class to separate package

d148cee

Delete duplicate parameters

1288610

Reactivate tests

319826e

Fix unused parameter warnings

08dfd5a

Remove duplicate LICENSE file

32f2f7f

Rename jtc plugin package

91b36b4

Add class_loader as persistent jtc-class member variable

c165317

Set PID parameters during tests only if plugin is loaded

9ada3e1

Add test to propery initialize PID plugin

deda645

Use lifecycle_node inside trajectory_controller plugin

7c797fe

Deactivate reconfigure of controller plugin

fc78ece

Compute gains on on_activate

d5a3b72

Let PID controller parse command joint names instead of joints

a9a2cbf

Rename pid_adapter variable and update gains with every new trajectory

74ae3b9

Split non-RT and RT methods

581f7b2

Use command joints instead of joints for gains structure

a97511b

Add RT buffer to base class

81828bf

Add trajectory start time

35ff9db

Rename variable

7055584

Remove local vars and just call compute-commands with duration in tra…

dc0793c

…jectory

Update control law also for hold position

74af177

Fix parameter updates and tests after rebase

5864456

Export dependencies and update comments

f30dc68

christophfroehlich added 2 commits December 6, 2023 13:01

Rename updateGains method

11b7d20

Call activate() of traj_contr_

4927435

christophfroehlich force-pushed the jtc/controller_plugin branch from de738e6 to 4927435 Compare December 6, 2023 13:15

christophfroehlich closed this Dec 6, 2023

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[JTC] Move PID to controller plugin #2

[JTC] Move PID to controller plugin #2

christophfroehlich commented Oct 14, 2023 •

edited

saikishor left a comment •

edited

saikishor Oct 30, 2023

christophfroehlich Oct 30, 2023

christophfroehlich Oct 30, 2023

saikishor Nov 1, 2023

christophfroehlich Nov 1, 2023

[JTC] Move PID to controller plugin #2

[JTC] Move PID to controller plugin #2

Conversation

christophfroehlich commented Oct 14, 2023 • edited

saikishor left a comment • edited

Choose a reason for hiding this comment

saikishor Oct 30, 2023

Choose a reason for hiding this comment

christophfroehlich Oct 30, 2023

Choose a reason for hiding this comment

christophfroehlich Oct 30, 2023

Choose a reason for hiding this comment

saikishor Nov 1, 2023

Choose a reason for hiding this comment

christophfroehlich Nov 1, 2023

Choose a reason for hiding this comment

christophfroehlich commented Oct 14, 2023 •

edited

saikishor left a comment •

edited