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

Rework PID class API #246

Open
wants to merge 20 commits into
base: ros2-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
263 changes: 242 additions & 21 deletions include/control_toolbox/pid.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
#include <stdexcept>
#include <string>

#include "rclcpp/duration.hpp"
#include "realtime_tools/realtime_buffer.hpp"

#include "control_toolbox/visibility_control.hpp"
Expand Down Expand Up @@ -88,13 +89,13 @@ namespace control_toolbox

\verbatim
control_toolbox::Pid pid;
pid.initPid(6.0, 1.0, 2.0, 0.3, -0.3);
double position_desi_ = 0.5;
pid.initialize(6.0, 1.0, 2.0, 0.3, -0.3);
double position_desi = 0.5;
...
rclcpp::Time last_time = get_clock()->now();
while (true) {
rclcpp::Time time = get_clock()->now();
double effort = pid.computeCommand(position_desi_ - currentPosition(), (time - last_time).nanoseconds());
double effort = pid.compute_command(position_desi - currentPosition(), time - last_time);
last_time = time;
}
\endverbatim
Expand Down Expand Up @@ -196,7 +197,26 @@ class CONTROL_TOOLBOX_PUBLIC Pid
*
* \note New gains are not applied if i_min_ > i_max_
*/
void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
void initialize(
double p, double i, double d, double i_max, double i_min, bool antiwindup = false);

/*!
* \brief Zeros out Pid values and initialize Pid-gains and integral term limits
* Does not initialize the node's parameter interface for PID gains
*
* \param p The proportional gain.
* \param i The integral gain.
* \param d The derivative gain.
* \param i_max The max integral windup.
* \param i_min The min integral windup.
* \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced
*
* \note New gains are not applied if i_min_ > i_max_
*/
[[deprecated("Use initialize() instead")]] void initPid(
double p, double i, double d, double i_max, double i_min, bool antiwindup = false) {
initialize(p, i, d, i_max, i_min, antiwindup);
}

/*!
* \brief Reset the state of this PID controller
Expand All @@ -211,7 +231,21 @@ class CONTROL_TOOLBOX_PUBLIC Pid
* \param i_max The max integral windup.
* \param i_min The min integral windup.
*/
void getGains(double & p, double & i, double & d, double & i_max, double & i_min);
void get_gains(double & p, double & i, double & d, double & i_max, double & i_min);

/*!
* \brief Get PID gains for the controller.
* \param p The proportional gain.
* \param i The integral gain.
* \param d The derivative gain.
* \param i_max The max integral windup.
* \param i_min The min integral windup.
*/
[[deprecated("Use get_gains() instead")]] void getGains(
double & p, double & i, double & d, double & i_max, double & i_min) {
get_gains(p, i, d, i_max, i_min);
}

/*!
* \brief Get PID gains for the controller.
* \param p The proportional gain.
Expand All @@ -221,14 +255,36 @@ class CONTROL_TOOLBOX_PUBLIC Pid
* \param i_min The min integral windup.
* \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced
*/
void getGains(
void get_gains(
double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup);

/*!
* \brief Get PID gains for the controller.
* \param p The proportional gain.
* \param i The integral gain.
* \param d The derivative gain.
* \param i_max The max integral windup.
* \param i_min The min integral windup.
* \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced
*/
[[deprecated("Use get_gains() instead")]] void getGains(
double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup) {
get_gains(p, i, d, i_max, i_min, antiwindup);
}

/*!
* \brief Get PID gains for the controller.
* \return gains A struct of the PID gain values
*/
Gains get_gains();

/*!
* \brief Get PID gains for the controller.
* \return gains A struct of the PID gain values
*/
Gains getGains();
[[deprecated("Use get_gains() instead")]] Gains getGains() {
return get_gains();
}

/*!
* \brief Set PID gains for the controller.
Expand All @@ -241,15 +297,53 @@ class CONTROL_TOOLBOX_PUBLIC Pid
*
* \note New gains are not applied if i_min > i_max
*/
void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);

