From aef281f43d6b1092d576593eae437822e63cc5f8 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Sun, 21 Jun 2026 20:20:21 -0700 Subject: [PATCH] [wpilib] Fix DifferentialDrivetrainSimTest (#8934) - Fix a dependency on other test global state via Vin by calling ResetData() to ensure consistent state - Clamp the ground truth voltages to match sim.SetInputs() - The C++ test was checking the noisy measurement output instead of using sim.GetState() - Make GetState() public in C++ and Java --- .../simulation/DifferentialDrivetrainSim.hpp | 26 +++++++++---------- .../simulation/DifferentialDrivetrainSim.yml | 4 +++ .../DifferentialDrivetrainSimTest.cpp | 18 ++++++++----- .../simulation/DifferentialDrivetrainSim.java | 12 ++++++--- .../DifferentialDrivetrainSimTest.java | 10 ++++--- 5 files changed, 44 insertions(+), 26 deletions(-) diff --git a/wpilibc/src/main/native/include/wpi/simulation/DifferentialDrivetrainSim.hpp b/wpilibc/src/main/native/include/wpi/simulation/DifferentialDrivetrainSim.hpp index 8fc0219a31..305fb8e00f 100644 --- a/wpilibc/src/main/native/include/wpi/simulation/DifferentialDrivetrainSim.hpp +++ b/wpilibc/src/main/native/include/wpi/simulation/DifferentialDrivetrainSim.hpp @@ -126,6 +126,19 @@ class DifferentialDrivetrainSim { */ wpi::math::Pose2d GetPose() const; + /** + * Returns the current state vector x. Note that this will not include noise! + */ + wpi::math::Vectord<7> GetState() const; + + /** + * Returns an element of the state vector. Note that this will not include + * noise! + * + * @param state The row of the state vector. + */ + double GetState(int state) const; + /** * Get the right encoder position in meters. * @return The encoder position. @@ -337,19 +350,6 @@ class DifferentialDrivetrainSim { */ wpi::math::Vectord<7> GetOutput() const; - /** - * Returns an element of the state vector. Note that this will not include - * noise! - * - * @param output The row of the output vector. - */ - double GetState(int state) const; - - /** - * Returns the current state vector x. Note that this will not include noise! - */ - wpi::math::Vectord<7> GetState() const; - wpi::math::LinearSystem<2, 2, 2> m_plant; wpi::units::meter_t m_rb; wpi::units::meter_t m_wheelRadius; diff --git a/wpilibc/src/main/python/semiwrap/simulation/DifferentialDrivetrainSim.yml b/wpilibc/src/main/python/semiwrap/simulation/DifferentialDrivetrainSim.yml index cc2bd28432..0d4bd53440 100644 --- a/wpilibc/src/main/python/semiwrap/simulation/DifferentialDrivetrainSim.yml +++ b/wpilibc/src/main/python/semiwrap/simulation/DifferentialDrivetrainSim.yml @@ -41,6 +41,10 @@ classes: param_override: measurementStdDevs: default: std::array{} + GetState: + overloads: + int [const]: + '[const]': wpi::sim::DifferentialDrivetrainSim::State: attributes: X: diff --git a/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp index 68b4bbbbe2..9b0ffd96b6 100644 --- a/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp @@ -14,11 +14,16 @@ #include "wpi/math/system/NumericalIntegration.hpp" #include "wpi/math/trajectory/TrajectoryGenerator.hpp" #include "wpi/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.hpp" +#include "wpi/simulation/RoboRioSim.hpp" #include "wpi/units/current.hpp" #include "wpi/units/math.hpp" #include "wpi/units/moment_of_inertia.hpp" TEST(DifferentialDrivetrainSimTest, Convergence) { + using State = wpi::sim::DifferentialDrivetrainSim::State; + + wpi::sim::RoboRioSim::ResetData(); + auto motor = wpi::math::DCMotor::NEO(2); auto plant = wpi::math::Models::DifferentialDriveFromPhysicalConstants( motor, 50_kg, 2_in, 12_in, 0.5_kg_sq_m, 1.0); @@ -50,10 +55,11 @@ TEST(DifferentialDrivetrainSimTest, Convergence) { auto [l, r] = kinematics.ToWheelVelocities(feedbackOut); auto voltages = feedforward.Calculate(wpi::math::Vectord<2>{l.value(), r.value()}); + auto clampedVoltages = sim.ClampInput(voltages); // Sim periodic code. - sim.SetInputs(wpi::units::volt_t{voltages(0, 0)}, - wpi::units::volt_t{voltages(1, 0)}); + sim.SetInputs(wpi::units::volt_t{clampedVoltages(0, 0)}, + wpi::units::volt_t{clampedVoltages(1, 0)}); sim.Update(20_ms); // Update ground truth. @@ -61,14 +67,14 @@ TEST(DifferentialDrivetrainSimTest, Convergence) { [&sim](const auto& x, const auto& u) -> wpi::math::Vectord<7> { return sim.Dynamics(x, u); }, - groundTruthX, voltages, 20_ms); + groundTruthX, clampedVoltages, 20_ms); } // 2 inch tolerance is OK since our ground truth is an approximation of the // ODE solution using wpi::math::RKDP anyway - EXPECT_NEAR(groundTruthX(0, 0), sim.GetPose().X().value(), 0.05); - EXPECT_NEAR(groundTruthX(1, 0), sim.GetPose().Y().value(), 0.05); - EXPECT_NEAR(groundTruthX(2, 0), sim.GetHeading().Radians().value(), 0.01); + EXPECT_NEAR(groundTruthX(0, 0), sim.GetState(State::X), 0.05); + EXPECT_NEAR(groundTruthX(1, 0), sim.GetState(State::Y), 0.05); + EXPECT_NEAR(groundTruthX(2, 0), sim.GetState(State::HEADING), 0.01); } TEST(DifferentialDrivetrainSimTest, Current) { diff --git a/wpilibj/src/main/java/org/wpilib/simulation/DifferentialDrivetrainSim.java b/wpilibj/src/main/java/org/wpilib/simulation/DifferentialDrivetrainSim.java index a3d3354f3a..fd196e1171 100644 --- a/wpilibj/src/main/java/org/wpilib/simulation/DifferentialDrivetrainSim.java +++ b/wpilibj/src/main/java/org/wpilib/simulation/DifferentialDrivetrainSim.java @@ -150,18 +150,22 @@ public class DifferentialDrivetrainSim { } } - /** Returns the full simulated state of the drivetrain. */ - Matrix getState() { + /** + * Returns the full simulated state of the drivetrain. Note that this will not include noise! + * + * @return The simulated state + */ + public Matrix getState() { return m_x; } /** - * Get one of the drivetrain states. + * Get one of the drivetrain states. Note that this will not include noise! * * @param state the state to get * @return the state */ - double getState(State state) { + public double getState(State state) { return m_x.get(state.value, 0); } diff --git a/wpilibj/src/test/java/org/wpilib/simulation/DifferentialDrivetrainSimTest.java b/wpilibj/src/test/java/org/wpilib/simulation/DifferentialDrivetrainSimTest.java index 852712926c..fc97b198d8 100644 --- a/wpilibj/src/test/java/org/wpilib/simulation/DifferentialDrivetrainSimTest.java +++ b/wpilibj/src/test/java/org/wpilib/simulation/DifferentialDrivetrainSimTest.java @@ -31,6 +31,8 @@ import org.wpilib.math.util.Units; class DifferentialDrivetrainSimTest { @Test void testConvergence() { + RoboRioSim.resetData(); + var motor = DCMotor.getNEO(2); var plant = Models.differentialDriveFromPhysicalConstants( @@ -44,7 +46,7 @@ class DifferentialDrivetrainSimTest { 1, kinematics.trackwidth, Units.inchesToMeters(2), - VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005)); + VecBuilder.fill(0.001, 0.001, 0.0001, 0.1, 0.1, 0.005, 0.005)); var feedforward = new LinearPlantInversionFeedforward<>(plant, 0.020); var feedback = new LTVUnicycleController(0.020); @@ -69,13 +71,15 @@ class DifferentialDrivetrainSimTest { var voltages = feedforward.calculate(VecBuilder.fill(wheelVelocities.left, wheelVelocities.right)); + var clampedVoltages = sim.clampInput(voltages); // Sim periodic code - sim.setInputs(voltages.get(0, 0), voltages.get(1, 0)); + sim.setInputs(clampedVoltages.get(0, 0), clampedVoltages.get(1, 0)); sim.update(0.020); // Update our ground truth - groundTruthX = NumericalIntegration.rkdp(sim::getDynamics, groundTruthX, voltages, 0.020); + groundTruthX = + NumericalIntegration.rkdp(sim::getDynamics, groundTruthX, clampedVoltages, 0.020); } // 2 inch tolerance is OK since our ground truth is an approximation of the