[wpimath] Add cosineScale and instance optimize to SwerveModuleState

[narmstro2020]

Please note this a very early draft. I am aware that providing for mutability can lead to some unforeseen results. I'm keeping this simple in the beginning.

Inspiration for this PR
A Swerve Drive typically has 4 modules with getter methods providing SwerveModulePosition and SwerveModuleState objects for the purposes of odometry. In most cases these objects are created and disposed of. This also requires creating and disposing of Rotation2d objects. These objects are relatively small, but would consume some amount of the processor's time with Garbage Collection when taken in the aggregate.

Changes this PR provides.

1. A subclass of the Rotation2d class as mutable version called MutRotation2d. Mutable methods comparable to Rotation2d's method exist. Note this has not been checked for documentation adjustments. I started with a clone of Rotation2d and began modifications.
2. SwerveModuleState's angle field is now a MutRotation2d object.
3. Methods have been given to SwerveModuleState to alter the state fields.
4. Made SwerveModuleState's optimize method an instance method. I realized now that I need to leave the original as deprecated, but that can be handled after feedback.
5. Added a cosineScaling method to SwerveModuleState instances.
6. Applied changes accordingly to the SwerveDrive examples.

Work to Do

1. First receive feedback which is always appreciated. I can be way off in my assumptions.
2. Make mods accordingly.
3. Make these changes applicable to SwerveModulePosition.
4. Apply MutRotation2d where needed to reduce GC.

This PR now only adds a cosineScale method and an instance optimize method for SwerveModuleState

[github-actions]

This PR modifies commands. Please open a corresponding PR in Python Commands and include a link to this PR.

[narmstro2020]

/format

[calcmogul]

I'm not a fan of this. We chose to only provide immutable APIs because they're much easier to reason about than mutable APIs. In particular mutable variables can be modified by library functions in surprising ways, which requires the user spamming clone() (and incuring a copy) to avoid the side effects.

This PR doubles the maintenance overhead for Rotation2d as well.

The GC implications of immutable APIs will likely be moot in 2027 (2 years from now) when we have a robot controller with more CPU and RAM (value classes notwithstanding).

[narmstro2020]

I'm not a fan of this. We chose to only provide immutable APIs because they're much easier to reason about than mutable APIs. This PR doubles the maintenance overhead for Rotation2d as well.

The GC implications of immutable APIs will likely be moot in 2027 (2 years from now) when we have a robot controller with more CPU and RAM (value classes notwithstanding).

No problem. It was fun to play around with this.

I had my doubts too. I just saw many instances of new objects being made and wondered what the impact would be if some were made mutable.

Would there still be value in an instance method for cosineScaling?

And could the optimize method be made an instance method?

[calcmogul]

Would there still be value in an instance method for cosineScaling?

Not really. It's a one-liner.

And could the optimize method be made an instance method?

I'm ambivalent on this. Is there a benefit to making it an instance method? If we made that change, we'd have to go through a deprecation cycle as usual.

[narmstro2020]

Would there still be value in an instance method for cosineScaling?

Not really. It's a one-liner.

True but it's easy to mess up.

I'm ambivalent on this. Is there a benefit to making it an instance method? If we made that change, we'd have to go through a deprecation cycle as usual.

The only benefit I can see would be less code clutter with var this = new that at the client code level.

[Gold856]

It's a bit cleaner:
Current:

SwerveModuleState.optimize(state, angle);

Instance method:

state.optimize(angle);

[narmstro2020]

It's a bit cleaner: Current:

SwerveModuleState.optimize(state, angle);

Instance method:

state.optimize(angle);

state.speedMetersPerSecond *= state.angle.minus(encoderRotation).getCos();
would become

state.cosineScale(encoderRotation)

[narmstro2020]

It would also be manipulating the state of the SwerveModuleState inside the class.

[calcmogul]

I'd accept instance methods for SwerveModuleState.cosineScale() and SwerveModuleState.optimize() as a separate PR (with deprecations for the static methods).