[wpimath] Make swerve and differential kinematics functions immutable (#8274)

Originally started with just swerve, but expanded to diff and mecanum
(docs only) for parity across the drivetrains. Return value checks are
applied when possible to make migration easier and to error loudly if
people forget.

---------

Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>

robotpyExamples/SwerveBot/drivetrain.py

@@ -67,22 +67,23 @@ class Drivetrain:
         :param fieldRelative: Whether the provided x and y velocities are relative to the field.
         :param periodSeconds: Time
         """
-        robot_velocities = wpimath.ChassisVelocities(xVelocity, yVelocity, rot)
+        chassisVelocities = wpimath.ChassisVelocities(xVelocity, yVelocity, rot)
         if fieldRelative:
-            robot_velocities = robot_velocities.toRobotRelative(
+            chassisVelocities = chassisVelocities.toRobotRelative(
                 self.imu.getRotation2d()
             )
 
-        swerveModuleStates = self.kinematics.toSwerveModuleVelocities(
-            wpimath.ChassisVelocities.discretize(robot_velocities, periodSeconds)
+        chassisVelocities = chassisVelocities.discretize(periodSeconds)
+
+        velocities = wpimath.SwerveDrive4Kinematics.desaturateWheelVelocities(
+            self.kinematics.toSwerveModuleVelocities(chassisVelocities),
+            kMaxVelocity,
         )
-        wpimath.SwerveDrive4Kinematics.desaturateWheelVelocities(
-            swerveModuleStates, kMaxVelocity
-        )
-        self.frontLeft.setDesiredVelocity(swerveModuleStates[0])
-        self.frontRight.setDesiredVelocity(swerveModuleStates[1])
-        self.backLeft.setDesiredVelocity(swerveModuleStates[2])
-        self.backRight.setDesiredVelocity(swerveModuleStates[3])
+
+        self.frontLeft.setDesiredVelocity(velocities[0])
+        self.frontRight.setDesiredVelocity(velocities[1])
+        self.backLeft.setDesiredVelocity(velocities[2])
+        self.backRight.setDesiredVelocity(velocities[3])
 
     def updateOdometry(self) -> None:
         """Updates the field relative position of the robot."""

robotpyExamples/SwerveBot/swervemodule.py

@@ -104,24 +104,24 @@ class SwerveModule:
 
         encoderRotation = wpimath.Rotation2d(self.turningEncoder.getDistance())
 
-        # Optimize the reference state to avoid spinning further than 90 degrees
-        desiredVelocity.optimize(encoderRotation)
-
-        # Scale velocity by cosine of angle error. This scales down movement perpendicular to the desired
-        # direction of travel that can occur when modules change directions. This results in smoother
-        # driving.
-        desiredVelocity.cosineScale(encoderRotation)
+        # Optimize the desired velocity to avoid spinning further than 90 degrees, then scale
+        # velocity by cosine of angle error. This scales down movement perpendicular to the desired
+        # direction of travel that can occur when modules change directions. This results in
+        # smoother driving.
+        velocity = desiredVelocity.optimize(encoderRotation).cosineScale(
+            encoderRotation
+        )
 
         # Calculate the drive output from the drive PID controller.
         driveOutput = self.drivePIDController.calculate(
-            self.driveEncoder.getRate(), desiredVelocity.velocity
+            self.driveEncoder.getRate(), velocity.velocity
         )
 
-        driveFeedforward = self.driveFeedforward.calculate(desiredVelocity.velocity)
+        driveFeedforward = self.driveFeedforward.calculate(velocity.velocity)
 
         # Calculate the turning motor output from the turning PID controller.
         turnOutput = self.turningPIDController.calculate(
-            self.turningEncoder.getDistance(), desiredVelocity.angle.radians()
+            self.turningEncoder.getDistance(), velocity.angle.radians()
         )
 
         turnFeedforward = self.turnFeedforward.calculate(

robotpyExamples/SwerveDrivePoseEstimator/drivetrain.py

@@ -81,15 +81,15 @@ class Drivetrain:
 
         chassisVelocities = chassisVelocities.discretize(period)
 
-        states = self.kinematics.toSwerveModuleVelocities(chassisVelocities)
-        wpimath.SwerveDrive4Kinematics.desaturateWheelVelocities(
-            states, self.kMaxVelocity
+        velocities = wpimath.SwerveDrive4Kinematics.desaturateWheelVelocities(
+            self.kinematics.toSwerveModuleVelocities(chassisVelocities),
+            self.kMaxVelocity,
         )
 
-        self.frontLeft.setDesiredVelocity(states[0])
-        self.frontRight.setDesiredVelocity(states[1])
-        self.backLeft.setDesiredVelocity(states[2])
-        self.backRight.setDesiredVelocity(states[3])
+        self.frontLeft.setDesiredVelocity(velocities[0])
+        self.frontRight.setDesiredVelocity(velocities[1])
+        self.backLeft.setDesiredVelocity(velocities[2])
+        self.backRight.setDesiredVelocity(velocities[3])
 
     def updateOdometry(self) -> None:
         """Updates the field relative position of the robot."""

robotpyExamples/SwerveDrivePoseEstimator/swervemodule.py

@@ -104,24 +104,24 @@ class SwerveModule:
         """
         encoderRotation = wpimath.Rotation2d(self.turningEncoder.getDistance())
 
-        # Optimize the reference state to avoid spinning further than 90 degrees
-        desiredVelocity.optimize(encoderRotation)
-
-        # Scale velocity by cosine of angle error. This scales down movement perpendicular to the
-        # desired direction of travel that can occur when modules change directions. This results
-        # in smoother driving.
-        desiredVelocity.cosineScale(encoderRotation)
+        # Optimize the desired velocity to avoid spinning further than 90 degrees, then scale
+        # velocity by cosine of angle error. This scales down movement perpendicular to the desired
+        # direction of travel that can occur when modules change directions. This results in
+        # smoother driving.
+        velocity = desiredVelocity.optimize(encoderRotation).cosineScale(
+            encoderRotation
+        )
 
         # Calculate the drive output from the drive PID controller.
         driveOutput = self.drivePIDController.calculate(
-            self.driveEncoder.getRate(), desiredVelocity.velocity
+            self.driveEncoder.getRate(), velocity.velocity
         )
 
-        driveFeedforward = self.driveFeedforward.calculate(desiredVelocity.velocity)
+        driveFeedforward = self.driveFeedforward.calculate(velocity.velocity)
 
         # Calculate the turning motor output from the turning PID controller.
         turnOutput = self.turningPIDController.calculate(
-            self.turningEncoder.getDistance(), desiredVelocity.angle.radians()
+            self.turningEncoder.getDistance(), velocity.angle.radians()
         )
 
         turnFeedforward = self.turnFeedforward.calculate(