LinearQuadraticRegulator_2_2

class wpimath.controller.LinearQuadraticRegulator_2_2(*args, **kwargs)

Bases: pybind11_object

Contains the controller coefficients and logic for a linear-quadratic regulator (LQR). LQRs use the control law u = K(r - x).

For more on the underlying math, read https://file.tavsys.net/control/controls-engineering-in-frc.pdf.

@tparam States Number of states. @tparam Inputs Number of inputs.

Overloaded function.

  1. __init__(self: wpimath._controls._controls.controller.LinearQuadraticRegulator_2_2, A: numpy.ndarray[numpy.float64[2, 2]], B: numpy.ndarray[numpy.float64[2, 2]], Qelems: Tuple[float, float], Relems: Tuple[float, float], dt: seconds) -> None

Constructs a controller with the given coefficients and plant.

Parameters:
  • A – Continuous system matrix of the plant being controlled.

  • B – Continuous input matrix of the plant being controlled.

  • Qelems – The maximum desired error tolerance for each state.

  • Relems – The maximum desired control effort for each input.

  • dt – Discretization timestep. @throws std::invalid_argument If the system is uncontrollable.

  1. __init__(self: wpimath._controls._controls.controller.LinearQuadraticRegulator_2_2, A: numpy.ndarray[numpy.float64[2, 2]], B: numpy.ndarray[numpy.float64[2, 2]], Q: numpy.ndarray[numpy.float64[2, 2]], R: numpy.ndarray[numpy.float64[2, 2]], dt: seconds) -> None

Constructs a controller with the given coefficients and plant.

Parameters:
  • A – Continuous system matrix of the plant being controlled.

  • B – Continuous input matrix of the plant being controlled.

  • Q – The state cost matrix.

  • R – The input cost matrix.

  • dt – Discretization timestep. @throws std::invalid_argument If the system is uncontrollable.

  1. __init__(self: wpimath._controls._controls.controller.LinearQuadraticRegulator_2_2, A: numpy.ndarray[numpy.float64[2, 2]], B: numpy.ndarray[numpy.float64[2, 2]], Q: numpy.ndarray[numpy.float64[2, 2]], R: numpy.ndarray[numpy.float64[2, 2]], N: numpy.ndarray[numpy.float64[2, 2]], dt: seconds) -> None

Constructs a controller with the given coefficients and plant.

Parameters:
  • A – Continuous system matrix of the plant being controlled.

  • B – Continuous input matrix of the plant being controlled.

  • Q – The state cost matrix.

  • R – The input cost matrix.

  • N – The state-input cross-term cost matrix.

  • dt – Discretization timestep. @throws std::invalid_argument If the system is uncontrollable.

  1. __init__(self: wpimath._controls._controls.controller.LinearQuadraticRegulator_2_2, arg0: wpimath._controls._controls.system.LinearSystem_2_2_1, arg1: Tuple[float, float], arg2: Tuple[float, float], arg3: seconds) -> None

  2. __init__(self: wpimath._controls._controls.controller.LinearQuadraticRegulator_2_2, arg0: wpimath._controls._controls.system.LinearSystem_2_2_2, arg1: Tuple[float, float], arg2: Tuple[float, float], arg3: seconds) -> None

K(*args, **kwargs)

Overloaded function.

  1. K(self: wpimath._controls._controls.controller.LinearQuadraticRegulator_2_2) -> numpy.ndarray[numpy.float64[2, 2]]

Returns the controller matrix K.

  1. K(self: wpimath._controls._controls.controller.LinearQuadraticRegulator_2_2, i: int, j: int) -> float

Returns an element of the controller matrix K.

Parameters:
  • i – Row of K.

  • j – Column of K.

R(*args, **kwargs)

Overloaded function.

  1. R(self: wpimath._controls._controls.controller.LinearQuadraticRegulator_2_2) -> numpy.ndarray[numpy.float64[2, 1]]

Returns the reference vector r.

Returns:

The reference vector.

  1. R(self: wpimath._controls._controls.controller.LinearQuadraticRegulator_2_2, i: int) -> float

Returns an element of the reference vector r.

Parameters:

i – Row of r.

Returns:

The row of the reference vector.

U(*args, **kwargs)

Overloaded function.

  1. U(self: wpimath._controls._controls.controller.LinearQuadraticRegulator_2_2) -> numpy.ndarray[numpy.float64[2, 1]]

Returns the control input vector u.

Returns:

The control input.

  1. U(self: wpimath._controls._controls.controller.LinearQuadraticRegulator_2_2, i: int) -> float

Returns an element of the control input vector u.

Parameters:

i – Row of u.

Returns:

The row of the control input vector.

calculate(*args, **kwargs)

Overloaded function.

  1. calculate(self: wpimath._controls._controls.controller.LinearQuadraticRegulator_2_2, x: numpy.ndarray[numpy.float64[2, 1]]) -> numpy.ndarray[numpy.float64[2, 1]]

Returns the next output of the controller.

Parameters:

x – The current state x.

  1. calculate(self: wpimath._controls._controls.controller.LinearQuadraticRegulator_2_2, x: numpy.ndarray[numpy.float64[2, 1]], nextR: numpy.ndarray[numpy.float64[2, 1]]) -> numpy.ndarray[numpy.float64[2, 1]]

Returns the next output of the controller.

Parameters:
  • x – The current state x.

  • nextR – The next reference vector r.

latencyCompensate(*args, **kwargs)

Overloaded function.

  1. latencyCompensate(self: wpimath._controls._controls.controller.LinearQuadraticRegulator_2_2, plant: wpimath._controls._controls.system.LinearSystem_2_2_1, dt: seconds, inputDelay: seconds) -> None

Adjusts LQR controller gain to compensate for a pure time delay in the input.

Linear-Quadratic regulator controller gains tend to be aggressive. If sensor measurements are time-delayed too long, the LQR may be unstable. However, if we know the amount of delay, we can compute the control based on where the system will be after the time delay.

See https://file.tavsys.net/control/controls-engineering-in-frc.pdf appendix C.4 for a derivation.

Parameters:
  • plant – The plant being controlled.

  • dt – Discretization timestep.

  • inputDelay – Input time delay.

  1. latencyCompensate(self: wpimath._controls._controls.controller.LinearQuadraticRegulator_2_2, plant: wpimath._controls._controls.system.LinearSystem_2_2_2, dt: seconds, inputDelay: seconds) -> None

Adjusts LQR controller gain to compensate for a pure time delay in the input.

Linear-Quadratic regulator controller gains tend to be aggressive. If sensor measurements are time-delayed too long, the LQR may be unstable. However, if we know the amount of delay, we can compute the control based on where the system will be after the time delay.

See https://file.tavsys.net/control/controls-engineering-in-frc.pdf appendix C.4 for a derivation.

Parameters:
  • plant – The plant being controlled.

  • dt – Discretization timestep.

  • inputDelay – Input time delay.

reset() None

Resets the controller.