/*!
* \brief Set PID gains for the controller.
* \param p The proportional gain.
* \param i The integral gain.
* \param d The derivative gain.
* \param i_max The max integral windup.
* \param i_min The min integral windup.
* \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced
*
* \note New gains are not applied if i_min > i_max
*/
[[deprecated("Use set_gains() instead")]] void setGains(
double p, double i, double d, double i_max, double i_min, bool antiwindup = false) {
set_gains(p, i, d, i_max, i_min, antiwindup);
}

/*!
* \brief Set PID gains for the controller.
* \param gains A struct of the PID gain values
*
* \note New gains are not applied if gains.i_min_ > gains.i_max_
*/
void set_gains(const Gains & gains);

/*!
* \brief Set PID gains for the controller.
* \param gains A struct of the PID gain values
*
* \note New gains are not applied if gains.i_min_ > gains.i_max_
*/
void setGains(const Gains & gains);
[[deprecated("Use set_gains() instead")]] void setGains(const Gains & gains) {
set_gains(gains);
}

/*!
* \brief Set the PID error and compute the PID command with nonuniform time
* step size. The derivative error is computed from the change in the error
* and the timestep \c dt.
*
* \param error Error since last call (error = target - state)
* \param dt Change in time since last call in seconds
*
* \returns PID command
*/
[[nodiscard]] double compute_command(double error, const double & dt_s);

/*!
* \brief Set the PID error and compute the PID command with nonuniform time
Expand All @@ -261,7 +355,59 @@ class CONTROL_TOOLBOX_PUBLIC Pid
*
* \returns PID command
*/
[[nodiscard]] double computeCommand(double error, uint64_t dt);
[[deprecated("Use compute_command() instead")]] [[nodiscard]] double computeCommand(
double error, uint64_t dt_ns) {
return compute_command(error, static_cast<double>(dt_ns) / 1.e9);
}

/*!
* \brief Set the PID error and compute the PID command with nonuniform time
* step size. The derivative error is computed from the change in the error
* and the timestep \c dt.
*
* \param error Error since last call (error = target - state)
* \param dt Change in time since last call, measured in nanoseconds.
*
* \returns PID command
*/
[[nodiscard]] double compute_command(double error, const rcl_duration_value_t & dt_ns);

/*!
* \brief Set the PID error and compute the PID command with nonuniform time
* step size. The derivative error is computed from the change in the error
* and the timestep \c dt.
*
* \param error Error since last call (error = target - state)
* \param dt Change in time since last call
*
* \returns PID command
*/
[[nodiscard]] double compute_command(double error, const rclcpp::Duration & dt);

/*!
* \brief Set the PID error and compute the PID command with nonuniform time
* step size. The derivative error is computed from the change in the error
* and the timestep \c dt.
*
* \param error Error since last call (error = target - state)
* \param dt Change in time since last call
*
* \returns PID command
*/
[[nodiscard]] double compute_command(double error, const std::chrono::nanoseconds & dt_ns);

/*!
* \brief Set the PID error and compute the PID command with nonuniform
* time step size. This also allows the user to pass in a precomputed
* derivative error.
*
* \param error Error since last call (error = target - state)
* \param error_dot d(Error)/dt since last call
* \param dt Change in time since last call in seconds
*
* \returns PID command
*/
[[nodiscard]] double compute_command(double error, double error_dot, const double & dt_s);

/*!
* \brief Set the PID error and compute the PID command with nonuniform
Expand All @@ -274,30 +420,104 @@ class CONTROL_TOOLBOX_PUBLIC Pid
*
* \returns PID command
*/
[[nodiscard]] double computeCommand(double error, double error_dot, uint64_t dt);
[[deprecated("Use compute_command() instead")]] [[nodiscard]] double computeCommand(
double error, double error_dot, uint64_t dt_ns) {
return compute_command(error, error_dot, static_cast<double>(dt_ns) / 1.e9);
}

