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

wpilibsuite · Jul 16, 2024 · 7b7d17c · 7b7d17c
1 parent 636450a
commit 7b7d17c
Show file tree

Hide file tree

Showing 6 changed files with 40 additions and 4 deletions.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
@@ -120,6 +120,13 @@ public DifferentialDriveWheelPositions copy(DifferentialDriveWheelPositions posi
     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,

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java b/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);
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
@@ -266,6 +266,14 @@ public MecanumDriveWheelPositions copy(MecanumDriveWheelPositions positions) {
         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) {

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java b/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 void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMet
     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 Pose2d update(Rotation2d gyroAngle, T wheelPositions) {
 
     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);
 

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
@@ -392,6 +392,17 @@ public SwerveModulePosition[] copy(SwerveModulePosition[] positions) {
     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) {

diff --git a/wpimath/src/main/native/include/frc/kinematics/Kinematics.h b/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:
   /**