From f209ecb0cba98d07c1d7f1107ee3af3b8fe17c11 Mon Sep 17 00:00:00 2001 From: Daniel Chen <108989218+Daniel1464@users.noreply.github.com> Date: Sun, 10 Aug 2025 14:45:36 -0400 Subject: [PATCH 01/15] [wpimath] Add structs for TrapezoidProfile.State and ExponentialProfile.State (#8163) --- docs/build.gradle | 1 + .../math/trajectory/ExponentialProfile.java | 7 +++- .../math/trajectory/TrapezoidProfile.java | 4 ++ .../struct/ExponentialProfileStateStruct.java | 42 +++++++++++++++++++ .../struct/TrapezoidProfileStateStruct.java | 42 +++++++++++++++++++ .../ExponentialProfileStateStructTest.java | 25 +++++++++++ .../TrapezoidProfileStateStructTest.java | 25 +++++++++++ 7 files changed, 145 insertions(+), 1 deletion(-) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/trajectory/struct/ExponentialProfileStateStruct.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/trajectory/struct/TrapezoidProfileStateStruct.java create mode 100644 wpimath/src/test/java/edu/wpi/first/math/trajectory/struct/ExponentialProfileStateStructTest.java create mode 100644 wpimath/src/test/java/edu/wpi/first/math/trajectory/struct/TrapezoidProfileStateStructTest.java diff --git a/docs/build.gradle b/docs/build.gradle index f18433901a..64ac63b3c7 100644 --- a/docs/build.gradle +++ b/docs/build.gradle @@ -206,6 +206,7 @@ task generateJavaDocs(type: Javadoc) { "-edu.wpi.first.math.system.plant.proto," + "-edu.wpi.first.math.system.plant.struct," + "-edu.wpi.first.math.trajectory.proto," + + "-edu.wpi.first.math.trajectory.struct," + // The .measure package contains generated source files for which automatic javadoc // generation is very difficult to do meaningfully. "-edu.wpi.first.units.measure", true) diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java index db6f8f7a1c..abb811b781 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java @@ -4,6 +4,8 @@ package edu.wpi.first.math.trajectory; +import edu.wpi.first.math.trajectory.struct.ExponentialProfileStateStruct; +import edu.wpi.first.util.struct.StructSerializable; import java.util.Objects; /** @@ -128,7 +130,10 @@ public class ExponentialProfile { } /** Profile state. */ - public static class State { + public static class State implements StructSerializable { + /** The struct that serializes this class. */ + public static final ExponentialProfileStateStruct struct = new ExponentialProfileStateStruct(); + /** The position at this state. */ public double position; diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java index c94f28ba13..f3de310944 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java @@ -6,6 +6,7 @@ package edu.wpi.first.math.trajectory; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUsageId; +import edu.wpi.first.math.trajectory.struct.TrapezoidProfileStateStruct; import java.util.Objects; /** @@ -76,6 +77,9 @@ public class TrapezoidProfile { /** Profile state. */ public static class State { + /** The struct used to serialize this class. */ + public static final TrapezoidProfileStateStruct struct = new TrapezoidProfileStateStruct(); + /** The position at this state. */ public double position; diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/struct/ExponentialProfileStateStruct.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/struct/ExponentialProfileStateStruct.java new file mode 100644 index 0000000000..55e7557380 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/struct/ExponentialProfileStateStruct.java @@ -0,0 +1,42 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.trajectory.struct; + +import edu.wpi.first.math.trajectory.ExponentialProfile; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class ExponentialProfileStateStruct implements Struct { + @Override + public Class getTypeClass() { + return ExponentialProfile.State.class; + } + + @Override + public String getTypeName() { + return "ExponentialProfileState"; + } + + @Override + public int getSize() { + return kSizeDouble * 2; + } + + @Override + public String getSchema() { + return "double position;double velocity"; + } + + @Override + public ExponentialProfile.State unpack(ByteBuffer bb) { + return new ExponentialProfile.State(bb.getDouble(), bb.getDouble()); + } + + @Override + public void pack(ByteBuffer bb, ExponentialProfile.State value) { + bb.putDouble(value.position); + bb.putDouble(value.velocity); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/struct/TrapezoidProfileStateStruct.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/struct/TrapezoidProfileStateStruct.java new file mode 100644 index 0000000000..b952790af3 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/struct/TrapezoidProfileStateStruct.java @@ -0,0 +1,42 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.trajectory.struct; + +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class TrapezoidProfileStateStruct implements Struct { + @Override + public Class getTypeClass() { + return TrapezoidProfile.State.class; + } + + @Override + public String getTypeName() { + return "TrapezoidProfileState"; + } + + @Override + public int getSize() { + return kSizeDouble * 2; + } + + @Override + public String getSchema() { + return "double position;double velocity"; + } + + @Override + public TrapezoidProfile.State unpack(ByteBuffer bb) { + return new TrapezoidProfile.State(bb.getDouble(), bb.getDouble()); + } + + @Override + public void pack(ByteBuffer bb, TrapezoidProfile.State value) { + bb.putDouble(value.position); + bb.putDouble(value.velocity); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/struct/ExponentialProfileStateStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/struct/ExponentialProfileStateStructTest.java new file mode 100644 index 0000000000..18a13bfffc --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/struct/ExponentialProfileStateStructTest.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.trajectory.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.trajectory.ExponentialProfile; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class ExponentialProfileStateStructTest { + private static final ExponentialProfile.State STATE = new ExponentialProfile.State(4.0, 5.0); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(ExponentialProfile.State.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + ExponentialProfile.State.struct.pack(buffer, STATE); + buffer.rewind(); + assertEquals(STATE, ExponentialProfile.State.struct.unpack(buffer)); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/struct/TrapezoidProfileStateStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/struct/TrapezoidProfileStateStructTest.java new file mode 100644 index 0000000000..84497c95e7 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/struct/TrapezoidProfileStateStructTest.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.trajectory.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class TrapezoidProfileStateStructTest { + private static final TrapezoidProfile.State STATE = new TrapezoidProfile.State(4.0, 5.0); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(TrapezoidProfile.State.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + TrapezoidProfile.State.struct.pack(buffer, STATE); + buffer.rewind(); + assertEquals(STATE, TrapezoidProfile.State.struct.unpack(buffer)); + } +} From 46a3318324bdeb5ef08bb5770fc3b03372968249 Mon Sep 17 00:00:00 2001 From: sciencewhiz Date: Mon, 11 Aug 2025 20:52:10 -0700 Subject: [PATCH 02/15] [build] Fix processstarter publishing (#8171) Artifacts weren't in OS and architecture subfolders in the zip like all the other C++ tools --- processstarter/publish.gradle | 1 + 1 file changed, 1 insertion(+) diff --git a/processstarter/publish.gradle b/processstarter/publish.gradle index b287bf930a..1fca11019c 100644 --- a/processstarter/publish.gradle +++ b/processstarter/publish.gradle @@ -42,6 +42,7 @@ model { } from(applicationPath) + into(nativeUtils.getPlatformPath(binary)) } task.dependsOn binary.tasks.link From 0d9e850e22ca51b5434392209a90c52ea08ac3ac Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 16 Aug 2025 22:51:13 -0700 Subject: [PATCH 03/15] [wpimath] Fix dt type in C++ tests (#8179) The UKF test was calling `.value()` on an implicit `units::millisecond_t` type assuming it was `units::second_t`. I normalized the rest of the dt declarations while I was at it. --- .../controller/LinearQuadraticRegulatorTest.java | 12 ++++++------ .../first/math/controller/LinearSystemLoopTest.java | 6 +++--- .../math/estimator/ExtendedKalmanFilterTest.java | 4 ++-- .../wpi/first/math/estimator/KalmanFilterTest.java | 2 +- .../DifferentialDriveAccelerationLimiterTest.cpp | 6 +++--- .../controller/DifferentialDriveFeedforwardTest.cpp | 4 ++-- .../cpp/controller/ImplicitModelFollowerTest.cpp | 4 ++-- .../cpp/controller/LinearQuadraticRegulatorTest.cpp | 12 ++++++------ .../cpp/estimator/ExtendedKalmanFilterTest.cpp | 4 ++-- .../cpp/estimator/UnscentedKalmanFilterTest.cpp | 6 +++--- .../test/native/cpp/system/DiscretizationTest.cpp | 4 ++-- 11 files changed, 32 insertions(+), 32 deletions(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java index 48278ced6e..abb26dc8da 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java @@ -29,12 +29,12 @@ class LinearQuadraticRegulatorTest { var qElms = VecBuilder.fill(0.02, 0.4); var rElms = VecBuilder.fill(12.0); - var dt = 0.00505; + var dt = 0.005; var K = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt).getK(); - assertEquals(522.153, K.get(0, 0), 0.1); - assertEquals(38.2, K.get(0, 1), 0.1); + assertEquals(522.87006795347486, K.get(0, 0), 1e-6); + assertEquals(38.239878385020411, K.get(0, 1), 1e-6); } @Test @@ -65,12 +65,12 @@ class LinearQuadraticRegulatorTest { var qElms = VecBuilder.fill(0.01745, 0.08726); var rElms = VecBuilder.fill(12.0); - var dt = 0.00505; + var dt = 0.005; var K = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt).getK(); - assertEquals(19.16, K.get(0, 0), 0.1); - assertEquals(3.32, K.get(0, 1), 0.1); + assertEquals(19.339349883583761, K.get(0, 0), 1e-6); + assertEquals(3.3542559517421582, K.get(0, 1), 1e-6); } /** diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java index a34b8b68a8..4baaf2d551 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java @@ -22,7 +22,7 @@ import java.util.Random; import org.junit.jupiter.api.Test; class LinearSystemLoopTest { - private static final double kDt = 0.00505; + private static final double kDt = 0.005; private static final double kPositionStddev = 0.0001; private static final Random random = new Random(); @@ -45,12 +45,12 @@ class LinearSystemLoopTest { (LinearSystem) m_plant.slice(0), VecBuilder.fill(0.02, 0.4), VecBuilder.fill(12.0), - 0.00505); + 0.005); @SuppressWarnings("unchecked") private final LinearSystemLoop m_loop = new LinearSystemLoop<>( - (LinearSystem) m_plant.slice(0), m_controller, m_observer, 12, 0.00505); + (LinearSystem) m_plant.slice(0), m_controller, m_observer, 12, 0.005); private static void updateTwoState( LinearSystem plant, LinearSystemLoop loop, double noise) { diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java index a1b5b3a1cf..7ea9c9ec5a 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java @@ -68,7 +68,7 @@ class ExtendedKalmanFilterTest { @Test void testInit() { - double dtSeconds = 0.00505; + double dtSeconds = 0.005; assertDoesNotThrow( () -> { @@ -98,7 +98,7 @@ class ExtendedKalmanFilterTest { @Test void testConvergence() { - double dtSeconds = 0.00505; + double dtSeconds = 0.005; double rbMeters = 0.8382 / 2.0; // Robot radius ExtendedKalmanFilter observer = diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java index 42fbd92db0..c3369161cf 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java @@ -29,7 +29,7 @@ import org.junit.jupiter.api.Test; class KalmanFilterTest { private static LinearSystem elevatorPlant; - private static final double kDt = 0.00505; + private static final double kDt = 0.005; static { createElevator(); diff --git a/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp b/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp index 452e017ecc..fe22274228 100644 --- a/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp +++ b/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp @@ -12,7 +12,7 @@ namespace frc { TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) { constexpr auto trackwidth = 0.9_m; - constexpr auto dt = 5_ms; + constexpr units::second_t dt = 5_ms; constexpr auto maxA = 2_mps_sq; constexpr auto maxAlpha = 2_rad_per_s_sq; @@ -105,7 +105,7 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) { TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) { constexpr auto trackwidth = 0.9_m; - constexpr auto dt = 5_ms; + constexpr units::second_t dt = 5_ms; using Kv_t = decltype(1_V / 1_mps); using Ka_t = decltype(1_V / 1_mps_sq); @@ -173,7 +173,7 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) { TEST(DifferentialDriveAccelerationLimiterTest, SeparateMinMaxLowLimits) { constexpr auto trackwidth = 0.9_m; - constexpr auto dt = 5_ms; + constexpr units::second_t dt = 5_ms; constexpr auto minA = -1_mps_sq; constexpr auto maxA = 2_mps_sq; constexpr auto maxAlpha = 2_rad_per_s_sq; diff --git a/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp index 5428d843ca..c0a0cdb02e 100644 --- a/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp @@ -20,7 +20,7 @@ TEST(DifferentialDriveFeedforwardTest, CalculateWithTrackwidth) { constexpr auto kVAngular = 1_V / 1_rad_per_s; constexpr auto kAAngular = 1_V / 1_rad_per_s_sq; constexpr auto trackwidth = 1_m; - constexpr auto dt = 20_ms; + constexpr units::second_t dt = 20_ms; frc::DifferentialDriveFeedforward differentialDriveFeedforward{ kVLinear, kALinear, kVAngular, kAAngular, trackwidth}; @@ -54,7 +54,7 @@ TEST(DifferentialDriveFeedforwardTest, CalculateWithoutTrackwidth) { constexpr auto kALinear = 1_V / 1_mps_sq; constexpr auto kVAngular = 1_V / 1_mps; constexpr auto kAAngular = 1_V / 1_mps_sq; - constexpr auto dt = 20_ms; + constexpr units::second_t dt = 20_ms; frc::DifferentialDriveFeedforward differentialDriveFeedforward{ kVLinear, kALinear, kVAngular, kAAngular}; diff --git a/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp b/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp index afa491a7b1..7994c84526 100644 --- a/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp @@ -10,7 +10,7 @@ namespace frc { TEST(ImplicitModelFollowerTest, SameModel) { - constexpr auto dt = 5_ms; + constexpr units::second_t dt = 5_ms; using Kv_t = decltype(1_V / 1_mps); using Ka_t = decltype(1_V / 1_mps_sq); @@ -54,7 +54,7 @@ TEST(ImplicitModelFollowerTest, SameModel) { } TEST(ImplicitModelFollowerTest, SlowerRefModel) { - constexpr auto dt = 5_ms; + constexpr units::second_t dt = 5_ms; using Kv_t = decltype(1_V / 1_mps); using Ka_t = decltype(1_V / 1_mps_sq); diff --git a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp index 3efb08d614..7f4a4e049a 100644 --- a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp +++ b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp @@ -31,10 +31,10 @@ TEST(LinearQuadraticRegulatorTest, ElevatorGains) { return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0); }(); Matrixd<1, 2> K = - LinearQuadraticRegulator<2, 1>{plant, {0.02, 0.4}, {12.0}, 5.05_ms}.K(); + LinearQuadraticRegulator<2, 1>{plant, {0.02, 0.4}, {12.0}, 5_ms}.K(); - EXPECT_NEAR(522.15314269, K(0, 0), 1e-6); - EXPECT_NEAR(38.20138596, K(0, 1), 1e-6); + EXPECT_NEAR(522.87006795347486, K(0, 0), 1e-6); + EXPECT_NEAR(38.239878385020411, K(0, 1), 1e-6); } TEST(LinearQuadraticRegulatorTest, ArmGains) { @@ -56,11 +56,11 @@ TEST(LinearQuadraticRegulatorTest, ArmGains) { }(); Matrixd<1, 2> K = - LinearQuadraticRegulator<2, 1>{plant, {0.01745, 0.08726}, {12.0}, 5.05_ms} + LinearQuadraticRegulator<2, 1>{plant, {0.01745, 0.08726}, {12.0}, 5_ms} .K(); - EXPECT_NEAR(19.16, K(0, 0), 1e-1); - EXPECT_NEAR(3.32, K(0, 1), 1e-1); + EXPECT_NEAR(19.339349883583761, K(0, 0), 1e-6); + EXPECT_NEAR(3.3542559517421582, K(0, 1), 1e-6); } TEST(LinearQuadraticRegulatorTest, FourMotorElevator) { diff --git a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp index 362de8b699..e54c8bb32c 100644 --- a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp @@ -61,7 +61,7 @@ frc::Vectord<5> GlobalMeasurementModel( } // namespace TEST(ExtendedKalmanFilterTest, Init) { - constexpr auto dt = 0.00505_s; + constexpr units::second_t dt = 5_ms; frc::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics, LocalMeasurementModel, @@ -80,7 +80,7 @@ TEST(ExtendedKalmanFilterTest, Init) { } TEST(ExtendedKalmanFilterTest, Convergence) { - constexpr auto dt = 0.00505_s; + constexpr units::second_t dt = 5_ms; constexpr auto rb = 0.8382_m / 2.0; // Robot radius frc::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics, diff --git a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp index 21d2df98f4..01a4ee8bcc 100644 --- a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp @@ -68,7 +68,7 @@ frc::Vectord<5> DriveGlobalMeasurementModel( } TEST(UnscentedKalmanFilterTest, DriveInit) { - constexpr auto dt = 5_ms; + constexpr units::second_t dt = 5_ms; frc::UnscentedKalmanFilter<5, 2, 3> observer{DriveDynamics, DriveLocalMeasurementModel, @@ -94,7 +94,7 @@ TEST(UnscentedKalmanFilterTest, DriveInit) { } TEST(UnscentedKalmanFilterTest, DriveConvergence) { - constexpr auto dt = 5_ms; + constexpr units::second_t dt = 5_ms; constexpr auto rb = 0.8382_m / 2.0; // Robot radius frc::UnscentedKalmanFilter<5, 2, 3> observer{DriveDynamics, @@ -206,7 +206,7 @@ TEST(UnscentedKalmanFilterTest, LinearUKF) { } TEST(UnscentedKalmanFilterTest, RoundTripP) { - constexpr auto dt = 5_ms; + constexpr units::second_t dt = 5_ms; frc::UnscentedKalmanFilter<2, 2, 2> observer{ [](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; }, diff --git a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp index 055ee49736..fec4447531 100644 --- a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp +++ b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp @@ -56,7 +56,7 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) { frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}}; frc::Matrixd<2, 2> contQ{{1, 0}, {0, 1}}; - constexpr auto dt = 1_s; + constexpr units::second_t dt = 1_s; // T // Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ @@ -88,7 +88,7 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) { frc::Matrixd<2, 2> contA{{0, 1}, {0, -1406.29}}; frc::Matrixd<2, 2> contQ{{0.0025, 0}, {0, 1}}; - constexpr auto dt = 5_ms; + constexpr units::second_t dt = 5_ms; // T // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ From 4e6b9706ff96d6556bf9bdc4670adb61bd3acc26 Mon Sep 17 00:00:00 2001 From: sciencewhiz Date: Tue, 26 Aug 2025 07:09:18 -0700 Subject: [PATCH 04/15] [build] Explicitly set Documentation archiveVersion (#8192) In the recent gradle or gradle dependencies update, the documentation zips were being published as documentation-version-unspecified, where the unspecified was coming from archiveVersion. It looks like we use a different method of setting the version, so make sure that archiveVersion is empty --- docs/build.gradle | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/build.gradle b/docs/build.gradle index 64ac63b3c7..93a41ef77f 100644 --- a/docs/build.gradle +++ b/docs/build.gradle @@ -168,6 +168,7 @@ doxygen.sourceSets.main { tasks.register("zipCppDocs", Zip) { archiveBaseName = zipBaseNameCpp + archiveVersion = "" destinationDirectory = outputsFolder dependsOn doxygenDox from ("$buildDir/docs/doxygen/html") @@ -247,6 +248,7 @@ task generateJavaDocs(type: Javadoc) { tasks.register("zipJavaDocs", Zip) { archiveBaseName = zipBaseNameJava + archiveVersion = "" destinationDirectory = outputsFolder dependsOn generateJavaDocs from ("$buildDir/docs/javadoc") From 9fd4ccf95b0845598dc3c2deb5d2ff5a2df5e70f Mon Sep 17 00:00:00 2001 From: Kevin-OConnor Date: Sat, 30 Aug 2025 14:36:26 -0400 Subject: [PATCH 05/15] [hal] Add CAN Mfgrs and Adjust Device Types (#8201) --- .../java/edu/wpi/first/hal/CANAPITypes.java | 20 ++++++++++++----- hal/src/main/native/include/hal/CANAPITypes.h | 22 ++++++++++++++----- 2 files changed, 31 insertions(+), 11 deletions(-) diff --git a/hal/src/main/java/edu/wpi/first/hal/CANAPITypes.java b/hal/src/main/java/edu/wpi/first/hal/CANAPITypes.java index cef69b65b2..f97cc18f92 100644 --- a/hal/src/main/java/edu/wpi/first/hal/CANAPITypes.java +++ b/hal/src/main/java/edu/wpi/first/hal/CANAPITypes.java @@ -35,10 +35,10 @@ public final class CANAPITypes { kGyroSensor(4), /** Accelerometer. */ kAccelerometer(5), - /** Ultrasonic sensor. */ - kUltrasonicSensor(6), - /** Gear tooth sensor. */ - kGearToothSensor(7), + /** Distance sensor. */ + kDistanceSensor(6), + /** Encoder. */ + kEncoder(7), /** Power distribution. */ kPowerDistribution(8), /** Pneumatics. */ @@ -49,6 +49,8 @@ public final class CANAPITypes { kIOBreakout(11), /** Servo Controller. */ kServoController(12), + /** Color Sensor. */ + kColorSensor(13), /** Firmware update. */ kFirmwareUpdate(31); @@ -105,7 +107,15 @@ public final class CANAPITypes { /** AndyMark. */ kAndyMark(15), /** Vivid-Hosting. */ - kVividHosting(16); + kVividHosting(16), + /** Vertos Robotics. */ + kVertosRobotics(17), + /** SWYFT Robotics. */ + kSWYFTRobotics(18), + /** Lumyn Labs. */ + kLumynLabs(19), + /** Brushland Labs. */ + kBrushlandLabs(20); /** The manufacturer ID. */ @SuppressWarnings("MemberName") diff --git a/hal/src/main/native/include/hal/CANAPITypes.h b/hal/src/main/native/include/hal/CANAPITypes.h index 7cfe93b4c4..6270697507 100644 --- a/hal/src/main/native/include/hal/CANAPITypes.h +++ b/hal/src/main/native/include/hal/CANAPITypes.h @@ -32,10 +32,10 @@ HAL_ENUM(HAL_CANDeviceType) { HAL_CAN_Dev_kGyroSensor = 4, /// Accelerometer. HAL_CAN_Dev_kAccelerometer = 5, - /// Ultrasonic sensor. - HAL_CAN_Dev_kUltrasonicSensor = 6, - /// Gear tooth sensor. - HAL_CAN_Dev_kGearToothSensor = 7, + /// Distance sensor. + HAL_CAN_Dev_kDistanceSensor = 6, + /// Encoder. + HAL_CAN_Dev_kEncoder = 7, /// Power distribution. HAL_CAN_Dev_kPowerDistribution = 8, /// Pneumatics. @@ -44,8 +44,10 @@ HAL_ENUM(HAL_CANDeviceType) { HAL_CAN_Dev_kMiscellaneous = 10, /// IO breakout. HAL_CAN_Dev_kIOBreakout = 11, - // Servo controller. + /// Servo controller. HAL_CAN_Dev_kServoController = 12, + /// Color Sensor. + HAL_CAN_Dev_ColorSensor = 13, /// Firmware update. HAL_CAN_Dev_kFirmwareUpdate = 31 }; @@ -89,6 +91,14 @@ HAL_ENUM(HAL_CANManufacturer) { /// AndyMark. HAL_CAN_Man_kAndyMark = 15, /// Vivid-Hosting. - HAL_CAN_Man_kVividHosting = 16 + HAL_CAN_Man_kVividHosting = 16, + /// Vertos Robotics. + HAL_CAN_Man_kVertosRobotics = 17, + /// SWYFT Robotics. + HAL_CAN_Man_kSWYFTRobotics = 18, + /// Lumyn Labs. + HAL_CAN_Man_kLumynLabs = 19, + /// Brushland Labs + HAL_CAN_Man_kBrushlandLabs = 20 }; /** @} */ From 45db0fd45e500bb267caa05d7c4a478a80d62cf3 Mon Sep 17 00:00:00 2001 From: Ryan Shavell Date: Sat, 30 Aug 2025 23:14:41 -0400 Subject: [PATCH 06/15] [epilogue] Use reflection to access non-public superclass fields (#7996) Co-authored-by: Sam Carlberg --- .../epilogue/processor/ArrayHandler.java | 11 +- .../epilogue/processor/CollectionHandler.java | 11 +- .../processor/ConfiguredLoggerHandler.java | 5 +- .../epilogue/processor/ElementHandler.java | 24 +- .../first/epilogue/processor/EnumHandler.java | 9 +- .../epilogue/processor/LoggableHandler.java | 6 +- .../epilogue/processor/LoggerGenerator.java | 102 ++- .../epilogue/processor/MeasureHandler.java | 9 +- .../epilogue/processor/PrimitiveHandler.java | 9 +- .../epilogue/processor/SendableHandler.java | 4 +- .../epilogue/processor/StructHandler.java | 5 +- .../epilogue/processor/SupplierHandler.java | 13 +- .../processor/AnnotationProcessorTest.java | 659 +++++------------- 13 files changed, 322 insertions(+), 545 deletions(-) diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ArrayHandler.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ArrayHandler.java index 8c31197e8d..37b9c421fd 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ArrayHandler.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ArrayHandler.java @@ -6,6 +6,7 @@ package edu.wpi.first.epilogue.processor; import javax.annotation.processing.ProcessingEnvironment; import javax.lang.model.element.Element; +import javax.lang.model.element.TypeElement; import javax.lang.model.type.ArrayType; import javax.lang.model.type.PrimitiveType; import javax.lang.model.type.TypeMirror; @@ -52,7 +53,7 @@ public class ArrayHandler extends ElementHandler { } @Override - public String logInvocation(Element element) { + public String logInvocation(Element element, TypeElement loggedClass) { var dataType = dataType(element); // known to be an array type (assuming isLoggable is checked first); this is a safe cast @@ -63,13 +64,17 @@ public class ArrayHandler extends ElementHandler { return "backend.log(\"" + loggedName(element) + "\", " - + elementAccess(element) + + elementAccess(element, loggedClass) + ", " + m_structHandler.structAccess(componentType) + ")"; } else { // Primitive or string array - return "backend.log(\"" + loggedName(element) + "\", " + elementAccess(element) + ")"; + return "backend.log(\"" + + loggedName(element) + + "\", " + + elementAccess(element, loggedClass) + + ")"; } } } diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/CollectionHandler.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/CollectionHandler.java index 3fe4abb3aa..d864f093d6 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/CollectionHandler.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/CollectionHandler.java @@ -6,6 +6,7 @@ package edu.wpi.first.epilogue.processor; import javax.annotation.processing.ProcessingEnvironment; import javax.lang.model.element.Element; +import javax.lang.model.element.TypeElement; import javax.lang.model.type.DeclaredType; import javax.lang.model.type.TypeMirror; @@ -38,7 +39,7 @@ public class CollectionHandler extends ElementHandler { } @Override - public String logInvocation(Element element) { + public String logInvocation(Element element, TypeElement loggedClass) { var dataType = dataType(element); var componentType = ((DeclaredType) dataType).getTypeArguments().get(0); @@ -46,12 +47,16 @@ public class CollectionHandler extends ElementHandler { return "backend.log(\"" + loggedName(element) + "\", " - + elementAccess(element) + + elementAccess(element, loggedClass) + ", " + m_structHandler.structAccess(componentType) + ")"; } else { - return "backend.log(\"" + loggedName(element) + "\", " + elementAccess(element) + ")"; + return "backend.log(\"" + + loggedName(element) + + "\", " + + elementAccess(element, loggedClass) + + ")"; } } } diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ConfiguredLoggerHandler.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ConfiguredLoggerHandler.java index f7e6744edf..086e7fbe40 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ConfiguredLoggerHandler.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ConfiguredLoggerHandler.java @@ -7,6 +7,7 @@ package edu.wpi.first.epilogue.processor; import java.util.Map; import javax.annotation.processing.ProcessingEnvironment; import javax.lang.model.element.Element; +import javax.lang.model.element.TypeElement; import javax.lang.model.type.DeclaredType; import javax.lang.model.type.TypeMirror; @@ -27,7 +28,7 @@ public class ConfiguredLoggerHandler extends ElementHandler { } @Override - public String logInvocation(Element element) { + public String logInvocation(Element element, TypeElement loggedClass) { var dataType = dataType(element); var loggerType = m_customLoggers.entrySet().stream() @@ -44,7 +45,7 @@ public class ConfiguredLoggerHandler extends ElementHandler { + ".tryUpdate(backend.getNested(\"" + loggedName(element) + "\"), " - + elementAccess(element) + + elementAccess(element, loggedClass) + ", Epilogue.getConfig().errorHandler)"; } } diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ElementHandler.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ElementHandler.java index d12acb7ecc..61e3d49974 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ElementHandler.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ElementHandler.java @@ -117,9 +117,9 @@ public abstract class ElementHandler { * @param element the element to generate the access for * @return the generated access snippet */ - public String elementAccess(Element element) { + public String elementAccess(Element element, TypeElement loggedClass) { if (element instanceof VariableElement field) { - return fieldAccess(field); + return fieldAccess(field, loggedClass); } else if (element instanceof ExecutableElement method) { return methodAccess(method); } else { @@ -127,8 +127,20 @@ public abstract class ElementHandler { } } - private static String fieldAccess(VariableElement field) { - if (!field.getModifiers().contains(Modifier.PUBLIC)) { + private static String fieldAccess(VariableElement field, TypeElement loggedClass) { + var mods = field.getModifiers(); + + // To be directly accessible, the field needs to be: + // - public; or + // - protected or package-private, and declared by a superclass in the same package + // However, we can't cleanly access package information, so we'll always emit a VarHandle + // for any field declared in a superclass unless it's public and we know we can read it. + boolean isVarHandle = + field.getEnclosingElement().equals(loggedClass) + ? mods.contains(Modifier.PRIVATE) + : !mods.contains(Modifier.PUBLIC); + + if (isVarHandle) { // ((com.example.Foo) $fooField.get(object)) // Extra parentheses so cast evaluates before appended methods // (e.g. when appending .getAsDouble()) @@ -136,7 +148,7 @@ public abstract class ElementHandler { if (type.getKind() == TypeKind.TYPEVAR) { type = ((TypeVariable) type).getUpperBound(); } - return "((" + type.toString() + ") $" + field.getSimpleName() + ".get(object))"; + return "((" + type.toString() + ") " + LoggerGenerator.varHandleName(field) + ".get(object))"; } else { // object.fooField return "object." + field.getSimpleName(); @@ -171,5 +183,5 @@ public abstract class ElementHandler { * @param element the field or method element to generate the logger call for * @return the generated log invocation */ - public abstract String logInvocation(Element element); + public abstract String logInvocation(Element element, TypeElement loggedClass); } diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/EnumHandler.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/EnumHandler.java index 78b14b4636..f8c05670b0 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/EnumHandler.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/EnumHandler.java @@ -6,6 +6,7 @@ package edu.wpi.first.epilogue.processor; import javax.annotation.processing.ProcessingEnvironment; import javax.lang.model.element.Element; +import javax.lang.model.element.TypeElement; import javax.lang.model.type.TypeMirror; public class EnumHandler extends ElementHandler { @@ -27,7 +28,11 @@ public class EnumHandler extends ElementHandler { } @Override - public String logInvocation(Element element) { - return "backend.log(\"" + loggedName(element) + "\", " + elementAccess(element) + ")"; + public String logInvocation(Element element, TypeElement loggedClass) { + return "backend.log(\"" + + loggedName(element) + + "\", " + + elementAccess(element, loggedClass) + + ")"; } } diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggableHandler.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggableHandler.java index ba14022a2c..001716a637 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggableHandler.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggableHandler.java @@ -39,7 +39,7 @@ public class LoggableHandler extends ElementHandler { } @Override - public String logInvocation(Element element) { + public String logInvocation(Element element, TypeElement loggedClass) { TypeMirror dataType = dataType(element); var declaredType = m_processingEnv @@ -61,7 +61,7 @@ public class LoggableHandler extends ElementHandler { // If there are no known loggable subtypes, return just the single logger call if (size == 1) { - return generateLoggerCall(element, declaredType, elementAccess(element)); + return generateLoggerCall(element, declaredType, elementAccess(element, loggedClass)); } // Otherwise, generate an if-else chain to compare the element with its known loggable subtypes @@ -73,7 +73,7 @@ public class LoggableHandler extends ElementHandler { StringBuilder builder = new StringBuilder(); // Cache the value in a variable so it's only read once - builder.append("var %s = %s;\n".formatted(varName, elementAccess(element))); + builder.append("var %s = %s;\n".formatted(varName, elementAccess(element, loggedClass))); for (int i = 0; i < size; i++) { TypeElement type = loggableSubtypes.get(i); diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggerGenerator.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggerGenerator.java index 7fbb82a931..20b1e1cebc 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggerGenerator.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggerGenerator.java @@ -18,9 +18,11 @@ import java.io.PrintWriter; import java.lang.annotation.Annotation; import java.util.ArrayDeque; import java.util.ArrayList; +import java.util.Comparator; import java.util.Deque; import java.util.EnumMap; import java.util.HashMap; +import java.util.LinkedHashMap; import java.util.List; import java.util.Map; import java.util.Objects; @@ -185,7 +187,21 @@ public class LoggerGenerator { var loggerFile = m_processingEnv.getFiler().createSourceFile(loggerClassName); var varHandleFields = - loggableFields.stream().filter(e -> !e.getModifiers().contains(Modifier.PUBLIC)).toList(); + loggableFields.stream() + .filter( + e -> { + if (e.getEnclosingElement().equals(clazz)) { + // The generated logger is in the same package as the logged class, so the + // only fields it can't read are private ones. + return e.getModifiers().contains(Modifier.PRIVATE); + } else { + // Logging from a superclass. Can only read public fields, unless the superclass + // is in the same package, in which case protected and package-private fields + // are also readable. + return !e.getModifiers().contains(Modifier.PUBLIC); + } + }) + .toList(); boolean requiresVarHandles = !varHandleFields.isEmpty(); try (var out = new PrintWriter(loggerFile.openWriter())) { @@ -214,41 +230,67 @@ public class LoggerGenerator { + "> {"); if (requiresVarHandles) { - for (var varHandleField : varHandleFields) { + for (var privateField : varHandleFields) { // This field needs a VarHandle to access. // Cache it in the class to avoid lookups - out.println(" private static final VarHandle $" + varHandleField.getSimpleName() + ";"); + out.printf( + " // Accesses private or superclass field %s.%s%n", + privateField.getEnclosingElement(), privateField.getSimpleName()); + out.printf(" private static final VarHandle %s;%n", varHandleName(privateField)); } out.println(); + } - var classReference = simpleClassName + ".class"; - + // Static initializer block to load VarHandles and reflection fields + if (requiresVarHandles) { out.println(" static {"); - out.println(" try {"); - out.println( - " var lookup = MethodHandles.privateLookupIn(" - + classReference - + ", MethodHandles.lookup());"); - for (var varHandleField : varHandleFields) { - var fieldName = varHandleField.getSimpleName(); - out.println( - " $" - + fieldName - + " = lookup.findVarHandle(" - + classReference - + ", \"" - + fieldName - + "\", " - + m_processingEnv.getTypeUtils().erasure(varHandleField.asType()) - + ".class);"); - } + out.println(" try {"); + + out.println(" var rootLookup = MethodHandles.lookup();"); + + // Group private fields by class, then generate a private lookup for each class + // and a VarHandle for each field using that lookup. Sorting and then collecting into a + // LinkedHashMap gives deterministic output ordering (mostly for tests, which check exact + // file contents, but also results in less churn when regenerating files for users who like + // to read the generated logger classes). + // + // This lets us read private fields from superclasses. + Map> privateFieldsByClass = + varHandleFields.stream() + .sorted(Comparator.comparing(e -> e.getSimpleName().toString())) + .collect( + Collectors.groupingBy( + VariableElement::getEnclosingElement, + LinkedHashMap::new, + Collectors.toList())); + + privateFieldsByClass.forEach( + (enclosingClass, fields) -> { + String className = enclosingClass.toString(); + String lookupName = "lookup$$" + className.replace(".", "_"); + out.printf( + " var %s = MethodHandles.privateLookupIn(%s.class, rootLookup);%n", + lookupName, className); + + for (var field : fields) { + var fieldname = field.getSimpleName(); + out.printf( + " %s = %s.findVarHandle(%s.class, \"%s\", %s.class);%n", + varHandleName(field), + lookupName, + className, + fieldname, + m_processingEnv.getTypeUtils().erasure(field.asType())); + } + }); out.println(" } catch (ReflectiveOperationException e) {"); out.println( " throw new RuntimeException(" + "\"[EPILOGUE] Could not load private fields for logging!\", e);"); out.println(" }"); + out.println(" }"); out.println(); } @@ -300,7 +342,7 @@ public class LoggerGenerator { // to be logged. For example, the sendable handler consumes all sendable types // but does not log commands or subsystems, to prevent excessive warnings about // unloggable commands. - var logInvocation = h.logInvocation(loggableElement); + var logInvocation = h.logInvocation(loggableElement, clazz); if (logInvocation != null) { out.println(logInvocation.indent(6).stripTrailing() + ";"); } @@ -315,6 +357,18 @@ public class LoggerGenerator { } } + /** + * Generates the name of a VarHandle for access to the given field. The VarHandle variable's name + * is guaranteed to be unique. + * + * @param field The field to generate a VarHandle for + * @return The name of the generated VarHandle variable + */ + public static String varHandleName(VariableElement field) { + return "$%s_%s" + .formatted(field.getEnclosingElement().toString().replace(".", "_"), field.getSimpleName()); + } + private void collectLoggables( TypeElement clazz, List fields, List methods) { var config = clazz.getAnnotation(Logged.class); diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/MeasureHandler.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/MeasureHandler.java index 8933306d65..bff3e8533a 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/MeasureHandler.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/MeasureHandler.java @@ -6,6 +6,7 @@ package edu.wpi.first.epilogue.processor; import javax.annotation.processing.ProcessingEnvironment; import javax.lang.model.element.Element; +import javax.lang.model.element.TypeElement; import javax.lang.model.type.TypeMirror; public class MeasureHandler extends ElementHandler { @@ -30,8 +31,12 @@ public class MeasureHandler extends ElementHandler { } @Override - public String logInvocation(Element element) { + public String logInvocation(Element element, TypeElement loggedClass) { // EpilogueBackend has builtin support for logging measures - return "backend.log(\"" + loggedName(element) + "\", " + elementAccess(element) + ")"; + return "backend.log(\"" + + loggedName(element) + + "\", " + + elementAccess(element, loggedClass) + + ")"; } } diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/PrimitiveHandler.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/PrimitiveHandler.java index 5563bd9b31..8da8ece405 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/PrimitiveHandler.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/PrimitiveHandler.java @@ -16,6 +16,7 @@ import static javax.lang.model.type.TypeKind.SHORT; import java.util.Set; import javax.annotation.processing.ProcessingEnvironment; import javax.lang.model.element.Element; +import javax.lang.model.element.TypeElement; import javax.lang.model.type.TypeMirror; public class PrimitiveHandler extends ElementHandler { @@ -35,7 +36,11 @@ public class PrimitiveHandler extends ElementHandler { } @Override - public String logInvocation(Element element) { - return "backend.log(\"" + loggedName(element) + "\", " + elementAccess(element) + ")"; + public String logInvocation(Element element, TypeElement loggedClass) { + return "backend.log(\"" + + loggedName(element) + + "\", " + + elementAccess(element, loggedClass) + + ")"; } } diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SendableHandler.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SendableHandler.java index d25ce09d31..635b565940 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SendableHandler.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SendableHandler.java @@ -44,7 +44,7 @@ public class SendableHandler extends ElementHandler { } @Override - public String logInvocation(Element element) { + public String logInvocation(Element element, TypeElement loggedClass) { var dataType = dataType(element); // Do not log commands or subsystems via their sendable implementations @@ -66,7 +66,7 @@ public class SendableHandler extends ElementHandler { return "logSendable(backend.getNested(\"" + loggedName(element) + "\"), " - + elementAccess(element) + + elementAccess(element, loggedClass) + ")"; } } diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StructHandler.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StructHandler.java index ddd753a818..1c658818e6 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StructHandler.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StructHandler.java @@ -6,6 +6,7 @@ package edu.wpi.first.epilogue.processor; import javax.annotation.processing.ProcessingEnvironment; import javax.lang.model.element.Element; +import javax.lang.model.element.TypeElement; import javax.lang.model.type.TypeMirror; import javax.lang.model.util.Types; @@ -38,11 +39,11 @@ public class StructHandler extends ElementHandler { } @Override - public String logInvocation(Element element) { + public String logInvocation(Element element, TypeElement loggedClass) { return "backend.log(\"" + loggedName(element) + "\", " - + elementAccess(element) + + elementAccess(element, loggedClass) + ", " + structAccess(dataType(element)) + ")"; diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SupplierHandler.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SupplierHandler.java index 173e371c05..49f2e06935 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SupplierHandler.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SupplierHandler.java @@ -6,6 +6,7 @@ package edu.wpi.first.epilogue.processor; import javax.annotation.processing.ProcessingEnvironment; import javax.lang.model.element.Element; +import javax.lang.model.element.TypeElement; import javax.lang.model.type.TypeMirror; public class SupplierHandler extends ElementHandler { @@ -42,15 +43,19 @@ public class SupplierHandler extends ElementHandler { } @Override - public String logInvocation(Element element) { - return "backend.log(\"" + loggedName(element) + "\", " + elementAccess(element) + ")"; + public String logInvocation(Element element, TypeElement loggedClass) { + return "backend.log(\"" + + loggedName(element) + + "\", " + + elementAccess(element, loggedClass) + + ")"; } @Override - public String elementAccess(Element element) { + public String elementAccess(Element element, TypeElement loggedClass) { var typeUtils = m_processingEnv.getTypeUtils(); var dataType = dataType(element); - String base = super.elementAccess(element); + String base = super.elementAccess(element, loggedClass); if (typeUtils.isAssignable(dataType, m_booleanSupplier)) { return base + ".getAsBoolean()"; diff --git a/epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java b/epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java index bc6189bf0b..65a7a2e736 100644 --- a/epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java +++ b/epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java @@ -42,21 +42,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $x; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $x = lookup.findVarHandle(Example.class, "x", double.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -64,7 +51,7 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", ((double) $x.get(object))); + backend.log("x", object.x); } } } @@ -93,23 +80,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $x; - private static final VarHandle $y; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $x = lookup.findVarHandle(Example.class, "x", double.class); - $y = lookup.findVarHandle(Example.class, "y", int.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -117,8 +89,8 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", ((double) $x.get(object))); - backend.log("y", ((int) $y.get(object))); + backend.log("x", object.x); + backend.log("y", object.y); } } } @@ -210,23 +182,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $x; - private static final VarHandle $y; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $x = lookup.findVarHandle(Example.class, "x", double.class); - $y = lookup.findVarHandle(Example.class, "y", double.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -234,8 +191,8 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", ((double) $x.get(object))); - backend.log("y", ((double) $y.get(object))); + backend.log("x", object.x); + backend.log("y", object.y); } } } @@ -268,12 +225,14 @@ class AnnotationProcessorTest { import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $x; + // Accesses private or superclass field edu.wpi.first.epilogue.Example.x + private static final VarHandle $edu_wpi_first_epilogue_Example_x; static { try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $x = lookup.findVarHandle(Example.class, "x", double.class); + var rootLookup = MethodHandles.lookup(); + var lookup$$edu_wpi_first_epilogue_Example = MethodHandles.privateLookupIn(edu.wpi.first.epilogue.Example.class, rootLookup); + $edu_wpi_first_epilogue_Example_x = lookup$$edu_wpi_first_epilogue_Example.findVarHandle(edu.wpi.first.epilogue.Example.class, "x", double.class); } catch (ReflectiveOperationException e) { throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); } @@ -286,7 +245,7 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", ((double) $x.get(object))); + backend.log("x", ((double) $edu_wpi_first_epilogue_Example_x.get(object))); } } } @@ -321,12 +280,14 @@ class AnnotationProcessorTest { import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $x; + // Accesses private or superclass field edu.wpi.first.epilogue.Example.x + private static final VarHandle $edu_wpi_first_epilogue_Example_x; static { try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $x = lookup.findVarHandle(Example.class, "x", java.util.function.DoubleSupplier.class); + var rootLookup = MethodHandles.lookup(); + var lookup$$edu_wpi_first_epilogue_Example = MethodHandles.privateLookupIn(edu.wpi.first.epilogue.Example.class, rootLookup); + $edu_wpi_first_epilogue_Example_x = lookup$$edu_wpi_first_epilogue_Example.findVarHandle(edu.wpi.first.epilogue.Example.class, "x", java.util.function.DoubleSupplier.class); } catch (ReflectiveOperationException e) { throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); } @@ -339,7 +300,7 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", ((java.util.function.DoubleSupplier) $x.get(object)).getAsDouble()); + backend.log("x", ((java.util.function.DoubleSupplier) $edu_wpi_first_epilogue_Example_x.get(object)).getAsDouble()); } } } @@ -372,12 +333,14 @@ class AnnotationProcessorTest { import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $chooser; + // Accesses private or superclass field edu.wpi.first.epilogue.Example.chooser + private static final VarHandle $edu_wpi_first_epilogue_Example_chooser; static { try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $chooser = lookup.findVarHandle(Example.class, "chooser", edu.wpi.first.wpilibj.smartdashboard.SendableChooser.class); + var rootLookup = MethodHandles.lookup(); + var lookup$$edu_wpi_first_epilogue_Example = MethodHandles.privateLookupIn(edu.wpi.first.epilogue.Example.class, rootLookup); + $edu_wpi_first_epilogue_Example_chooser = lookup$$edu_wpi_first_epilogue_Example.findVarHandle(edu.wpi.first.epilogue.Example.class, "chooser", edu.wpi.first.wpilibj.smartdashboard.SendableChooser.class); } catch (ReflectiveOperationException e) { throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); } @@ -390,7 +353,7 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - logSendable(backend.getNested("chooser"), ((edu.wpi.first.wpilibj.smartdashboard.SendableChooser) $chooser.get(object))); + logSendable(backend.getNested("chooser"), ((edu.wpi.first.wpilibj.smartdashboard.SendableChooser) $edu_wpi_first_epilogue_Example_chooser.get(object))); } } } @@ -421,25 +384,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $low; - private static final VarHandle $medium; - private static final VarHandle $high; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $low = lookup.findVarHandle(Example.class, "low", double.class); - $medium = lookup.findVarHandle(Example.class, "medium", int.class); - $high = lookup.findVarHandle(Example.class, "high", long.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -447,13 +393,13 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("low", ((double) $low.get(object))); + backend.log("low", object.low); } if (Epilogue.shouldLog(Logged.Importance.INFO)) { - backend.log("medium", ((int) $medium.get(object))); + backend.log("medium", object.medium); } if (Epilogue.shouldLog(Logged.Importance.CRITICAL)) { - backend.log("high", ((long) $high.get(object))); + backend.log("high", object.high); } } } @@ -486,21 +432,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $enumValue; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $enumValue = lookup.findVarHandle(Example.class, "enumValue", edu.wpi.first.epilogue.Example.E.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -508,7 +441,7 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("enumValue", ((edu.wpi.first.epilogue.Example.E) $enumValue.get(object))); + backend.log("enumValue", object.enumValue); } } } @@ -535,6 +468,52 @@ class AnnotationProcessorTest { } """; + String expectedGeneratedSource = + """ + package edu.wpi.first.epilogue; + + import edu.wpi.first.epilogue.Logged; + import edu.wpi.first.epilogue.Epilogue; + import edu.wpi.first.epilogue.logging.ClassSpecificLogger; + import edu.wpi.first.epilogue.logging.EpilogueBackend; + + public class ExampleLogger extends ClassSpecificLogger { + public ExampleLogger() { + super(Example.class); + } + + @Override + public void update(EpilogueBackend backend, Example object) { + if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { + backend.log("y", object.y); + } + } + } + """; + + assertLoggerGenerates(source, expectedGeneratedSource); + } + + @Test + void simpleSuperclass() { + String source = + """ + package edu.wpi.first.epilogue; + + @Logged + class BaseExample { + public double a; + protected double b; + private double c; + double d; + } + + @Logged + class Example extends BaseExample { + double e; + } + """; + String expectedGeneratedSource = """ package edu.wpi.first.epilogue; @@ -547,12 +526,20 @@ class AnnotationProcessorTest { import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $y; + // Accesses private or superclass field edu.wpi.first.epilogue.BaseExample.b + private static final VarHandle $edu_wpi_first_epilogue_BaseExample_b; + // Accesses private or superclass field edu.wpi.first.epilogue.BaseExample.c + private static final VarHandle $edu_wpi_first_epilogue_BaseExample_c; + // Accesses private or superclass field edu.wpi.first.epilogue.BaseExample.d + private static final VarHandle $edu_wpi_first_epilogue_BaseExample_d; static { try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $y = lookup.findVarHandle(Example.class, "y", double.class); + var rootLookup = MethodHandles.lookup(); + var lookup$$edu_wpi_first_epilogue_BaseExample = MethodHandles.privateLookupIn(edu.wpi.first.epilogue.BaseExample.class, rootLookup); + $edu_wpi_first_epilogue_BaseExample_b = lookup$$edu_wpi_first_epilogue_BaseExample.findVarHandle(edu.wpi.first.epilogue.BaseExample.class, "b", double.class); + $edu_wpi_first_epilogue_BaseExample_c = lookup$$edu_wpi_first_epilogue_BaseExample.findVarHandle(edu.wpi.first.epilogue.BaseExample.class, "c", double.class); + $edu_wpi_first_epilogue_BaseExample_d = lookup$$edu_wpi_first_epilogue_BaseExample.findVarHandle(edu.wpi.first.epilogue.BaseExample.class, "d", double.class); } catch (ReflectiveOperationException e) { throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); } @@ -565,7 +552,11 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("y", ((double) $y.get(object))); + backend.log("e", object.e); + backend.log("a", object.a); + backend.log("b", ((double) $edu_wpi_first_epilogue_BaseExample_b.get(object))); + backend.log("c", ((double) $edu_wpi_first_epilogue_BaseExample_c.get(object))); + backend.log("d", ((double) $edu_wpi_first_epilogue_BaseExample_d.get(object))); } } } @@ -575,7 +566,7 @@ class AnnotationProcessorTest { } @Test - void superclass() { + void complexSuperclass() { String source = """ package edu.wpi.first.epilogue; @@ -600,6 +591,7 @@ class AnnotationProcessorTest { @Logged class Example extends BaseExample { double h; + private double i; } """; @@ -615,18 +607,24 @@ class AnnotationProcessorTest { import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $h; - private static final VarHandle $d; - private static final VarHandle $f; - private static final VarHandle $g; + // Accesses private or superclass field edu.wpi.first.epilogue.Example.i + private static final VarHandle $edu_wpi_first_epilogue_Example_i; + // Accesses private or superclass field edu.wpi.first.epilogue.BaseExample.d + private static final VarHandle $edu_wpi_first_epilogue_BaseExample_d; + // Accesses private or superclass field edu.wpi.first.epilogue.BaseExample.f + private static final VarHandle $edu_wpi_first_epilogue_BaseExample_f; + // Accesses private or superclass field edu.wpi.first.epilogue.BaseExample.g + private static final VarHandle $edu_wpi_first_epilogue_BaseExample_g; static { try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $h = lookup.findVarHandle(Example.class, "h", double.class); - $d = lookup.findVarHandle(Example.class, "d", double.class); - $f = lookup.findVarHandle(Example.class, "f", double.class); - $g = lookup.findVarHandle(Example.class, "g", double.class); + var rootLookup = MethodHandles.lookup(); + var lookup$$edu_wpi_first_epilogue_BaseExample = MethodHandles.privateLookupIn(edu.wpi.first.epilogue.BaseExample.class, rootLookup); + $edu_wpi_first_epilogue_BaseExample_d = lookup$$edu_wpi_first_epilogue_BaseExample.findVarHandle(edu.wpi.first.epilogue.BaseExample.class, "d", double.class); + $edu_wpi_first_epilogue_BaseExample_f = lookup$$edu_wpi_first_epilogue_BaseExample.findVarHandle(edu.wpi.first.epilogue.BaseExample.class, "f", double.class); + $edu_wpi_first_epilogue_BaseExample_g = lookup$$edu_wpi_first_epilogue_BaseExample.findVarHandle(edu.wpi.first.epilogue.BaseExample.class, "g", double.class); + var lookup$$edu_wpi_first_epilogue_Example = MethodHandles.privateLookupIn(edu.wpi.first.epilogue.Example.class, rootLookup); + $edu_wpi_first_epilogue_Example_i = lookup$$edu_wpi_first_epilogue_Example.findVarHandle(edu.wpi.first.epilogue.Example.class, "i", double.class); } catch (ReflectiveOperationException e) { throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); } @@ -639,11 +637,12 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("h", ((double) $h.get(object))); - backend.log("d", ((double) $d.get(object))); + backend.log("h", object.h); + backend.log("i", ((double) $edu_wpi_first_epilogue_Example_i.get(object))); + backend.log("d", ((double) $edu_wpi_first_epilogue_BaseExample_d.get(object))); backend.log("e", object.e); - backend.log("f", ((double) $f.get(object))); - backend.log("g", ((double) $g.get(object))); + backend.log("f", ((double) $edu_wpi_first_epilogue_BaseExample_f.get(object))); + backend.log("g", ((double) $edu_wpi_first_epilogue_BaseExample_g.get(object))); backend.log("a", object.a); backend.log("getValue", object.getValue()); backend.log("getB", object.getB()); @@ -681,23 +680,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $x; - private static final VarHandle $arr1; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $x = lookup.findVarHandle(Example.class, "x", byte.class); - $arr1 = lookup.findVarHandle(Example.class, "arr1", byte[].class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -705,8 +689,8 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", ((byte) $x.get(object))); - backend.log("arr1", ((byte[]) $arr1.get(object))); + backend.log("x", object.x); + backend.log("arr1", object.arr1); backend.log("getX", object.getX()); backend.log("getArr1", object.getArr1()); } @@ -743,21 +727,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $x; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $x = lookup.findVarHandle(Example.class, "x", char.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -765,7 +736,7 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", ((char) $x.get(object))); + backend.log("x", object.x); backend.log("getX", object.getX()); } } @@ -801,21 +772,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $x; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $x = lookup.findVarHandle(Example.class, "x", short.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -823,7 +781,7 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", ((short) $x.get(object))); + backend.log("x", object.x); backend.log("getX", object.getX()); } } @@ -859,23 +817,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $x; - private static final VarHandle $arr1; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $x = lookup.findVarHandle(Example.class, "x", int.class); - $arr1 = lookup.findVarHandle(Example.class, "arr1", int[].class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -883,8 +826,8 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", ((int) $x.get(object))); - backend.log("arr1", ((int[]) $arr1.get(object))); + backend.log("x", object.x); + backend.log("arr1", object.arr1); backend.log("getX", object.getX()); backend.log("getArr1", object.getArr1()); } @@ -921,23 +864,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $x; - private static final VarHandle $arr1; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $x = lookup.findVarHandle(Example.class, "x", long.class); - $arr1 = lookup.findVarHandle(Example.class, "arr1", long[].class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -945,8 +873,8 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", ((long) $x.get(object))); - backend.log("arr1", ((long[]) $arr1.get(object))); + backend.log("x", object.x); + backend.log("arr1", object.arr1); backend.log("getX", object.getX()); backend.log("getArr1", object.getArr1()); } @@ -983,23 +911,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $x; - private static final VarHandle $arr1; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $x = lookup.findVarHandle(Example.class, "x", float.class); - $arr1 = lookup.findVarHandle(Example.class, "arr1", float[].class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -1007,8 +920,8 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", ((float) $x.get(object))); - backend.log("arr1", ((float[]) $arr1.get(object))); + backend.log("x", object.x); + backend.log("arr1", object.arr1); backend.log("getX", object.getX()); backend.log("getArr1", object.getArr1()); } @@ -1048,23 +961,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $x; - private static final VarHandle $arr1; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $x = lookup.findVarHandle(Example.class, "x", double.class); - $arr1 = lookup.findVarHandle(Example.class, "arr1", double[].class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -1072,8 +970,8 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", ((double) $x.get(object))); - backend.log("arr1", ((double[]) $arr1.get(object))); + backend.log("x", object.x); + backend.log("arr1", object.arr1); backend.log("getX", object.getX()); backend.log("getArr1", object.getArr1()); } @@ -1112,23 +1010,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $x; - private static final VarHandle $arr1; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $x = lookup.findVarHandle(Example.class, "x", boolean.class); - $arr1 = lookup.findVarHandle(Example.class, "arr1", boolean[].class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -1136,8 +1019,8 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", ((boolean) $x.get(object))); - backend.log("arr1", ((boolean[]) $arr1.get(object))); + backend.log("x", object.x); + backend.log("arr1", object.arr1); backend.log("getX", object.getX()); backend.log("getArr1", object.getArr1()); } @@ -1177,25 +1060,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $x; - private static final VarHandle $arr1; - private static final VarHandle $list; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $x = lookup.findVarHandle(Example.class, "x", java.lang.String.class); - $arr1 = lookup.findVarHandle(Example.class, "arr1", java.lang.String[].class); - $list = lookup.findVarHandle(Example.class, "list", java.util.List.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -1203,9 +1069,9 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", ((java.lang.String) $x.get(object))); - backend.log("arr1", ((java.lang.String[]) $arr1.get(object))); - backend.log("list", ((java.util.List) $list.get(object))); + backend.log("x", object.x); + backend.log("arr1", object.arr1); + backend.log("list", object.list); backend.log("getX", object.getX()); backend.log("getArr1", object.getArr1()); } @@ -1253,25 +1119,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $x; - private static final VarHandle $arr1; - private static final VarHandle $list; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $x = lookup.findVarHandle(Example.class, "x", edu.wpi.first.epilogue.Example.Structable.class); - $arr1 = lookup.findVarHandle(Example.class, "arr1", edu.wpi.first.epilogue.Example.Structable[].class); - $list = lookup.findVarHandle(Example.class, "list", java.util.List.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -1279,9 +1128,9 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", ((edu.wpi.first.epilogue.Example.Structable) $x.get(object)), edu.wpi.first.epilogue.Example.Structable.struct); - backend.log("arr1", ((edu.wpi.first.epilogue.Example.Structable[]) $arr1.get(object)), edu.wpi.first.epilogue.Example.Structable.struct); - backend.log("list", ((java.util.List) $list.get(object)), edu.wpi.first.epilogue.Example.Structable.struct); + backend.log("x", object.x, edu.wpi.first.epilogue.Example.Structable.struct); + backend.log("arr1", object.arr1, edu.wpi.first.epilogue.Example.Structable.struct); + backend.log("list", object.list, edu.wpi.first.epilogue.Example.Structable.struct); backend.log("getX", object.getX(), edu.wpi.first.epilogue.Example.Structable.struct); backend.log("getArr1", object.getArr1(), edu.wpi.first.epilogue.Example.Structable.struct); } @@ -1321,27 +1170,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $list; - private static final VarHandle $set; - private static final VarHandle $queue; - private static final VarHandle $stack; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $list = lookup.findVarHandle(Example.class, "list", java.util.List.class); - $set = lookup.findVarHandle(Example.class, "set", java.util.Set.class); - $queue = lookup.findVarHandle(Example.class, "queue", java.util.Queue.class); - $stack = lookup.findVarHandle(Example.class, "stack", java.util.Stack.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -1349,10 +1179,10 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("list", ((java.util.List) $list.get(object))); - backend.log("set", ((java.util.Set) $set.get(object))); - backend.log("queue", ((java.util.Queue) $queue.get(object))); - backend.log("stack", ((java.util.Stack) $stack.get(object))); + backend.log("list", object.list); + backend.log("set", object.set); + backend.log("queue", object.queue); + backend.log("stack", object.stack); } } } @@ -1507,9 +1337,10 @@ class AnnotationProcessorTest { @Logged class Example { - T value; + T valueA; + private T valueB; - public S upcast() { return (S) value; } + public S upcast() { return (S) valueA; } } """; @@ -1525,12 +1356,14 @@ class AnnotationProcessorTest { import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $value; + // Accesses private or superclass field edu.wpi.first.epilogue.Example.valueB + private static final VarHandle $edu_wpi_first_epilogue_Example_valueB; static { try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $value = lookup.findVarHandle(Example.class, "value", java.lang.String.class); + var rootLookup = MethodHandles.lookup(); + var lookup$$edu_wpi_first_epilogue_Example = MethodHandles.privateLookupIn(edu.wpi.first.epilogue.Example.class, rootLookup); + $edu_wpi_first_epilogue_Example_valueB = lookup$$edu_wpi_first_epilogue_Example.findVarHandle(edu.wpi.first.epilogue.Example.class, "valueB", java.lang.String.class); } catch (ReflectiveOperationException e) { throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); } @@ -1543,7 +1376,8 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("value", ((java.lang.String) $value.get(object))); + backend.log("valueA", object.valueA); + backend.log("valueB", ((java.lang.String) $edu_wpi_first_epilogue_Example_valueB.get(object))); backend.log("upcast", object.upcast()); } } @@ -1586,23 +1420,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $child; - private static final VarHandle $io; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $child = lookup.findVarHandle(Example.class, "child", edu.wpi.first.epilogue.Child.class); - $io = lookup.findVarHandle(Example.class, "io", edu.wpi.first.epilogue.IO.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -1610,8 +1429,8 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - Epilogue.childLogger.tryUpdate(backend.getNested("child"), ((edu.wpi.first.epilogue.Child) $child.get(object)), Epilogue.getConfig().errorHandler); - Epilogue.ioLogger.tryUpdate(backend.getNested("io"), ((edu.wpi.first.epilogue.IO) $io.get(object)), Epilogue.getConfig().errorHandler); + Epilogue.childLogger.tryUpdate(backend.getNested("child"), object.child, Epilogue.getConfig().errorHandler); + Epilogue.ioLogger.tryUpdate(backend.getNested("io"), object.io, Epilogue.getConfig().errorHandler); } } } @@ -1688,27 +1507,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $asInterface; - private static final VarHandle $firstImpl; - private static final VarHandle $secondImpl; - private static final VarHandle $complex; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $asInterface = lookup.findVarHandle(Example.class, "asInterface", edu.wpi.first.epilogue.IFace.class); - $firstImpl = lookup.findVarHandle(Example.class, "firstImpl", edu.wpi.first.epilogue.Impl1.class); - $secondImpl = lookup.findVarHandle(Example.class, "secondImpl", edu.wpi.first.epilogue.Impl2.class); - $complex = lookup.findVarHandle(Example.class, "complex", edu.wpi.first.epilogue.I.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -1716,7 +1516,7 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - var $$asInterface = ((edu.wpi.first.epilogue.IFace) $asInterface.get(object)); + var $$asInterface = object.asInterface; if ($$asInterface instanceof edu.wpi.first.epilogue.Impl1 edu_wpi_first_epilogue_Impl1) { Epilogue.impl1Logger.tryUpdate(backend.getNested("asInterface"), edu_wpi_first_epilogue_Impl1, Epilogue.getConfig().errorHandler); } else if ($$asInterface instanceof edu.wpi.first.epilogue.Impl2 edu_wpi_first_epilogue_Impl2) { @@ -1725,9 +1525,9 @@ class AnnotationProcessorTest { // Base type edu.wpi.first.epilogue.IFace Epilogue.iFaceLogger.tryUpdate(backend.getNested("asInterface"), $$asInterface, Epilogue.getConfig().errorHandler); }; - Epilogue.impl1Logger.tryUpdate(backend.getNested("firstImpl"), ((edu.wpi.first.epilogue.Impl1) $firstImpl.get(object)), Epilogue.getConfig().errorHandler); - Epilogue.impl2Logger.tryUpdate(backend.getNested("secondImpl"), ((edu.wpi.first.epilogue.Impl2) $secondImpl.get(object)), Epilogue.getConfig().errorHandler); - var $$complex = ((edu.wpi.first.epilogue.I) $complex.get(object)); + Epilogue.impl1Logger.tryUpdate(backend.getNested("firstImpl"), object.firstImpl, Epilogue.getConfig().errorHandler); + Epilogue.impl2Logger.tryUpdate(backend.getNested("secondImpl"), object.secondImpl, Epilogue.getConfig().errorHandler); + var $$complex = object.complex; if ($$complex instanceof edu.wpi.first.epilogue.ConcreteLogged edu_wpi_first_epilogue_ConcreteLogged) { Epilogue.concreteLoggedLogger.tryUpdate(backend.getNested("complex"), edu_wpi_first_epilogue_ConcreteLogged, Epilogue.getConfig().errorHandler); } else if ($$complex instanceof edu.wpi.first.epilogue.I4 edu_wpi_first_epilogue_I4) { @@ -1770,21 +1570,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class Outer$ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $x; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Outer.Example.class, MethodHandles.lookup()); - $x = lookup.findVarHandle(Outer.Example.class, "x", double.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public Outer$ExampleLogger() { super(Outer.Example.class); } @@ -1792,7 +1579,7 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Outer.Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", ((double) $x.get(object))); + backend.log("x", object.x); } } } @@ -1829,21 +1616,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class A$B$C$D$ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $x; - - static { - try { - var lookup = MethodHandles.privateLookupIn(A.B.C.D.Example.class, MethodHandles.lookup()); - $x = lookup.findVarHandle(A.B.C.D.Example.class, "x", double.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public A$B$C$D$ExampleLogger() { super(A.B.C.D.Example.class); } @@ -1851,7 +1625,7 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, A.B.C.D.Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", ((double) $x.get(object))); + backend.log("x", object.x); } } } @@ -1882,21 +1656,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class CustomExampleLogger extends ClassSpecificLogger { - private static final VarHandle $x; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Outer.Example.class, MethodHandles.lookup()); - $x = lookup.findVarHandle(Outer.Example.class, "x", double.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public CustomExampleLogger() { super(Outer.Example.class); } @@ -1904,7 +1665,7 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Outer.Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", ((double) $x.get(object))); + backend.log("x", object.x); } } } @@ -1948,21 +1709,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $theField; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $theField = lookup.findVarHandle(Example.class, "theField", edu.wpi.first.epilogue.I.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -1970,7 +1718,7 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - var $$theField = ((edu.wpi.first.epilogue.I) $theField.get(object)); + var $$theField = object.theField; if ($$theField instanceof edu.wpi.first.epilogue.Base edu_wpi_first_epilogue_Base) { Epilogue.baseLogger.tryUpdate(backend.getNested("theField"), edu_wpi_first_epilogue_Base, Epilogue.getConfig().errorHandler); } else if ($$theField instanceof edu.wpi.first.epilogue.ExtendingInterface edu_wpi_first_epilogue_ExtendingInterface) { @@ -2017,12 +1765,14 @@ class AnnotationProcessorTest { import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $theField; + // Accesses private or superclass field edu.wpi.first.epilogue.Example.theField + private static final VarHandle $edu_wpi_first_epilogue_Example_theField; static { try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $theField = lookup.findVarHandle(Example.class, "theField", edu.wpi.first.epilogue.I.class); + var rootLookup = MethodHandles.lookup(); + var lookup$$edu_wpi_first_epilogue_Example = MethodHandles.privateLookupIn(edu.wpi.first.epilogue.Example.class, rootLookup); + $edu_wpi_first_epilogue_Example_theField = lookup$$edu_wpi_first_epilogue_Example.findVarHandle(edu.wpi.first.epilogue.Example.class, "theField", edu.wpi.first.epilogue.I.class); } catch (ReflectiveOperationException e) { throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); } @@ -2035,7 +1785,7 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - var $$theField = ((edu.wpi.first.epilogue.I) $theField.get(object)); + var $$theField = ((edu.wpi.first.epilogue.I) $edu_wpi_first_epilogue_Example_theField.get(object)); if ($$theField instanceof edu.wpi.first.epilogue.Base edu_wpi_first_epilogue_Base) { Epilogue.baseLogger.tryUpdate(backend.getNested("theField"), edu_wpi_first_epilogue_Base, Epilogue.getConfig().errorHandler); } else { @@ -2073,21 +1823,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $i; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $i = lookup.findVarHandle(Example.class, "i", edu.wpi.first.epilogue.Implicit.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -2095,7 +1832,7 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - Epilogue.implicitLogger.tryUpdate(backend.getNested("i"), ((edu.wpi.first.epilogue.Implicit) $i.get(object)), Epilogue.getConfig().errorHandler); + Epilogue.implicitLogger.tryUpdate(backend.getNested("i"), object.i, Epilogue.getConfig().errorHandler); } } } @@ -2140,21 +1877,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $point; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $point = lookup.findVarHandle(Example.class, "point", edu.wpi.first.epilogue.Point.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -2162,7 +1886,7 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - Epilogue.customPointLogger.tryUpdate(backend.getNested("point"), ((edu.wpi.first.epilogue.Point) $point.get(object)), Epilogue.getConfig().errorHandler); + Epilogue.customPointLogger.tryUpdate(backend.getNested("point"), object.point, Epilogue.getConfig().errorHandler); } } } @@ -2208,21 +1932,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $vec; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $vec = lookup.findVarHandle(Example.class, "vec", edu.wpi.first.math.Vector.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -2230,7 +1941,7 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - Epilogue.vectorLogger.tryUpdate(backend.getNested("vec"), ((edu.wpi.first.math.Vector) $vec.get(object)), Epilogue.getConfig().errorHandler); + Epilogue.vectorLogger.tryUpdate(backend.getNested("vec"), object.vec, Epilogue.getConfig().errorHandler); } } } @@ -2417,21 +2128,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $x; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $x = lookup.findVarHandle(Example.class, "x", double.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -2439,7 +2137,7 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", ((double) $x.get(object))); + backend.log("x", object.x); backend.log("withANoOpTransform", object.withANoOpTransform()); backend.log("withTemp", object.withTemp()); } @@ -2482,27 +2180,8 @@ class AnnotationProcessorTest { import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; - import java.lang.invoke.MethodHandles; - import java.lang.invoke.VarHandle; public class ExampleLogger extends ClassSpecificLogger { - private static final VarHandle $m_memberPrefix; - private static final VarHandle $kConstantPrefix; - private static final VarHandle $k_otherConstantPrefix; - private static final VarHandle $s_otherPrefix; - - static { - try { - var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); - $m_memberPrefix = lookup.findVarHandle(Example.class, "m_memberPrefix", double.class); - $kConstantPrefix = lookup.findVarHandle(Example.class, "kConstantPrefix", double.class); - $k_otherConstantPrefix = lookup.findVarHandle(Example.class, "k_otherConstantPrefix", double.class); - $s_otherPrefix = lookup.findVarHandle(Example.class, "s_otherPrefix", double.class); - } catch (ReflectiveOperationException e) { - throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); - } - } - public ExampleLogger() { super(Example.class); } @@ -2510,10 +2189,10 @@ class AnnotationProcessorTest { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("Member Prefix", ((double) $m_memberPrefix.get(object))); - backend.log("Constant Prefix", ((double) $kConstantPrefix.get(object))); - backend.log("Other Constant Prefix", ((double) $k_otherConstantPrefix.get(object))); - backend.log("Other Prefix", ((double) $s_otherPrefix.get(object))); + backend.log("Member Prefix", object.m_memberPrefix); + backend.log("Constant Prefix", object.kConstantPrefix); + backend.log("Other Constant Prefix", object.k_otherConstantPrefix); + backend.log("Other Prefix", object.s_otherPrefix); backend.log("The Getter Method", object.getTheGetterMethod()); backend.log("optedOut", object.optedOut()); } From 129cbbe11d8ab5b4941de43ed4819e9852ee96c2 Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Sat, 30 Aug 2025 23:15:22 -0400 Subject: [PATCH 07/15] [epilogue] Optimize time and memory usage of epilogue backends (#8190) --- .../epilogue/logging/ClassSpecificLogger.java | 17 +- .../first/epilogue/logging/FileBackend.java | 35 +++- .../first/epilogue/logging/LazyBackend.java | 8 +- .../first/epilogue/logging/MultiBackend.java | 8 +- .../epilogue/logging/NTEpilogueBackend.java | 108 +++++++---- .../first/epilogue/logging/NestedBackend.java | 67 +++++-- .../epilogue/logging/NestedBackendTest.java | 180 ++++++++++++++++++ 7 files changed, 354 insertions(+), 69 deletions(-) create mode 100644 epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/NestedBackendTest.java diff --git a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/ClassSpecificLogger.java b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/ClassSpecificLogger.java index c433d739a5..9cbd2ba2f7 100644 --- a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/ClassSpecificLogger.java +++ b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/ClassSpecificLogger.java @@ -106,14 +106,13 @@ public abstract class ClassSpecificLogger { return; } - var builder = - m_sendables.computeIfAbsent( - sendable, - s -> { - var b = new LogBackedSendableBuilder(backend); - s.initSendable(b); - return b; - }); - builder.update(); + if (m_sendables.containsKey(sendable)) { + m_sendables.get(sendable).update(); + } else { + var builder = new LogBackedSendableBuilder(backend); + sendable.initSendable(builder); + m_sendables.put(sendable, builder); + builder.update(); + } } } diff --git a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/FileBackend.java b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/FileBackend.java index 2b5b6b2071..0a116a03d9 100644 --- a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/FileBackend.java +++ b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/FileBackend.java @@ -23,7 +23,9 @@ import edu.wpi.first.util.datalog.StructArrayLogEntry; import edu.wpi.first.util.datalog.StructLogEntry; import edu.wpi.first.util.struct.Struct; import java.util.HashMap; +import java.util.HashSet; import java.util.Map; +import java.util.Set; import java.util.function.BiFunction; /** A backend implementation that saves information to a WPILib {@link DataLog} file on disk. */ @@ -31,6 +33,7 @@ public class FileBackend implements EpilogueBackend { private final DataLog m_dataLog; private final Map m_entries = new HashMap<>(); private final Map m_subLoggers = new HashMap<>(); + private final Set> m_seenSchemas = new HashSet<>(); /** * Creates a new file-based backend. @@ -43,7 +46,13 @@ public class FileBackend implements EpilogueBackend { @Override public EpilogueBackend getNested(String path) { - return m_subLoggers.computeIfAbsent(path, k -> new NestedBackend(k, this)); + if (!m_subLoggers.containsKey(path)) { + var nested = new NestedBackend(path, this); + m_subLoggers.put(path, nested); + return nested; + } + + return m_subLoggers.get(path); } @SuppressWarnings("unchecked") @@ -131,14 +140,30 @@ public class FileBackend implements EpilogueBackend { @Override @SuppressWarnings("unchecked") public void log(String identifier, S value, Struct struct) { - m_dataLog.addSchema(struct); - getEntry(identifier, (log, k) -> StructLogEntry.create(log, k, struct)).append(value); + // DataLog.addSchema has checks that we're able to skip, avoiding allocations + if (m_seenSchemas.add(struct)) { + m_dataLog.addSchema(struct); + } + + if (!m_entries.containsKey(identifier)) { + m_entries.put(identifier, StructLogEntry.create(m_dataLog, identifier, struct)); + } + + ((StructLogEntry) m_entries.get(identifier)).append(value); } @Override @SuppressWarnings("unchecked") public void log(String identifier, S[] value, Struct struct) { - m_dataLog.addSchema(struct); - getEntry(identifier, (log, k) -> StructArrayLogEntry.create(log, k, struct)).append(value); + // DataLog.addSchema has checks that we're able to skip, avoiding allocations + if (m_seenSchemas.add(struct)) { + m_dataLog.addSchema(struct); + } + + if (!m_entries.containsKey(identifier)) { + m_entries.put(identifier, StructArrayLogEntry.create(m_dataLog, identifier, struct)); + } + + ((StructArrayLogEntry) m_entries.get(identifier)).append(value); } } diff --git a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LazyBackend.java b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LazyBackend.java index adad963e07..bd925165e0 100644 --- a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LazyBackend.java +++ b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LazyBackend.java @@ -40,7 +40,13 @@ public class LazyBackend implements EpilogueBackend { @Override public EpilogueBackend getNested(String path) { - return m_subLoggers.computeIfAbsent(path, k -> new NestedBackend(k, this)); + if (!m_subLoggers.containsKey(path)) { + var nested = new NestedBackend(path, this); + m_subLoggers.put(path, nested); + return nested; + } + + return m_subLoggers.get(path); } @Override diff --git a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/MultiBackend.java b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/MultiBackend.java index 408d65aad6..575fde05b2 100644 --- a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/MultiBackend.java +++ b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/MultiBackend.java @@ -24,7 +24,13 @@ public class MultiBackend implements EpilogueBackend { @Override public EpilogueBackend getNested(String path) { - return m_nestedBackends.computeIfAbsent(path, k -> new NestedBackend(k, this)); + if (!m_nestedBackends.containsKey(path)) { + var nested = new NestedBackend(path, this); + m_nestedBackends.put(path, nested); + return nested; + } + + return m_nestedBackends.get(path); } @Override diff --git a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NTEpilogueBackend.java b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NTEpilogueBackend.java index cf381a2f02..e398172e77 100644 --- a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NTEpilogueBackend.java +++ b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NTEpilogueBackend.java @@ -21,7 +21,10 @@ import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.util.struct.Struct; import java.util.HashMap; +import java.util.HashSet; import java.util.Map; +import java.util.Set; +import java.util.function.Function; /** * A backend implementation that sends data over network tables. Be careful when using this, since @@ -32,61 +35,81 @@ public class NTEpilogueBackend implements EpilogueBackend { private final Map m_publishers = new HashMap<>(); private final Map m_nestedBackends = new HashMap<>(); + private final Set> m_seenSchemas = new HashSet<>(); + private final Function m_createIntPublisher; + private final Function m_createFloatPublisher; + private final Function m_createDoublePublisher; + private final Function m_createBooleanPublisher; + private final Function m_createRawPublisher; + private final Function m_createIntegerArrayPublisher; + private final Function m_createFloatArrayPublisher; + private final Function m_createDoubleArrayPublisher; + private final Function m_createBooleanArrayPublisher; + private final Function m_createStringPublisher; + private final Function m_createStringArrayPublisher; /** * Creates a logging backend that sends information to NetworkTables. * * @param nt the NetworkTable instance to use to send data to */ + @SuppressWarnings("unchecked") public NTEpilogueBackend(NetworkTableInstance nt) { this.m_nt = nt; + m_createIntPublisher = identifier -> m_nt.getIntegerTopic(identifier).publish(); + m_createFloatPublisher = identifier -> m_nt.getFloatTopic(identifier).publish(); + m_createDoublePublisher = identifier -> m_nt.getDoubleTopic(identifier).publish(); + m_createBooleanPublisher = identifier -> m_nt.getBooleanTopic(identifier).publish(); + m_createRawPublisher = identifier -> m_nt.getRawTopic(identifier).publish("raw"); + m_createIntegerArrayPublisher = identifier -> m_nt.getIntegerArrayTopic(identifier).publish(); + m_createFloatArrayPublisher = identifier -> m_nt.getFloatArrayTopic(identifier).publish(); + m_createDoubleArrayPublisher = identifier -> m_nt.getDoubleArrayTopic(identifier).publish(); + m_createBooleanArrayPublisher = identifier -> m_nt.getBooleanArrayTopic(identifier).publish(); + m_createStringPublisher = identifier -> m_nt.getStringTopic(identifier).publish(); + m_createStringArrayPublisher = identifier -> m_nt.getStringArrayTopic(identifier).publish(); } @Override public EpilogueBackend getNested(String path) { - return m_nestedBackends.computeIfAbsent(path, k -> new NestedBackend(k, this)); + if (!m_nestedBackends.containsKey(path)) { + var nested = new NestedBackend(path, this); + m_nestedBackends.put(path, nested); + return nested; + } + + return m_nestedBackends.get(path); } @Override public void log(String identifier, int value) { - ((IntegerPublisher) - m_publishers.computeIfAbsent(identifier, k -> m_nt.getIntegerTopic(k).publish())) - .set(value); + ((IntegerPublisher) m_publishers.computeIfAbsent(identifier, m_createIntPublisher)).set(value); } @Override public void log(String identifier, long value) { - ((IntegerPublisher) - m_publishers.computeIfAbsent(identifier, k -> m_nt.getIntegerTopic(k).publish())) - .set(value); + ((IntegerPublisher) m_publishers.computeIfAbsent(identifier, m_createIntPublisher)).set(value); } @Override public void log(String identifier, float value) { - ((FloatPublisher) - m_publishers.computeIfAbsent(identifier, k -> m_nt.getFloatTopic(k).publish())) - .set(value); + ((FloatPublisher) m_publishers.computeIfAbsent(identifier, m_createFloatPublisher)).set(value); } @Override public void log(String identifier, double value) { - ((DoublePublisher) - m_publishers.computeIfAbsent(identifier, k -> m_nt.getDoubleTopic(k).publish())) + ((DoublePublisher) m_publishers.computeIfAbsent(identifier, m_createDoublePublisher)) .set(value); } @Override public void log(String identifier, boolean value) { - ((BooleanPublisher) - m_publishers.computeIfAbsent(identifier, k -> m_nt.getBooleanTopic(k).publish())) + ((BooleanPublisher) m_publishers.computeIfAbsent(identifier, m_createBooleanPublisher)) .set(value); } @Override public void log(String identifier, byte[] value) { - ((RawPublisher) - m_publishers.computeIfAbsent(identifier, k -> m_nt.getRawTopic(k).publish("raw"))) - .set(value); + ((RawPublisher) m_publishers.computeIfAbsent(identifier, m_createRawPublisher)).set(value); } @Override @@ -100,68 +123,79 @@ public class NTEpilogueBackend implements EpilogueBackend { } ((IntegerArrayPublisher) - m_publishers.computeIfAbsent(identifier, k -> m_nt.getIntegerArrayTopic(k).publish())) + m_publishers.computeIfAbsent(identifier, m_createIntegerArrayPublisher)) .set(widened); } @Override public void log(String identifier, long[] value) { ((IntegerArrayPublisher) - m_publishers.computeIfAbsent(identifier, k -> m_nt.getIntegerArrayTopic(k).publish())) + m_publishers.computeIfAbsent(identifier, m_createIntegerArrayPublisher)) .set(value); } @Override public void log(String identifier, float[] value) { - ((FloatArrayPublisher) - m_publishers.computeIfAbsent(identifier, k -> m_nt.getFloatArrayTopic(k).publish())) + ((FloatArrayPublisher) m_publishers.computeIfAbsent(identifier, m_createFloatArrayPublisher)) .set(value); } @Override public void log(String identifier, double[] value) { - ((DoubleArrayPublisher) - m_publishers.computeIfAbsent(identifier, k -> m_nt.getDoubleArrayTopic(k).publish())) + ((DoubleArrayPublisher) m_publishers.computeIfAbsent(identifier, m_createDoubleArrayPublisher)) .set(value); } @Override public void log(String identifier, boolean[] value) { ((BooleanArrayPublisher) - m_publishers.computeIfAbsent(identifier, k -> m_nt.getBooleanArrayTopic(k).publish())) + m_publishers.computeIfAbsent(identifier, m_createBooleanArrayPublisher)) .set(value); } @Override public void log(String identifier, String value) { - ((StringPublisher) - m_publishers.computeIfAbsent(identifier, k -> m_nt.getStringTopic(k).publish())) + ((StringPublisher) m_publishers.computeIfAbsent(identifier, m_createStringPublisher)) .set(value); } @Override public void log(String identifier, String[] value) { - ((StringArrayPublisher) - m_publishers.computeIfAbsent(identifier, k -> m_nt.getStringArrayTopic(k).publish())) + ((StringArrayPublisher) m_publishers.computeIfAbsent(identifier, m_createStringArrayPublisher)) .set(value); } @Override @SuppressWarnings("unchecked") public void log(String identifier, S value, Struct struct) { - m_nt.addSchema(struct); - ((StructPublisher) - m_publishers.computeIfAbsent(identifier, k -> m_nt.getStructTopic(k, struct).publish())) - .set(value); + // NetworkTableInstance.addSchema has checks that we're able to skip, avoiding allocations + if (m_seenSchemas.add(struct)) { + m_nt.addSchema(struct); + } + + if (m_publishers.containsKey(identifier)) { + ((StructPublisher) m_publishers.get(identifier)).set(value); + } else { + StructPublisher publisher = m_nt.getStructTopic(identifier, struct).publish(); + m_publishers.put(identifier, publisher); + publisher.set(value); + } } @Override @SuppressWarnings("unchecked") public void log(String identifier, S[] value, Struct struct) { - m_nt.addSchema(struct); - ((StructArrayPublisher) - m_publishers.computeIfAbsent( - identifier, k -> m_nt.getStructArrayTopic(k, struct).publish())) - .set(value); + // NetworkTableInstance.addSchema has checks that we're able to skip, avoiding allocations + if (m_seenSchemas.add(struct)) { + m_nt.addSchema(struct); + } + + if (m_publishers.containsKey(identifier)) { + ((StructArrayPublisher) m_publishers.get(identifier)).set(value); + } else { + StructArrayPublisher publisher = m_nt.getStructArrayTopic(identifier, struct).publish(); + m_publishers.put(identifier, publisher); + publisher.set(value); + } } } diff --git a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NestedBackend.java b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NestedBackend.java index 50003b24ca..f288566085 100644 --- a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NestedBackend.java +++ b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NestedBackend.java @@ -17,6 +17,15 @@ public class NestedBackend implements EpilogueBackend { private final EpilogueBackend m_impl; private final Map m_nestedBackends = new HashMap<>(); + // String concatenation can be expensive, especially for deeply nested hierarchies with many + // logged fields. For example, logging a hypothetical Robot.elevator.io.getHeight() would result + // in "/Robot/" + "elevator/" + "io/" + "getHeight"; three concatenations and string and byte + // array allocations that need to be cleaned up by the GC. Caching the results means those + // allocations only occur once, resulting in no GC (the strings are always referenced in the + // cache), and minimal time costs (the String object caches its own hash code, so all we do is an + // O(1) table lookup per concatenation) + private final Map m_prefixedIdentifiers = new HashMap<>(); + /** * Creates a new nested backed underneath another backend. * @@ -33,83 +42,109 @@ public class NestedBackend implements EpilogueBackend { this.m_impl = impl; } + /** + * Fast lookup to avoid redundant `m_prefix + identifier` concatenations. If the identifier has + * not been seen before, we compute the concatenation and cache the result for later invocations + * to read. This avoids redundantly recomputing the same concatenations every loop and + * significantly cuts down on the CPU and memory overhead of the Epilogue library. + * + * @param identifier The identifier to prepend with {@link #m_prefix}. + * @return The concatenated string. + */ + private String withPrefix(String identifier) { + // Using computeIfAbsent would result in a new lambda object allocation on every call + if (m_prefixedIdentifiers.containsKey(identifier)) { + return m_prefixedIdentifiers.get(identifier); + } + + String result = m_prefix + identifier; + m_prefixedIdentifiers.put(identifier, result); + return result; + } + @Override public EpilogueBackend getNested(String path) { - return m_nestedBackends.computeIfAbsent(path, k -> new NestedBackend(k, this)); + if (!m_nestedBackends.containsKey(path)) { + var nested = new NestedBackend(path, this); + m_nestedBackends.put(path, nested); + return nested; + } + + return m_nestedBackends.get(path); } @Override public void log(String identifier, int value) { - m_impl.log(m_prefix + identifier, value); + m_impl.log(withPrefix(identifier), value); } @Override public void log(String identifier, long value) { - m_impl.log(m_prefix + identifier, value); + m_impl.log(withPrefix(identifier), value); } @Override public void log(String identifier, float value) { - m_impl.log(m_prefix + identifier, value); + m_impl.log(withPrefix(identifier), value); } @Override public void log(String identifier, double value) { - m_impl.log(m_prefix + identifier, value); + m_impl.log(withPrefix(identifier), value); } @Override public void log(String identifier, boolean value) { - m_impl.log(m_prefix + identifier, value); + m_impl.log(withPrefix(identifier), value); } @Override public void log(String identifier, byte[] value) { - m_impl.log(m_prefix + identifier, value); + m_impl.log(withPrefix(identifier), value); } @Override public void log(String identifier, int[] value) { - m_impl.log(m_prefix + identifier, value); + m_impl.log(withPrefix(identifier), value); } @Override public void log(String identifier, long[] value) { - m_impl.log(m_prefix + identifier, value); + m_impl.log(withPrefix(identifier), value); } @Override public void log(String identifier, float[] value) { - m_impl.log(m_prefix + identifier, value); + m_impl.log(withPrefix(identifier), value); } @Override public void log(String identifier, double[] value) { - m_impl.log(m_prefix + identifier, value); + m_impl.log(withPrefix(identifier), value); } @Override public void log(String identifier, boolean[] value) { - m_impl.log(m_prefix + identifier, value); + m_impl.log(withPrefix(identifier), value); } @Override public void log(String identifier, String value) { - m_impl.log(m_prefix + identifier, value); + m_impl.log(withPrefix(identifier), value); } @Override public void log(String identifier, String[] value) { - m_impl.log(m_prefix + identifier, value); + m_impl.log(withPrefix(identifier), value); } @Override public void log(String identifier, S value, Struct struct) { - m_impl.log(m_prefix + identifier, value, struct); + m_impl.log(withPrefix(identifier), value, struct); } @Override public void log(String identifier, S[] value, Struct struct) { - m_impl.log(m_prefix + identifier, value, struct); + m_impl.log(withPrefix(identifier), value, struct); } } diff --git a/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/NestedBackendTest.java b/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/NestedBackendTest.java new file mode 100644 index 0000000000..e9ee64ac6a --- /dev/null +++ b/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/NestedBackendTest.java @@ -0,0 +1,180 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.logging; + +import static org.junit.jupiter.api.Assertions.assertArrayEquals; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertSame; + +import org.junit.jupiter.api.Test; + +class NestedBackendTest { + @Test + void prefixesAppliedAndNested() { + var root = new TestBackend(); + var nested = new NestedBackend("/Robot", root); + + nested.log("int", 1); + nested.log("string", "hello"); + + var arm = nested.getNested("arm"); + arm.log("position", 2.0); + arm.log("enabled", true); + + assertEquals(4, root.getEntries().size()); + assertEquals("/Robot/int", root.getEntries().get(0).identifier()); + assertEquals(1, root.getEntries().get(0).value()); + + assertEquals("/Robot/string", root.getEntries().get(1).identifier()); + assertEquals("hello", root.getEntries().get(1).value()); + + assertEquals("/Robot/arm/position", root.getEntries().get(2).identifier()); + assertEquals(2.0, root.getEntries().get(2).value()); + + assertEquals("/Robot/arm/enabled", root.getEntries().get(3).identifier()); + assertEquals(true, root.getEntries().get(3).value()); + } + + @Test + void handlesTrailingSlashOnPrefix() { + var root = new TestBackend(); + var a = new NestedBackend("/Robot", root); + var b = new NestedBackend("/Robot/", root); + + a.log("x", 1); + b.log("y", 2); + + assertEquals("/Robot/x", root.getEntries().get(0).identifier()); + assertEquals("/Robot/y", root.getEntries().get(1).identifier()); + } + + @Test + void getNestedIsCached() { + var root = new TestBackend(); + var nested = new NestedBackend("/Robot", root); + + var arm1 = nested.getNested("arm"); + var arm2 = nested.getNested("arm"); + + assertSame(arm1, arm2); + } + + @Test + void usesPrefixedIdentifierCacheForSameField() { + var root = new TestBackend(); + var nested = new NestedBackend("/Robot", root); + + // Same field logged multiple times - identifier object should be the same (cached) + // We use assertSame to check that the references are identical + nested.log("x", 0); + nested.log("x", 1); + + String id0 = root.getEntries().get(0).identifier(); + String id1 = root.getEntries().get(1).identifier(); + assertSame( + id0, + id1, + "Identifier %s (id: %d) was not reused (new id: %d)" + .formatted(id0, System.identityHashCode(id0), System.identityHashCode(id1))); + + // Also verify through a nested backend path + var arm = nested.getNested("arm"); + arm.log("position", 0.0); + arm.log("position", 1.0); + + String id2 = root.getEntries().get(2).identifier(); + String id3 = root.getEntries().get(3).identifier(); + assertSame( + id2, + id3, + "Identifier %s (id: %d) was not reused (new id: %d)" + .formatted(id2, System.identityHashCode(id2), System.identityHashCode(id3))); + + // Sanity check actual full values + assertEquals("/Robot/x", id0); + assertEquals("/Robot/arm/position", id2); + } + + @Test + void logsAllOverloads() { + var root = new TestBackend(); + var nested = new NestedBackend("/Robot", root); + + // Scalars + nested.log("int", 1); + nested.log("long", 2L); + nested.log("float", 3.0f); + nested.log("double", 4.0); + nested.log("boolean", true); + nested.log("string", "hello"); + + // Arrays + nested.log("bytes", new byte[] {1, 2}); + nested.log("ints", new int[] {3, 4}); + nested.log("longs", new long[] {5L, 6L}); + nested.log("floats", new float[] {7.0f, 8.0f}); + nested.log("doubles", new double[] {9.0, 10.0}); + nested.log("booleans", new boolean[] {true, false}); + nested.log("strings", new String[] {"x", "y"}); + + // Structs + nested.log("customStruct", new CustomStruct(7), CustomStruct.struct); + nested.log( + "customStructs", + new CustomStruct[] {new CustomStruct(0), new CustomStruct(1)}, + CustomStruct.struct); + + var entries = root.getEntries(); + int idx = 0; + + // Scalars + assertEquals(new TestBackend.LogEntry<>("/Robot/int", 1), entries.get(idx++)); + assertEquals(new TestBackend.LogEntry<>("/Robot/long", 2L), entries.get(idx++)); + assertEquals(new TestBackend.LogEntry<>("/Robot/float", 3.0f), entries.get(idx++)); + assertEquals(new TestBackend.LogEntry<>("/Robot/double", 4.0), entries.get(idx++)); + assertEquals(new TestBackend.LogEntry<>("/Robot/boolean", true), entries.get(idx++)); + assertEquals(new TestBackend.LogEntry<>("/Robot/string", "hello"), entries.get(idx++)); + + // Arrays + assertEquals("/Robot/bytes", entries.get(idx).identifier()); + assertArrayEquals(new byte[] {1, 2}, (byte[]) entries.get(idx++).value()); + + assertEquals("/Robot/ints", entries.get(idx).identifier()); + assertArrayEquals(new int[] {3, 4}, (int[]) entries.get(idx++).value()); + + assertEquals("/Robot/longs", entries.get(idx).identifier()); + assertArrayEquals(new long[] {5L, 6L}, (long[]) entries.get(idx++).value()); + + assertEquals("/Robot/floats", entries.get(idx).identifier()); + assertArrayEquals(new float[] {7.0f, 8.0f}, (float[]) entries.get(idx++).value()); + + assertEquals("/Robot/doubles", entries.get(idx).identifier()); + assertArrayEquals(new double[] {9.0, 10.0}, (double[]) entries.get(idx++).value()); + + assertEquals("/Robot/booleans", entries.get(idx).identifier()); + assertArrayEquals(new boolean[] {true, false}, (boolean[]) entries.get(idx++).value()); + + assertEquals("/Robot/strings", entries.get(idx).identifier()); + assertArrayEquals(new String[] {"x", "y"}, (String[]) entries.get(idx++).value()); + + // Structs are serialized to bytes + assertEquals("/Robot/customStruct", entries.get(idx).identifier()); + assertArrayEquals(new byte[] {0x07, 0x00, 0x00, 0x00}, (byte[]) entries.get(idx++).value()); + + assertEquals("/Robot/customStructs", entries.get(idx).identifier()); + // two int32 values, little-endian + assertArrayEquals( + new byte[] { + 0x00, 0x00, 0x00, 0x00, // 0 (first element) + 0x01, 0x00, 0x00, 0x00, // 1 (second element) + 0x00, 0x00, 0x00, 0x00, // 0 (empty space allocated by StructBuffer) + 0x00, 0x00, 0x00, 0x00 // 0 (empty space allocated by StructBuffer) + }, + (byte[]) entries.get(idx++).value()); + + // Ensure we covered all calls + assertEquals(idx, entries.size()); + } +} From 2cfd58f119c1890714b9bfa8f4493f248c7f8895 Mon Sep 17 00:00:00 2001 From: Wispy <101812473+WispySparks@users.noreply.github.com> Date: Sun, 31 Aug 2025 00:54:53 -0500 Subject: [PATCH 08/15] [commands] Add Subsystem.idle() (#7815) --- .../java/edu/wpi/first/wpilibj2/command/Subsystem.java | 9 +++++++++ .../src/main/native/cpp/frc2/command/Subsystem.cpp | 4 ++++ .../src/main/native/include/frc2/command/Subsystem.h | 9 +++++++++ 3 files changed, 22 insertions(+) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java index c0e694774c..4a0c056149 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java @@ -98,6 +98,15 @@ public interface Subsystem { CommandScheduler.getInstance().registerSubsystem(this); } + /** + * Constructs a command that does nothing until interrupted. Requires this subsystem. + * + * @return the command + */ + default Command idle() { + return Commands.idle(this); + } + /** * Constructs a command that runs an action once and finishes. Requires this subsystem. * diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp index 876269a1b4..8282682b91 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp @@ -46,6 +46,10 @@ void Subsystem::Register() { return CommandScheduler::GetInstance().RegisterSubsystem(this); } +CommandPtr Subsystem::Idle() { + return cmd::Idle({this}); +} + CommandPtr Subsystem::RunOnce(std::function action) { return cmd::RunOnce(std::move(action), {this}); } diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h index ea7fbb7591..2ec07f5d88 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h @@ -121,6 +121,15 @@ class Subsystem { */ void Register(); + /** + * Constructs a command that does nothing until interrupted. Requires this + * subsystem. + * + * @return the command + */ + [[nodiscard]] + CommandPtr Idle(); + /** * Constructs a command that runs an action once and finishes. Requires this * subsystem. From ed904851eb7592bbb6e4401d3a00116c20203b28 Mon Sep 17 00:00:00 2001 From: Ryan Blue Date: Mon, 1 Sep 2025 00:55:13 -0400 Subject: [PATCH 09/15] [ci] Fix CMake Android build caching (#8206) sccache was enabled but didn't have write credentials --- .github/workflows/cmake-android.yml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.github/workflows/cmake-android.yml b/.github/workflows/cmake-android.yml index 338eedee1f..da7a67f362 100644 --- a/.github/workflows/cmake-android.yml +++ b/.github/workflows/cmake-android.yml @@ -46,6 +46,12 @@ jobs: - name: configure run: cmake --preset with-sccache -DCMAKE_BUILD_TYPE=RelWithDebInfo -DWITH_WPILIB=OFF -DWITH_GUI=OFF -DWITH_CSCORE=OFF -DWITH_TESTS=OFF -DWITH_SIMULATION_MODULES=OFF -DWITH_PROTOBUF=OFF -DWITH_JAVA=ON -DBUILD_SHARED_LIBS=ON -DCMAKE_TOOLCHAIN_FILE=${{ steps.setup-ndk.outputs.ndk-path }}/build/cmake/android.toolchain.cmake -DANDROID_ABI="${{ matrix.abi }}" -DANDROID_PLATFORM=android-24 + env: + SCCACHE_WEBDAV_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }} + SCCACHE_WEBDAV_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }} - name: build run: cmake --build build-cmake --parallel $(nproc) + env: + SCCACHE_WEBDAV_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }} + SCCACHE_WEBDAV_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }} From b0829356fa7841f2295bb5f5a4377700fc44ecc7 Mon Sep 17 00:00:00 2001 From: Jade Date: Mon, 1 Sep 2025 23:11:39 +0800 Subject: [PATCH 10/15] [wpimath] Fix sysid links (NFC) (#8204) Signed-off-by: Jade Turner --- .../first/math/system/plant/LinearSystemId.java | 15 ++++++++++----- .../include/frc/system/plant/LinearSystemId.h | 10 +++++----- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java index ce1129b059..bc64c5c73c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java @@ -138,7 +138,8 @@ public final class LinearSystemId { * @param kA The acceleration gain, in volts/(unit/sec²) * @return A LinearSystem representing the given characterized constants. * @throws IllegalArgumentException if kV < 0 or kA <= 0. - * @see https://github.com/wpilibsuite/sysid + * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid */ public static LinearSystem createDCMotorSystem(double kV, double kA) { if (kV < 0.0) { @@ -259,7 +260,8 @@ public final class LinearSystemId { * @param kA The acceleration gain, in volts/(unit/sec²) * @return A LinearSystem representing the given characterized constants. * @throws IllegalArgumentException if kV < 0 or kA <= 0. - * @see https://github.com/wpilibsuite/sysid + * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid */ public static LinearSystem identifyVelocitySystem(double kV, double kA) { if (kV < 0.0) { @@ -292,7 +294,8 @@ public final class LinearSystemId { * @param kA The acceleration gain, in volts/(unit/sec²) * @return A LinearSystem representing the given characterized constants. * @throws IllegalArgumentException if kV < 0 or kA <= 0. - * @see https://github.com/wpilibsuite/sysid + * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid */ public static LinearSystem identifyPositionSystem(double kV, double kA) { if (kV < 0.0) { @@ -325,7 +328,8 @@ public final class LinearSystemId { * @return A LinearSystem representing the given characterized constants. * @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, or * kAAngular <= 0. - * @see https://github.com/wpilibsuite/sysid + * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid */ public static LinearSystem identifyDrivetrainSystem( double kVLinear, double kALinear, double kVAngular, double kAAngular) { @@ -372,7 +376,8 @@ public final class LinearSystemId { * @return A LinearSystem representing the given characterized constants. * @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, * kAAngular <= 0, or trackwidth <= 0. - * @see https://github.com/wpilibsuite/sysid + * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid */ public static LinearSystem identifyDrivetrainSystem( double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) { diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h index f26c8a0376..49ed38bbfb 100644 --- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h +++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h @@ -122,7 +122,7 @@ class WPILIB_DLLEXPORT LinearSystemId { * @param kA The acceleration gain, in volts/(unit/sec²). * @throws std::domain_error if kV < 0 or kA <= 0. * @see https://github.com/wpilibsuite/sysid + * href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid */ template requires std::same_as || @@ -165,7 +165,7 @@ class WPILIB_DLLEXPORT LinearSystemId { * * @throws std::domain_error if kV < 0 or kA <= 0. * @see https://github.com/wpilibsuite/sysid + * href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid */ template requires std::same_as || @@ -208,7 +208,7 @@ class WPILIB_DLLEXPORT LinearSystemId { * @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, * or kAAngular <= 0. * @see https://github.com/wpilibsuite/sysid + * href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid */ static constexpr LinearSystem<2, 2, 2> IdentifyDrivetrainSystem( decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear, @@ -269,7 +269,7 @@ class WPILIB_DLLEXPORT LinearSystemId { * @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, * kAAngular <= 0, or trackwidth <= 0. * @see https://github.com/wpilibsuite/sysid + * href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid */ static constexpr LinearSystem<2, 2, 2> IdentifyDrivetrainSystem( decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear, @@ -346,7 +346,7 @@ class WPILIB_DLLEXPORT LinearSystemId { * @param gearing Gear ratio from motor to output. * @throws std::domain_error if J <= 0 or gearing <= 0. * @see https://github.com/wpilibsuite/sysid + * href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid */ static constexpr LinearSystem<2, 1, 2> DCMotorSystem( DCMotor motor, units::kilogram_square_meter_t J, double gearing) { From 632749e6f36f4eed33dc1f5df4a25a544dd2bae1 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 1 Sep 2025 08:13:46 -0700 Subject: [PATCH 11/15] [build] Upgrade Maven dependencies (#8173) --- WORKSPACE | 12 ++++++------ wpimath/CMakeLists.txt | 30 +++++++++++++++--------------- wpimath/build.gradle | 10 +++++----- wpiutil/CMakeLists.txt | 20 ++++++++++---------- wpiutil/build.gradle | 8 ++++---- 5 files changed, 40 insertions(+), 40 deletions(-) diff --git a/WORKSPACE b/WORKSPACE index eb731acfd2..c3af356395 100644 --- a/WORKSPACE +++ b/WORKSPACE @@ -15,12 +15,12 @@ rules_jvm_external_deps() load("@rules_jvm_external//:defs.bzl", "maven_install") maven_artifacts = [ - "org.ejml:ejml-simple:0.43.1", - "com.fasterxml.jackson.core:jackson-annotations:2.15.2", - "com.fasterxml.jackson.core:jackson-core:2.15.2", - "com.fasterxml.jackson.core:jackson-databind:2.15.2", - "us.hebi.quickbuf:quickbuf-runtime:1.3.3", - "com.google.code.gson:gson:2.10.1", + "org.ejml:ejml-simple:0.44.0", + "com.fasterxml.jackson.core:jackson-annotations:2.19.2", + "com.fasterxml.jackson.core:jackson-core:2.19.2", + "com.fasterxml.jackson.core:jackson-databind:2.19.2", + "us.hebi.quickbuf:quickbuf-runtime:1.4", + "com.google.code.gson:gson:2.13.1", ] maven_install( diff --git a/wpimath/CMakeLists.txt b/wpimath/CMakeLists.txt index 348fef5e99..a36670f502 100644 --- a/wpimath/CMakeLists.txt +++ b/wpimath/CMakeLists.txt @@ -21,39 +21,39 @@ file( if(WITH_JAVA) include(UseJava) - if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/ejml-simple-0.43.1.jar") + if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/ejml-simple-0.44.0.jar") set(BASE_URL "https://search.maven.org/remotecontent?filepath=") set(JAR_ROOT "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml") message(STATUS "Downloading EJML jarfiles...") download_and_check( - "${BASE_URL}org/ejml/ejml-cdense/0.43.1/ejml-cdense-0.43.1.jar" - "${JAR_ROOT}/ejml-cdense-0.43.1.jar" + "${BASE_URL}org/ejml/ejml-cdense/0.44.0/ejml-cdense-0.44.0.jar" + "${JAR_ROOT}/ejml-cdense-0.44.0.jar" ) download_and_check( - "${BASE_URL}org/ejml/ejml-core/0.43.1/ejml-core-0.43.1.jar" - "${JAR_ROOT}/ejml-core-0.43.1.jar" + "${BASE_URL}org/ejml/ejml-core/0.44.0/ejml-core-0.44.0.jar" + "${JAR_ROOT}/ejml-core-0.44.0.jar" ) download_and_check( - "${BASE_URL}org/ejml/ejml-ddense/0.43.1/ejml-ddense-0.43.1.jar" - "${JAR_ROOT}/ejml-ddense-0.43.1.jar" + "${BASE_URL}org/ejml/ejml-ddense/0.44.0/ejml-ddense-0.44.0.jar" + "${JAR_ROOT}/ejml-ddense-0.44.0.jar" ) download_and_check( - "${BASE_URL}org/ejml/ejml-dsparse/0.43.1/ejml-dsparse-0.43.1.jar" - "${JAR_ROOT}/ejml-dsparse-0.43.1.jar" + "${BASE_URL}org/ejml/ejml-dsparse/0.44.0/ejml-dsparse-0.44.0.jar" + "${JAR_ROOT}/ejml-dsparse-0.44.0.jar" ) download_and_check( - "${BASE_URL}org/ejml/ejml-fdense/0.43.1/ejml-fdense-0.43.1.jar" - "${JAR_ROOT}/ejml-fdense-0.43.1.jar" + "${BASE_URL}org/ejml/ejml-fdense/0.44.0/ejml-fdense-0.44.0.jar" + "${JAR_ROOT}/ejml-fdense-0.44.0.jar" ) download_and_check( - "${BASE_URL}org/ejml/ejml-simple/0.43.1/ejml-simple-0.43.1.jar" - "${JAR_ROOT}/ejml-simple-0.43.1.jar" + "${BASE_URL}org/ejml/ejml-simple/0.44.0/ejml-simple-0.44.0.jar" + "${JAR_ROOT}/ejml-simple-0.44.0.jar" ) download_and_check( - "${BASE_URL}org/ejml/ejml-zdense/0.43.1/ejml-zdense-0.43.1.jar" - "${JAR_ROOT}/ejml-zdense-0.43.1.jar" + "${BASE_URL}org/ejml/ejml-zdense/0.44.0/ejml-zdense-0.44.0.jar" + "${JAR_ROOT}/ejml-zdense-0.44.0.jar" ) message(STATUS "All files downloaded.") diff --git a/wpimath/build.gradle b/wpimath/build.gradle index 9cb07b416e..a36af333e8 100644 --- a/wpimath/build.gradle +++ b/wpimath/build.gradle @@ -83,11 +83,11 @@ nativeUtils.exportsConfigs { dependencies { api project(":wpiunits") - api "org.ejml:ejml-simple:0.43.1" - api "com.fasterxml.jackson.core:jackson-annotations:2.15.2" - api "com.fasterxml.jackson.core:jackson-core:2.15.2" - api "com.fasterxml.jackson.core:jackson-databind:2.15.2" - api "us.hebi.quickbuf:quickbuf-runtime:1.3.3" + api "org.ejml:ejml-simple:0.44.0" + api "com.fasterxml.jackson.core:jackson-annotations:2.19.2" + api "com.fasterxml.jackson.core:jackson-core:2.19.2" + api "com.fasterxml.jackson.core:jackson-databind:2.19.2" + api "us.hebi.quickbuf:quickbuf-runtime:1.4" } sourceSets.main.java.srcDir "${projectDir}/src/generated/main/java" diff --git a/wpiutil/CMakeLists.txt b/wpiutil/CMakeLists.txt index 15e0c7b85c..05359d90b6 100644 --- a/wpiutil/CMakeLists.txt +++ b/wpiutil/CMakeLists.txt @@ -16,23 +16,23 @@ file( if(WITH_JAVA) include(UseJava) - if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/jackson-core-2.15.2.jar") + if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/jackson-core-2.19.2.jar") set(BASE_URL "https://search.maven.org/remotecontent?filepath=") set(JAR_ROOT "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson") message(STATUS "Downloading Jackson jarfiles...") download_and_check( - "${BASE_URL}com/fasterxml/jackson/core/jackson-core/2.15.2/jackson-core-2.15.2.jar" - "${JAR_ROOT}/jackson-core-2.15.2.jar" + "${BASE_URL}com/fasterxml/jackson/core/jackson-core/2.19.2/jackson-core-2.19.2.jar" + "${JAR_ROOT}/jackson-core-2.19.2.jar" ) download_and_check( - "${BASE_URL}com/fasterxml/jackson/core/jackson-databind/2.15.2/jackson-databind-2.15.2.jar" - "${JAR_ROOT}/jackson-databind-2.15.2.jar" + "${BASE_URL}com/fasterxml/jackson/core/jackson-databind/2.19.2/jackson-databind-2.19.2.jar" + "${JAR_ROOT}/jackson-databind-2.19.2.jar" ) download_and_check( - "${BASE_URL}com/fasterxml/jackson/core/jackson-annotations/2.15.2/jackson-annotations-2.15.2.jar" - "${JAR_ROOT}/jackson-annotations-2.15.2.jar" + "${BASE_URL}com/fasterxml/jackson/core/jackson-annotations/2.19.2/jackson-annotations-2.19.2.jar" + "${JAR_ROOT}/jackson-annotations-2.19.2.jar" ) message(STATUS "All files downloaded.") @@ -40,14 +40,14 @@ if(WITH_JAVA) file(GLOB JACKSON_JARS ${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar) - if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/quickbuf/quickbuf-runtime-1.3.3.jar") + if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/quickbuf/quickbuf-runtime-1.4.jar") set(BASE_URL "https://search.maven.org/remotecontent?filepath=") set(JAR_ROOT "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/quickbuf") message(STATUS "Downloading Quickbuf jarfile...") download_and_check( - "${BASE_URL}us/hebi/quickbuf/quickbuf-runtime/1.3.3/quickbuf-runtime-1.3.3.jar" - "${JAR_ROOT}/quickbuf-runtime-1.3.3.jar" + "${BASE_URL}us/hebi/quickbuf/quickbuf-runtime/1.4/quickbuf-runtime-1.4.jar" + "${JAR_ROOT}/quickbuf-runtime-1.4.jar" ) message(STATUS "Downloaded.") diff --git a/wpiutil/build.gradle b/wpiutil/build.gradle index b4c0aab333..40a98907a7 100644 --- a/wpiutil/build.gradle +++ b/wpiutil/build.gradle @@ -301,10 +301,10 @@ task runPrintLog(type: JavaExec) { } dependencies { - api "com.fasterxml.jackson.core:jackson-annotations:2.15.2" - api "com.fasterxml.jackson.core:jackson-core:2.15.2" - api "com.fasterxml.jackson.core:jackson-databind:2.15.2" - api 'us.hebi.quickbuf:quickbuf-runtime:1.3.3' + api "com.fasterxml.jackson.core:jackson-annotations:2.19.2" + api "com.fasterxml.jackson.core:jackson-core:2.19.2" + api "com.fasterxml.jackson.core:jackson-databind:2.19.2" + api 'us.hebi.quickbuf:quickbuf-runtime:1.4' printlogImplementation sourceSets.main.output } From 08f11488b0c047a6618b1d6f926d76e0038d9df0 Mon Sep 17 00:00:00 2001 From: Ryan Blue Date: Sun, 7 Sep 2025 09:16:26 -0400 Subject: [PATCH 12/15] [ci] Add 2027 development repo to cleanup task (#8217) --- .github/workflows/aql/wpilib-mvn-development_unused.aql | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.github/workflows/aql/wpilib-mvn-development_unused.aql b/.github/workflows/aql/wpilib-mvn-development_unused.aql index 365094310c..7dc91c9af9 100644 --- a/.github/workflows/aql/wpilib-mvn-development_unused.aql +++ b/.github/workflows/aql/wpilib-mvn-development_unused.aql @@ -3,7 +3,10 @@ { "aql": { "items.find": { - "repo": "wpilib-mvn-development-local", + "$or":[ + { "repo": "wpilib-mvn-development-local" }, + { "repo": "wpilib-mvn-development-2027-local" } + ], "path": { "$nmatch":"*edu/wpi/first/thirdparty*" }, "$or":[ { From 2639e0365b7a48bbe7c90dfe625e92ec9fba8e94 Mon Sep 17 00:00:00 2001 From: Ryan Blue Date: Mon, 8 Sep 2025 00:33:19 -0400 Subject: [PATCH 13/15] [ci] Update sentinel build with Windows FFI changes (#8218) --- .github/workflows/sentinel-build.yml | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/.github/workflows/sentinel-build.yml b/.github/workflows/sentinel-build.yml index 7ab1e7dbee..c059efda2c 100644 --- a/.github/workflows/sentinel-build.yml +++ b/.github/workflows/sentinel-build.yml @@ -103,9 +103,16 @@ jobs: task: "build" outputs: "build/allOutputs" - os: windows-2022 - artifact-name: Win32 + artifact-name: Win32FFI architecture: x86 task: ":ntcoreffi:build" + build-options: "-Pntcoreffibuild \"-Dorg.gradle.jvmargs=-Xmx1096m\"" + outputs: "ntcoreffi/build/outputs" + - os: windows-2022 + artifact-name: Win64FFI + architecture: x64 + task: ":ntcoreffi:build" + build-options: "-Pntcoreffibuild -Pbuildwinarm64" outputs: "ntcoreffi/build/outputs" name: "Build - ${{ matrix.artifact-name }}" runs-on: ${{ matrix.os }} From dbffe6e8ac2443a47d8b63391ca3bc267d5a57dc Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 8 Sep 2025 17:26:22 -0700 Subject: [PATCH 14/15] [wpimath] Add readme (#8209) --- CONTRIBUTING.md | 33 ------------------------ wpimath/README.md | 66 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 66 insertions(+), 33 deletions(-) create mode 100644 wpimath/README.md diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index f04042415e..9ca8d049d2 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -51,39 +51,6 @@ Have an idea to make WPILib better? Here's some steps to go from idea to impleme WPILib uses modified Google style guides for both C++ and Java, which can be found in the [styleguide repository](https://github.com/wpilibsuite/styleguide). Autoformatters are available for many popular editors at https://github.com/google/styleguide. Running wpiformat is required for all contributions and is enforced by our continuous integration system. While the library should be fully formatted according to the styles, additional elements of the style guide were not followed when the library was initially created. All new code should follow the guidelines. If you are looking for some easy ramp-up tasks, finding areas that don't follow the style guide and fixing them is very welcome. -### Math documentation - -When writing math expressions in documentation, use https://www.unicodeit.net/ to convert LaTeX to a Unicode equivalent that's easier to read. Not all expressions will translate (e.g., superscripts of superscripts) so focus on making it readable by someone who isn't familiar with LaTeX. If content on multiple lines needs to be aligned in Doxygen/Javadoc comments (e.g., integration/summation limits, matrices packed with square brackets and superscripts for them), put them in @verbatim/@endverbatim blocks in Doxygen or `
` tags in Javadoc so they render with monospace font.
-
-The LaTeX to Unicode conversions can also be done locally via the unicodeit Python package. To install it, execute:
-```bash
-pip install --user unicodeit
-```
-
-Here's example usage:
-```bash
-$ python -m unicodeit.cli 'x_{k+1} = Ax_k + Bu_k'
-xₖ₊₁ = Axₖ + Buₖ
-```
-
-On Linux, this process can be streamlined further by adding the following Bash function to your .bashrc (requires `wl-clipboard` on Wayland or `xclip` on X11):
-```bash
-# Converts LaTeX to Unicode, prints the result, and copies it to the clipboard
-uc() {
-  if [ $WAYLAND_DISPLAY ]; then
-    python -m unicodeit.cli $@ | tee >(wl-copy -n)
-  else
-    python -m unicodeit.cli $@ | tee >(xclip -sel)
-  fi
-}
-```
-
-Here's example usage:
-```bash
-$ uc 'x_{k+1} = Ax_k + Bu_k'
-xₖ₊₁ = Axₖ + Buₖ
-```
-
 ## Submitting Changes
 
 ### Pull Request Format
diff --git a/wpimath/README.md b/wpimath/README.md
new file mode 100644
index 0000000000..c4de2c78bd
--- /dev/null
+++ b/wpimath/README.md
@@ -0,0 +1,66 @@
+# wpimath
+
+wpimath contains utilities for robot control (feedforward/feedback), state estimation (filters, Kalman and otherwise), 2D/3D geometry, kinematics, trajectory generation, and trajectory optimization.
+
+## Implementation guidelines
+
+A lot of wpimath features directly implement equations from books or papers. The following guidelines make that code easier to maintain and audit for correctness.
+
+### Citations
+
+Cite source books/papers at the top of the function (e.g., `See section 5.6 of "book name".`). If multiple items from a given work are referenced, write a bibliography entry for the work to reference later.
+
+Cite the equation numbers each line of code implements, if applicable. For example, `See equation (#.#) of [1].` where `[1]` is a bibliography reference number.
+
+### Comments
+
+Comment each line of code with its pretty-printed math equivalent.
+```cpp
+// xₖ₊₁ = Axₖ + Buₖ
+x = A * x + B * u;
+```
+
+Link to explanatory material where appropriate to explain background knowledge and/or notation choice.
+
+### Variable naming
+
+Follow established mathematical convention where possible (e.g., use A, B, C, D for state-space notation instead of `stateTransitionMatrix`).
+
+Use math symbols in variable names (see [Unicodeit](#Unicodeit)) to match source papers. This usually entails some Greek letters (α), but diacritics, superscripts, and subscripts need to be spelled out (`ẋ` → `x_dot`, `αₖ²` → `α_k_sq`) since compilers reject them, and the small features make variable names difficult to read.
+
+### Derivations
+
+Put small derivations in comments within the function. Put large derivations in algorithms.md and link to them.
+
+## Unicodeit
+
+When writing math expressions in documentation, use https://www.unicodeit.net/ to convert LaTeX to a Unicode equivalent that's easier to read. Not all expressions will translate (e.g., superscripts of superscripts) so focus on making it readable by someone who isn't familiar with LaTeX. If content on multiple lines needs to be aligned in Doxygen/Javadoc comments (e.g., integration/summation limits, matrices packed with square brackets and superscripts for them), put them in @verbatim/@endverbatim blocks in Doxygen or `
` tags in Javadoc so they render with monospace font.
+
+The LaTeX to Unicode conversions can also be done locally via the unicodeit Python package. To install it, execute:
+```bash
+pip install --user unicodeit
+```
+
+Here's example usage:
+```bash
+$ python -m unicodeit.cli 'x_{k+1} = Ax_k + Bu_k'
+xₖ₊₁ = Axₖ + Buₖ
+```
+
+On Linux, this process can be streamlined further by adding the following Bash function to your .bashrc (requires `wl-clipboard` on Wayland or `xclip` on X11):
+```bash
+# Converts LaTeX to Unicode, prints the result, and copies it to the clipboard
+uc() {
+  if [ $WAYLAND_DISPLAY ]; then
+    python -m unicodeit.cli $@ | tee >(wl-copy -n)
+  else
+    python -m unicodeit.cli $@ | tee >(xclip -sel)
+  fi
+}
+```
+
+Here's example usage:
+```bash
+$ uc 'x_{k+1} = Ax_k + Bu_k'
+xₖ₊₁ = Axₖ + Buₖ
+```

From ee3d55e8488449f7b89e28182df0acdf37f94e17 Mon Sep 17 00:00:00 2001
From: Tyler Veness 
Date: Fri, 19 Sep 2025 16:52:48 -0700
Subject: [PATCH 15/15] [upstream_utils] Upgrade Eigen to latest (#8228)

---
 upstream_utils/eigen.py                       |   8 +-
 .../eigen_patches/0002-Intellisense-fix.patch |   2 +-
 .../thirdparty/eigen/include/Eigen/Core       |  21 +-
 .../include/Eigen/src/Core/CoreEvaluators.h   |  50 +-
 .../eigen/include/Eigen/src/Core/DenseBase.h  |  12 +
 .../include/Eigen/src/Core/DenseCoeffsBase.h  |   8 +-
 .../eigen/include/Eigen/src/Core/Fill.h       |   3 +-
 .../eigen/include/Eigen/src/Core/FindCoeff.h  | 464 ++++++++++++++++++
 .../Eigen/src/Core/GenericPacketMath.h        | 132 +++--
 .../include/Eigen/src/Core/IndexedView.h      |   6 +
 .../include/Eigen/src/Core/MathFunctions.h    |  82 +++-
 .../Eigen/src/Core/MathFunctionsImpl.h        |  48 +-
 .../eigen/include/Eigen/src/Core/NumTraits.h  |  18 +-
 .../Eigen/src/Core/PermutationMatrix.h        |  18 +-
 .../Eigen/src/Core/ProductEvaluators.h        |   4 +-
 .../eigen/include/Eigen/src/Core/RandomImpl.h |   7 +
 .../eigen/include/Eigen/src/Core/RealView.h   | 250 ++++++++++
 .../eigen/include/Eigen/src/Core/Select.h     |  92 +---
 .../Eigen/src/Core/SelfCwiseBinaryOp.h        |  26 +-
 .../include/Eigen/src/Core/VectorwiseOp.h     |  52 +-
 .../eigen/include/Eigen/src/Core/Visitor.h    | 261 ----------
 .../Eigen/src/Core/arch/AVX/PacketMath.h      | 210 ++------
 .../Eigen/src/Core/arch/AVX/Reductions.h      | 353 +++++++++++++
 .../Eigen/src/Core/arch/Default/BFloat16.h    |   6 +
 .../arch/Default/GenericPacketMathFunctions.h |  61 +--
 .../Eigen/src/Core/arch/Default/Half.h        |  66 ++-
 .../Eigen/src/Core/arch/NEON/Complex.h        |  46 +-
 .../Eigen/src/Core/arch/NEON/PacketMath.h     | 406 ++++++---------
 .../Eigen/src/Core/arch/SSE/PacketMath.h      | 234 +--------
 .../Eigen/src/Core/arch/SSE/Reductions.h      | 324 ++++++++++++
 .../Eigen/src/Core/functors/BinaryFunctors.h  |   6 +-
 .../Core/products/SelfadjointMatrixVector.h   |   5 +
 .../src/Core/util/ConfigureVectorization.h    |   2 +-
 .../Eigen/src/Core/util/ForwardDeclarations.h |   4 +-
 .../include/Eigen/src/Core/util/Macros.h      |  21 +-
 .../include/Eigen/src/Core/util/Memory.h      |  42 +-
 .../include/Eigen/src/Geometry/Quaternion.h   |  75 +++
 .../src/SparseCholesky/SimplicialCholesky.h   |  20 +-
 .../SparseCholesky/SimplicialCholesky_impl.h  |   4 +
 .../include/Eigen/src/SparseCore/SparseDot.h  |   6 +-
 .../Eigen/src/SparseCore/SparseMatrix.h       |   6 +-
 .../Eigen/src/SparseCore/SparseMatrixBase.h   |  66 ++-
 .../Eigen/src/SparseCore/TriangularSolver.h   |  20 +-
 43 files changed, 2250 insertions(+), 1297 deletions(-)
 create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/FindCoeff.h
 create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/RealView.h
 create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/Reductions.h
 create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/Reductions.h

diff --git a/upstream_utils/eigen.py b/upstream_utils/eigen.py
index fdd9f580f9..6cb3facfa9 100755
--- a/upstream_utils/eigen.py
+++ b/upstream_utils/eigen.py
@@ -38,6 +38,10 @@ def eigen_inclusions(dp: Path, f: str):
     if "MKL" in f:
         return False
 
+    # Exclude HIP CUDA support
+    if "GpuHip" in f:
+        return False
+
     # Include architectures we care about by filtering for Core/arch
     if "Core" in dp.parts and "arch" in dp.parts:
         return (
@@ -140,8 +144,8 @@ def copy_upstream_src(wpilib_root: Path):
 def main():
     name = "eigen"
     url = "https://gitlab.com/libeigen/eigen.git"
-    # master on 2025-05-18
-    tag = "d81aa18f4dc56264b2cd7e2f230807d776a2d385"
+    # master on 2025-09-08
+    tag = "e0a59e5a66e6d16fa93ab4f5e48bf539205e837f"
 
     eigen = Lib(name, url, tag, copy_upstream_src)
     eigen.main()
diff --git a/upstream_utils/eigen_patches/0002-Intellisense-fix.patch b/upstream_utils/eigen_patches/0002-Intellisense-fix.patch
index 5522d69109..7f2aaeea10 100644
--- a/upstream_utils/eigen_patches/0002-Intellisense-fix.patch
+++ b/upstream_utils/eigen_patches/0002-Intellisense-fix.patch
@@ -8,7 +8,7 @@ Subject: [PATCH 2/2] Intellisense fix
  1 file changed, 7 insertions(+)
 
 diff --git a/Eigen/src/Core/util/ConfigureVectorization.h b/Eigen/src/Core/util/ConfigureVectorization.h
-index 49f307c734e937f013e659e931286a17ef6756f9..a9430716a320327aed81ea0cdffabc051aeb0ce2 100644
+index c2546a083898154a1d4bd741722a5544cbdb1d92..8b5cc16b2092a73804af87ed7f59722ae3fdab0c 100644
 --- a/Eigen/src/Core/util/ConfigureVectorization.h
 +++ b/Eigen/src/Core/util/ConfigureVectorization.h
 @@ -178,6 +178,13 @@
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core
index 69aa1f8e5c..2fedfd3b80 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core
@@ -92,6 +92,7 @@
 #include 
 
 #include 
+#include 
 #include 
 
 // for std::is_nothrow_move_assignable
@@ -102,6 +103,11 @@
 #include 
 #endif
 
+// for std::bit_cast()
+#if defined(__cpp_lib_bit_cast) && __cpp_lib_bit_cast >= 201806L
+#include 
+#endif
+
 // for outputting debug info
 #ifdef EIGEN_DEBUG_ASSIGN
 #include 
@@ -121,7 +127,6 @@
 #undef isfinite
 #include 
 #include 
-#include 
 #include 
 #include 
 #ifndef EIGEN_SYCL_LOCAL_THREAD_DIM0
@@ -194,8 +199,11 @@ using std::ptrdiff_t;
 
 #if defined EIGEN_VECTORIZE_AVX512
 #include "src/Core/arch/SSE/PacketMath.h"
+#include "src/Core/arch/SSE/Reductions.h"
 #include "src/Core/arch/AVX/PacketMath.h"
+#include "src/Core/arch/AVX/Reductions.h"
 // #include "src/Core/arch/AVX512/PacketMath.h"
+// #include "src/Core/arch/AVX512/Reductions.h"
 #if defined EIGEN_VECTORIZE_AVX512FP16
 // #include "src/Core/arch/AVX512/PacketMathFP16.h"
 #endif
@@ -216,21 +224,26 @@ using std::ptrdiff_t;
 #endif
 // #include "src/Core/arch/AVX512/TrsmKernel.h"
 #elif defined EIGEN_VECTORIZE_AVX
-   // Use AVX for floats and doubles, SSE for integers
+// Use AVX for floats and doubles, SSE for integers
 #include "src/Core/arch/SSE/PacketMath.h"
+#include "src/Core/arch/SSE/Reductions.h"
 #include "src/Core/arch/SSE/TypeCasting.h"
 #include "src/Core/arch/SSE/Complex.h"
 #include "src/Core/arch/AVX/PacketMath.h"
+#include "src/Core/arch/AVX/Reductions.h"
 #include "src/Core/arch/AVX/TypeCasting.h"
 #include "src/Core/arch/AVX/Complex.h"
 #include "src/Core/arch/SSE/MathFunctions.h"
 #include "src/Core/arch/AVX/MathFunctions.h"
 #elif defined EIGEN_VECTORIZE_SSE
 #include "src/Core/arch/SSE/PacketMath.h"
+#include "src/Core/arch/SSE/Reductions.h"
 #include "src/Core/arch/SSE/TypeCasting.h"
 #include "src/Core/arch/SSE/MathFunctions.h"
 #include "src/Core/arch/SSE/Complex.h"
-#elif defined(EIGEN_VECTORIZE_ALTIVEC) || defined(EIGEN_VECTORIZE_VSX)
+#endif
+
+#if defined(EIGEN_VECTORIZE_ALTIVEC) || defined(EIGEN_VECTORIZE_VSX)
 // #include "src/Core/arch/AltiVec/PacketMath.h"
 // #include "src/Core/arch/AltiVec/TypeCasting.h"
 // #include "src/Core/arch/AltiVec/MathFunctions.h"
@@ -311,6 +324,7 @@ using std::ptrdiff_t;
 #include "src/Core/Product.h"
 #include "src/Core/CoreEvaluators.h"
 #include "src/Core/AssignEvaluator.h"
+#include "src/Core/RealView.h"
 #include "src/Core/Assign.h"
 
 #include "src/Core/ArrayBase.h"
@@ -350,6 +364,7 @@ using std::ptrdiff_t;
 #include "src/Core/SkewSymmetricMatrix3.h"
 #include "src/Core/Redux.h"
 #include "src/Core/Visitor.h"
+#include "src/Core/FindCoeff.h"
 #include "src/Core/Fuzzy.h"
 #include "src/Core/Swap.h"
 #include "src/Core/CommaInitializer.h"
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h
index 63f1895d2a..c9b2d2d282 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h
@@ -707,7 +707,7 @@ struct unary_evaluator, ArgType>, In
     Index packetOffset = offset * PacketSize;
     Index actualRow = IsRowMajor ? row : row + packetOffset;
     Index actualCol = IsRowMajor ? col + packetOffset : col;
-    eigen_assert(check_array_bounds(actualRow, actualCol, 0, count) && "Array index out of bounds");
+    eigen_assert(check_array_bounds(actualRow, actualCol, begin, count) && "Array index out of bounds");
     return m_argImpl.template packetSegment(actualRow, actualCol, begin, count);
   }
   template 
@@ -715,8 +715,8 @@ struct unary_evaluator, ArgType>, In
                                                                     Index offset) const {
     constexpr int PacketSize = unpacket_traits::size;
     Index packetOffset = offset * PacketSize;
-    Index actualIndex = index + packetOffset + begin;
-    eigen_assert(check_array_bounds(actualIndex, 0, count) && "Array index out of bounds");
+    Index actualIndex = index + packetOffset;
+    eigen_assert(check_array_bounds(actualIndex, begin, count) && "Array index out of bounds");
     return m_argImpl.template packetSegment(actualIndex, begin, count);
   }
 
@@ -1565,50 +1565,6 @@ struct block_evaluator
-struct evaluator>
-    : evaluator_base> {
-  typedef Select XprType;
-  enum {
-    CoeffReadCost = evaluator::CoeffReadCost +
-                    plain_enum_max(evaluator::CoeffReadCost, evaluator::CoeffReadCost),
-
-    Flags = (unsigned int)evaluator::Flags & evaluator::Flags & HereditaryBits,
-
-    Alignment = plain_enum_min(evaluator::Alignment, evaluator::Alignment)
-  };
-
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& select)
-      : m_conditionImpl(select.conditionMatrix()), m_thenImpl(select.thenMatrix()), m_elseImpl(select.elseMatrix()) {
-    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
-  }
-
-  typedef typename XprType::CoeffReturnType CoeffReturnType;
-
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const {
-    if (m_conditionImpl.coeff(row, col))
-      return m_thenImpl.coeff(row, col);
-    else
-      return m_elseImpl.coeff(row, col);
-  }
-
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const {
-    if (m_conditionImpl.coeff(index))
-      return m_thenImpl.coeff(index);
-    else
-      return m_elseImpl.coeff(index);
-  }
-
- protected:
-  evaluator m_conditionImpl;
-  evaluator m_thenImpl;
-  evaluator m_elseImpl;
-};
-
 // -------------------- Replicate --------------------
 
 template 
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h
index 4f6894280e..0333ad167a 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h
@@ -367,7 +367,12 @@ class DenseBase
   EIGEN_DEVICE_FUNC inline bool allFinite() const;
 
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator*=(const Scalar& other);
+  template ::value, typename = std::enable_if_t>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator*=(const RealScalar& other);
+
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator/=(const Scalar& other);
+  template ::value, typename = std::enable_if_t>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator/=(const RealScalar& other);
 
   typedef internal::add_const_on_value_type_t::type> EvalReturnType;
   /** \returns the matrix or vector obtained by evaluating this expression.
@@ -597,6 +602,13 @@ class DenseBase
   inline const_iterator end() const;
   inline const_iterator cend() const;
 
+  using RealViewReturnType = std::conditional_t::IsComplex, RealView, Derived&>;
+  using ConstRealViewReturnType =
+      std::conditional_t::IsComplex, RealView, const Derived&>;
+
+  EIGEN_DEVICE_FUNC RealViewReturnType realView();
+  EIGEN_DEVICE_FUNC ConstRealViewReturnType realView() const;
+
 #define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::DenseBase
 #define EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
 #define EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(COND)
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseCoeffsBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseCoeffsBase.h
index cff104c366..377df574ff 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseCoeffsBase.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseCoeffsBase.h
@@ -45,10 +45,16 @@ class DenseCoeffsBase : public EigenBase {
   // - This is the return type of the coeff() method.
   // - The LvalueBit means exactly that we can offer a coeffRef() method, which means exactly that we can get references
   // to coeffs, which means exactly that we can have coeff() return a const reference (as opposed to returning a value).
+  // - The DirectAccessBit means exactly that the underlying data of coefficients can be directly accessed as a plain
+  // strided array, which means exactly that the underlying data of coefficients does exist in memory, which means
+  // exactly that the coefficients is const-referencable, which means exactly that we can have coeff() return a const
+  // reference. For example, Map have DirectAccessBit but not LvalueBit, so that Map.coeff()
+  // does points to a const Scalar& which exists in memory, while does not allow coeffRef() as it would not provide a
+  // lvalue. Notice that DirectAccessBit and LvalueBit are mutually orthogonal.
   // - The is_arithmetic check is required since "const int", "const double", etc. will cause warnings on some systems
   // while the declaration of "const T", where T is a non arithmetic type does not. Always returning "const Scalar&" is
   // not possible, since the underlying expressions might not offer a valid address the reference could be referring to.
-  typedef std::conditional_t::Flags& LvalueBit), const Scalar&,
+  typedef std::conditional_t::Flags&(LvalueBit | DirectAccessBit)), const Scalar&,
                              std::conditional_t::value, Scalar, const Scalar>>
       CoeffReturnType;
 
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Fill.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Fill.h
index 9d4ecd445a..f40d56db6b 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Fill.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Fill.h
@@ -78,8 +78,9 @@ template 
 struct eigen_fill_impl {
   using Scalar = typename Xpr::Scalar;
   static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Xpr& dst, const Scalar& val) {
+    const Scalar val_copy = val;
     using std::fill_n;
-    fill_n(dst.data(), dst.size(), val);
+    fill_n(dst.data(), dst.size(), val_copy);
   }
   template 
   static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Xpr& dst, const SrcXpr& src) {
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/FindCoeff.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/FindCoeff.h
new file mode 100644
index 0000000000..0102e8af3a
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/FindCoeff.h
@@ -0,0 +1,464 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2025 Charlie Schlosser 
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_FIND_COEFF_H
+#define EIGEN_FIND_COEFF_H
+
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
+
+namespace internal {
+
+template ::IsInteger>
+struct max_coeff_functor {
+  EIGEN_DEVICE_FUNC inline bool compareCoeff(const Scalar& incumbent, const Scalar& candidate) const {
+    return candidate > incumbent;
+  }
+  template 
+  EIGEN_DEVICE_FUNC inline Packet comparePacket(const Packet& incumbent, const Packet& candidate) const {
+    return pcmp_lt(incumbent, candidate);
+  }
+  template 
+  EIGEN_DEVICE_FUNC inline Scalar predux(const Packet& a) const {
+    return predux_max(a);
+  }
+};
+
+template 
+struct max_coeff_functor {
+  EIGEN_DEVICE_FUNC inline Scalar compareCoeff(const Scalar& incumbent, const Scalar& candidate) {
+    return (candidate > incumbent) || ((candidate != candidate) && (incumbent == incumbent));
+  }
+  template 
+  EIGEN_DEVICE_FUNC inline Packet comparePacket(const Packet& incumbent, const Packet& candidate) {
+    return pandnot(pcmp_lt_or_nan(incumbent, candidate), pisnan(incumbent));
+  }
+  template 
+  EIGEN_DEVICE_FUNC inline Scalar predux(const Packet& a) const {
+    return predux_max(a);
+  }
+};
+
+template 
+struct max_coeff_functor {
+  EIGEN_DEVICE_FUNC inline bool compareCoeff(const Scalar& incumbent, const Scalar& candidate) const {
+    return (candidate > incumbent) || ((candidate == candidate) && (incumbent != incumbent));
+  }
+  template 
+  EIGEN_DEVICE_FUNC inline Packet comparePacket(const Packet& incumbent, const Packet& candidate) const {
+    return pandnot(pcmp_lt_or_nan(incumbent, candidate), pisnan(candidate));
+  }
+  template 
+  EIGEN_DEVICE_FUNC inline Scalar predux(const Packet& a) const {
+    return predux_max(a);
+  }
+};
+
+template ::IsInteger>
+struct min_coeff_functor {
+  EIGEN_DEVICE_FUNC inline bool compareCoeff(const Scalar& incumbent, const Scalar& candidate) const {
+    return candidate < incumbent;
+  }
+  template 
+  EIGEN_DEVICE_FUNC inline Packet comparePacket(const Packet& incumbent, const Packet& candidate) const {
+    return pcmp_lt(candidate, incumbent);
+  }
+  template 
+  EIGEN_DEVICE_FUNC inline Scalar predux(const Packet& a) const {
+    return predux_min(a);
+  }
+};
+
+template 
+struct min_coeff_functor {
+  EIGEN_DEVICE_FUNC inline Scalar compareCoeff(const Scalar& incumbent, const Scalar& candidate) {
+    return (candidate < incumbent) || ((candidate != candidate) && (incumbent == incumbent));
+  }
+  template 
+  EIGEN_DEVICE_FUNC inline Packet comparePacket(const Packet& incumbent, const Packet& candidate) {
+    return pandnot(pcmp_lt_or_nan(candidate, incumbent), pisnan(incumbent));
+  }
+  template 
+  EIGEN_DEVICE_FUNC inline Scalar predux(const Packet& a) const {
+    return predux_min(a);
+  }
+};
+
+template 
+struct min_coeff_functor {
+  EIGEN_DEVICE_FUNC inline bool compareCoeff(const Scalar& incumbent, const Scalar& candidate) const {
+    return (candidate < incumbent) || ((candidate == candidate) && (incumbent != incumbent));
+  }
+  template 
+  EIGEN_DEVICE_FUNC inline Packet comparePacket(const Packet& incumbent, const Packet& candidate) const {
+    return pandnot(pcmp_lt_or_nan(candidate, incumbent), pisnan(candidate));
+  }
+  template 
+  EIGEN_DEVICE_FUNC inline Scalar predux(const Packet& a) const {
+    return predux_min(a);
+  }
+};
+
+template 
+struct min_max_traits {
+  static constexpr bool PacketAccess = packet_traits::Vectorizable;
+};
+template 
+struct functor_traits> : min_max_traits {};
+template 
+struct functor_traits> : min_max_traits {};
+
+template 
+struct find_coeff_loop;
+template 
+struct find_coeff_loop {
+  using Scalar = typename Evaluator::Scalar;
+  static EIGEN_DEVICE_FUNC inline void run(const Evaluator& eval, Func& func, Scalar& res, Index& outer, Index& inner) {
+    Index outerSize = eval.outerSize();
+    Index innerSize = eval.innerSize();
+
+    /* initialization performed in calling function */
+    /* result = eval.coeff(0, 0); */
+    /* outer = 0; */
+    /* inner = 0; */
+
+    for (Index j = 0; j < outerSize; j++) {
+      for (Index i = 0; i < innerSize; i++) {
+        Scalar xprCoeff = eval.coeffByOuterInner(j, i);
+        bool newRes = func.compareCoeff(res, xprCoeff);
+        if (newRes) {
+          outer = j;
+          inner = i;
+          res = xprCoeff;
+        }
+      }
+    }
+  }
+};
+template 
+struct find_coeff_loop {
+  using Scalar = typename Evaluator::Scalar;
+  static EIGEN_DEVICE_FUNC inline void run(const Evaluator& eval, Func& func, Scalar& res, Index& index) {
+    Index size = eval.size();
+
+    /* initialization performed in calling function */
+    /* result = eval.coeff(0); */
+    /* index = 0; */
+
+    for (Index k = 0; k < size; k++) {
+      Scalar xprCoeff = eval.coeff(k);
+      bool newRes = func.compareCoeff(res, xprCoeff);
+      if (newRes) {
+        index = k;
+        res = xprCoeff;
+      }
+    }
+  }
+};
+template 
+struct find_coeff_loop {
+  using ScalarImpl = find_coeff_loop;
+  using Scalar = typename Evaluator::Scalar;
+  using Packet = typename Evaluator::Packet;
+  static constexpr int PacketSize = unpacket_traits::size;
+  static EIGEN_DEVICE_FUNC inline void run(const Evaluator& eval, Func& func, Scalar& result, Index& outer,
+                                           Index& inner) {
+    Index outerSize = eval.outerSize();
+    Index innerSize = eval.innerSize();
+    Index packetEnd = numext::round_down(innerSize, PacketSize);
+
+    /* initialization performed in calling function */
+    /* result = eval.coeff(0, 0); */
+    /* outer = 0; */
+    /* inner = 0; */
+
+    bool checkPacket = false;
+
+    for (Index j = 0; j < outerSize; j++) {
+      Packet resultPacket = pset1(result);
+      for (Index i = 0; i < packetEnd; i += PacketSize) {
+        Packet xprPacket = eval.template packetByOuterInner(j, i);
+        if (predux_any(func.comparePacket(resultPacket, xprPacket))) {
+          outer = j;
+          inner = i;
+          result = func.predux(xprPacket);
+          resultPacket = pset1(result);
+          checkPacket = true;
+        }
+      }
+
+      for (Index i = packetEnd; i < innerSize; i++) {
+        Scalar xprCoeff = eval.coeffByOuterInner(j, i);
+        if (func.compareCoeff(result, xprCoeff)) {
+          outer = j;
+          inner = i;
+          result = xprCoeff;
+          checkPacket = false;
+        }
+      }
+    }
+
+    if (checkPacket) {
+      result = eval.coeffByOuterInner(outer, inner);
+      Index i_end = inner + PacketSize;
+      for (Index i = inner; i < i_end; i++) {
+        Scalar xprCoeff = eval.coeffByOuterInner(outer, i);
+        if (func.compareCoeff(result, xprCoeff)) {
+          inner = i;
+          result = xprCoeff;
+        }
+      }
+    }
+  }
+};
+template 
+struct find_coeff_loop {
+  using ScalarImpl = find_coeff_loop;
+  using Scalar = typename Evaluator::Scalar;
+  using Packet = typename Evaluator::Packet;
+  static constexpr int PacketSize = unpacket_traits::size;
+  static constexpr int Alignment = Evaluator::Alignment;
+
+  static EIGEN_DEVICE_FUNC inline void run(const Evaluator& eval, Func& func, Scalar& result, Index& index) {
+    Index size = eval.size();
+    Index packetEnd = numext::round_down(size, PacketSize);
+
+    /* initialization performed in calling function */
+    /* result = eval.coeff(0); */
+    /* index = 0; */
+
+    Packet resultPacket = pset1(result);
+    bool checkPacket = false;
+
+    for (Index k = 0; k < packetEnd; k += PacketSize) {
+      Packet xprPacket = eval.template packet(k);
+      if (predux_any(func.comparePacket(resultPacket, xprPacket))) {
+        index = k;
+        result = func.predux(xprPacket);
+        resultPacket = pset1(result);
+        checkPacket = true;
+      }
+    }
+
+    for (Index k = packetEnd; k < size; k++) {
+      Scalar xprCoeff = eval.coeff(k);
+      if (func.compareCoeff(result, xprCoeff)) {
+        index = k;
+        result = xprCoeff;
+        checkPacket = false;
+      }
+    }
+
+    if (checkPacket) {
+      result = eval.coeff(index);
+      Index k_end = index + PacketSize;
+      for (Index k = index; k < k_end; k++) {
+        Scalar xprCoeff = eval.coeff(k);
+        if (func.compareCoeff(result, xprCoeff)) {
+          index = k;
+          result = xprCoeff;
+        }
+      }
+    }
+  }
+};
+
+template 
+struct find_coeff_evaluator : public evaluator {
+  using Base = evaluator;
+  using Scalar = typename Derived::Scalar;
+  using Packet = typename packet_traits::type;
+  static constexpr int Flags = Base::Flags;
+  static constexpr bool IsRowMajor = bool(Flags & RowMajorBit);
+  EIGEN_DEVICE_FUNC inline find_coeff_evaluator(const Derived& xpr) : Base(xpr), m_xpr(xpr) {}
+
+  EIGEN_DEVICE_FUNC inline Scalar coeffByOuterInner(Index outer, Index inner) const {
+    Index row = IsRowMajor ? outer : inner;
+    Index col = IsRowMajor ? inner : outer;
+    return Base::coeff(row, col);
+  }
+  template 
+  EIGEN_DEVICE_FUNC inline PacketType packetByOuterInner(Index outer, Index inner) const {
+    Index row = IsRowMajor ? outer : inner;
+    Index col = IsRowMajor ? inner : outer;
+    return Base::template packet(row, col);
+  }
+
+  EIGEN_DEVICE_FUNC inline Index innerSize() const { return m_xpr.innerSize(); }
+  EIGEN_DEVICE_FUNC inline Index outerSize() const { return m_xpr.outerSize(); }
+  EIGEN_DEVICE_FUNC inline Index size() const { return m_xpr.size(); }
+
+  const Derived& m_xpr;
+};
+
+template 
+struct find_coeff_impl {
+  using Evaluator = find_coeff_evaluator;
+  static constexpr int Flags = Evaluator::Flags;
+  static constexpr int Alignment = Evaluator::Alignment;
+  static constexpr bool IsRowMajor = Derived::IsRowMajor;
+  static constexpr int MaxInnerSizeAtCompileTime =
+      IsRowMajor ? Derived::MaxColsAtCompileTime : Derived::MaxRowsAtCompileTime;
+  static constexpr int MaxSizeAtCompileTime = Derived::MaxSizeAtCompileTime;
+
+  using Scalar = typename Derived::Scalar;
+  using Packet = typename Evaluator::Packet;
+
+  static constexpr int PacketSize = unpacket_traits::size;
+  static constexpr bool Linearize = bool(Flags & LinearAccessBit);
+  static constexpr bool DontVectorize =
+      enum_lt_not_dynamic(Linearize ? MaxSizeAtCompileTime : MaxInnerSizeAtCompileTime, PacketSize);
+  static constexpr bool Vectorize =
+      !DontVectorize && bool(Flags & PacketAccessBit) && functor_traits::PacketAccess;
+
+  using Loop = find_coeff_loop;
+
+  template  = true>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(const Derived& xpr, Func& func, Scalar& res, Index& outer,
+                                                        Index& inner) {
+    Evaluator eval(xpr);
+    Loop::run(eval, func, res, outer, inner);
+  }
+  template  = true>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(const Derived& xpr, Func& func, Scalar& res, Index& outer,
+                                                        Index& inner) {
+    // where possible, use the linear loop and back-calculate the outer and inner indices
+    Index index = 0;
+    run(xpr, func, res, index);
+    outer = index / xpr.innerSize();
+    inner = index % xpr.innerSize();
+  }
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(const Derived& xpr, Func& func, Scalar& res, Index& index) {
+    Evaluator eval(xpr);
+    Loop::run(eval, func, res, index);
+  }
+};
+
+template 
+EIGEN_DEVICE_FUNC typename internal::traits::Scalar findCoeff(const DenseBase& mat, Func& func,
+                                                                       IndexType* rowPtr, IndexType* colPtr) {
+  eigen_assert(mat.rows() > 0 && mat.cols() > 0 && "you are using an empty matrix");
+  using Scalar = typename DenseBase::Scalar;
+  using FindCoeffImpl = internal::find_coeff_impl;
+  Index outer = 0;
+  Index inner = 0;
+  Scalar res = mat.coeff(0, 0);
+  FindCoeffImpl::run(mat.derived(), func, res, outer, inner);
+  *rowPtr = internal::convert_index(Derived::IsRowMajor ? outer : inner);
+  if (colPtr) *colPtr = internal::convert_index(Derived::IsRowMajor ? inner : outer);
+  return res;
+}
+
+template 
+EIGEN_DEVICE_FUNC typename internal::traits::Scalar findCoeff(const DenseBase& mat, Func& func,
+                                                                       IndexType* indexPtr) {
+  eigen_assert(mat.size() > 0 && "you are using an empty matrix");
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  using Scalar = typename DenseBase::Scalar;
+  using FindCoeffImpl = internal::find_coeff_impl;
+  Index index = 0;
+  Scalar res = mat.coeff(0);
+  FindCoeffImpl::run(mat.derived(), func, res, index);
+  *indexPtr = internal::convert_index(index);
+  return res;
+}
+
+}  // namespace internal
+
+/** \fn DenseBase::minCoeff(IndexType* rowId, IndexType* colId) const
+ * \returns the minimum of all coefficients of *this and puts in *row and *col its location.
+ *
+ * If there are multiple coefficients with the same extreme value, the location of the first instance is returned.
+ *
+ * In case \c *this contains NaN, NaNPropagation determines the behavior:
+ *   NaNPropagation == PropagateFast : undefined
+ *   NaNPropagation == PropagateNaN : result is NaN
+ *   NaNPropagation == PropagateNumbers : result is maximum of elements that are not NaN
+ * \warning the matrix must be not empty, otherwise an assertion is triggered.
+ *
+ * \sa DenseBase::minCoeff(Index*), DenseBase::maxCoeff(Index*,Index*), DenseBase::visit(), DenseBase::minCoeff()
+ */
+template 
+template 
+EIGEN_DEVICE_FUNC typename internal::traits::Scalar DenseBase::minCoeff(IndexType* rowPtr,
+                                                                                          IndexType* colPtr) const {
+  using Func = internal::min_coeff_functor;
+  Func func;
+  return internal::findCoeff(derived(), func, rowPtr, colPtr);
+}
+
+/** \returns the minimum of all coefficients of *this and puts in *index its location.
+ *
+ * If there are multiple coefficients with the same extreme value, the location of the first instance is returned.
+ *
+ * In case \c *this contains NaN, NaNPropagation determines the behavior:
+ *   NaNPropagation == PropagateFast : undefined
+ *   NaNPropagation == PropagateNaN : result is NaN
+ *   NaNPropagation == PropagateNumbers : result is maximum of elements that are not NaN
+ * \warning the matrix must be not empty, otherwise an assertion is triggered.
+ *
+ * \sa DenseBase::minCoeff(IndexType*,IndexType*), DenseBase::maxCoeff(IndexType*,IndexType*), DenseBase::visit(),
+ * DenseBase::minCoeff()
+ */
+template 
+template 
+EIGEN_DEVICE_FUNC typename internal::traits::Scalar DenseBase::minCoeff(IndexType* indexPtr) const {
+  using Func = internal::min_coeff_functor;
+  Func func;
+  return internal::findCoeff(derived(), func, indexPtr);
+}
+
+/** \fn DenseBase::maxCoeff(IndexType* rowId, IndexType* colId) const
+ * \returns the maximum of all coefficients of *this and puts in *row and *col its location.
+ *
+ * If there are multiple coefficients with the same extreme value, the location of the first instance is returned.
+ *
+ * In case \c *this contains NaN, NaNPropagation determines the behavior:
+ *   NaNPropagation == PropagateFast : undefined
+ *   NaNPropagation == PropagateNaN : result is NaN
+ *   NaNPropagation == PropagateNumbers : result is maximum of elements that are not NaN
+ * \warning the matrix must be not empty, otherwise an assertion is triggered.
+ *
+ * \sa DenseBase::minCoeff(IndexType*,IndexType*), DenseBase::visit(), DenseBase::maxCoeff()
+ */
+template 
+template 
+EIGEN_DEVICE_FUNC typename internal::traits::Scalar DenseBase::maxCoeff(IndexType* rowPtr,
+                                                                                          IndexType* colPtr) const {
+  using Func = internal::max_coeff_functor;
+  Func func;
+  return internal::findCoeff(derived(), func, rowPtr, colPtr);
+}
+
+/** \returns the maximum of all coefficients of *this and puts in *index its location.
+ *
+ * If there are multiple coefficients with the same extreme value, the location of the first instance is returned.
+ *
+ * In case \c *this contains NaN, NaNPropagation determines the behavior:
+ *   NaNPropagation == PropagateFast : undefined
+ *   NaNPropagation == PropagateNaN : result is NaN
+ *   NaNPropagation == PropagateNumbers : result is maximum of elements that are not NaN
+ * \warning the matrix must be not empty, otherwise an assertion is triggered.
+ *
+ * \sa DenseBase::maxCoeff(IndexType*,IndexType*), DenseBase::minCoeff(IndexType*,IndexType*), DenseBase::visitor(),
+ * DenseBase::maxCoeff()
+ */
+template 
+template 
+EIGEN_DEVICE_FUNC typename internal::traits::Scalar DenseBase::maxCoeff(IndexType* indexPtr) const {
+  using Func = internal::max_coeff_functor;
+  Func func;
+  return internal::findCoeff(derived(), func, indexPtr);
+}
+
+}  // namespace Eigen
+
+#endif  // EIGEN_FIND_COEFF_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h
index d45cb4bf4a..139b10e8a4 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h
@@ -253,6 +253,12 @@ struct preinterpret_generic {
   static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet run(const Packet& a) { return a; }
 };
 
+template 
+struct preinterpret_generic::as_real, ComplexPacket, false> {
+  using RealPacket = typename unpacket_traits::as_real;
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE RealPacket run(const ComplexPacket& a) { return a.v; }
+};
+
 /** \internal \returns reinterpret_cast(a) */
 template 
 EIGEN_DEVICE_FUNC inline Target preinterpret(const Packet& a) {
@@ -375,7 +381,7 @@ EIGEN_DEVICE_FUNC inline bool pdiv(const bool& a, const bool& b) {
   return a && b;
 }
 
-// In the generic case, memset to all one bits.
+// In the generic packet case, memset to all one bits.
 template 
 struct ptrue_impl {
   static EIGEN_DEVICE_FUNC inline Packet run(const Packet& /*a*/) {
@@ -385,19 +391,16 @@ struct ptrue_impl {
   }
 };
 
+// Use a value of one for scalars.
+template 
+struct ptrue_impl::value>> {
+  static EIGEN_DEVICE_FUNC inline Scalar run(const Scalar&) { return Scalar(1); }
+};
+
 // For booleans, we can only directly set a valid `bool` value to avoid UB.
 template <>
 struct ptrue_impl {
-  static EIGEN_DEVICE_FUNC inline bool run(const bool& /*a*/) { return true; }
-};
-
-// For non-trivial scalars, set to Scalar(1) (i.e. a non-zero value).
-// Although this is technically not a valid bitmask, the scalar path for pselect
-// uses a comparison to zero, so this should still work in most cases. We don't
-// have another option, since the scalar type requires initialization.
-template 
-struct ptrue_impl::value && NumTraits::RequireInitialization>> {
-  static EIGEN_DEVICE_FUNC inline T run(const T& /*a*/) { return T(1); }
+  static EIGEN_DEVICE_FUNC inline bool run(const bool&) { return true; }
 };
 
 /** \internal \returns one bits. */
@@ -406,7 +409,7 @@ EIGEN_DEVICE_FUNC inline Packet ptrue(const Packet& a) {
   return ptrue_impl::run(a);
 }
 
-// In the general case, memset to zero.
+// In the general packet case, memset to zero.
 template 
 struct pzero_impl {
   static EIGEN_DEVICE_FUNC inline Packet run(const Packet& /*a*/) {
@@ -608,7 +611,7 @@ EIGEN_DEVICE_FUNC inline bool pselect(const bool& cond, const bool& a, con
 
 /** \internal \returns the min or of \a a and \a b (coeff-wise)
     If either \a a or \a b are NaN, the result is implementation defined. */
-template 
+template 
 struct pminmax_impl {
   template 
   static EIGEN_DEVICE_FUNC inline Packet run(const Packet& a, const Packet& b, Op op) {
@@ -619,7 +622,7 @@ struct pminmax_impl {
 /** \internal \returns the min or max of \a a and \a b (coeff-wise)
     If either \a a or \a b are NaN, NaN is returned. */
 template <>
-struct pminmax_impl {
+struct pminmax_impl {
   template 
   static EIGEN_DEVICE_FUNC inline Packet run(const Packet& a, const Packet& b, Op op) {
     Packet not_nan_mask_a = pcmp_eq(a, a);
@@ -632,7 +635,7 @@ struct pminmax_impl {
     If both \a a and \a b are NaN, NaN is returned.
     Equivalent to std::fmin(a, b).  */
 template <>
-struct pminmax_impl {
+struct pminmax_impl {
   template 
   static EIGEN_DEVICE_FUNC inline Packet run(const Packet& a, const Packet& b, Op op) {
     Packet not_nan_mask_a = pcmp_eq(a, a);
@@ -641,7 +644,7 @@ struct pminmax_impl {
   }
 };
 
-#define EIGEN_BINARY_OP_NAN_PROPAGATION(Type, Func) [](const Type& a, const Type& b) { return Func(a, b); }
+#define EIGEN_BINARY_OP_NAN_PROPAGATION(Type, Func) [](const Type& aa, const Type& bb) { return Func(aa, bb); }
 
 /** \internal \returns the min of \a a and \a b  (coeff-wise).
     If \a a or \b b is NaN, the return value is implementation defined. */
@@ -654,7 +657,8 @@ EIGEN_DEVICE_FUNC inline Packet pmin(const Packet& a, const Packet& b) {
     NaNPropagation determines the NaN propagation semantics. */
 template 
 EIGEN_DEVICE_FUNC inline Packet pmin(const Packet& a, const Packet& b) {
-  return pminmax_impl::run(a, b, EIGEN_BINARY_OP_NAN_PROPAGATION(Packet, (pmin)));
+  constexpr bool IsInteger = NumTraits::type>::IsInteger;
+  return pminmax_impl::run(a, b, EIGEN_BINARY_OP_NAN_PROPAGATION(Packet, (pmin)));
 }
 
 /** \internal \returns the max of \a a and \a b  (coeff-wise)
@@ -668,7 +672,8 @@ EIGEN_DEVICE_FUNC inline Packet pmax(const Packet& a, const Packet& b) {
     NaNPropagation determines the NaN propagation semantics. */
 template 
 EIGEN_DEVICE_FUNC inline Packet pmax(const Packet& a, const Packet& b) {
-  return pminmax_impl::run(a, b, EIGEN_BINARY_OP_NAN_PROPAGATION(Packet, (pmax)));
+  constexpr bool IsInteger = NumTraits::type>::IsInteger;
+  return pminmax_impl::run(a, b, EIGEN_BINARY_OP_NAN_PROPAGATION(Packet, (pmax)));
 }
 
 /** \internal \returns the absolute value of \a a */
@@ -873,17 +878,29 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet plset(const typename unpacket_trait
   return a;
 }
 
+template 
+struct peven_mask_impl {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet run(const Packet&) {
+    typedef typename unpacket_traits::type Scalar;
+    const size_t n = unpacket_traits::size;
+    EIGEN_ALIGN_TO_BOUNDARY(sizeof(Packet)) Scalar elements[n];
+    for (size_t i = 0; i < n; ++i) {
+      memset(elements + i, ((i & 1) == 0 ? 0xff : 0), sizeof(Scalar));
+    }
+    return ploadu(elements);
+  }
+};
+
+template 
+struct peven_mask_impl::value>> {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run(const Scalar&) { return Scalar(1); }
+};
+
 /** \internal \returns a packet with constant coefficients \a a, e.g.: (x, 0, x, 0),
      where x is the value of all 1-bits. */
 template 
-EIGEN_DEVICE_FUNC inline Packet peven_mask(const Packet& /*a*/) {
-  typedef typename unpacket_traits::type Scalar;
-  const size_t n = unpacket_traits::size;
-  EIGEN_ALIGN_TO_BOUNDARY(sizeof(Packet)) Scalar elements[n];
-  for (size_t i = 0; i < n; ++i) {
-    memset(elements + i, ((i & 1) == 0 ? 0xff : 0), sizeof(Scalar));
-  }
-  return ploadu(elements);
+EIGEN_DEVICE_FUNC inline Packet peven_mask(const Packet& a) {
+  return peven_mask_impl::run(a);
 }
 
 /** \internal copy the packet \a from to \a *to, \a to must be properly aligned */
@@ -1244,26 +1261,46 @@ EIGEN_DEVICE_FUNC inline typename unpacket_traits::type predux_mul(const
 template 
 EIGEN_DEVICE_FUNC inline typename unpacket_traits::type predux_min(const Packet& a) {
   typedef typename unpacket_traits::type Scalar;
-  return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmin)));
+  return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmin)));
 }
 
-template 
-EIGEN_DEVICE_FUNC inline typename unpacket_traits::type predux_min(const Packet& a) {
-  typedef typename unpacket_traits::type Scalar;
-  return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmin)));
-}
-
-/** \internal \returns the min of the elements of \a a */
+/** \internal \returns the max of the elements of \a a */
 template 
 EIGEN_DEVICE_FUNC inline typename unpacket_traits::type predux_max(const Packet& a) {
   typedef typename unpacket_traits::type Scalar;
-  return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmax)));
+  return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmax)));
+}
+
+template 
+struct predux_min_max_helper_impl {
+  using Scalar = typename unpacket_traits::type;
+  static constexpr bool UsePredux_ = NaNPropagation == PropagateFast || NumTraits::IsInteger;
+  template  = true>
+  static EIGEN_DEVICE_FUNC inline Scalar run_min(const Packet& a) {
+    return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmin)));
+  }
+  template  = true>
+  static EIGEN_DEVICE_FUNC inline Scalar run_max(const Packet& a) {
+    return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmax)));
+  }
+  template  = true>
+  static EIGEN_DEVICE_FUNC inline Scalar run_min(const Packet& a) {
+    return predux_min(a);
+  }
+  template  = true>
+  static EIGEN_DEVICE_FUNC inline Scalar run_max(const Packet& a) {
+    return predux_max(a);
+  }
+};
+
+template 
+EIGEN_DEVICE_FUNC inline typename unpacket_traits::type predux_min(const Packet& a) {
+  return predux_min_max_helper_impl::run_min(a);
 }
 
 template 
 EIGEN_DEVICE_FUNC inline typename unpacket_traits::type predux_max(const Packet& a) {
-  typedef typename unpacket_traits::type Scalar;
-  return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmax)));
+  return predux_min_max_helper_impl::run_max(a);
 }
 
 #undef EIGEN_BINARY_OP_NAN_PROPAGATION
@@ -1313,20 +1350,20 @@ struct pmadd_impl {
 template 
 struct pmadd_impl::value && NumTraits::IsSigned>> {
   static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar pmadd(const Scalar& a, const Scalar& b, const Scalar& c) {
-    return numext::fma(a, b, c);
+    return numext::madd(a, b, c);
   }
   static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar pmsub(const Scalar& a, const Scalar& b, const Scalar& c) {
-    return numext::fma(a, b, Scalar(-c));
+    return numext::madd(a, b, Scalar(-c));
   }
   static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar pnmadd(const Scalar& a, const Scalar& b, const Scalar& c) {
-    return numext::fma(Scalar(-a), b, c);
+    return numext::madd(Scalar(-a), b, c);
   }
   static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar pnmsub(const Scalar& a, const Scalar& b, const Scalar& c) {
-    return -Scalar(numext::fma(a, b, c));
+    return -Scalar(numext::madd(a, b, c));
   }
 };
 
-// FMA instructions.
+// Multiply-add instructions.
 /** \internal \returns a * b + c (coeff-wise) */
 template 
 EIGEN_DEVICE_FUNC inline Packet pmadd(const Packet& a, const Packet& b, const Packet& c) {
@@ -1565,9 +1602,10 @@ EIGEN_DEVICE_FUNC inline Packet ploaduSegment(const typename unpacket_traits::type;
   constexpr Index PacketSize = unpacket_traits::size;
   eigen_assert((begin >= 0 && count >= 0 && begin + count <= PacketSize) && "invalid range");
-  Scalar aux[PacketSize];
-  memset(static_cast(aux), 0x00, sizeof(Scalar) * PacketSize);
-  smart_copy(from + begin, from + begin + count, aux + begin);
+  Scalar aux[PacketSize] = {};
+  for (Index k = begin; k < begin + count; k++) {
+    aux[k] = from[k];
+  }
   return ploadu(aux);
 }
 
@@ -1588,7 +1626,9 @@ EIGEN_DEVICE_FUNC inline void pstoreuSegment(Scalar* to, const Packet& from, Ind
   eigen_assert((begin >= 0 && count >= 0 && begin + count <= PacketSize) && "invalid range");
   Scalar aux[PacketSize];
   pstoreu(aux, from);
-  smart_copy(aux + begin, aux + begin + count, to + begin);
+  for (Index k = begin; k < begin + count; k++) {
+    to[k] = aux[k];
+  }
 }
 
 /** \internal copy the packet \a from in the range [begin, begin + count) to \a *to.
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IndexedView.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IndexedView.h
index 358239ca86..10562c1943 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IndexedView.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IndexedView.h
@@ -308,6 +308,12 @@ struct unary_evaluator, IndexBased>
   const XprType& m_xpr;
 };
 
+// Catch assignments to an IndexedView.
+template 
+struct evaluator_assume_aliasing> {
+  static const bool value = true;
+};
+
 }  // end namespace internal
 
 }  // end namespace Eigen
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctions.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctions.h
index 941961d99e..155fdad20e 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctions.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctions.h
@@ -182,10 +182,6 @@ struct imag_ref_retval {
   typedef typename NumTraits::Real& type;
 };
 
-// implementation in MathFunctionsImpl.h
-template ::value>
-struct scalar_select_mask;
-
 }  // namespace internal
 
 namespace numext {
@@ -211,9 +207,9 @@ EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(imag, Scalar) imag(const Scalar&
   return EIGEN_MATHFUNC_IMPL(imag, Scalar)::run(x);
 }
 
-template 
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar select(const Mask& mask, const Scalar& a, const Scalar& b) {
-  return internal::scalar_select_mask::run(mask) ? b : a;
+template 
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar select(const Scalar& mask, const Scalar& a, const Scalar& b) {
+  return numext::is_exactly_zero(mask) ? b : a;
 }
 
 }  // namespace numext
@@ -945,23 +941,43 @@ struct nearest_integer_impl {
   static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_trunc(const Scalar& x) { return x; }
 };
 
+// Extra namespace to prevent leaking std::fma into Eigen::internal.
+namespace has_fma_detail {
+
+template 
+struct has_fma_impl : public std::false_type {};
+
+using std::fma;
+
+template 
+struct has_fma_impl<
+    T, std::enable_if_t(), std::declval(), std::declval()))>::value>>
+    : public std::true_type {};
+
+}  // namespace has_fma_detail
+
+template 
+struct has_fma : public has_fma_detail::has_fma_impl {};
+
 // Default implementation.
-template 
+template 
 struct fma_impl {
-  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run(const Scalar& a, const Scalar& b, const Scalar& c) {
-    return a * b + c;
+  static_assert(has_fma::value, "No function fma(...) for type.  Please provide an implementation.");
+};
+
+// STD or ADL version if it exists.
+template 
+struct fma_impl::value>> {
+  static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T run(const T& a, const T& b, const T& c) {
+    using std::fma;
+    return fma(a, b, c);
   }
 };
 
-// ADL version if it exists.
-template 
-struct fma_impl<
-    T,
-    std::enable_if_t(), std::declval(), std::declval()))>::value>> {
-  static T run(const T& a, const T& b, const T& c) { return fma(a, b, c); }
-};
-
 #if defined(EIGEN_GPUCC)
+template <>
+struct has_fma : public true_type {};
+
 template <>
 struct fma_impl {
   static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE float run(const float& a, const float& b, const float& c) {
@@ -969,6 +985,9 @@ struct fma_impl {
   }
 };
 
+template <>
+struct has_fma : public true_type {};
+
 template <>
 struct fma_impl {
   static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE double run(const double& a, const double& b, const double& c) {
@@ -977,6 +996,24 @@ struct fma_impl {
 };
 #endif
 
+// Basic multiply-add.
+template 
+struct madd_impl {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run(const Scalar& x, const Scalar& y, const Scalar& z) {
+    return x * y + z;
+  }
+};
+
+// Use FMA if there is a single CPU instruction.
+#ifdef EIGEN_VECTORIZE_FMA
+template 
+struct madd_impl::value>> {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run(const Scalar& x, const Scalar& y, const Scalar& z) {
+    return fma_impl::run(x, y, z);
+  }
+};
+#endif
+
 }  // end namespace internal
 
 /****************************************************************************
@@ -1890,15 +1927,18 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar arithmetic_shift_right(const Scalar
   return bit_cast(bit_cast(a) >> n);
 }
 
-// Use std::fma if available.
-using std::fma;
-
 // Otherwise, rely on template implementation.
 template 
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar fma(const Scalar& x, const Scalar& y, const Scalar& z) {
   return internal::fma_impl::run(x, y, z);
 }
 
+// Multiply-add.
+template 
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar madd(const Scalar& x, const Scalar& y, const Scalar& z) {
+  return internal::madd_impl::run(x, y, z);
+}
+
 }  // end namespace numext
 
 namespace internal {
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctionsImpl.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctionsImpl.h
index cbac1c2a4c..c4b5da3cc9 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctionsImpl.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctionsImpl.h
@@ -28,7 +28,7 @@ namespace internal {
    2. If a is zero, approx_a_recip must be infinite with the same sign as a.
    3. If a is infinite, approx_a_recip must be zero with the same sign as a.
 
-   If the preconditions are satisfied, which they are for for the _*_rcp_ps
+   If the preconditions are satisfied, which they are for the _*_rcp_ps
    instructions on x86, the result has a maximum relative error of 2 ulps,
    and correctly handles reciprocals of zero, infinity, and NaN.
 */
@@ -66,7 +66,7 @@ struct generic_reciprocal_newton_step {
    2. If a is zero, approx_a_recip must be infinite with the same sign as a.
    3. If a is infinite, approx_a_recip must be zero with the same sign as a.
 
-   If the preconditions are satisfied, which they are for for the _*_rcp_ps
+   If the preconditions are satisfied, which they are for the _*_rcp_ps
    instructions on x86, the result has a maximum relative error of 2 ulps,
    and correctly handles zero, infinity, and NaN. Positive denormals are
    treated as zero.
@@ -116,7 +116,7 @@ struct generic_rsqrt_newton_step {
    2. If a is zero, approx_rsqrt must be infinite.
    3. If a is infinite, approx_rsqrt must be zero.
 
-   If the preconditions are satisfied, which they are for for the _*_rsqrt_ps
+   If the preconditions are satisfied, which they are for the _*_rsqrt_ps
    instructions on x86, the result has a maximum relative error of 2 ulps,
    and correctly handles zero and infinity, and NaN. Positive denormal inputs
    are treated as zero.
@@ -256,48 +256,6 @@ EIGEN_DEVICE_FUNC ComplexT complex_log(const ComplexT& z) {
   return ComplexT(numext::log(a), b);
 }
 
-// For generic scalars, use ternary select.
-template 
-struct scalar_select_mask {
-  static EIGEN_DEVICE_FUNC inline bool run(const Mask& mask) { return numext::is_exactly_zero(mask); }
-};
-
-// For built-in float mask, bitcast the mask to its integer counterpart and use ternary select.
-template 
-struct scalar_select_mask {
-  using IntegerType = typename numext::get_integer_by_size::unsigned_type;
-  static EIGEN_DEVICE_FUNC inline bool run(const Mask& mask) {
-    return numext::is_exactly_zero(numext::bit_cast(std::abs(mask)));
-  }
-};
-
-template 
-struct ldbl_select_mask {
-  static constexpr int MantissaDigits = std::numeric_limits::digits;
-  static constexpr int NumBytes = (MantissaDigits == 64 ? 80 : 128) / CHAR_BIT;
-  static EIGEN_DEVICE_FUNC inline bool run(const long double& mask) {
-    const uint8_t* mask_bytes = reinterpret_cast(&mask);
-    for (Index i = 0; i < NumBytes; i++) {
-      if (mask_bytes[i] != 0) return false;
-    }
-    return true;
-  }
-};
-
-template <>
-struct ldbl_select_mask : scalar_select_mask {};
-
-template <>
-struct scalar_select_mask : ldbl_select_mask<> {};
-
-template 
-struct scalar_select_mask, false> {
-  using impl = scalar_select_mask;
-  static EIGEN_DEVICE_FUNC inline bool run(const std::complex& mask) {
-    return impl::run(numext::real(mask)) && impl::run(numext::imag(mask));
-  }
-};
-
 }  // end namespace internal
 
 }  // end namespace Eigen
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NumTraits.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NumTraits.h
index 5e4e5c2ff6..bf41c3bb6c 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NumTraits.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NumTraits.h
@@ -95,9 +95,22 @@ struct default_max_digits10_impl  // Integer
 }  // end namespace internal
 
 namespace numext {
-/** \internal bit-wise cast without changing the underlying bit representation. */
 
-// TODO: Replace by std::bit_cast (available in C++20)
+/** \internal bit-wise cast without changing the underlying bit representation. */
+#if defined(__cpp_lib_bit_cast) && __cpp_lib_bit_cast >= 201806L
+template 
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC constexpr Tgt bit_cast(const Src& src) {
+  return std::bit_cast(src);
+}
+#elif EIGEN_HAS_BUILTIN(__builtin_bit_cast)
+template 
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC constexpr Tgt bit_cast(const Src& src) {
+  EIGEN_STATIC_ASSERT(std::is_trivially_copyable::value, THIS_TYPE_IS_NOT_SUPPORTED)
+  EIGEN_STATIC_ASSERT(std::is_trivially_copyable::value, THIS_TYPE_IS_NOT_SUPPORTED)
+  EIGEN_STATIC_ASSERT(sizeof(Src) == sizeof(Tgt), THIS_TYPE_IS_NOT_SUPPORTED)
+  return __builtin_bit_cast(Tgt, src);
+}
+#else
 template 
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Tgt bit_cast(const Src& src) {
   // The behaviour of memcpy is not specified for non-trivially copyable types
@@ -113,6 +126,7 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Tgt bit_cast(const Src& src) {
   memcpy(static_cast(&tgt), static_cast(&staged), sizeof(Tgt));
   return tgt;
 }
+#endif
 }  // namespace numext
 
 // clang-format off
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PermutationMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PermutationMatrix.h
index 4748b118a6..eb8e797820 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PermutationMatrix.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PermutationMatrix.h
@@ -468,17 +468,17 @@ class PermutationWrapper : public PermutationBase
-EIGEN_DEVICE_FUNC const Product operator*(
+EIGEN_DEVICE_FUNC const Product operator*(
     const MatrixBase& matrix, const PermutationBase& permutation) {
-  return Product(matrix.derived(), permutation.derived());
+  return Product(matrix.derived(), permutation.derived());
 }
 
 /** \returns the matrix with the permutation applied to the rows.
  */
 template 
-EIGEN_DEVICE_FUNC const Product operator*(
+EIGEN_DEVICE_FUNC const Product operator*(
     const PermutationBase& permutation, const MatrixBase& matrix) {
-  return Product(permutation.derived(), matrix.derived());
+  return Product(permutation.derived(), matrix.derived());
 }
 
 template 
@@ -520,16 +520,16 @@ class InverseImpl : public EigenBase
-  friend const Product operator*(const MatrixBase& matrix,
-                                                                              const InverseType& trPerm) {
-    return Product(matrix.derived(), trPerm.derived());
+  friend const Product operator*(const MatrixBase& matrix,
+                                                                            const InverseType& trPerm) {
+    return Product(matrix.derived(), trPerm.derived());
   }
 
   /** \returns the matrix with the inverse permutation applied to the rows.
    */
   template 
-  const Product operator*(const MatrixBase& matrix) const {
-    return Product(derived(), matrix.derived());
+  const Product operator*(const MatrixBase& matrix) const {
+    return Product(derived(), matrix.derived());
   }
 };
 
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h
index ce8d954bff..a230044583 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h
@@ -846,7 +846,7 @@ struct generic_product_impl
 
   template 
   static EIGEN_DEVICE_FUNC void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) {
-    selfadjoint_product_impl::run(
+    selfadjoint_product_impl::run(
         dst, lhs.nestedExpression(), rhs, alpha);
   }
 };
@@ -858,7 +858,7 @@ struct generic_product_impl
 
   template 
   static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) {
-    selfadjoint_product_impl::run(
+    selfadjoint_product_impl::run(
         dst, lhs, rhs.nestedExpression(), alpha);
   }
 };
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/RandomImpl.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/RandomImpl.h
index efba33680d..1a82e62536 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/RandomImpl.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/RandomImpl.h
@@ -131,8 +131,15 @@ struct random_longdouble_impl {
     uint64_t randomBits[2];
     long double result = 2.0L;
     memcpy(&randomBits, &result, Size);
+#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
     randomBits[0] |= getRandomBits(numLowBits);
     randomBits[1] |= getRandomBits(numHighBits);
+#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
+    randomBits[0] |= getRandomBits(numHighBits);
+    randomBits[1] |= getRandomBits(numLowBits);
+#else
+#error Unexpected or undefined __BYTE_ORDER__
+#endif
     memcpy(&result, &randomBits, Size);
     result -= 3.0L;
     return result;
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/RealView.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/RealView.h
new file mode 100644
index 0000000000..7ba42f9a1f
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/RealView.h
@@ -0,0 +1,250 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2025 Charlie Schlosser 
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_REALVIEW_H
+#define EIGEN_REALVIEW_H
+
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
+
+namespace internal {
+
+// Vectorized assignment to RealView requires array-oriented access to the real and imaginary components.
+// From https://en.cppreference.com/w/cpp/numeric/complex.html:
+// For any pointer to an element of an array of std::complex named p and any valid array index i,
+// reinterpret_cast(p)[2 * i] is the real part of the complex number p[i], and
+// reinterpret_cast(p)[2 * i + 1] is the imaginary part of the complex number p[i].
+
+template 
+struct complex_array_access : std::false_type {};
+template <>
+struct complex_array_access> : std::true_type {};
+template <>
+struct complex_array_access> : std::true_type {};
+template <>
+struct complex_array_access> : std::true_type {};
+
+template 
+struct traits> : public traits {
+  template 
+  static constexpr int double_size(T size, bool times_two) {
+    int size_as_int = int(size);
+    if (size_as_int == Dynamic) return Dynamic;
+    return times_two ? (2 * size_as_int) : size_as_int;
+  }
+  using Base = traits;
+  using ComplexScalar = typename Base::Scalar;
+  using Scalar = typename NumTraits::Real;
+  static constexpr int ActualDirectAccessBit = complex_array_access::value ? DirectAccessBit : 0;
+  static constexpr int ActualPacketAccessBit = packet_traits::Vectorizable ? PacketAccessBit : 0;
+  static constexpr int FlagMask =
+      ActualDirectAccessBit | ActualPacketAccessBit | HereditaryBits | LinearAccessBit | LvalueBit;
+  static constexpr int BaseFlags = int(evaluator::Flags) | int(Base::Flags);
+  static constexpr int Flags = BaseFlags & FlagMask;
+  static constexpr bool IsRowMajor = Flags & RowMajorBit;
+  static constexpr int RowsAtCompileTime = double_size(Base::RowsAtCompileTime, !IsRowMajor);
+  static constexpr int ColsAtCompileTime = double_size(Base::ColsAtCompileTime, IsRowMajor);
+  static constexpr int SizeAtCompileTime = size_at_compile_time(RowsAtCompileTime, ColsAtCompileTime);
+  static constexpr int MaxRowsAtCompileTime = double_size(Base::MaxRowsAtCompileTime, !IsRowMajor);
+  static constexpr int MaxColsAtCompileTime = double_size(Base::MaxColsAtCompileTime, IsRowMajor);
+  static constexpr int MaxSizeAtCompileTime = size_at_compile_time(MaxRowsAtCompileTime, MaxColsAtCompileTime);
+  static constexpr int OuterStrideAtCompileTime = double_size(outer_stride_at_compile_time::ret, true);
+  static constexpr int InnerStrideAtCompileTime = inner_stride_at_compile_time::ret;
+};
+
+template 
+struct evaluator> : private evaluator {
+  using BaseEvaluator = evaluator;
+  using XprType = RealView;
+  using ExpressionTraits = traits;
+  using ComplexScalar = typename ExpressionTraits::ComplexScalar;
+  using ComplexCoeffReturnType = typename BaseEvaluator::CoeffReturnType;
+  using Scalar = typename ExpressionTraits::Scalar;
+
+  static constexpr bool IsRowMajor = ExpressionTraits::IsRowMajor;
+  static constexpr int Flags = ExpressionTraits::Flags;
+  static constexpr int CoeffReadCost = BaseEvaluator::CoeffReadCost;
+  static constexpr int Alignment = BaseEvaluator::Alignment;
+
+  EIGEN_DEVICE_FUNC explicit evaluator(XprType realView) : BaseEvaluator(realView.m_xpr) {}
+
+  template ::value, typename = std::enable_if_t>
+  constexpr EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar coeff(Index row, Index col) const {
+    ComplexCoeffReturnType cscalar = BaseEvaluator::coeff(IsRowMajor ? row : row / 2, IsRowMajor ? col / 2 : col);
+    Index p = (IsRowMajor ? col : row) & 1;
+    return p ? numext::real(cscalar) : numext::imag(cscalar);
+  }
+
+  template ::value, typename = std::enable_if_t>
+  constexpr EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& coeff(Index row, Index col) const {
+    ComplexCoeffReturnType cscalar = BaseEvaluator::coeff(IsRowMajor ? row : row / 2, IsRowMajor ? col / 2 : col);
+    Index p = (IsRowMajor ? col : row) & 1;
+    return reinterpret_cast(cscalar)[p];
+  }
+
+  constexpr EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) {
+    ComplexScalar& cscalar = BaseEvaluator::coeffRef(IsRowMajor ? row : row / 2, IsRowMajor ? col / 2 : col);
+    Index p = (IsRowMajor ? col : row) & 1;
+    return reinterpret_cast(cscalar)[p];
+  }
+
+  template ::value, typename = std::enable_if_t>
+  constexpr EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar coeff(Index index) const {
+    ComplexCoeffReturnType cscalar = BaseEvaluator::coeff(index / 2);
+    Index p = index & 1;
+    return p ? numext::real(cscalar) : numext::imag(cscalar);
+  }
+
+  template ::value, typename = std::enable_if_t>
+  constexpr EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& coeff(Index index) const {
+    ComplexCoeffReturnType cscalar = BaseEvaluator::coeff(index / 2);
+    Index p = index & 1;
+    return reinterpret_cast(cscalar)[p];
+  }
+
+  constexpr EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) {
+    ComplexScalar& cscalar = BaseEvaluator::coeffRef(index / 2);
+    Index p = index & 1;
+    return reinterpret_cast(cscalar)[p];
+  }
+
+  template 
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const {
+    constexpr int RealPacketSize = unpacket_traits::size;
+    using ComplexPacket = typename find_packet_by_size::type;
+    EIGEN_STATIC_ASSERT((find_packet_by_size::value),
+                        MISSING COMPATIBLE COMPLEX PACKET TYPE)
+    eigen_assert(((IsRowMajor ? col : row) % 2 == 0) && "the inner index must be even");
+
+    Index crow = IsRowMajor ? row : row / 2;
+    Index ccol = IsRowMajor ? col / 2 : col;
+    ComplexPacket cpacket = BaseEvaluator::template packet(crow, ccol);
+    return preinterpret(cpacket);
+  }
+
+  template 
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packet(Index index) const {
+    constexpr int RealPacketSize = unpacket_traits::size;
+    using ComplexPacket = typename find_packet_by_size::type;
+    EIGEN_STATIC_ASSERT((find_packet_by_size::value),
+                        MISSING COMPATIBLE COMPLEX PACKET TYPE)
+    eigen_assert((index % 2 == 0) && "the index must be even");
+
+    Index cindex = index / 2;
+    ComplexPacket cpacket = BaseEvaluator::template packet(cindex);
+    return preinterpret(cpacket);
+  }
+
+  template 
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index row, Index col, Index begin, Index count) const {
+    constexpr int RealPacketSize = unpacket_traits::size;
+    using ComplexPacket = typename find_packet_by_size::type;
+    EIGEN_STATIC_ASSERT((find_packet_by_size::value),
+                        MISSING COMPATIBLE COMPLEX PACKET TYPE)
+    eigen_assert(((IsRowMajor ? col : row) % 2 == 0) && "the inner index must be even");
+    eigen_assert((begin % 2 == 0) && (count % 2 == 0) && "begin and count must be even");
+
+    Index crow = IsRowMajor ? row : row / 2;
+    Index ccol = IsRowMajor ? col / 2 : col;
+    Index cbegin = begin / 2;
+    Index ccount = count / 2;
+    ComplexPacket cpacket = BaseEvaluator::template packetSegment(crow, ccol, cbegin, ccount);
+    return preinterpret(cpacket);
+  }
+
+  template 
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index index, Index begin, Index count) const {
+    constexpr int RealPacketSize = unpacket_traits::size;
+    using ComplexPacket = typename find_packet_by_size::type;
+    EIGEN_STATIC_ASSERT((find_packet_by_size::value),
+                        MISSING COMPATIBLE COMPLEX PACKET TYPE)
+    eigen_assert((index % 2 == 0) && "the index must be even");
+    eigen_assert((begin % 2 == 0) && (count % 2 == 0) && "begin and count must be even");
+
+    Index cindex = index / 2;
+    Index cbegin = begin / 2;
+    Index ccount = count / 2;
+    ComplexPacket cpacket = BaseEvaluator::template packetSegment(cindex, cbegin, ccount);
+    return preinterpret(cpacket);
+  }
+};
+
+}  // namespace internal
+
+template 
+class RealView : public internal::dense_xpr_base>::type {
+  using ExpressionTraits = internal::traits;
+  EIGEN_STATIC_ASSERT(NumTraits::IsComplex, SCALAR MUST BE COMPLEX)
+ public:
+  using Scalar = typename ExpressionTraits::Scalar;
+  using Nested = RealView;
+
+  EIGEN_DEVICE_FUNC explicit RealView(Xpr& xpr) : m_xpr(xpr) {}
+  EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return Xpr::IsRowMajor ? m_xpr.rows() : 2 * m_xpr.rows(); }
+  EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return Xpr::IsRowMajor ? 2 * m_xpr.cols() : m_xpr.cols(); }
+  EIGEN_DEVICE_FUNC constexpr Index size() const noexcept { return 2 * m_xpr.size(); }
+  EIGEN_DEVICE_FUNC constexpr Index innerStride() const noexcept { return m_xpr.innerStride(); }
+  EIGEN_DEVICE_FUNC constexpr Index outerStride() const noexcept { return 2 * m_xpr.outerStride(); }
+  EIGEN_DEVICE_FUNC void resize(Index rows, Index cols) {
+    m_xpr.resize(Xpr::IsRowMajor ? rows : rows / 2, Xpr::IsRowMajor ? cols / 2 : cols);
+  }
+  EIGEN_DEVICE_FUNC void resize(Index size) { m_xpr.resize(size / 2); }
+  EIGEN_DEVICE_FUNC Scalar* data() { return reinterpret_cast(m_xpr.data()); }
+  EIGEN_DEVICE_FUNC const Scalar* data() const { return reinterpret_cast(m_xpr.data()); }
+
+  EIGEN_DEVICE_FUNC RealView(const RealView&) = default;
+
+  EIGEN_DEVICE_FUNC RealView& operator=(const RealView& other);
+
+  template 
+  EIGEN_DEVICE_FUNC RealView& operator=(const RealView& other);
+
+  template 
+  EIGEN_DEVICE_FUNC RealView& operator=(const DenseBase& other);
+
+ protected:
+  friend struct internal::evaluator>;
+  Xpr& m_xpr;
+};
+
+template 
+EIGEN_DEVICE_FUNC RealView& RealView::operator=(const RealView& other) {
+  internal::call_assignment(*this, other);
+  return *this;
+}
+
+template 
+template 
+EIGEN_DEVICE_FUNC RealView& RealView::operator=(const RealView& other) {
+  internal::call_assignment(*this, other);
+  return *this;
+}
+
+template 
+template 
+EIGEN_DEVICE_FUNC RealView& RealView::operator=(const DenseBase& other) {
+  internal::call_assignment(*this, other.derived());
+  return *this;
+}
+
+template 
+EIGEN_DEVICE_FUNC typename DenseBase::RealViewReturnType DenseBase::realView() {
+  return RealViewReturnType(derived());
+}
+
+template 
+EIGEN_DEVICE_FUNC typename DenseBase::ConstRealViewReturnType DenseBase::realView() const {
+  return ConstRealViewReturnType(derived());
+}
+
+}  // namespace Eigen
+
+#endif  // EIGEN_REALVIEW_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Select.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Select.h
index 0fa5f1e178..61a67c2f7a 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Select.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Select.h
@@ -15,7 +15,7 @@
 
 namespace Eigen {
 
-/** \class Select
+/** \typedef Select
  * \ingroup Core_Module
  *
  * \brief Expression of a coefficient wise version of the C++ ternary operator ?:
@@ -24,73 +24,16 @@ namespace Eigen {
  * \tparam ThenMatrixType the type of the \em then expression
  * \tparam ElseMatrixType the type of the \em else expression
  *
- * This class represents an expression of a coefficient wise version of the C++ ternary operator ?:.
+ * This type represents an expression of a coefficient wise version of the C++ ternary operator ?:.
  * It is the return type of DenseBase::select() and most of the time this is the only way it is used.
  *
  * \sa DenseBase::select(const DenseBase&, const DenseBase&) const
  */
-
-namespace internal {
 template 
-struct traits > : traits {
-  typedef typename traits::Scalar Scalar;
-  typedef Dense StorageKind;
-  typedef typename traits::XprKind XprKind;
-  typedef typename ConditionMatrixType::Nested ConditionMatrixNested;
-  typedef typename ThenMatrixType::Nested ThenMatrixNested;
-  typedef typename ElseMatrixType::Nested ElseMatrixNested;
-  enum {
-    RowsAtCompileTime = ConditionMatrixType::RowsAtCompileTime,
-    ColsAtCompileTime = ConditionMatrixType::ColsAtCompileTime,
-    MaxRowsAtCompileTime = ConditionMatrixType::MaxRowsAtCompileTime,
-    MaxColsAtCompileTime = ConditionMatrixType::MaxColsAtCompileTime,
-    Flags = (unsigned int)ThenMatrixType::Flags & ElseMatrixType::Flags & RowMajorBit
-  };
-};
-}  // namespace internal
-
-template 
-class Select : public internal::dense_xpr_base >::type,
-               internal::no_assignment_operator {
- public:
-  typedef typename internal::dense_xpr_base