Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Remove LinearSystem from LinearSystemLoop (#2968)

Browse Source 
The system wasn't being used internally, and as LinearSystem is stateless, it doesn't need to be held by LinearSystemLoop.
This commit is contained in:
Matt
2020-12-28 10:35:51 -08:00
committed by GitHub
parent aa89744c95
commit 254931b9a8
4 changed files with 43 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java
View File

@@ -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.
*
* <p>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<States extends Num, Inputs extends Num,
Outputs extends Num> {
private final LinearSystem<States, Inputs, Outputs> m_plant;
private final LinearQuadraticRegulator<States, Inputs, Outputs> m_controller;
private final LinearPlantInversionFeedforward<States, Inputs, Outputs> m_feedforward;
private final KalmanFilter<States, Inputs, Outputs> m_observer;
@@ -59,7 +58,7 @@ public class LinearSystemLoop<States extends Num, Inputs extends Num,
KalmanFilter<States, Inputs, Outputs> 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<States extends Num, Inputs extends Num,
KalmanFilter<States, Inputs, Outputs> observer,
Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> 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<States, Inputs, Outputs> plant,
LinearQuadraticRegulator<States, Inputs, Outputs> controller,
public LinearSystemLoop(LinearQuadraticRegulator<States, Inputs, Outputs> controller,
LinearPlantInversionFeedforward<States, Inputs, Outputs> feedforward,
KalmanFilter<States, Inputs, Outputs> 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<States, Inputs, Outputs> plant,
LinearQuadraticRegulator<States, Inputs, Outputs> controller,
public LinearSystemLoop(LinearQuadraticRegulator<States, Inputs, Outputs> controller,
LinearPlantInversionFeedforward<States, Inputs, Outputs> feedforward,
KalmanFilter<States, Inputs, Outputs> observer,
Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction) {
this.m_plant = plant;
this.m_controller = controller;
this.m_feedforward = feedforward;
this.m_observer = observer;
@@ -232,15 +226,6 @@ public class LinearSystemLoop<States extends Num, Inputs extends Num,
return getU().get(row, 0);
}
/**
* Return the plant used internally.
*
* @return the plant used internally.
*/
public LinearSystem<States, Inputs, Outputs> getPlant() {
return m_plant;
}
/**
* Return the controller used internally.
*

31
wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
View File

@@ -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<States, Inputs>{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<States, Inputs, Outputs>& plant,
LinearQuadraticRegulator<States, Inputs>& controller,
const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
KalmanFilter<States, Inputs, Outputs>& observer, units::volt_t maxVoltage)
: LinearSystemLoop(plant, controller, feedforward, observer,
: LinearSystemLoop(controller, feedforward, observer,
[=](Eigen::Matrix<double, Inputs, 1> u) {
return frc::NormalizeInputVector<Inputs>(
u, maxVoltage.template to<double>());
}) {}
/**
* 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<States, Inputs, Outputs>& plant,
LinearQuadraticRegulator<States, Inputs>& controller,
const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
KalmanFilter<States, Inputs, Outputs>& observer,
std::function<Eigen::Matrix<double, Inputs, 1>(
const Eigen::Matrix<double, Inputs, 1>&)>
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<States, Inputs, Outputs>& Plant() const { return m_plant; }
/**
* Return the controller used internally.
*/
@@ -281,7 +271,6 @@ class LinearSystemLoop {
}
protected:
LinearSystem<States, Inputs, Outputs>& m_plant;
LinearQuadraticRegulator<States, Inputs>& m_controller;
LinearPlantInversionFeedforward<States, Inputs> m_feedforward;
KalmanFilter<States, Inputs, Outputs>& m_observer;

44
wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearSystemLoopTest.java
View File

@@ -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<N2, N1, N1> m_loop;
LinearSystem<N2, N1, N1> m_plant = LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5,
0.0181864, 1.0);
KalmanFilter<N2, N1, N1> m_observer = new KalmanFilter<>(Nat.N2(), Nat.N1(), m_plant,
VecBuilder.fill(0.05, 1.0),
VecBuilder.fill(0.0001), kDt);
LinearQuadraticRegulator<N2, N1, N1> m_controller = new LinearQuadraticRegulator<>(
m_plant, VecBuilder.fill(0.02, 0.4), VecBuilder.fill(12.0),
0.00505);
private final LinearSystemLoop<N2, N1, N1> m_loop =
new LinearSystemLoop<>(m_plant, m_controller, m_observer, 12, 0.00505);
@SuppressWarnings("LocalVariableName")
public LinearSystemLoopTest() {
LinearSystem<N2, N1, N1> plant = LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5,
0.0181864, 1.0);
KalmanFilter<N2, N1, N1> 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<N2, N1, N1> loop, double noise) {
Matrix<N1, N1> y = loop.getPlant().calculateY(loop.getXHat(), loop.getU()).plus(
private static void updateTwoState(LinearSystem<N2, N1, N1> plant, LinearSystemLoop<N2, N1, N1>
loop, double noise) {
Matrix<N1, N1> 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<N1, N1> y = loop.getPlant().calculateY(loop.getXHat(), loop.getU()).plus(
Matrix<N1, N1> y = plant.calculateY(loop.getXHat(), loop.getU()).plus(
VecBuilder.fill(random.nextGaussian() * kPositionStddev)
);

10
wpimath/src/test/native/cpp/StateSpaceTest.cpp
View File

@@ -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<double, 1, 1> y =
loop.Plant().CalculateY(loop.Xhat(), loop.U()) +
Eigen::Matrix<double, 1, 1>(noise);
void Update(const LinearSystem<2, 1, 1>& plant, LinearSystemLoop<2, 1, 1>& loop,
double noise) {
Eigen::Matrix<double, 1, 1> y = plant.CalculateY(loop.Xhat(), loop.U()) +
Eigen::Matrix<double, 1, 1>(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);
}