From 7ed1d1d1557da2ea6588a599dc5ab0f49b8aa23e Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Sat, 13 Jul 2024 12:24:26 -0700 Subject: [PATCH] Only support fully-mutable wheel positions objects We never had any like this, and C++ required full mutability anyways --- .../DifferentialDriveKinematics.java | 9 +++-- .../wpi/first/math/kinematics/Kinematics.java | 36 +++---------------- .../kinematics/MecanumDriveKinematics.java | 12 +++---- .../wpi/first/math/kinematics/Odometry.java | 6 ++-- .../kinematics/SwerveDriveKinematics.java | 12 +++---- 5 files changed, 22 insertions(+), 53 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 index c11553f2474..d67a7816475 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java @@ -121,11 +121,10 @@ public DifferentialDriveWheelPositions copy(DifferentialDriveWheelPositions posi } @Override - public DifferentialDriveWheelPositions copyInto( - DifferentialDriveWheelPositions positions, DifferentialDriveWheelPositions buffer) { - buffer.leftMeters = positions.leftMeters; - buffer.rightMeters = positions.rightMeters; - return buffer; + public void copyInto( + DifferentialDriveWheelPositions positions, DifferentialDriveWheelPositions output) { + output.leftMeters = positions.leftMeters; + output.rightMeters = positions.rightMeters; } @Override 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 index 90fc08cf79d..307999d6552 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java @@ -49,43 +49,17 @@ public interface Kinematics extends Interpolator

{ /** * Returns a copy of the wheel positions object. * - *

Implementation note: - * - *

- * * @param positions The wheel positions object to copy. * @return A copy. */ P copy(P positions); /** - * Returns a copy of the wheel positions object, possibly reusing storage in the buffer. - * - *

This is meant to be used like this: - * - * 

-   * m_buffer = m_kinematics.copyInto(positions, m_buffer);
-   *
- * - *

Implementation note: - * - *

+ * Copies the value of the wheel positions object into the output. * - * @param positions The wheel positions object to copy. Must not be modified. - * @param buffer A buffer available for overwriting. The state after the call is undefined. - * @return The copy. Identity is undefined. + * @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. */ - default P copyInto(P positions, P buffer) { - return copy(positions); - } + 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 index 522cdcccfa9..26108088c63 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java @@ -267,13 +267,11 @@ public MecanumDriveWheelPositions copy(MecanumDriveWheelPositions positions) { } @Override - public MecanumDriveWheelPositions copyInto( - MecanumDriveWheelPositions positions, MecanumDriveWheelPositions buffer) { - buffer.frontLeftMeters = positions.frontLeftMeters; - buffer.frontRightMeters = positions.frontRightMeters; - buffer.rearLeftMeters = positions.rearLeftMeters; - buffer.rearRightMeters = positions.rearRightMeters; - return buffer; + public void copyInto(MecanumDriveWheelPositions positions, MecanumDriveWheelPositions output) { + output.frontLeftMeters = positions.frontLeftMeters; + output.frontRightMeters = positions.frontRightMeters; + output.rearLeftMeters = positions.rearLeftMeters; + output.rearRightMeters = positions.rearRightMeters; } @Override 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 index f45fe4e8549..4db2e9651a2 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java @@ -24,7 +24,7 @@ public class Odometry { private Rotation2d m_gyroOffset; private Rotation2d m_previousAngle; - private T m_previousWheelPositions; + private final T m_previousWheelPositions; /** * Constructs an Odometry object. @@ -60,7 +60,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.copyInto(wheelPositions, m_previousWheelPositions); + m_kinematics.copyInto(wheelPositions, m_previousWheelPositions); } /** @@ -90,7 +90,7 @@ public Pose2d update(Rotation2d gyroAngle, T wheelPositions) { var newPose = m_poseMeters.exp(twist); - m_previousWheelPositions = m_kinematics.copyInto(wheelPositions, m_previousWheelPositions); + 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 index d260c1fc3d8..a25a2bed7da 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java @@ -393,16 +393,14 @@ public SwerveModulePosition[] copy(SwerveModulePosition[] positions) { } @Override - public SwerveModulePosition[] copyInto( - SwerveModulePosition[] positions, SwerveModulePosition[] buffer) { - if (positions.length != buffer.length) { - return copy(positions); + 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) { - buffer[i].distanceMeters = positions[i].distanceMeters; - buffer[i].angle = positions[i].angle; + output[i].distanceMeters = positions[i].distanceMeters; + output[i].angle = positions[i].angle; } - return buffer; } @Override