[wpilib] LinearSystemSim.setState: calculate new output state (#7799)

Establishes the invariant that the state and measurement always match up, even immediately after construction.

wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h

@@ -52,11 +52,11 @@ class LinearSystemSim {
    * @param dt The time between updates.
    */
   void Update(units::second_t dt) {
-    // Update x. By default, this is the linear system dynamics x_k+1 = Ax_k +
-    // Bu_k
+    // Update x. By default, this is the linear system dynamics xₖ₊₁ = Axₖ +
+    // Buₖ.
     m_x = UpdateX(m_x, m_u, dt);
 
-    // y = Cx + Du
+    // yₖ = Cxₖ + Duₖ
     m_y = m_plant.CalculateY(m_x, m_u);
 
     // Add noise. If the user did not pass a noise vector to the
@@ -115,7 +115,14 @@ class LinearSystemSim {
    *
    * @param state The new state.
    */
-  void SetState(const Vectord<States>& state) { m_x = state; }
+  void SetState(const Vectord<States>& state) {
+    m_x = state;
+
+    // Update the output to reflect the new state.
+    //
+    //   yₖ = Cxₖ + Duₖ
+    m_y = m_plant.CalculateY(m_x, m_u);
+  }
 
  protected:
   /**

wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp

@@ -44,6 +44,15 @@ TEST(ElevatorSimTest, StateSpaceSim) {
   EXPECT_NEAR(controller.GetSetpoint(), sim.GetPosition().value(), 0.2);
 }
 
+TEST(ElevatorSimTest, InitialState) {
+  constexpr auto startingHeight = 0.5_m;
+  frc::sim::ElevatorSim sim(frc::DCMotor::KrakenX60(2), 20, 8_kg, 0.1_m, 0_m,
+                            1_m, true, startingHeight, {0.01, 0.0});
+
+  EXPECT_DOUBLE_EQ(startingHeight.value(), sim.GetPosition().value());
+  EXPECT_DOUBLE_EQ(0, sim.GetVelocity().value());
+}
+
 TEST(ElevatorSimTest, MinMax) {
   frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 14.67, 8_kg, 0.75_in,
                             0_m, 1_m, true, 0_m, {0.01});

wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp

@@ -21,3 +21,12 @@ TEST(SingleJointedArmTest, Disabled) {
   // The arm should swing down.
   EXPECT_NEAR(sim.GetAngle().value(), -std::numbers::pi / 2, 0.01);
 }
+
+TEST(SingleJointedArmTest, InitialState) {
+  constexpr auto startingAngle = 45_deg;
+  frc::sim::SingleJointedArmSim sim(frc::DCMotor::KrakenX60(2), 125, 3_kg_sq_m,
+                                    30_in, 0_deg, 90_deg, true, startingAngle);
+
+  EXPECT_EQ(startingAngle, sim.GetAngle());
+  EXPECT_DOUBLE_EQ(0, sim.GetVelocity().value());
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java

@@ -75,10 +75,10 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
    * @param dtSeconds The time between updates.
    */
   public void update(double dtSeconds) {
-    // Update X. By default, this is the linear system dynamics X = Ax + Bu
+    // Update x. By default, this is the linear system dynamics xₖ₊₁ = Axₖ + Buₖ.
     m_x = updateX(m_x, m_u, dtSeconds);
 
-    // y = cx + du
+    // yₖ = Cxₖ + Duₖ
     m_y = m_plant.calculateY(m_x, m_u);
 
     // Add measurement noise.
@@ -164,6 +164,11 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
    */
   public void setState(Matrix<States, N1> state) {
     m_x = state;
+
+    // Update the output to reflect the new state.
+    //
+    //   yₖ = Cxₖ + Duₖ
+    m_y = m_plant.calculateY(m_x, m_u);
   }
 
   /**

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java

@@ -63,6 +63,17 @@ class ElevatorSimTest {
     }
   }
 
+  @Test
+  void testInitialState() {
+    double startingHeightMeters = 0.5;
+    var sim =
+        new ElevatorSim(
+            DCMotor.getKrakenX60(2), 20, 8.0, 0.1, 0.0, 1.0, true, startingHeightMeters, 0.01, 0.0);
+
+    assertEquals(startingHeightMeters, sim.getPositionMeters());
+    assertEquals(0, sim.getVelocityMetersPerSecond());
+  }
+
   @Test
   void testMinMax() {
     var sim =

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java

@@ -12,28 +12,46 @@ import edu.wpi.first.math.util.Units;
 import org.junit.jupiter.api.Test;
 
 class SingleJointedArmSimTest {
-  SingleJointedArmSim m_sim =
-      new SingleJointedArmSim(
-          DCMotor.getVex775Pro(2),
-          300,
-          3.0,
-          Units.inchesToMeters(30.0),
-          -Math.PI,
-          0.0,
-          true,
-          Math.PI / 2.0);
-
   @Test
   void testArmDisabled() {
+    SingleJointedArmSim sim =
+        new SingleJointedArmSim(
+            DCMotor.getVex775Pro(2),
+            300,
+            3.0,
+            Units.inchesToMeters(30.0),
+            -Math.PI,
+            0.0,
+            true,
+            Math.PI / 2.0);
+
     // Reset Arm angle to 0
-    m_sim.setState(VecBuilder.fill(0.0, 0.0));
+    sim.setState(VecBuilder.fill(0.0, 0.0));
 
     for (int i = 0; i < 12 / 0.02; i++) {
-      m_sim.setInput(0.0);
-      m_sim.update(0.020);
+      sim.setInput(0.0);
+      sim.update(0.020);
     }
 
     // the arm should swing down
-    assertEquals(-Math.PI / 2.0, m_sim.getAngleRads(), 0.1);
+    assertEquals(-Math.PI / 2.0, sim.getAngleRads(), 0.1);
+  }
+
+  @Test
+  void testInitialState() {
+    double startingAngleRads = Math.PI / 4.0;
+    SingleJointedArmSim sim =
+        new SingleJointedArmSim(
+            DCMotor.getKrakenX60(2),
+            125,
+            3.0,
+            Units.inchesToMeters(30.0),
+            0,
+            Math.PI / 2.0,
+            true,
+            startingAngleRads);
+
+    assertEquals(startingAngleRads, sim.getAngleRads());
+    assertEquals(0, sim.getVelocityRadPerSec());
   }
 }