LQR-based Swerve Module; Ambiguous Error, No Stacktrace Provided

Jelatinone · September 24, 2023, 11:12am

Hello! I’m working on a project utilizing LQR to achieve accurate positions for Swerve Module azimuths. as a proof of concept that it could possibly be a better approach in season as opposed to my team’s tradition approach of PID control (Which cost us a lot of time this season). I’ve built my Module class around a pre-defined Linear System Loop. However, my error originates in constants, where each module is defined when the Modules are created.

Here’s what the Constants file which defines the Module(s) and where the error originates from looks like:

public static final class FL_Module {

          public static final class Values {
            public static final Double MAXIMUM_AZIMUTH_ACCELERATION = (Math.PI*2);    
            public static final Double AZIMUTH_ENCODER_POSITION_OFFSET = (-313.006);
            public static final Double MAXIMUM_AZIMUTH_VOLTAGE = (12.0);            
            public static final Double MAXIMUM_AZIMUTH_VELOCITY = (5.4);
            public static final Double MAXIMUM_LINEAR_VELOCITY = (5.4);
            public static final Double ACCELERATION_GAIN = (1.0); //TODO <---- Find Acceleration Gain (kA) of Linear Position System, Volts/(Units/Sec^2), DO NOT DEPLOY THIS UNTIL FOUND
            public static final Double VELOCITY_GAIN = (1.0); //TODO <---- Find Velocity Gain (kV) of Linear Position System, Volts/(Units/Sec), DO NOT DEPLOY THIS UNTIL FOUND

            public static final Double LINEAR_ENCODER_SENSITIVITY = (2048.0);     
            public static final Double ENCODER_SENSITIVITY = (4096.0);   

            public static final Double MODEL_VELOCITY_ACCURACY_DEGREES = (7e-5);
            public static final Double MODEL_POSITION_ACCURACY_RPM = (2.5e-4);
            public static final Double DISCRETIZATION_TIMESTEP = (1/50.0);

            public static final Integer AZIMUTH_ID = (21);                       
            public static final Integer LINEAR_ID = (11);    
            public static final Integer SENSOR_ID = (4);
          }

          public static final class Components {
            public static final WPI_CANCoder AZIMUTH_SENSOR = DrivebaseModule.configureRotationEncoder((new WPI_CANCoder(Values.SENSOR_ID)), Values.AZIMUTH_ENCODER_POSITION_OFFSET);
            public static final WPI_TalonFX LINEAR_CONTROLLER = DrivebaseModule.configureTranslationController(new WPI_TalonFX(Values.LINEAR_ID), Constants.Values.ComponentData.DRIVE_CURRENT_LIMIT);            
            public static final WPI_TalonFX AZIMUTH_CONTROLLER = DrivebaseModule.configureRotationController(new WPI_TalonFX(Values.AZIMUTH_ID), AZIMUTH_SENSOR, Constants.Values.ComponentData.AZIMUTH_CURRENT_LIMIT, Constants.Values.ComponentData.AZIMUTH_DEADBAND);                                 
            public static final Supplier<SwerveModuleState> STATE_SENSOR = () -> new SwerveModuleState(
              (2.0)*(((LINEAR_CONTROLLER.getSelectedSensorVelocity() / Values.LINEAR_ENCODER_SENSITIVITY) * (10)) / Constants.Values.Chassis.DRIVETRAIN_GEAR_RATIO) * Math.PI * Constants.Values.Chassis.WHEEL_DIAMETER,
              new Rotation2d(AZIMUTH_SENSOR.getAbsolutePosition()));            
            public static final DrivebaseModule MODULE = new DrivebaseModule(new MotorControllerGroup(AZIMUTH_CONTROLLER), new MotorControllerGroup(AZIMUTH_CONTROLLER), STATE_SENSOR, () -> Values.MAXIMUM_LINEAR_VELOCITY, () -> Control.CONSTRAINTS, Control.CONTROL_LOOP);
          }
      

