From 7fc17811fabffc6064771fa0e619e2a3789609e6 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Wed, 15 May 2024 09:23:22 -0400 Subject: [PATCH] [wpimath] Add full state support to LinearSystemId functions (#6554) Co-authored-by: Tyler Veness --- .../main/native/cpp/simulation/DCMotorSim.cpp | 2 +- .../native/cpp/simulation/ElevatorSim.cpp | 8 +- .../native/cpp/simulation/FlywheelSim.cpp | 2 +- .../cpp/simulation/SingleJointedArmSim.cpp | 8 +- .../include/frc/simulation/ElevatorSim.h | 10 +- .../frc/simulation/SingleJointedArmSim.h | 10 +- .../native/cpp/simulation/ElevatorSimTest.cpp | 6 +- .../cpp/examples/StateSpaceArm/cpp/Robot.cpp | 3 +- .../examples/StateSpaceElevator/cpp/Robot.cpp | 3 +- .../first/wpilibj/simulation/DCMotorSim.java | 2 +- .../first/wpilibj/simulation/ElevatorSim.java | 17 +- .../simulation/SingleJointedArmSim.java | 14 +- .../wpilibj/simulation/ElevatorSimTest.java | 4 +- .../armsimulation/subsystems/Arm.java | 3 +- .../subsystems/Elevator.java | 2 +- .../subsystems/Elevator.java | 2 +- .../wpilibj/examples/statespacearm/Robot.java | 13 +- .../examples/statespaceelevator/Robot.java | 17 +- .../wpi/first/math/system/LinearSystem.java | 159 ++++++++++++++++++ .../math/system/plant/LinearSystemId.java | 18 +- .../cpp/system/plant/LinearSystemId.cpp | 16 +- .../native/include/frc/system/LinearSystem.h | 54 ++++++ .../include/frc/system/plant/LinearSystemId.h | 4 +- .../math/controller/LinearSystemLoopTest.java | 26 ++- .../math/estimator/KalmanFilterTest.java | 8 +- .../first/math/system/LinearSystemIDTest.java | 4 +- .../src/test/native/cpp/StateSpaceTest.cpp | 2 +- .../LinearQuadraticRegulatorTest.cpp | 11 +- .../native/cpp/system/LinearSystemIDTest.cpp | 3 +- 29 files changed, 343 insertions(+), 88 deletions(-) diff --git a/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp index c0448ead59..7023a218d4 100644 --- a/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp @@ -41,7 +41,7 @@ units::ampere_t DCMotorSim::GetCurrentDraw() const { // I = V / R - omega / (Kv * R) // Reductions are greater than 1, so a reduction of 10:1 would mean the motor // is spinning 10x faster than the output. - return m_gearbox.Current(GetAngularVelocity() * m_gearing, + return m_gearbox.Current(units::radians_per_second_t{m_x(1)} * m_gearing, units::volt_t{m_u(0)}) * wpi::sgn(m_u(0)); } diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 0281730214..911d828171 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -12,11 +12,11 @@ using namespace frc; using namespace frc::sim; -ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant, +ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 2>& plant, const DCMotor& gearbox, units::meter_t minHeight, units::meter_t maxHeight, bool simulateGravity, units::meter_t startingHeight, - const std::array& measurementStdDevs) + const std::array& measurementStdDevs) : LinearSystemSim(plant, measurementStdDevs), m_gearbox(gearbox), m_minHeight(minHeight), @@ -30,7 +30,7 @@ ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing, units::meter_t drumRadius, units::meter_t minHeight, units::meter_t maxHeight, bool simulateGravity, units::meter_t startingHeight, - const std::array& measurementStdDevs) + const std::array& measurementStdDevs) : ElevatorSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass, drumRadius, gearing), gearbox, minHeight, maxHeight, simulateGravity, @@ -44,7 +44,7 @@ ElevatorSim::ElevatorSim(decltype(1_V / Velocity_t(1)) kV, const DCMotor& gearbox, units::meter_t minHeight, units::meter_t maxHeight, bool simulateGravity, units::meter_t startingHeight, - const std::array& measurementStdDevs) + const std::array& measurementStdDevs) : ElevatorSim(LinearSystemId::IdentifyPositionSystem(kV, kA), gearbox, minHeight, maxHeight, simulateGravity, startingHeight, measurementStdDevs) {} diff --git a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp index f4b9a81556..1eb529b3c8 100644 --- a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp @@ -36,7 +36,7 @@ units::ampere_t FlywheelSim::GetCurrentDraw() const { // I = V / R - omega / (Kv * R) // Reductions are greater than 1, so a reduction of 10:1 would mean the motor // is spinning 10x faster than the output. - return m_gearbox.Current(GetAngularVelocity() * m_gearing, + return m_gearbox.Current(units::radians_per_second_t{m_x(0)} * m_gearing, units::volt_t{m_u(0)}) * wpi::sgn(m_u(0)); } diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp index d9969ca15a..ab68cbcc70 100644 --- a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp @@ -16,12 +16,12 @@ using namespace frc; using namespace frc::sim; SingleJointedArmSim::SingleJointedArmSim( - const LinearSystem<2, 1, 1>& system, const DCMotor& gearbox, double gearing, + const LinearSystem<2, 1, 2>& system, const DCMotor& gearbox, double gearing, units::meter_t armLength, units::radian_t minAngle, units::radian_t maxAngle, bool simulateGravity, units::radian_t startingAngle, - const std::array& measurementStdDevs) - : LinearSystemSim<2, 1, 1>(system, measurementStdDevs), + const std::array& measurementStdDevs) + : LinearSystemSim<2, 1, 2>(system, measurementStdDevs), m_armLen(armLength), m_minAngle(minAngle), m_maxAngle(maxAngle), @@ -36,7 +36,7 @@ SingleJointedArmSim::SingleJointedArmSim( units::meter_t armLength, units::radian_t minAngle, units::radian_t maxAngle, bool simulateGravity, units::radian_t startingAngle, - const std::array& measurementStdDevs) + const std::array& measurementStdDevs) : SingleJointedArmSim( LinearSystemId::SingleJointedArmSystem(gearbox, moi, gearing), gearbox, gearing, armLength, minAngle, maxAngle, simulateGravity, diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h index 3d5c43398e..fd883bb295 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h @@ -17,7 +17,7 @@ namespace frc::sim { /** * Represents a simulated elevator mechanism. */ -class ElevatorSim : public LinearSystemSim<2, 1, 1> { +class ElevatorSim : public LinearSystemSim<2, 1, 2> { public: template using Velocity_t = units::unit_t< @@ -42,10 +42,10 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { * @param startingHeight The starting height of the elevator. * @param measurementStdDevs The standard deviation of the measurements. */ - ElevatorSim(const LinearSystem<2, 1, 1>& plant, const DCMotor& gearbox, + ElevatorSim(const LinearSystem<2, 1, 2>& plant, const DCMotor& gearbox, units::meter_t minHeight, units::meter_t maxHeight, bool simulateGravity, units::meter_t startingHeight, - const std::array& measurementStdDevs = {0.0}); + const std::array& measurementStdDevs = {0.0, 0.0}); /** * Constructs a simulated elevator mechanism. @@ -67,7 +67,7 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { units::kilogram_t carriageMass, units::meter_t drumRadius, units::meter_t minHeight, units::meter_t maxHeight, bool simulateGravity, units::meter_t startingHeight, - const std::array& measurementStdDevs = {0.0}); + const std::array& measurementStdDevs = {0.0, 0.0}); /** * Constructs a simulated elevator mechanism. @@ -90,7 +90,7 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { const DCMotor& gearbox, units::meter_t minHeight, units::meter_t maxHeight, bool simulateGravity, units::meter_t startingHeight, - const std::array& measurementStdDevs = {0.0}); + const std::array& measurementStdDevs = {0.0, 0.0}); using LinearSystemSim::SetState; /** diff --git a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h index 12a719b55e..2476cea67c 100644 --- a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h @@ -18,7 +18,7 @@ namespace frc::sim { /** * Represents a simulated arm mechanism. */ -class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { +class SingleJointedArmSim : public LinearSystemSim<2, 1, 2> { public: /** * Creates a simulated arm mechanism. @@ -36,12 +36,13 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { * @param startingAngle The initial position of the arm. * @param measurementStdDevs The standard deviations of the measurements. */ - SingleJointedArmSim(const LinearSystem<2, 1, 1>& system, + SingleJointedArmSim(const LinearSystem<2, 1, 2>& system, const DCMotor& gearbox, double gearing, units::meter_t armLength, units::radian_t minAngle, units::radian_t maxAngle, bool simulateGravity, units::radian_t startingAngle, - const std::array& measurementStdDevs = {0.0}); + const std::array& measurementStdDevs = {0.0, + 0.0}); /** * Creates a simulated arm mechanism. * @@ -62,7 +63,8 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { units::meter_t armLength, units::radian_t minAngle, units::radian_t maxAngle, bool simulateGravity, units::radian_t startingAngle, - const std::array& measurementStdDevs = {0.0}); + const std::array& measurementStdDevs = {0.0, + 0.0}); using LinearSystemSim::SetState; diff --git a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp index 28777bdbff..dd835650cd 100644 --- a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp @@ -74,8 +74,10 @@ TEST(ElevatorSimTest, Stability) { sim.Update(20_ms); } - frc::LinearSystem<2, 1, 1> system = frc::LinearSystemId::ElevatorSystem( - frc::DCMotor::Vex775Pro(4), 4_kg, 0.5_in, 100); + frc::LinearSystem<2, 1, 1> system = + frc::LinearSystemId::ElevatorSystem(frc::DCMotor::Vex775Pro(4), 4_kg, + 0.5_in, 100) + .Slice(0); EXPECT_NEAR_UNITS( units::meter_t{system.CalculateX(frc::Vectord<2>{0.0, 0.0}, frc::Vectord<1>{12.0}, 20_ms * 50)(0)}, diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp index 38e9262613..9b91e205c6 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp @@ -47,7 +47,8 @@ class Robot : public frc::TimedRobot { // Outputs (what we can measure): [position], in radians. frc::LinearSystem<2, 1, 1> m_armPlant = frc::LinearSystemId::SingleJointedArmSystem(frc::DCMotor::NEO(2), kArmMOI, - kArmGearing); + kArmGearing) + .Slice(0); // The observer fuses our encoder data and voltage inputs to reject noise. frc::KalmanFilter<2, 1, 1> m_observer{ diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp index ceadd8392b..7b982b581b 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp @@ -45,7 +45,8 @@ class Robot : public frc::TimedRobot { // Outputs (what we can measure): [position], in meters. frc::LinearSystem<2, 1, 1> m_elevatorPlant = frc::LinearSystemId::ElevatorSystem(frc::DCMotor::NEO(2), kCarriageMass, - kDrumRadius, kGearRatio); + kDrumRadius, kGearRatio) + .Slice(0); // The observer fuses our encoder data and voltage inputs to reject noise. frc::KalmanFilter<2, 1, 1> m_observer{ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java index 5561fc1057..67962a9626 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java @@ -141,7 +141,7 @@ public class DCMotorSim extends LinearSystemSim { // I = V / R - omega / (Kv * R) // Reductions are output over input, so a reduction of 2:1 means the motor is spinning // 2x faster than the output - return m_gearbox.getCurrent(getAngularVelocityRadPerSec() * m_gearing, m_u.get(0, 0)) + return m_gearbox.getCurrent(m_x.get(1, 0) * m_gearing, m_u.get(0, 0)) * Math.signum(m_u.get(0, 0)); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index 6ec8002730..2f3bf83e97 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -15,7 +15,7 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; /** Represents a simulated elevator mechanism. */ -public class ElevatorSim extends LinearSystemSim { +public class ElevatorSim extends LinearSystemSim { // Gearbox for the elevator. private final DCMotor m_gearbox; @@ -43,13 +43,13 @@ public class ElevatorSim extends LinearSystemSim { */ @SuppressWarnings("this-escape") public ElevatorSim( - LinearSystem plant, + LinearSystem plant, DCMotor gearbox, double minHeightMeters, double maxHeightMeters, boolean simulateGravity, double startingHeightMeters, - Matrix measurementStdDevs) { + Matrix measurementStdDevs) { super(plant, measurementStdDevs); m_gearbox = gearbox; m_minHeight = minHeightMeters; @@ -72,7 +72,7 @@ public class ElevatorSim extends LinearSystemSim { * @param simulateGravity Whether gravity should be simulated or not. */ public ElevatorSim( - LinearSystem plant, + LinearSystem plant, DCMotor gearbox, double minHeightMeters, double maxHeightMeters, @@ -138,7 +138,7 @@ public class ElevatorSim extends LinearSystemSim { double maxHeightMeters, boolean simulateGravity, double startingHeightMeters, - Matrix measurementStdDevs) { + Matrix measurementStdDevs) { this( LinearSystemId.identifyPositionSystem(kV, kA), gearbox, @@ -171,7 +171,7 @@ public class ElevatorSim extends LinearSystemSim { double maxHeightMeters, boolean simulateGravity, double startingHeightMeters, - Matrix measurementStdDevs) { + Matrix measurementStdDevs) { this( LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing), gearbox, @@ -281,7 +281,7 @@ public class ElevatorSim extends LinearSystemSim { * @return The velocity of the elevator. */ public double getVelocityMetersPerSecond() { - return m_x.get(1, 0); + return getOutput(1); } /** @@ -297,8 +297,7 @@ public class ElevatorSim extends LinearSystemSim { // v = r w, so w = v/r double kA = 1 / m_plant.getB().get(1, 0); double kV = -m_plant.getA().get(1, 1) * kA; - double motorVelocityRadPerSec = - getVelocityMetersPerSecond() * kV * m_gearbox.KvRadPerSecPerVolt; + double motorVelocityRadPerSec = m_x.get(1, 0) * kV * m_gearbox.KvRadPerSecPerVolt; var appliedVoltage = m_u.get(0, 0); return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage) * Math.signum(appliedVoltage); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java index 46e04411e1..b2f155771d 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java @@ -15,7 +15,7 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; /** Represents a simulated single jointed arm mechanism. */ -public class SingleJointedArmSim extends LinearSystemSim { +public class SingleJointedArmSim extends LinearSystemSim { // The gearbox for the arm. private final DCMotor m_gearbox; @@ -51,7 +51,7 @@ public class SingleJointedArmSim extends LinearSystemSim { */ @SuppressWarnings("this-escape") public SingleJointedArmSim( - LinearSystem plant, + LinearSystem plant, DCMotor gearbox, double gearing, double armLengthMeters, @@ -59,7 +59,7 @@ public class SingleJointedArmSim extends LinearSystemSim { double maxAngleRads, boolean simulateGravity, double startingAngleRads, - Matrix measurementStdDevs) { + Matrix measurementStdDevs) { super(plant, measurementStdDevs); m_gearbox = gearbox; m_gearing = gearing; @@ -86,7 +86,7 @@ public class SingleJointedArmSim extends LinearSystemSim { * @param startingAngleRads The initial position of the Arm simulation in radians. */ public SingleJointedArmSim( - LinearSystem plant, + LinearSystem plant, DCMotor gearbox, double gearing, double armLengthMeters, @@ -161,7 +161,7 @@ public class SingleJointedArmSim extends LinearSystemSim { double maxAngleRads, boolean simulateGravity, double startingAngleRads, - Matrix measurementStdDevs) { + Matrix measurementStdDevs) { this( LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing), gearbox, @@ -230,7 +230,7 @@ public class SingleJointedArmSim extends LinearSystemSim { * @return The current arm angle. */ public double getAngleRads() { - return m_y.get(0, 0); + return getOutput(0); } /** @@ -239,7 +239,7 @@ public class SingleJointedArmSim extends LinearSystemSim { * @return The current arm velocity. */ public double getVelocityRadPerSec() { - return m_x.get(1, 0); + return getOutput(1); } /** 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 47eb9ae259..73a28100ce 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 @@ -35,7 +35,7 @@ class ElevatorSimTest { 3.0, true, 0.0, - VecBuilder.fill(0.01)); + VecBuilder.fill(0.01, 0.00)); try (var motor = new PWMVictorSPX(0); var encoder = new Encoder(0, 1)) { @@ -74,7 +74,7 @@ class ElevatorSimTest { 1.0, true, 0.0, - VecBuilder.fill(0.01)); + VecBuilder.fill(0.01, 0.00)); for (int i = 0; i < 100; i++) { sim.setInput(VecBuilder.fill(0)); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java index 3cce47d58c..fcf53d5f50 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java @@ -51,7 +51,8 @@ public class Arm implements AutoCloseable { Constants.kMaxAngleRads, true, 0, - VecBuilder.fill(Constants.kArmEncoderDistPerPulse) // Add noise with a std-dev of 1 tick + VecBuilder.fill( + Constants.kArmEncoderDistPerPulse, 0.0) // Add noise with a std-dev of 1 tick ); private final EncoderSim m_encoderSim = new EncoderSim(m_encoder); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java index 6d176e8c35..66fb6ec58d 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java @@ -60,7 +60,7 @@ public class Elevator implements AutoCloseable { Constants.kMaxElevatorHeightMeters, true, 0, - VecBuilder.fill(0.005)); + VecBuilder.fill(0.005, 0.0)); private final EncoderSim m_encoderSim = new EncoderSim(m_encoder); private final PWMSim m_motorSim = new PWMSim(m_motor); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java index 1c9d876764..8c0b0f6bbc 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java @@ -55,7 +55,7 @@ public class Elevator implements AutoCloseable { Constants.kMaxElevatorHeightMeters, true, 0, - VecBuilder.fill(0.01)); + VecBuilder.fill(0.01, 0.0)); private final EncoderSim m_encoderSim = new EncoderSim(m_encoder); private final PWMSim m_motorSim = new PWMSim(m_motor); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java index 3f91c93ab2..b031430517 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java @@ -52,15 +52,16 @@ public class Robot extends TimedRobot { // States: [position, velocity], in radians and radians per second. // Inputs (what we can "put in"): [voltage], in volts. // Outputs (what we can measure): [position], in radians. - private final LinearSystem m_armPlant = + private final LinearSystem m_armPlant = LinearSystemId.createSingleJointedArmSystem(DCMotor.getNEO(2), kArmMOI, kArmGearing); // The observer fuses our encoder data and voltage inputs to reject noise. + @SuppressWarnings("unchecked") private final KalmanFilter m_observer = new KalmanFilter<>( Nat.N2(), Nat.N1(), - m_armPlant, + (LinearSystem) m_armPlant.slice(0), VecBuilder.fill(0.015, 0.17), // How accurate we // think our model is, in radians and radians/sec VecBuilder.fill(0.01), // How accurate we think our encoder position @@ -68,9 +69,10 @@ public class Robot extends TimedRobot { 0.020); // A LQR uses feedback to create voltage commands. + @SuppressWarnings("unchecked") private final LinearQuadraticRegulator m_controller = new LinearQuadraticRegulator<>( - m_armPlant, + (LinearSystem) m_armPlant.slice(0), VecBuilder.fill(Units.degreesToRadians(1.0), Units.degreesToRadians(10.0)), // qelms. // Position and velocity error tolerances, in radians and radians per second. Decrease // this @@ -82,11 +84,14 @@ public class Robot extends TimedRobot { // heavily penalize control effort, or make the controller less aggressive. 12 is a good // starting point because that is the (approximate) maximum voltage of a battery. 0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be + // lower if using notifiers. // The state-space loop combines a controller, observer, feedforward and plant for easy control. + @SuppressWarnings("unchecked") private final LinearSystemLoop m_loop = - new LinearSystemLoop<>(m_armPlant, m_controller, m_observer, 12.0, 0.020); + new LinearSystemLoop<>( + (LinearSystem) m_armPlant.slice(0), m_controller, m_observer, 12.0, 0.020); // An encoder set up to measure arm position in radians. private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java index 52941913d1..0e9c045c9c 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java @@ -57,16 +57,17 @@ public class Robot extends TimedRobot { This elevator is driven by two NEO motors. */ - private final LinearSystem m_elevatorPlant = + private final LinearSystem m_elevatorPlant = LinearSystemId.createElevatorSystem( DCMotor.getNEO(2), kCarriageMass, kDrumRadius, kElevatorGearing); // The observer fuses our encoder data and voltage inputs to reject noise. + @SuppressWarnings("unchecked") private final KalmanFilter m_observer = new KalmanFilter<>( Nat.N2(), Nat.N1(), - m_elevatorPlant, + (LinearSystem) m_elevatorPlant.slice(0), VecBuilder.fill(Units.inchesToMeters(2), Units.inchesToMeters(40)), // How accurate we // think our model is, in meters and meters/second. VecBuilder.fill(0.001), // How accurate we think our encoder position @@ -74,9 +75,10 @@ public class Robot extends TimedRobot { 0.020); // A LQR uses feedback to create voltage commands. + @SuppressWarnings("unchecked") private final LinearQuadraticRegulator m_controller = new LinearQuadraticRegulator<>( - m_elevatorPlant, + (LinearSystem) m_elevatorPlant.slice(0), VecBuilder.fill(Units.inchesToMeters(1.0), Units.inchesToMeters(10.0)), // qelms. Position // and velocity error tolerances, in meters and meters per second. Decrease this to more // heavily penalize state excursion, or make the controller behave more aggressively. In @@ -86,11 +88,18 @@ public class Robot extends TimedRobot { // heavily penalize control effort, or make the controller less aggressive. 12 is a good // starting point because that is the (approximate) maximum voltage of a battery. 0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be + // lower if using notifiers. // The state-space loop combines a controller, observer, feedforward and plant for easy control. + @SuppressWarnings("unchecked") private final LinearSystemLoop m_loop = - new LinearSystemLoop<>(m_elevatorPlant, m_controller, m_observer, 12.0, 0.020); + new LinearSystemLoop<>( + (LinearSystem) m_elevatorPlant.slice(0), + m_controller, + m_observer, + 12.0, + 0.020); // An encoder set up to measure elevator height in meters. private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel); diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java index 703498fe52..923f11e48f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java @@ -7,6 +7,30 @@ package edu.wpi.first.math.system; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Num; import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N10; +import edu.wpi.first.math.numbers.N11; +import edu.wpi.first.math.numbers.N12; +import edu.wpi.first.math.numbers.N13; +import edu.wpi.first.math.numbers.N14; +import edu.wpi.first.math.numbers.N15; +import edu.wpi.first.math.numbers.N16; +import edu.wpi.first.math.numbers.N17; +import edu.wpi.first.math.numbers.N18; +import edu.wpi.first.math.numbers.N19; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.numbers.N20; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N4; +import edu.wpi.first.math.numbers.N5; +import edu.wpi.first.math.numbers.N6; +import edu.wpi.first.math.numbers.N7; +import edu.wpi.first.math.numbers.N8; +import edu.wpi.first.math.numbers.N9; +import java.util.Arrays; +import java.util.Collections; +import java.util.List; +import java.util.stream.Collectors; +import org.ejml.simple.SimpleMatrix; /** * A plant defined using state-space notation. @@ -196,6 +220,141 @@ public class LinearSystemThis is used by state observers such as the Kalman Filter. + * + * @param outputIndices the list of output indices to include in the sliced system. + * @return the sliced LinearSystem with outputs set to row vectors of LinearSystem. + * @throws IllegalArgumentException if any outputIndices are outside the range of system outputs. + * @throws IllegalArgumentException if number of outputIndices exceeds the number of system + * outputs. + * @throws IllegalArgumentException if duplication exists in outputIndices. + */ + public LinearSystem slice(int... outputIndices) { + for (int index : outputIndices) { + if (index < 0 || index >= m_C.getNumRows()) { + throw new IllegalArgumentException( + "Output indices out of range. This is usually due to model implementation errors."); + } + } + + if (outputIndices.length >= m_C.getNumRows()) { + throw new IllegalArgumentException( + "More outputs requested than available. This is usually due to model implementation " + + "errors."); + } + + List outputIndicesList = + Arrays.stream(outputIndices).distinct().boxed().collect(Collectors.toList()); + Collections.sort(outputIndicesList); + + if (outputIndices.length != outputIndicesList.size()) { + throw new IllegalArgumentException( + "Duplicate indices exist. This is usually due to model implementation " + "errors."); + } + + SimpleMatrix new_C_Storage = new SimpleMatrix(outputIndices.length, m_C.getNumCols()); + int row = 0; + for (var index : outputIndicesList) { + var current_row_data = m_C.extractRowVector(index).getData(); + new_C_Storage.setRow(row, 0, current_row_data); + row++; + } + + SimpleMatrix new_D_Storage = new SimpleMatrix(outputIndices.length, m_D.getNumCols()); + row = 0; + for (var index : outputIndicesList) { + var current_row_data = m_D.extractRowVector(index).getData(); + new_D_Storage.setRow(row, 0, current_row_data); + row++; + } + + switch (outputIndices.length) { + case 20: + Matrix new_C20 = new Matrix<>(new_C_Storage); + Matrix new_D20 = new Matrix<>(new_D_Storage); + return new LinearSystem<>(m_A, m_B, new_C20, new_D20); + case 19: + Matrix new_C19 = new Matrix<>(new_C_Storage); + Matrix new_D19 = new Matrix<>(new_D_Storage); + return new LinearSystem<>(m_A, m_B, new_C19, new_D19); + case 18: + Matrix new_C18 = new Matrix<>(new_C_Storage); + Matrix new_D18 = new Matrix<>(new_D_Storage); + return new LinearSystem<>(m_A, m_B, new_C18, new_D18); + case 17: + Matrix new_C17 = new Matrix<>(new_C_Storage); + Matrix new_D17 = new Matrix<>(new_D_Storage); + return new LinearSystem<>(m_A, m_B, new_C17, new_D17); + case 16: + Matrix new_C16 = new Matrix<>(new_C_Storage); + Matrix new_D16 = new Matrix<>(new_D_Storage); + return new LinearSystem<>(m_A, m_B, new_C16, new_D16); + case 15: + Matrix new_C15 = new Matrix<>(new_C_Storage); + Matrix new_D15 = new Matrix<>(new_D_Storage); + return new LinearSystem<>(m_A, m_B, new_C15, new_D15); + case 14: + Matrix new_C14 = new Matrix<>(new_C_Storage); + Matrix new_D14 = new Matrix<>(new_D_Storage); + return new LinearSystem<>(m_A, m_B, new_C14, new_D14); + case 13: + Matrix new_C13 = new Matrix<>(new_C_Storage); + Matrix new_D13 = new Matrix<>(new_D_Storage); + return new LinearSystem<>(m_A, m_B, new_C13, new_D13); + case 12: + Matrix new_C12 = new Matrix<>(new_C_Storage); + Matrix new_D12 = new Matrix<>(new_D_Storage); + return new LinearSystem<>(m_A, m_B, new_C12, new_D12); + case 11: + Matrix new_C11 = new Matrix<>(new_C_Storage); + Matrix new_D11 = new Matrix<>(new_D_Storage); + return new LinearSystem<>(m_A, m_B, new_C11, new_D11); + case 10: + Matrix new_C10 = new Matrix<>(new_C_Storage); + Matrix new_D10 = new Matrix<>(new_D_Storage); + return new LinearSystem<>(m_A, m_B, new_C10, new_D10); + case 9: + Matrix new_C9 = new Matrix<>(new_C_Storage); + Matrix new_D9 = new Matrix<>(new_D_Storage); + return new LinearSystem<>(m_A, m_B, new_C9, new_D9); + case 8: + Matrix new_C8 = new Matrix<>(new_C_Storage); + Matrix new_D8 = new Matrix<>(new_D_Storage); + return new LinearSystem<>(m_A, m_B, new_C8, new_D8); + case 7: + Matrix new_C7 = new Matrix<>(new_C_Storage); + Matrix new_D7 = new Matrix<>(new_D_Storage); + return new LinearSystem<>(m_A, m_B, new_C7, new_D7); + case 6: + Matrix new_C6 = new Matrix<>(new_C_Storage); + Matrix new_D6 = new Matrix<>(new_D_Storage); + return new LinearSystem<>(m_A, m_B, new_C6, new_D6); + case 5: + Matrix new_C5 = new Matrix<>(new_C_Storage); + Matrix new_D5 = new Matrix<>(new_D_Storage); + return new LinearSystem<>(m_A, m_B, new_C5, new_D5); + case 4: + Matrix new_C4 = new Matrix<>(new_C_Storage); + Matrix new_D4 = new Matrix<>(new_D_Storage); + return new LinearSystem<>(m_A, m_B, new_C4, new_D4); + case 3: + Matrix new_C3 = new Matrix<>(new_C_Storage); + Matrix new_D3 = new Matrix<>(new_D_Storage); + return new LinearSystem<>(m_A, m_B, new_C3, new_D3); + case 2: + Matrix new_C2 = new Matrix<>(new_C_Storage); + Matrix new_D2 = new Matrix<>(new_D_Storage); + return new LinearSystem<>(m_A, m_B, new_C2, new_D2); + default: + Matrix new_C1 = new Matrix<>(new_C_Storage); + Matrix new_D1 = new Matrix<>(new_D_Storage); + return new LinearSystem<>(m_A, m_B, new_C1, new_D1); + } + } + @Override public String toString() { return String.format( 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 6d8e39a23e..761148a0f2 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 @@ -29,7 +29,7 @@ public final class LinearSystemId { * @return A LinearSystem representing the given characterized constants. * @throws IllegalArgumentException if massKg <= 0, radiusMeters <= 0, or gearing <= 0. */ - public static LinearSystem createElevatorSystem( + public static LinearSystem createElevatorSystem( DCMotor motor, double massKg, double radiusMeters, double gearing) { if (massKg <= 0.0) { throw new IllegalArgumentException("massKg must be greater than zero."); @@ -52,8 +52,8 @@ public final class LinearSystemId { * motor.KtNMPerAmp / (motor.rOhms * radiusMeters * radiusMeters * massKg * motor.KvRadPerSecPerVolt)), VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * radiusMeters * massKg)), - MatBuilder.fill(Nat.N1(), Nat.N2(), 1, 0), - new Matrix<>(Nat.N1(), Nat.N1())); + Matrix.eye(Nat.N2()), + new Matrix<>(Nat.N2(), Nat.N1())); } /** @@ -219,7 +219,7 @@ public final class LinearSystemId { * this will be greater than 1. * @return A LinearSystem representing the given characterized constants. */ - public static LinearSystem createSingleJointedArmSystem( + public static LinearSystem createSingleJointedArmSystem( DCMotor motor, double JKgSquaredMeters, double gearing) { if (JKgSquaredMeters <= 0.0) { throw new IllegalArgumentException("JKgSquaredMeters must be greater than zero."); @@ -239,8 +239,8 @@ public final class LinearSystemId { * motor.KtNMPerAmp / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgSquaredMeters)), VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * JKgSquaredMeters)), - MatBuilder.fill(Nat.N1(), Nat.N2(), 1, 0), - new Matrix<>(Nat.N1(), Nat.N1())); + Matrix.eye(Nat.N2()), + new Matrix<>(Nat.N2(), Nat.N1())); } /** @@ -294,7 +294,7 @@ public final class LinearSystemId { * @throws IllegalArgumentException if kV < 0 or kA <= 0. * @see https://github.com/wpilibsuite/sysid */ - public static LinearSystem identifyPositionSystem(double kV, double kA) { + public static LinearSystem identifyPositionSystem(double kV, double kA) { if (kV < 0.0) { throw new IllegalArgumentException("Kv must be greater than or equal to zero."); } @@ -305,8 +305,8 @@ public final class LinearSystemId { return new LinearSystem<>( MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA), VecBuilder.fill(0.0, 1.0 / kA), - MatBuilder.fill(Nat.N1(), Nat.N2(), 1.0, 0.0), - VecBuilder.fill(0.0)); + Matrix.eye(Nat.N2()), + new Matrix<>(Nat.N2(), Nat.N1())); } /** diff --git a/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp b/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp index 11c65eada3..26d239fed4 100644 --- a/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp +++ b/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp @@ -6,7 +6,7 @@ using namespace frc; -LinearSystem<2, 1, 1> LinearSystemId::ElevatorSystem(DCMotor motor, +LinearSystem<2, 1, 2> LinearSystemId::ElevatorSystem(DCMotor motor, units::kilogram_t mass, units::meter_t radius, double gearing) { @@ -27,13 +27,13 @@ LinearSystem<2, 1, 1> LinearSystemId::ElevatorSystem(DCMotor motor, .value()}}; Matrixd<2, 1> B{0.0, (gearing * motor.Kt / (motor.R * radius * mass)).value()}; - Matrixd<1, 2> C{1.0, 0.0}; - Matrixd<1, 1> D{0.0}; + Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; + Matrixd<2, 1> D{0.0, 0.0}; - return LinearSystem<2, 1, 1>(A, B, C, D); + return LinearSystem<2, 1, 2>(A, B, C, D); } -LinearSystem<2, 1, 1> LinearSystemId::SingleJointedArmSystem( +LinearSystem<2, 1, 2> LinearSystemId::SingleJointedArmSystem( DCMotor motor, units::kilogram_square_meter_t J, double gearing) { if (J <= 0_kg_sq_m) { throw std::domain_error("J must be greater than zero."); @@ -47,10 +47,10 @@ LinearSystem<2, 1, 1> LinearSystemId::SingleJointedArmSystem( {0.0, (-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}}; Matrixd<2, 1> B{0.0, (gearing * motor.Kt / (motor.R * J)).value()}; - Matrixd<1, 2> C{1.0, 0.0}; - Matrixd<1, 1> D{0.0}; + Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; + Matrixd<2, 1> D{{0.0}, {0.0}}; - return LinearSystem<2, 1, 1>(A, B, C, D); + return LinearSystem<2, 1, 2>(A, B, C, D); } LinearSystem<2, 2, 2> LinearSystemId::IdentifyDrivetrainSystem( diff --git a/wpimath/src/main/native/include/frc/system/LinearSystem.h b/wpimath/src/main/native/include/frc/system/LinearSystem.h index dff24338ac..21a3783173 100644 --- a/wpimath/src/main/native/include/frc/system/LinearSystem.h +++ b/wpimath/src/main/native/include/frc/system/LinearSystem.h @@ -5,9 +5,13 @@ #pragma once #include +#include #include #include +#include +#include + #include "frc/EigenCore.h" #include "frc/StateSpaceUtil.h" #include "frc/system/Discretization.h" @@ -164,6 +168,56 @@ class LinearSystem { return m_C * x + m_D * clampedU; } + /** + * Returns the LinearSystem with the outputs listed in outputIndices. + * + *

This is used by state observers such as the Kalman Filter. + * + * @param outputIndices the list of output indices to include in the sliced + * system. + * @return the sliced LinearSystem with outputs set to row vectors of + * LinearSystem. + * @throws std::domain_error if any outputIndices are outside the range of + * system outputs. + * @throws std::domain_error if number of outputIndices exceeds the system + * outputs. + * @throws std::domain_error if duplication exists in outputIndices. + */ + template ... OutputIndices> + LinearSystem Slice( + OutputIndices... outputIndices) { + static_assert(sizeof...(OutputIndices) <= Outputs, + "More outputs requested than available. This is usually due " + "to model implementation errors."); + + wpi::for_each( + [](size_t i, const auto& elem) { + if (elem < 0 || elem >= Outputs) { + throw std::domain_error( + "Slice indices out of range. This is usually due to model " + "implementation errors."); + } + }, + outputIndices...); + + // Sort and deduplicate output indices + wpi::SmallVector outputIndicesArray{outputIndices...}; + std::sort(outputIndicesArray.begin(), outputIndicesArray.end()); + auto last = + std::unique(outputIndicesArray.begin(), outputIndicesArray.end()); + outputIndicesArray.erase(last, outputIndicesArray.end()); + + if (outputIndicesArray.size() != sizeof...(outputIndices)) { + throw std::domain_error( + "Duplicate indices exist. This is usually due to model " + "implementation errors."); + } + + return LinearSystem{ + m_A, m_B, m_C(outputIndicesArray, Eigen::placeholders::all), + m_D(outputIndicesArray, Eigen::placeholders::all)}; + } + private: /** * Continuous system matrix. 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 2b1508c063..d149f8b65a 100644 --- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h +++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h @@ -44,7 +44,7 @@ class WPILIB_DLLEXPORT LinearSystemId { * @param gearing Gear ratio from motor to carriage. * @throws std::domain_error if mass <= 0, radius <= 0, or gearing <= 0. */ - static LinearSystem<2, 1, 1> ElevatorSystem(DCMotor motor, + static LinearSystem<2, 1, 2> ElevatorSystem(DCMotor motor, units::kilogram_t mass, units::meter_t radius, double gearing); @@ -59,7 +59,7 @@ class WPILIB_DLLEXPORT LinearSystemId { * @param gearing Gear ratio from motor to arm. * @throws std::domain_error if J <= 0 or gearing <= 0. */ - static LinearSystem<2, 1, 1> SingleJointedArmSystem( + static LinearSystem<2, 1, 2> SingleJointedArmSystem( DCMotor motor, units::kilogram_square_meter_t J, double gearing); /** 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 7ea8caa715..a34b8b68a8 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 @@ -26,19 +26,31 @@ class LinearSystemLoopTest { private static final double kPositionStddev = 0.0001; private static final Random random = new Random(); - LinearSystem m_plant = + LinearSystem m_plant = LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5, 0.0181864, 1.0); + @SuppressWarnings("unchecked") KalmanFilter m_observer = new KalmanFilter<>( - Nat.N2(), Nat.N1(), m_plant, VecBuilder.fill(0.05, 1.0), VecBuilder.fill(0.0001), kDt); + Nat.N2(), + Nat.N1(), + (LinearSystem) m_plant.slice(0), + VecBuilder.fill(0.05, 1.0), + VecBuilder.fill(0.0001), + kDt); + @SuppressWarnings("unchecked") LinearQuadraticRegulator m_controller = new LinearQuadraticRegulator<>( - m_plant, VecBuilder.fill(0.02, 0.4), VecBuilder.fill(12.0), 0.00505); + (LinearSystem) m_plant.slice(0), + VecBuilder.fill(0.02, 0.4), + VecBuilder.fill(12.0), + 0.00505); + @SuppressWarnings("unchecked") private final LinearSystemLoop m_loop = - new LinearSystemLoop<>(m_plant, m_controller, m_observer, 12, 0.00505); + new LinearSystemLoop<>( + (LinearSystem) m_plant.slice(0), m_controller, m_observer, 12, 0.00505); private static void updateTwoState( LinearSystem plant, LinearSystemLoop loop, double noise) { @@ -49,6 +61,7 @@ class LinearSystemLoopTest { } @Test + @SuppressWarnings("unchecked") void testStateSpaceEnabled() { m_loop.reset(VecBuilder.fill(0, 0)); Matrix references = VecBuilder.fill(2.0, 0.0); @@ -66,7 +79,10 @@ class LinearSystemLoopTest { new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0))); m_loop.setNextR(VecBuilder.fill(state.position, state.velocity)); - updateTwoState(m_plant, m_loop, (random.nextGaussian()) * kPositionStddev); + updateTwoState( + (LinearSystem) m_plant.slice(0), + m_loop, + (random.nextGaussian()) * kPositionStddev); var u = m_loop.getU(0); assertTrue(u >= -12.1 && u <= 12.1, "U out of bounds! Got " + u); 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 d0609b8b13..42fbd92db0 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 @@ -27,7 +27,7 @@ import java.util.Random; import org.junit.jupiter.api.Test; class KalmanFilterTest { - private static LinearSystem elevatorPlant; + private static LinearSystem elevatorPlant; private static final double kDt = 0.00505; @@ -61,11 +61,15 @@ class KalmanFilterTest { new Matrix<>(Nat.N3(), Nat.N3())); // D @Test + @SuppressWarnings("unchecked") void testElevatorKalmanFilter() { var Q = VecBuilder.fill(0.05, 1.0); var R = VecBuilder.fill(0.0001); - assertDoesNotThrow(() -> new KalmanFilter<>(Nat.N2(), Nat.N1(), elevatorPlant, Q, R, kDt)); + assertDoesNotThrow( + () -> + new KalmanFilter<>( + Nat.N2(), Nat.N1(), (LinearSystem) elevatorPlant.slice(0), Q, R, kDt)); } @Test diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java index 0c2ca235f5..21ab6acf42 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java @@ -48,9 +48,9 @@ class LinearSystemIDTest { assertTrue(model.getB().isEqual(VecBuilder.fill(0, 20.8), 0.001)); - assertTrue(model.getC().isEqual(MatBuilder.fill(Nat.N1(), Nat.N2(), 1, 0), 0.001)); + assertTrue(model.getC().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1), 0.001)); - assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001)); + assertTrue(model.getD().isEqual(VecBuilder.fill(0, 0), 0.001)); } @Test diff --git a/wpimath/src/test/native/cpp/StateSpaceTest.cpp b/wpimath/src/test/native/cpp/StateSpaceTest.cpp index 07907e255c..eb3f3a4dd2 100644 --- a/wpimath/src/test/native/cpp/StateSpaceTest.cpp +++ b/wpimath/src/test/native/cpp/StateSpaceTest.cpp @@ -36,7 +36,7 @@ class StateSpaceTest : public testing::Test { // Gear ratio constexpr double G = 40.0 / 40.0; - return frc::LinearSystemId::ElevatorSystem(motors, m, r, G); + return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0); }(); LinearQuadraticRegulator<2, 1> controller{plant, {0.02, 0.4}, {12.0}, kDt}; KalmanFilter<2, 1, 1> observer{plant, {0.05, 1.0}, {0.0001}, kDt}; diff --git a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp index 45f6473129..ab7004ef72 100644 --- a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp +++ b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp @@ -28,7 +28,7 @@ TEST(LinearQuadraticRegulatorTest, ElevatorGains) { // Gear ratio constexpr double G = 40.0 / 40.0; - return frc::LinearSystemId::ElevatorSystem(motors, m, r, G); + 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(); @@ -50,8 +50,9 @@ TEST(LinearQuadraticRegulatorTest, ArmGains) { // Gear ratio constexpr double G = 100.0; - return frc::LinearSystemId::SingleJointedArmSystem( - motors, 1.0 / 3.0 * m * r * r, G); + return frc::LinearSystemId::SingleJointedArmSystem(motors, + 1.0 / 3.0 * m * r * r, G) + .Slice(0); }(); Matrixd<1, 2> K = @@ -75,7 +76,7 @@ TEST(LinearQuadraticRegulatorTest, FourMotorElevator) { // Gear ratio constexpr double G = 14.67; - return frc::LinearSystemId::ElevatorSystem(motors, m, r, G); + return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0); }(); Matrixd<1, 2> K = LinearQuadraticRegulator<2, 1>{plant, {0.1, 0.2}, {12.0}, 20_ms}.K(); @@ -177,7 +178,7 @@ TEST(LinearQuadraticRegulatorTest, LatencyCompensate) { // Gear ratio constexpr double G = 14.67; - return frc::LinearSystemId::ElevatorSystem(motors, m, r, G); + return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0); }(); LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 0.02_s}; diff --git a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp index 4dc6263823..7b59f59f70 100644 --- a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp +++ b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp @@ -28,7 +28,8 @@ TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) { TEST(LinearSystemIDTest, ElevatorSystem) { auto model = frc::LinearSystemId::ElevatorSystem(frc::DCMotor::NEO(2), 5_kg, - 0.05_m, 12); + 0.05_m, 12) + .Slice(0); ASSERT_TRUE(model.A().isApprox( frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -99.05473}}, 0.001)); ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0.0, 20.8}, 0.001));