diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java index e01d16a5d0..93f90cda7a 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java @@ -18,7 +18,7 @@ import edu.wpi.first.wpiutil.math.Num; import edu.wpi.first.wpiutil.math.numbers.N1; /** - * Combines a plant, controller, and observer for controlling a mechanism with + * Combines a controller, feedforward, and observer for controlling a mechanism with * full state feedback. * *

For everything in this file, "inputs" and "outputs" are defined from the @@ -35,7 +35,6 @@ import edu.wpi.first.wpiutil.math.numbers.N1; public class LinearSystemLoop { - private final LinearSystem m_plant; private final LinearQuadraticRegulator m_controller; private final LinearPlantInversionFeedforward m_feedforward; private final KalmanFilter m_observer; @@ -59,7 +58,7 @@ public class LinearSystemLoop observer, double maxVoltageVolts, double dtSeconds) { - this(plant, controller, + this(controller, new LinearPlantInversionFeedforward<>(plant, dtSeconds), observer, u -> StateSpaceUtil.normalizeInputVector(u, maxVoltageVolts)); } @@ -80,49 +79,44 @@ public class LinearSystemLoop observer, Function, Matrix> clampFunction, double dtSeconds) { - this(plant, controller, new LinearPlantInversionFeedforward<>(plant, dtSeconds), + this(controller, new LinearPlantInversionFeedforward<>(plant, dtSeconds), observer, clampFunction); } /** - * Constructs a state-space loop with the given plant, controller, and + * Constructs a state-space loop with the given controller, feedforward and * observer. By default, the initial reference is all zeros. Users should * call reset with the initial system state before enabling the loop. * - * @param plant State-space plant. * @param controller State-space controller. * @param feedforward Plant inversion feedforward. * @param observer State-space observer. * @param maxVoltageVolts The maximum voltage that can be applied. Assumes that the * inputs are voltages. */ - public LinearSystemLoop(LinearSystem plant, - LinearQuadraticRegulator controller, + public LinearSystemLoop(LinearQuadraticRegulator controller, LinearPlantInversionFeedforward feedforward, KalmanFilter observer, double maxVoltageVolts ) { - this(plant, controller, feedforward, + this(controller, feedforward, observer, u -> StateSpaceUtil.normalizeInputVector(u, maxVoltageVolts)); } /** - * Constructs a state-space loop with the given plant, controller, and + * Constructs a state-space loop with the given controller, feedforward, and * observer. By default, the initial reference is all zeros. Users should * call reset with the initial system state before enabling the loop. * - * @param plant State-space plant. * @param controller State-space controller. * @param feedforward Plant inversion feedforward. * @param observer State-space observer. * @param clampFunction The function used to clamp the input U. */ - public LinearSystemLoop(LinearSystem plant, - LinearQuadraticRegulator controller, + public LinearSystemLoop(LinearQuadraticRegulator controller, LinearPlantInversionFeedforward feedforward, KalmanFilter observer, Function, Matrix> clampFunction) { - this.m_plant = plant; this.m_controller = controller; this.m_feedforward = feedforward; this.m_observer = observer; @@ -232,15 +226,6 @@ public class LinearSystemLoop getPlant() { - return m_plant; - } - /** * Return the controller used internally. * diff --git a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h index 494b0d1c7e..b90618a17f 100644 --- a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h +++ b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h @@ -15,8 +15,8 @@ namespace frc { /** - * Combines a plant, controller, and observer for controlling a mechanism with - * full state feedback. + * Combines a controller, feedforward, and observer for controlling a mechanism + * with full state feedback. * * For everything in this file, "inputs" and "outputs" are defined from the * perspective of the plant. This means U is an input and Y is an output @@ -37,7 +37,6 @@ class LinearSystemLoop { * call reset with the initial system state before enabling the loop. This * constructor assumes that the input(s) to this system are voltage. * - * @param plant State-space plant. * @param controller State-space controller. * @param observer State-space observer. * @param maxVoltage The maximum voltage that can be applied. Commonly 12. @@ -75,16 +74,15 @@ class LinearSystemLoop { clampFunction, units::second_t dt) : LinearSystemLoop( - plant, controller, + controller, LinearPlantInversionFeedforward{plant, dt}, observer, clampFunction) {} /** - * Constructs a state-space loop with the given plant, controller, and + * Constructs a state-space loop with the given controller, feedforward and * observer. By default, the initial reference is all zeros. Users should - * call reset with the initial system state before enabling the loop. + * call reset with the initial system state. * - * @param plant State-space plant. * @param controller State-space controller. * @param feedforward Plant inversion feedforward. * @param observer State-space observer. @@ -92,36 +90,33 @@ class LinearSystemLoop { * that the inputs are voltages. */ LinearSystemLoop( - LinearSystem& plant, LinearQuadraticRegulator& controller, const LinearPlantInversionFeedforward& feedforward, KalmanFilter& observer, units::volt_t maxVoltage) - : LinearSystemLoop(plant, controller, feedforward, observer, + : LinearSystemLoop(controller, feedforward, observer, [=](Eigen::Matrix u) { return frc::NormalizeInputVector( u, maxVoltage.template to()); }) {} /** - * Constructs a state-space loop with the given plant, controller, and - * observer. + * Constructs a state-space loop with the given controller, feedforward, + * observer and clamp function. By default, the initial reference is all + * zeros. Users should call reset with the initial system state. * - * @param plant State-space plant. * @param controller State-space controller. * @param feedforward Plant-inversion feedforward. * @param observer State-space observer. * @param clampFunction The function used to clamp the input vector. */ LinearSystemLoop( - LinearSystem& plant, LinearQuadraticRegulator& controller, const LinearPlantInversionFeedforward& feedforward, KalmanFilter& observer, std::function( const Eigen::Matrix&)> clampFunction) - : m_plant(plant), - m_controller(controller), + : m_controller(controller), m_feedforward(feedforward), m_observer(observer), m_clampFunc(clampFunction) { @@ -195,11 +190,6 @@ class LinearSystemLoop { m_nextR = nextR; } - /** - * Return the plant used internally. - */ - const LinearSystem& Plant() const { return m_plant; } - /** * Return the controller used internally. */ @@ -281,7 +271,6 @@ class LinearSystemLoop { } protected: - LinearSystem& m_plant; LinearQuadraticRegulator& m_controller; LinearPlantInversionFeedforward m_feedforward; KalmanFilter& m_observer; diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearSystemLoopTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearSystemLoopTest.java index c84e76e3a3..0ba638ab44 100644 --- a/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearSystemLoopTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearSystemLoopTest.java @@ -29,29 +29,25 @@ public class LinearSystemLoopTest { public static final double kDt = 0.00505; private static final double kPositionStddev = 0.0001; private static final Random random = new Random(); - private final LinearSystemLoop m_loop; + + LinearSystem m_plant = LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5, + 0.0181864, 1.0); + + KalmanFilter m_observer = new KalmanFilter<>(Nat.N2(), Nat.N1(), m_plant, + VecBuilder.fill(0.05, 1.0), + VecBuilder.fill(0.0001), kDt); + + LinearQuadraticRegulator m_controller = new LinearQuadraticRegulator<>( + m_plant, VecBuilder.fill(0.02, 0.4), VecBuilder.fill(12.0), + 0.00505); + + private final LinearSystemLoop m_loop = + new LinearSystemLoop<>(m_plant, m_controller, m_observer, 12, 0.00505); @SuppressWarnings("LocalVariableName") - public LinearSystemLoopTest() { - LinearSystem plant = LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5, - 0.0181864, 1.0); - KalmanFilter observer = new KalmanFilter<>(Nat.N2(), Nat.N1(), plant, - VecBuilder.fill(0.05, 1.0), - VecBuilder.fill(0.0001), kDt); - - var qElms = VecBuilder.fill(0.02, 0.4); - var rElms = VecBuilder.fill(12.0); - var dt = 0.00505; - - var controller = new LinearQuadraticRegulator<>( - plant, qElms, rElms, dt); - - m_loop = new LinearSystemLoop<>(plant, controller, observer, 12, dt); - } - - @SuppressWarnings("LocalVariableName") - private static void updateTwoState(LinearSystemLoop loop, double noise) { - Matrix y = loop.getPlant().calculateY(loop.getXHat(), loop.getU()).plus( + private static void updateTwoState(LinearSystem plant, LinearSystemLoop + loop, double noise) { + Matrix y = plant.calculateY(loop.getXHat(), loop.getU()).plus( VecBuilder.fill(noise) ); @@ -78,7 +74,7 @@ public class LinearSystemLoopTest { state = profile.calculate(kDt); m_loop.setNextR(VecBuilder.fill(state.position, state.velocity)); - updateTwoState(m_loop, (random.nextGaussian()) * kPositionStddev); + updateTwoState(m_plant, m_loop, (random.nextGaussian()) * kPositionStddev); var u = m_loop.getU(0); assertTrue(u >= -12.1 && u <= 12.1, "U out of bounds! Got " + u); @@ -107,7 +103,7 @@ public class LinearSystemLoopTest { var feedforward = new LinearPlantInversionFeedforward<>(plant, kDt); - var loop = new LinearSystemLoop<>(plant, controller, feedforward, observer, 12); + var loop = new LinearSystemLoop<>(controller, feedforward, observer, 12); loop.reset(VecBuilder.fill(0.0)); var references = VecBuilder.fill(3000 / 60d * 2 * Math.PI); @@ -129,7 +125,7 @@ public class LinearSystemLoopTest { loop.setNextR(references); - Matrix y = loop.getPlant().calculateY(loop.getXHat(), loop.getU()).plus( + Matrix y = plant.calculateY(loop.getXHat(), loop.getU()).plus( VecBuilder.fill(random.nextGaussian() * kPositionStddev) ); diff --git a/wpimath/src/test/native/cpp/StateSpaceTest.cpp b/wpimath/src/test/native/cpp/StateSpaceTest.cpp index 80d3c8a6c4..cd8b083ba3 100644 --- a/wpimath/src/test/native/cpp/StateSpaceTest.cpp +++ b/wpimath/src/test/native/cpp/StateSpaceTest.cpp @@ -43,10 +43,10 @@ class StateSpace : public testing::Test { LinearSystemLoop<2, 1, 1> loop{plant, controller, observer, 12_V, kDt}; }; -void Update(LinearSystemLoop<2, 1, 1>& loop, double noise) { - Eigen::Matrix y = - loop.Plant().CalculateY(loop.Xhat(), loop.U()) + - Eigen::Matrix(noise); +void Update(const LinearSystem<2, 1, 1>& plant, LinearSystemLoop<2, 1, 1>& loop, + double noise) { + Eigen::Matrix y = plant.CalculateY(loop.Xhat(), loop.U()) + + Eigen::Matrix(noise); loop.Correct(y); loop.Predict(kDt); } @@ -60,7 +60,7 @@ TEST_F(StateSpace, CorrectPredictLoop) { loop.SetNextR(references); for (int i = 0; i < 1000; i++) { - Update(loop, dist(generator)); + Update(plant, loop, dist(generator)); EXPECT_PRED_FORMAT2(testing::DoubleLE, -12.0, loop.U(0)); EXPECT_PRED_FORMAT2(testing::DoubleLE, loop.U(0), 12.0); }