Mutating returned SwerveModuleState[] from SwerveDriveKinematics.toSwerveModuleStates() affects angles returned when called again with ChassisSpeeds(0,0,0)

[person4268]

Describe the bug
If you take the states returned from SwerveDriveKinematics.toSwerveModuleStates, and take one of the angles, and mutate it (e.g. states[0].angle = new Rotation2d()), the returned array of states is the same object as the one in m_moduleStates, thus affecting it. This is visible when you call SwerveDriveKinematics.toSwerveModuleStates(new ChassisSpeeds()), which tries to persist the angles using the states in m_moduleStates.

To Reproduce
Run the following code:

package frc.robot;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;

public final class Main {
  private Main() {}

  public static void main(String... args) {
    var kinematics = new SwerveDriveKinematics(
      new Translation2d(1, 1),
      new Translation2d(1, -1),
      new Translation2d(-1, 1),
      new Translation2d(-1, -1)
    );

    var states = kinematics.toSwerveModuleStates(new ChassisSpeeds(1, 1, 0));
    System.out.println("states[0] pre-mutation: " + states[0].toString());
    states[0].angle = states[0].angle.plus(Rotation2d.fromDegrees(32));
    System.out.println("states[0] post-mutation " + states[0].toString());

    states = kinematics.toSwerveModuleStates(new ChassisSpeeds(0, 0, 0));
    System.out.println("states[0] when stopped: " + states[0].toString());
  }
}

Output:

states[0] pre-mutation: SwerveModuleState(Speed: 1.41 m/s, Angle: Rotation2d(Rads: 0.79, Deg: 45.00))
states[0] post-mutation SwerveModuleState(Speed: 1.41 m/s, Angle: Rotation2d(Rads: 1.34, Deg: 77.00))
states[0] when stopped: SwerveModuleState(Speed: 0.00 m/s, Angle: Rotation2d(Rads: 1.34, Deg: 77.00))

Expected behavior

states[0] pre-mutation: SwerveModuleState(Speed: 1.41 m/s, Angle: Rotation2d(Rads: 0.79, Deg: 45.00))
states[0] post-mutation SwerveModuleState(Speed: 1.41 m/s, Angle: Rotation2d(Rads: 1.34, Deg: 77.00))
states[0] when stopped: SwerveModuleState(Speed: 0.00 m/s, Angle: Rotation2d(Rads: 0.79, Deg: 45.00))

Desktop:

• WPILib Version: 2023.2.1
• OS: Arch Linux
• Java version: OpenJDK Runtime Environment (build 11.0.18+10)

[calcmogul]

Heh, this suppressed warning seems relevant:

  @SuppressWarnings("PMD.MethodReturnsInternalArray")
  public SwerveModuleState[] toSwerveModuleStates(

[Starlight220]

I'm apprehensive that deep-copying everywhere would be a lot of extra allocations.

[rzblue]

The other option would be to have the method take an array as an out param and copy the states into the passed array.

[Starlight220]

Out params aren't intuitive for Java programmers (especially those in high school), and we shouldn't use them in our API.

[calcmogul]

The simplest solution is making the Java API match the C++ API by returning a new object (C++ returns by value). The GC should be able to handle small, short-lived objects because that's a very common memory usage pattern.

[person4268]

Out params aren't intuitive for Java programmers (especially those in high school), and we shouldn't use them in our API.

FWIW, desaturateWheelSpeeds already takes in module states as an in/out parameter, so it's not like out params are entirely unused as it is now. I do agree with the sentiment that it'd be better to just return a new object, though (out params aren't really used particularly much [in wpilib] afaik).

[person4268]

Given that it's been roughly a week without comment, should I make a PR for this? Or is debate still open on the approach?