[wpimath] add cosine optimization method to SwerveModuleState

[Alextopher]

Motivation

Many teams are using so called 'cosine compensation' within their swerve drive subsystems. This optimization reduces 'skew' caused by changing direction.

For those unfamiliar, the idea is we scale a module's desired velocity by the cosine of it's azimuth error. At the extremes:

• If a module is already pointing in the right direction (no error) there is no change.
• If a module is off by 90 degrees then speed is set to 0, allowing time for the module to correct its angle without moving in the wrong direction.
• Everything in between is scaled by the cosine of the error.

Adding this feature to wpilib would help with discoverability and may reduce ecosystem code-duplication.

Discussion

I think there is room for discussion around the API for this feature. I see 2 possible usages:

// Call 2 methods to preform both optimizations
state = SwerveModuleStates.optimize(state, currentAngle);
state = SwerveModuleStates.cosineCompensation(state, currentAngle);

// Call a single one to preform both
state = SwerveModuleStates.cosineCompensation(state, currentAngle);

In the case where cosineCompensation does not preform the same angle optimization users will see odd behavior. For example, if there is 180 degrees error then it would initially seem like the module is moving in the right direction (cos(180) = -1). Since it isn't at the right angle we would watch the module start turning and slowing down (stopping as it passes through 90) for it to then start speeding back up again as it approaches 0.

It seems like a footgun in the world where we have 2 methods to only use cosineCompensation. Although, maybe preforming no optimization at-all is already a footgun.

[calcmogul]

The swerve example already does this as a one-liner, so I don't see a reason to complicate SwerveModuleState's implementation and tests. https://github.com/wpilibsuite/allwpilib/blob/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java#L116-L119

[Alextopher]

So maybe the vision for this change would be better fulfilled as outlining this idea in paragraph + code example on frc-docs?

https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html

[Alextopher]

I'll definitely defer to your judgement on this. Maybe we can leave this PR open for a day to see if there are any other opinions on the matter? Making a change on frc-docs seems like a good alternative, I will look into that process tomorrow.

[Alextopher]

Closing in favor of https://github.com/wpilibsuite/frc-docs/pull/2661