[wpimath] Add and use kinematics.copyInto() (#6789)

wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java

@@ -120,6 +120,13 @@ public class DifferentialDriveKinematics
     return new DifferentialDriveWheelPositions(positions.leftMeters, positions.rightMeters);
   }
 
+  @Override
+  public void copyInto(
+      DifferentialDriveWheelPositions positions, DifferentialDriveWheelPositions output) {
+    output.leftMeters = positions.leftMeters;
+    output.rightMeters = positions.rightMeters;
+  }
+
   @Override
   public DifferentialDriveWheelPositions interpolate(
       DifferentialDriveWheelPositions startValue,

wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java

@@ -53,4 +53,13 @@ public interface Kinematics<S, P> extends Interpolator<P> {
    * @return A copy.
    */
   P copy(P positions);
+
+  /**
+   * Copies the value of the wheel positions object into the output.
+   *
+   * @param positions The wheel positions object to copy. Will not be modified.
+   * @param output The output variable of the copy operation. Will have the same value as the
+   *     positions object after the call.
+   */
+  void copyInto(P positions, P output);
 }

wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java

@@ -266,6 +266,14 @@ public class MecanumDriveKinematics
         positions.rearRightMeters);
   }
 
+  @Override
+  public void copyInto(MecanumDriveWheelPositions positions, MecanumDriveWheelPositions output) {
+    output.frontLeftMeters = positions.frontLeftMeters;
+    output.frontRightMeters = positions.frontRightMeters;
+    output.rearLeftMeters = positions.rearLeftMeters;
+    output.rearRightMeters = positions.rearRightMeters;
+  }
+
   @Override
   public MecanumDriveWheelPositions interpolate(
       MecanumDriveWheelPositions startValue, MecanumDriveWheelPositions endValue, double t) {

wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java

@@ -25,7 +25,7 @@ public class Odometry<T> {
 
   private Rotation2d m_gyroOffset;
   private Rotation2d m_previousAngle;
-  private T m_previousWheelPositions;
+  private final T m_previousWheelPositions;
 
   /**
    * Constructs an Odometry object.
@@ -61,7 +61,7 @@ public class Odometry<T> {
     m_poseMeters = poseMeters;
     m_previousAngle = m_poseMeters.getRotation();
     m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    m_previousWheelPositions = m_kinematics.copy(wheelPositions);
+    m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
   }
 
   /**
@@ -122,7 +122,7 @@ public class Odometry<T> {
 
     var newPose = m_poseMeters.exp(twist);
 
-    m_previousWheelPositions = m_kinematics.copy(wheelPositions);
+    m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
     m_previousAngle = angle;
     m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
 

wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java

@@ -392,6 +392,17 @@ public class SwerveDriveKinematics
     return newPositions;
   }
 
+  @Override
+  public void copyInto(SwerveModulePosition[] positions, SwerveModulePosition[] output) {
+    if (positions.length != output.length) {
+      throw new IllegalArgumentException("Inconsistent number of modules!");
+    }
+    for (int i = 0; i < positions.length; ++i) {
+      output[i].distanceMeters = positions[i].distanceMeters;
+      output[i].angle = positions[i].angle;
+    }
+  }
+
   @Override
   public SwerveModulePosition[] interpolate(
       SwerveModulePosition[] startValue, SwerveModulePosition[] endValue, double t) {

wpimath/src/main/native/include/frc/kinematics/Kinematics.h

@@ -20,7 +20,8 @@ namespace frc {
  * forward kinematics converts wheel speeds into chassis speed.
  */
 template <typename WheelSpeeds, typename WheelPositions>
-  requires std::copy_constructible<WheelPositions>
+  requires std::copy_constructible<WheelPositions> &&
+           std::assignable_from<WheelPositions&, const WheelPositions&>
 class WPILIB_DLLEXPORT Kinematics {
  public:
   /**