/*!
* \brief Set the PID error and compute the PID command with nonuniform
* time step size. This also allows the user to pass in a precomputed
* derivative error.
*
* \param error Error since last call (error = target - state)
* \param error_dot d(Error)/dt since last call
* \param dt Change in time since last call, measured in nanoseconds.
*
* \returns PID command
*/
[[nodiscard]] double compute_command(
double error, double error_dot, const rcl_duration_value_t & dt_ns);

/*!
* \brief Set the PID error and compute the PID command with nonuniform
* time step size. This also allows the user to pass in a precomputed
* derivative error.
*
* \param error Error since last call (error = target - state)
* \param error_dot d(Error)/dt since last call
* \param dt Change in time since last call
*
* \returns PID command
*/
[[nodiscard]] double compute_command(double error, double error_dot, const rclcpp::Duration & dt);

/*!
* \brief Set the PID error and compute the PID command with nonuniform
* time step size. This also allows the user to pass in a precomputed
* derivative error.
*
* \param error Error since last call (error = target - state)
* \param error_dot d(Error)/dt since last call
* \param dt Change in time since last call, measured in nanoseconds.
*
* \returns PID command
*/
[[nodiscard]] double compute_command(
double error, double error_dot, const std::chrono::nanoseconds & dt_ns);

/*!
* \brief Set current command for this PID controller
*/
void setCurrentCmd(double cmd);
void set_current_cmd(double cmd);

/*!
* \brief Set current command for this PID controller
*/
[[deprecated("Use set_current_cmd() instead")]] void setCurrentCmd(double cmd) {
set_current_cmd(cmd);
}

/*!
* \brief Return current command for this PID controller
*/
double getCurrentCmd();
double get_current_cmd();

/*!
* \brief Return current command for this PID controller
*/
[[deprecated("Use get_current_cmd() instead")]] double getCurrentCmd() {
return get_current_cmd();
}

/*!
* \brief Return derivative error
*/
double getDerivativeError();
[[deprecated("Use get_current_pid_errors() instead")]]
double getDerivativeError() {
double pe, ie, de;
get_current_pid_errors(pe, ie, de);
return de;
}

/*!
* \brief Return PID error terms for the controller.
* \param pe The proportional error.
* \param ie The integral error.
* \param de The derivative error.
*/
void get_current_pid_errors(double & pe, double & ie, double & de);

/*!
* \brief Return PID error terms for the controller.
* \param pe The proportional error.
* \param ie The integral error.
* \param de The derivative error.
*/
void getCurrentPIDErrors(double & pe, double & ie, double & de);
[[deprecated("Use get_current_pid_errors() instead")]] void getCurrentPIDErrors(
double & pe, double & ie, double & de) {
get_current_pid_errors(pe, ie, de);
}

/*!
* @brief Custom assignment operator
Expand All @@ -323,12 +543,13 @@ class CONTROL_TOOLBOX_PUBLIC Pid
// blocking the realtime update loop
realtime_tools::RealtimeBuffer<Gains> gains_buffer_;

double p_error_last_; /**< _Save position state for derivative state calculation. */
double p_error_; /**< Position error. */
double i_error_; /**< Integral of position error. */
double d_error_; /**< Derivative of position error. */
double cmd_; /**< Command to send. */
double error_dot_; /**< Derivative error */
double p_error_last_; /** Save state for derivative state calculation. */
double p_error_; /** Error. */
double i_error_; /** Integral of error. */
double d_error_; /** Derivative of error. */
double cmd_; /** Command to send. */
// TODO(christophfroehlich) remove this -> breaks ABI
[[deprecated("Use d_error_")]] double error_dot_; /** Derivative error */
};

} // namespace control_toolbox
Expand Down
Loading
Loading