[wpimath] Add a class to calculate PD gains using LQR

[TheComputer314]

Implementation is stolen from SysID.

TODO:

• Give it a nicer API shape
• Documenting

[TheComputer314]

I'm not the happiest with the API shape, (big block of doubles is not a fun parameter list) will prolly change later

[ArchdukeTim]

Since the comment above describes the special case, why not put the special case in this if block?

[TheComputer314]

IDK man that's what the comments in SysID do

[TheComputer314]

I deffo do agree that's kinda confusing. That's a problem for later me.

[calcmogul]

The comment refers to the whole if statement. It's a style thing.

[TheComputer314]

Ahhh ok.

[ArchdukeTim]

Like you do here

[calcmogul]

I'm not a fan of this API either. wpimath's design intent is to provide composable functions you can mix-and-match instead of big interfaces that automate plumbing for specific use cases. The latter is less flexible and maintainable in the long term (e.g., PIDSubsystem, PIDCommand, RamseteCommand, ProfiledPIDController).

The code in this PR can be shortened considerably by using a LinearSystemId factory function and making LQR take lists instead of vectors. At least in C++, you could shorten this enough that it's not really worth having the convenience function.

With all that said, I'd rather see a more general class like LTVController.

/**
 * Constructor.
 *
 * @param dynamics function
 * @param state tolerance vector
 * @param input tolerance vector
 * @param period (defaulted to 20 ms)
 * @param measurement delay (defaulted to 0 ms)
 */

/**
 * Returns the next output of the linearized LQR.
 *
 * @param measurement vector
 * @param reference vector
 * @returns control input vector
 */

I assume the main reason teams don't use the state-space API directly is because Java makes anything math-related difficult to write.

[calcmogul]

We probably don't need a C++ version, since it's really short:

auto system = frc::LinearSystemId::IdentifyPositionSystem<units::meters>(Kv, Ka);
frc::LinearQuadraticRegulator<2, 1> controller{system, {qp, qv}, {r}, period};
controller.LatencyCompensate(system, period, measurementDelay);

return {controller.K(0, 0), controller.K(0, 1)};

If your kₐ value is super small, or zero because you didn't measure it, you probably shouldn't be using LQR.

[TheComputer314]

I assume the main reason teams don't use the state-space API directly is because Java makes anything math-related difficult to write.

That's exactly it. I wanted to use LQR stuff in my own team code, but the verbosity of math in Java made it just easier to write a helper class that eventually ended up as this PR. Java sucks.