[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

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;

wpilibc/src/main/python/semiwrap/simulation/DifferentialDrivetrainSim.yml

@@ -41,6 +41,10 @@ classes:
             param_override:
               measurementStdDevs:
                 default: std::array<double, 7>{}
+      GetState:
+        overloads:
+          int [const]:
+          '[const]':
   wpi::sim::DifferentialDrivetrainSim::State:
     attributes:
       X:

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) {

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<N7, N1> getState() {
+  /**
+   * Returns the full simulated state of the drivetrain. Note that this will not include noise!
+   *
+   * @return The simulated state
+   */
+  public Matrix<N7, N1> 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);
   }
 

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