You're reading the documentation for an older, but still supported, version of ROS 2. For information on the latest version, please have a look at Jazzy.
PID Controller
PID Controller implementation that uses PidROS implementation from control_toolbox package. The controller can be used directly by sending references through a topic or in a chain having preceding or following controllers. It also enables to use the first derivative of the reference and its feedback to have second-order PID control.
Depending on the reference/state and command interface of the hardware a different parameter setup of PidROS should be used as for example:
reference/state POSITION; command VELOCITY –> PI CONTROLLER
reference/state VELOCITY; command ACCELERATION –> PI CONTROLLER
reference/state VELOCITY; command POSITION –> PD CONTROLLER
reference/state ACCELERATION; command VELOCITY –> PD CONTROLLER
reference/state POSITION; command POSITION –> PID CONTROLLER
reference/state VELOCITY; command VELOCITY –> PID CONTROLLER
reference/state ACCELERATION; command ACCELERATION –> PID CONTROLLER
reference/state EFFORT; command EFFORT –> PID CONTROLLER
Note
Theoretically one can misuse Joint Trajectory Controller (JTC) for the same purpose by sending only one reference point into it.
Nevertheless, this is not recommended. JTC should be used if you need to interpolate between trajectory points using linear, cubic or quintic interpolation. PID Controller doesn’t do that.
PID term of JTC has different purpose - it enables commanding only velocity
or effort
interfaces to hardware.
Execution logic of the controller
The controller can be also used in “feed-forward” mode where feed-forward gain is used to increase controllers dynamics.
If one type of the reference and state interfaces is used, only immediate error is used. If there are two, then the second interface type is considered to be the first derivative of the first type.
For example a valid combination would be position
and velocity
interface types.
Using the controller
Pluginlib-Library: pid_controller Plugin name: pid_controller/PidController
Description of controller’s interfaces
References (from a preceding controller)
<reference_and_state_dof_names[i]>/<reference_and_state_interfaces[j]> [double] NOTE:
reference_and_state_dof_names[i]
can be fromreference_and_state_dof_names
parameter, or if it is empty thendof_names
.
Commands
<dof_names[i]>/<command_interface> [double]
States
<reference_and_state_dof_names[i]>/<reference_and_state_interfaces[j]> [double] NOTE:
reference_and_state_dof_names[i]
can be fromreference_and_state_dof_names
parameter, or if it is empty thendof_names
.
Subscribers
If controller is not in chained mode (in_chained_mode == false
):
<controller_name>/reference [control_msgs/msg/MultiDOFCommand]
If controller parameter use_external_measured_states
is true:
<controller_name>/measured_state [control_msgs/msg/MultiDOFCommand]
Services
<controller_name>/set_feedforward_control [std_srvs/srv/SetBool]
Publishers
<controller_name>/controller_state [control_msgs/msg/MultiDOFStateStamped]
Parameters
The PID controller uses the generate_parameter_library to handle its parameters.
List of parameters
- dof_names (string_array)
Specifies dof_names or axes used by the controller. If ‘reference_and_state_dof_names’ parameter is defined, then only command dof names are defined with this parameter.
Read only: True
Default: {}
Constraints:
contains no duplicates
parameter is not empty
- reference_and_state_dof_names (string_array)
(optional) Specifies dof_names or axes for getting reference and reading states. This parameter is only relevant when state dof names are different then command dof names, i.e., when a following controller is used.
Read only: True
Default: {}
Constraints:
contains no duplicates
- command_interface (string)
Name of the interface used by the controller for writing commands to the hardware.
Read only: True
Default: “”
Constraints:
parameter is not empty
- reference_and_state_interfaces (string_array)
Name of the interfaces used by the controller getting hardware states and reference commands. The second interface should be the derivative of the first.
Read only: True
Default: {}
Constraints:
parameter is not empty
length is less than 3
- use_external_measured_states (bool)
Use external states from a topic instead from state interfaces.
Default: false
- gains.<dof_names>.p (double)
Proportional gain for PID
Default: 0.0
- gains.<dof_names>.i (double)
Integral gain for PID
Default: 0.0
- gains.<dof_names>.d (double)
Derivative gain for PID
Default: 0.0
- gains.<dof_names>.antiwindup (bool)
Antiwindup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_clamp_max and i_clamp_min are applied in both scenarios.
Default: false
- gains.<dof_names>.i_clamp_max (double)
Upper integral clamp.
Default: 0.0
- gains.<dof_names>.i_clamp_min (double)
Lower integral clamp.
Default: 0.0
- gains.<dof_names>.feedforward_gain (double)
Gain for the feed-forward part.
Default: 0.0
- gains.<dof_names>.angle_wraparound (bool)
For joints that wrap around (i.e., are continuous). Normalizes position-error to -pi to pi.
Default: false
An example parameter file
An example parameter file for this controller can be found in the test folder (standalone):
test_pid_controller:
ros__parameters:
dof_names:
- joint1
command_interface: position
reference_and_state_interfaces: ["position"]
gains:
joint1: {p: 1.0, i: 2.0, d: 10.0, i_clamp_max: 5.0, i_clamp_min: -5.0}
or as preceding controller:
test_pid_controller:
ros__parameters:
dof_names:
- joint1
command_interface: position
reference_and_state_interfaces: ["position"]
reference_and_state_dof_names:
- joint1state