          public static final class Control {
            public static final TrapezoidProfile.Constraints CONSTRAINTS = new TrapezoidProfile.Constraints(Values.MAXIMUM_AZIMUTH_VELOCITY, Values.MAXIMUM_AZIMUTH_ACCELERATION);
            public static final LinearSystem<N2,N1,N1> PLANT = LinearSystemId.identifyPositionSystem(Values.VELOCITY_GAIN, Values.ACCELERATION_GAIN);        
            public static final LinearSystemLoop<N2,N1,N1> CONTROL_LOOP = new LinearSystemLoop<>(
              PLANT,
              new LinearQuadraticRegulator<>(
                PLANT,
                VecBuilder.fill(Units.degreesToRadians(Components.AZIMUTH_SENSOR.getAbsolutePosition()), Units.rotationsPerMinuteToRadiansPerSecond(Components.AZIMUTH_SENSOR.getVelocity())), //<---- Controller error weight, how much we want to conserve controller error, or how accurate we want to be, increased time to target
                VecBuilder.fill(Values.MAXIMUM_AZIMUTH_VOLTAGE), //<---- Controller effort weight, how much we want to conserve controller expenditure or how fast we want to be; increased state error
                Values.DISCRETIZATION_TIMESTEP),
              new KalmanFilter<>(
                  Nat.N2(),
                  Nat.N1(),
                  PLANT,
                  VecBuilder.fill(Units.degreesToRadians(Values.MODEL_VELOCITY_ACCURACY_DEGREES), Units.rotationsPerMinuteToRadiansPerSecond(Values.MODEL_POSITION_ACCURACY_RPM)), //<---- How accurate we think our models are
                  VecBuilder.fill(Units.rotationsPerMinuteToRadiansPerSecond(1 / Values.ENCODER_SENSITIVITY)), //<---- How accurate we think our encoders are 
                  Values.DISCRETIZATION_TIMESTEP),
              Values.MAXIMUM_AZIMUTH_VOLTAGE,
              Values.DISCRETIZATION_TIMESTEP);
          }
        }

For some additional context, I’m also trying to utilize Advantage Kit for the replay and logging features it has, but I can’t get much use out of it so far considering nothing can be logged if it crashed prior to logging starting.

This is what the console looks like:

********** Robot program starting **********
NT: Listening on NT3 port 1735, NT4 port 5810
DataLog: Logging to 'src\main\java\frc\deploy\logs\Log_f8b5b944bd570ad7.wpilog'
[phoenix] CANbus Connected: sim
[phoenix] CANbus Network Up: sim
CTR: CAN frame not received/too-stale.
        CANCoder 4 GetAbsolutePosition
CTR: CAN frame not received/too-stale.
        CANCoder 4 GetAbsolutePosition
abort: Failure at D:\a\allwpilib\allwpilib\wpimath\src\main\native\thirdparty\drake\src\math\discrete_algebraic_riccati_equation.cpp:417 in DiscreteAlgebraicRiccatiEquation(): condition 'is_approx_equal_abstol(Q, Q.transpose(), 1e-10)' failed.

My initial solution was to remove all references to special values such as Double.NaN and Infinity as per this: drake Namespace Reference, and Github Issue.

At some point during my debugging session, I was able to yield a different (Much more revealing) error, (With a stacktrace!):

********** Robot program starting **********
NT: Listening on NT3 port 1735, NT4 port 5810
DataLog: Logging to 'src\main\java\frc\deploy\logs\Log_9ed21a554a9f2c61.wpilog'
[phoenix] CANbus Connected: sim
[phoenix] CANbus Network Up: sim
CTR: CAN frame not received/too-stale.
        CANCoder 4 GetAbsolutePosition
CTR: CAN frame not received/too-stale.
        CANCoder 4 GetAbsolutePosition
