Swerve Drive Optimize does not consider full robot

[scarmain]

Is your feature request related to a problem? Please describe. SwerveModuleState currently has a great function optimize() that considers the wheel and will take the shortest angle and reverse direction if needed to get the swerve corner to the correct position the quickest way possible. This strategy has a major flaw though by being per wheel.

Because it does not consider the whole robot, you can end up in a scenario where 3 motors are 88 off command, the last one is 92 off command, and the swerve corners turn in opposite directions. This induces a HUGE yaw on the incorrect wheel (we were seeing 30*+ sometimes) and makes driving less smooth.

Describe the solution you'd like SwerveDriveKinematics adds a optimize() function similar to desaturateWheelSpeeds() that considers all the swerve drive states, and makes all the wheels spin the same way in the shortest total direction.

For the example, if the 3 wheels at 88 need CW, and the 92 needs CCW, all the wheels should go CW to minimize the difference in command. Not sure if the drive velocity should also be considered.

Describe alternatives you've considered None really, just the proposed solution

Additional context none

[scarmain]

I just thought, there is a big flaw with this, if the wheels are +/-1, then you don't want to spin one 179 to get back to straight... You need like a threshold to start considering this strategy. (guestimating 30-45*)