From 7b7d17ccd79bcf8ff0edda6f54c86cfa0d2aeaf0 Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Mon, 15 Jul 2024 17:19:31 -0700 Subject: [PATCH] [wpimath] Add and use kinematics.copyInto() (#6789) --- .../math/kinematics/DifferentialDriveKinematics.java | 7 +++++++ .../edu/wpi/first/math/kinematics/Kinematics.java | 9 +++++++++ .../first/math/kinematics/MecanumDriveKinematics.java | 8 ++++++++ .../java/edu/wpi/first/math/kinematics/Odometry.java | 6 +++--- .../first/math/kinematics/SwerveDriveKinematics.java | 11 +++++++++++ .../main/native/include/frc/kinematics/Kinematics.h | 3 ++- 6 files changed, 40 insertions(+), 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 index 8bcb6a74cfa..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 @@ -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 index a64c9a20091..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 @@ -53,4 +53,13 @@ public interface Kinematics extends Interpolator

{ * @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 index 4589a448533..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 @@ -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 index 66ce592957d..6a09f92bffd 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 @@ -25,7 +25,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. @@ -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 index 39306d0ac69..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 @@ -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 index 181c9a0abbe..f683dffa943 100644 --- 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 - requires std::copy_constructible + requires std::copy_constructible && + std::assignable_from class WPILIB_DLLEXPORT Kinematics { public: /**