Error at frc.robot.subsystems.drivebase.Constants$Hardware$Modules$FL_Module$Control.<clinit>(Constants.java:109): Unhandled exception: java.lang.RuntimeException: Failure at D:\a\allwpilib\allwpilib\wpimath\src\main\native\thirdparty\drake\src\math\discrete_algebraic_riccati_equation.cpp:420 in DiscreteAlgebraicRiccatiEquation(): condition 'es.eigenvalues()[i] >= 0' failed.
        at edu.wpi.first.math.WPIMathJNI.discreteAlgebraicRiccatiEquation(Native Method)
        at edu.wpi.first.math.Drake.discreteAlgebraicRiccatiEquation(Drake.java:24)
        at edu.wpi.first.math.Drake.discreteAlgebraicRiccatiEquation(Drake.java:53)
        at edu.wpi.first.math.controller.LinearQuadraticRegulator.<init>(LinearQuadraticRegulator.java:114)
        at edu.wpi.first.math.controller.LinearQuadraticRegulator.<init>(LinearQuadraticRegulator.java:49)
        at frc.robot.subsystems.drivebase.Constants$Hardware$Modules$FL_Module$Control.<clinit>(Constants.java:109)
        at frc.robot.subsystems.drivebase.Constants$Hardware$Modules$FL_Module$Components.<clinit>(Constants.java:100)
        at frc.robot.subsystems.drivebase.DrivebaseSubsystem.<clinit>(DrivebaseSubsystem.java:59)
        at frc.robot.Constants$Subsystems.<clinit>(Constants.java:14)
        at frc.robot.RobotContainer.<init>(RobotContainer.java:16)
        at frc.robot.RobotContainer.getInstance(RobotContainer.java:33)
        at frc.robot.Robot.robotInit(Robot.java:68)
        at org.littletonrobotics.junction.LoggedRobot.startCompetition(LoggedRobot.java:59)
        at edu.wpi.first.wpilibj.RobotBase.runRobot(RobotBase.java:358)
        at edu.wpi.first.wpilibj.RobotBase.lambda$startRobot$0(RobotBase.java:431)
        at java.base/java.lang.Thread.run(Thread.java:833)

Warning at edu.wpi.first.wpilibj.RobotBase.runRobot(RobotBase.java:373): The robot program quit unexpectedly. This is usually due to a code error.
  The above stacktrace can help determine where the error occurred.
  See https://wpilib.org/stacktrace for more information.
Error at edu.wpi.first.wpilibj.RobotBase.runRobot(RobotBase.java:380): The startCompetition() method (or methods called by it) should have handled the exception above.      
[phoenix-diagnostics] Server shutdown cleanly. (dur:0)

[phoenix] Library shutdown cleanly

Unfortunately, I have since gone back to the original error message, which isn’t very helpful for debugging my system. Looking at those two issues since then again, I tried removing any references to negative Qelms and Relms, but I don’t actually have any when defining LinearSystemLoop or in my Constants (Where the modules are defined) as a whole. The only place they could appear is getting simulated encoder Absolute values, or From Joystick Inputs (Which wouldn’t make sense because the Joystick isn’t reference yet or even plugged in for that matter).

Therefore, as my last option and not making any progress for the last 3 hours, I have posted here. Anybody have any suggestions? Is this just limited to Simulated code because of something to do with the JNI?

Oblarg · September 24, 2023, 12:57pm

For a simple linear system, LQR is going to be equivalent to PID with properly-tuned gains. The main reason to use LQR is for conceptual clarity and to keep your code in more interpretable units, rather than for performance. I don’t mean to discourage you from pursuing LQR; I just want to make sure you have realistic expectations about how it’s likely to function.

The LinearSystemLoop abstraction is, in my opinion, a little bit overwrought and I think you might have better luck using the Kalman filter and LQR objects directly with your own plumbing.

yotam201410 · September 24, 2023, 2:17pm

In addition, keep in mind that robot Rio is very limited and has a hard time processing a lot of LQR loops and with it being a loop per module it can really take a lot of memory and processing power from the rest of the robot

calcmogul · September 24, 2023, 5:39pm

