Class LQRController

java.lang.Object
yams.core.math.LQRController
public class LQRController extends Object
Linear Quadratic Regulator (Regulator is a type of controller)

An LQR computes optimal control inputs (voltages) that minimize a quadratic cost function combining state error and control effort. It requires a linearized plant model, which is built automatically from the physical parameters supplied via LQRConfig. Three mechanism types are supported: ARM, ELEVATOR, and FLYWHEEL.

On each robot loop iteration, call the appropriate calculate() overload with the current sensor measurement and the desired setpoint. The controller internally runs a LinearSystemLoop that fuses a LinearQuadraticRegulator with a KalmanFilter observer.

Example construct from LQRConfig and calculate arm voltage

import static org.wpilib.units.Units.*;

// Build configuration (see LQRConfig for full parameter details)
LQRConfig config = new LQRConfig(
        DCMotor.getNEO(1),
        MechanismGearing.ofReduction(60.0),
        KilogramSquareMeters.of(0.25))
    .withArm(
        Radians.of(0.01),
        RadiansPerSecond.of(0.5),
        Radians.of(0.05),
        RadiansPerSecond.of(0.5),
        Radians.of(0.01));

LQRController controller = new LQRController(config);

// In robotInit / subsystem initialization, reset to the current sensor state:
controller.reset(encoder.getAngle(), encoder.getAngularVelocity());

// In periodic, calculate and apply voltage:
Voltage output = controller.calculate(
        encoder.getAngle(),           // measured position
        Radians.of(Math.PI / 4),      // position setpoint
        RadiansPerSecond.of(0));       // velocity setpoint
motor.setVoltage(output.in(Volts));

  • Constructor Summary

    Constructors
    Constructor
    Description
    LQRController(LQRConfig config)
    Create a LQR Controller.
    LQRController(LQRConfig.LQRType type, org.wpilib.math.system.LinearSystemLoop<?,?,?> loop, org.wpilib.units.measure.Time period)
    Create a LQR Controller.

  • Method Summary

    Modifier and Type
    Method
    Description
    org.wpilib.units.measure.Voltage
    calculate(org.wpilib.units.measure.Angle measured, org.wpilib.units.measure.Angle position, org.wpilib.units.measure.AngularVelocity velocity)
    Calculate the voltage for an Arm LQR
    org.wpilib.units.measure.Voltage
    calculate(org.wpilib.units.measure.AngularVelocity measured, org.wpilib.units.measure.AngularVelocity velocity)
    Calculate the voltage for a Flywheel LQR
    org.wpilib.units.measure.Voltage
    calculate(org.wpilib.units.measure.Distance measured, org.wpilib.units.measure.Distance position, org.wpilib.units.measure.LinearVelocity velocity)
    Calculate the voltage for a Elevator LQR
    org.wpilib.units.measure.Voltage
    calculate(org.wpilib.units.measure.LinearVelocity measured, org.wpilib.units.measure.LinearVelocity velocity)
    Calculate the voltage for a Flywheel LQR
    Optional<LQRConfig>
    getConfig()
    Get the LQRConfig for the LQR.
    LQRConfig.LQRType
    getType()
    Get the type of LQR.
    void
    reset(org.wpilib.units.measure.Angle angle, org.wpilib.units.measure.AngularVelocity velocity)
    Reset the Arm or Flywheel LQR.
    void
    reset(org.wpilib.units.measure.Distance distance, org.wpilib.units.measure.LinearVelocity velocity)
    Reset the Elevator LQR.
    void
    updateConfig(LQRConfig config)
    Update LQR based off config.

    Methods inherited from class Object

    clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

  • Constructor Details

    • LQRController

      public LQRController(LQRConfig.LQRType type, org.wpilib.math.system.LinearSystemLoop<?,?,?> loop, org.wpilib.units.measure.Time period)
      Create a LQR Controller.
      Parameters:
      type - LQR Type.
      loop - LinearSystemLoop which can be derived from LQRConfig
      period - Loop time.

    • LQRController

      public LQRController(LQRConfig config)
      Create a LQR Controller.
      Parameters:
      config - LQRConfig to create the controller from.
      Throws:
      NoSuchElementException - if config has no mechanism type configured (none of withFlyWheel, withArm, or withElevator was called).

  • Method Details

    • updateConfig

      public void updateConfig(LQRConfig config)
      Update LQR based off config.
      Parameters:
      config - LQRConfig
      Throws:
      NoSuchElementException - if config has no mechanism type configured (none of withFlyWheel, withArm, or withElevator was called).

    • reset

      public void reset(org.wpilib.units.measure.Angle angle, org.wpilib.units.measure.AngularVelocity velocity)
      Reset the Arm or Flywheel LQR.
      Parameters:
      angle - Current angle.
      velocity - Current velocity.

    • reset

      public void reset(org.wpilib.units.measure.Distance distance, org.wpilib.units.measure.LinearVelocity velocity)
      Reset the Elevator LQR.
      Parameters:
      distance - Current distance.
      velocity - Current velocity.

    • calculate

      public org.wpilib.units.measure.Voltage calculate(org.wpilib.units.measure.Angle measured, org.wpilib.units.measure.Angle position, org.wpilib.units.measure.AngularVelocity velocity)
      Calculate the voltage for an Arm LQR
      Parameters:
      measured - Measured angle from the arm.
      position - Angle setpoint to go to.
      velocity - AngularVelocity setpoint to go to.
      Returns:
      Voltage to apply to the arm.

    • calculate

      public org.wpilib.units.measure.Voltage calculate(org.wpilib.units.measure.Distance measured, org.wpilib.units.measure.Distance position, org.wpilib.units.measure.LinearVelocity velocity)
      Calculate the voltage for a Elevator LQR
      Parameters:
      measured - Measured distance of the elevator.
      position - Setpoint distance of the elevator from the motion profile.
      velocity - Setpoint velocity of the elevator from the motion profile.
      Returns:
      Voltage to apply to the elevator.

    • calculate

      public org.wpilib.units.measure.Voltage calculate(org.wpilib.units.measure.AngularVelocity measured, org.wpilib.units.measure.AngularVelocity velocity)
      Calculate the voltage for a Flywheel LQR
      Parameters:
      measured - Measured velocity of the flywheel.
      velocity - Setpoint velocity of the flywheel.
      Returns:
      Voltage to apply to the flywheel.

    • calculate

      public org.wpilib.units.measure.Voltage calculate(org.wpilib.units.measure.LinearVelocity measured, org.wpilib.units.measure.LinearVelocity velocity)
      Calculate the voltage for a Flywheel LQR
      Parameters:
      measured - Measured velocity of the flywheel.
      velocity - Setpoint velocity of the flywheel.
      Returns:
      Voltage to apply to the flywheel.

    • getType

      public LQRConfig.LQRType getType()
      Get the type of LQR.
      Returns:
      LQRConfig.LQRType

    • getConfig

      public Optional<LQRConfig> getConfig()
      Get the LQRConfig for the LQR.
      Returns:
      LQRConfig