From c898853b4dbe8cbda153d91a109c0c64e9c79679 Mon Sep 17 00:00:00 2001 From: HarryXChen <51322624+HarryXChen3@users.noreply.github.com> Date: Wed, 19 Feb 2025 01:49:28 -0500 Subject: [PATCH] [wpilib] LinearSystemSim.setState: calculate new output state (#7799) Establishes the invariant that the state and measurement always match up, even immediately after construction. --- .../include/frc/simulation/LinearSystemSim.h | 15 ++++-- .../native/cpp/simulation/ElevatorSimTest.cpp | 9 ++++ .../simulation/SingleJointedArmSimTest.cpp | 9 ++++ .../wpilibj/simulation/LinearSystemSim.java | 9 +++- .../wpilibj/simulation/ElevatorSimTest.java | 11 +++++ .../simulation/SingleJointedArmSimTest.java | 48 +++++++++++++------ 6 files changed, 80 insertions(+), 21 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h index 788d5ceeed..8747a0d18c 100644 --- a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h +++ b/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& state) { m_x = state; } + void SetState(const Vectord& 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: /** diff --git a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp index dd835650cd..2777ee48c0 100644 --- a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp +++ b/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}); diff --git a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp index 1311a0ebf6..3f6a1446ce 100644 --- a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp +++ b/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()); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java index ec296fde3d..e277902530 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java @@ -75,10 +75,10 @@ public class LinearSystemSim state) { m_x = state; + + // Update the output to reflect the new state. + // + // yₖ = Cxₖ + Duₖ + m_y = m_plant.calculateY(m_x, m_u); } /** diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java index deade245fa..99ffbcd976 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java +++ b/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 = diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java index 928fd3edc8..4c7a876592 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java +++ b/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()); } }