Jelatinone:

              new LinearQuadraticRegulator<>(
                PLANT,
                VecBuilder.fill(Units.degreesToRadians(Components.AZIMUTH_SENSOR.getAbsolutePosition()), Units.rotationsPerMinuteToRadiansPerSecond(Components.AZIMUTH_SENSOR.getVelocity())), //<---- Controller error weight, how much we want to conserve controller error, or how accurate we want to be, increased time to target
                VecBuilder.fill(Values.MAXIMUM_AZIMUTH_VOLTAGE), //<---- Controller effort weight, how much we want to conserve controller expenditure or how fast we want to be; increased state error
                Values.DISCRETIZATION_TIMESTEP),

You should be passing positive constants to the VecBuilder.fill() calls. The encoder getters can return negative values, which would throw the second exception you saw.

The first one is from Q not being symmetric. It should be symmetric by construction though with the LQR overload you used.

By the way, we rewrote our DARE solver for 2024, and it will have better error messages than Drake’s (we call the C++ solver in JNI, which forwards the exception messages to Java).

github.com

wpilibsuite/allwpilib/blob/main/wpimath/src/main/native/include/frc/DARE.h#L45-L94


      
          // Require Q be symmetric
          if ((Q - Q.transpose()).norm() > 1e-10) {
            std::string msg = fmt::format("Q isn't symmetric!\n\nQ =\n{}\n", Q);
            throw std::invalid_argument(msg);
          }
          
          // Require Q be positive semidefinite
          //
          // If Q is a symmetric matrix with a decomposition LDLᵀ, the number of
          // positive, negative, and zero diagonal entries in D equals the number of
          // positive, negative, and zero eigenvalues respectively in Q (see
          // https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia).
          //
          // Therefore, D having no negative diagonal entries is sufficient to prove Q
          // is positive semidefinite.
          auto Q_ldlt = Q.ldlt();
          if (Q_ldlt.info() != Eigen::Success ||
              (Q_ldlt.vectorD().array() < 0.0).any()) {
            std::string msg =
                fmt::format("Q isn't positive semidefinite!\n\nQ =\n{}\n", Q);

This file has been truncated. show original

I disagree with this. There’s construction cost from solving the DARE, but that should only happen at boot. After, that, it’s just a few floating point multiplies and adds in calculate() called every loop. Granted, EJML adds some intermediate matrix object allocations in there too, because Java doesn’t let us have nice things; C++ doesn’t have this overhead.

github.com

wpilibsuite/allwpilib/blob/v2023.4.3/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java#L229-L232


      
          public Matrix<Inputs, N1> calculate(Matrix<States, N1> x) {
            m_u = m_K.times(m_r.minus(x));
            return m_u;
          }

To make snippets of code like that more performant, we could rewrite it to use free functions on double[] instances (would be way less readable), or make the Java classes JNI wrappers around the C++ versions (would be a lot more boilerplate in the backend).

In any case, the first rule of performance is to measure. Once someone demonstrates it’s a performance bottleneck in a benchmark, we’ll optimize it.
Rotation3d.rotateBy() was a recent instance of this; it was being called hundreds of times per loop, and we sped it up by 100x by replacing EJML vector operations in the underlying quaternion object with raw doubles.

Jelatinone · September 24, 2023, 9:52pm

I tried this last night too actually, did it like this:

            public static final Double AZIMUTH_EPSILON_POSITION = Components.AZIMUTH_SENSOR.getAbsolutePosition();
            public static final Double AZIMUTH_EPSILON_VELOCITY = Components.AZIMUTH_SENSOR.getVelocity();
            public static final TrapezoidProfile.Constraints CONSTRAINTS = new TrapezoidProfile.Constraints(Values.MAXIMUM_AZIMUTH_VELOCITY, Values.MAXIMUM_AZIMUTH_ACCELERATION);
            public static final LinearSystem<N2,N1,N1> PLANT = LinearSystemId.identifyPositionSystem(Values.VELOCITY_GAIN, Values.ACCELERATION_GAIN);        
            public static final LinearSystemLoop<N2,N1,N1> CONTROL_LOOP = new LinearSystemLoop<>(
              PLANT,
              new LinearQuadraticRegulator<>(
                PLANT,
                VecBuilder.fill(Units.degreesToRadians((AZIMUTH_EPSILON_POSITION >= 0)? (AZIMUTH_EPSILON_POSITION): (1.0)), Units.rotationsPerMinuteToRadiansPerSecond((AZIMUTH_EPSILON_VELOCITY >= 0)? (AZIMUTH_EPSILON_VELOCITY): (1.0))), //<---- Controller error weight, how much we want to conserve controller error, or how accurate we want to be, increased time to target
                VecBuilder.fill(Values.MAXIMUM_AZIMUTH_VOLTAGE), //<---- Controller effort weight, how much we want to conserve controller expenditure or how fast we want to be; increased state error
                Values.DISCRETIZATION_TIMESTEP),

Still got the same error, but I just realized something, it should be > not >=, I think one of the errors about eigenvalues that I had read said >= 0 and that’s where I got this notion from. So used > and set the default to 1 radian, and good news it now simulates!

              new LinearQuadraticRegulator<>(
                PLANT,
                VecBuilder.fill(Units.degreesToRadians((AZIMUTH_EPSILON_POSITION > 0)? (AZIMUTH_EPSILON_POSITION): (1.0)), Units.rotationsPerMinuteToRadiansPerSecond((AZIMUTH_EPSILON_VELOCITY > 0)? (AZIMUTH_EPSILON_VELOCITY): (1.0))),
                VecBuilder.fill(Values.MAXIMUM_AZIMUTH_VOLTAGE), 
                Values.DISCRETIZATION_TIMESTEP),

But now I have a new question, what should the actual value defaults be since they cannot be less than or equal to 0, Math.abs() or change the CANCoder configurations to do .setPosition(0.0) as opposed to setAbsolutePosition()?

Jelatinone · September 24, 2023, 10:11pm

I will admit i’m not the greatest when it comes to math (I’m only in Pre-calc), so I can only really understand LQR from the videos i’ve watched, and I don’t fully understand the underlying equations, but I do understand how it works from a programming perspective, and tuning it., But wouldn’t LQR be easier to tune in theory given that you only need to tune for Qelms and Relms?

calcmogul · September 24, 2023, 10:11pm

The vector constructor overload does the following to compute Q and R (with ρ = 1):

from section 7.7 of Controls Engineering in FRC

Passing infinity results in a zero entry in Q, which means the state doesn’t contribute to the sum squared cost (this is a valid thing to be able to do). Passing zero results in an infinity in Q, which is numerically unstable (i.e., not valid).

This explains how to pick the constants:

Oblarg · September 25, 2023, 12:25pm

It’ll be easier to tune in the sense that the tuning handles are way more sensible, yeah. That’s the primary reason to use LQR.

Jelatinone · October 25, 2023, 3:24am

Also I would like to note that I was using the wrong constants here, I was under the assumption that the Qelms and Relms should be the initial position of the robots, I assumed ‘epsilon’ meant starting position when observing the constructor for an LQR-based Single jointed arm from lib5k.

Admlvntv · October 25, 2023, 12:55pm

@Jelatinone do you have a link to your teams code? I’d like to check out something like this

Jelatinone · October 25, 2023, 2:28pm

Of course, Here’s the whole repository, more specifically LinearSystemModule.java contains all of the code to control a swerve module using LinearSystems, but the LQR, Kalman Filter, and Feedback, and LinearSystem itself is all created in the drivebase susbystem folder’s Constants.java It was done this way so that I could be as abstracted as possible at the time, but i’ve moved to a new approach with modularly implementing LinearSystem based code using matrices which will soon be on the Rework branch.

A forewarning though, on the main code i’m trying to fix and issue where the CANCoders on the azimuths dont report the angles, and when we around a perfect 45 degree angle on the azimuths they seem to get stuck switching back and forth when using the integrated encoders on the CANSparkMax.

Good news is it works in simulation using WPILib’s FlywheelSim Classes:
Working LQRgif