Merge "Sundry fixes to statespace controllers"
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 7680688..3fa138c 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -41,7 +41,7 @@
coefs.emplace_back(new StateFeedbackPlantCoefficients<4, 2, 2, double>(
dt_config.make_drivetrain_loop().plant().coefficients(ii)));
}
- return StateFeedbackPlant<4, 2, 2, double>(&coefs);
+ return StateFeedbackPlant<4, 2, 2, double>(std::move(coefs));
}
} // namespace
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index 6285024..23a3d21 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -63,8 +63,8 @@
StateFeedbackHybridPlant(
::std::vector<::std::unique_ptr<StateFeedbackHybridPlantCoefficients<
number_of_states, number_of_inputs, number_of_outputs>>>
- *coefficients)
- : coefficients_(::std::move(*coefficients)), index_(0) {
+ &&coefficients)
+ : coefficients_(::std::move(coefficients)), index_(0) {
Reset();
}
@@ -106,14 +106,14 @@
Scalar U_max(int i, int j) const { return U_max()(i, j); }
const Eigen::Matrix<Scalar, number_of_states, 1> &X() const { return X_; }
- Scalar X(int i, int j) const { return X()(i, j); }
+ Scalar X(int i) const { return X()(i); }
const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y() const { return Y_; }
- Scalar Y(int i, int j) const { return Y()(i, j); }
+ Scalar Y(int i) const { return Y()(i); }
Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X() { return X_; }
- Scalar &mutable_X(int i, int j) { return mutable_X()(i, j); }
+ Scalar &mutable_X(int i) { return mutable_X()(i); }
Eigen::Matrix<Scalar, number_of_outputs, 1> &mutable_Y() { return Y_; }
- Scalar &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
+ Scalar &mutable_Y(int i) { return mutable_Y()(i); }
const StateFeedbackHybridPlantCoefficients<number_of_states, number_of_inputs,
number_of_outputs, Scalar>
@@ -247,8 +247,12 @@
explicit HybridKalman(
::std::vector<::std::unique_ptr<HybridKalmanCoefficients<
number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
- *observers)
- : coefficients_(::std::move(*observers)) {}
+ &&observers)
+ : coefficients_(::std::move(observers)) {
+ R_.setZero();
+ X_hat_.setZero();
+ P_ = coefficients().P_steady_state;
+ }
HybridKalman(HybridKalman &&other) : index_(other.index_) {
::std::swap(coefficients_, other.coefficients_);
@@ -281,22 +285,37 @@
return X_hat_;
}
Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
+ Scalar &mutable_X_hat(int i) { return mutable_X_hat()(i); }
+ Scalar X_hat(int i) const { return X_hat_(i); }
void Reset(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
number_of_outputs> *plant) {
X_hat_.setZero();
P_ = coefficients().P_steady_state;
- UpdateQR(plant, ::std::chrono::milliseconds(5));
+ UpdateQR(plant, coefficients().Q_continuous, coefficients().R_continuous,
+ ::std::chrono::milliseconds(5));
}
void Predict(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
number_of_outputs, Scalar> *plant,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
::std::chrono::nanoseconds dt) {
+ Predict(plant, new_u, dt, coefficients().Q_continuous,
+ coefficients().R_continuous);
+ }
+
+ void Predict(
+ StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs, Scalar> *plant,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
+ ::std::chrono::nanoseconds dt,
+ Eigen::Matrix<Scalar, number_of_states, number_of_states> Q_continuous,
+ Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs>
+ R_continuous) {
// Trigger the predict step. This will update A() and B() in the plant.
mutable_X_hat() = plant->Update(X_hat(), new_u, dt);
- UpdateQR(plant, dt);
+ UpdateQR(plant, Q_continuous, R_continuous, dt);
P_ = plant->A() * P_ * plant->A().transpose() + Q_;
}
@@ -305,18 +324,29 @@
number_of_outputs, Scalar> &plant,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y) {
+ DynamicCorrect(plant.C(), plant.D(), U, Y, R_);
+ }
+
+ // Corrects based on the sensor information available.
+ template <int number_of_measurements>
+ void DynamicCorrect(
+ const Eigen::Matrix<Scalar, number_of_measurements, number_of_states> &C,
+ const Eigen::Matrix<Scalar, number_of_measurements, number_of_inputs> &D,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
+ const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y,
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> &R) {
Eigen::Matrix<Scalar, number_of_outputs, 1> Y_bar =
- Y - (plant.C() * X_hat_ + plant.D() * U);
+ Y - (C * X_hat_ + D * U);
Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> S =
- plant.C() * P_ * plant.C().transpose() + R_;
+ C * P_ * C.transpose() + R;
Eigen::Matrix<Scalar, number_of_states, number_of_outputs> KalmanGain;
- KalmanGain =
- (S.transpose().ldlt().solve((P() * plant.C().transpose()).transpose()))
- .transpose();
+ KalmanGain = (S.transpose().ldlt().solve((P() * C.transpose()).transpose()))
+ .transpose();
X_hat_ = X_hat_ + KalmanGain * Y_bar;
- P_ = (plant.coefficients().A_continuous.Identity() -
- KalmanGain * plant.C()) *
- P();
+ P_ =
+ (Eigen::Matrix<Scalar, number_of_states, number_of_states>::Identity() -
+ KalmanGain * C) *
+ P();
}
// Sets the current controller to be index, clamped to be within range.
@@ -345,17 +375,17 @@
}
private:
- void UpdateQR(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
- number_of_outputs> *plant,
- ::std::chrono::nanoseconds dt) {
- ::frc971::controls::DiscretizeQ(coefficients().Q_continuous,
- plant->coefficients().A_continuous, dt,
- &Q_);
+ void UpdateQR(
+ StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs> *plant,
+ Eigen::Matrix<Scalar, number_of_states, number_of_states> Q_continuous,
+ Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> R_continuous,
+ ::std::chrono::nanoseconds dt) {
+ frc971::controls::DiscretizeQ(Q_continuous,
+ plant->coefficients().A_continuous, dt, &Q_);
Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> Rtemp =
- (coefficients().R_continuous +
- coefficients().R_continuous.transpose()) /
- static_cast<Scalar>(2.0);
+ (R_continuous + R_continuous.transpose()) / static_cast<Scalar>(2.0);
R_ = Rtemp / ::aos::time::TypedDurationInSeconds<Scalar>(dt);
}
diff --git a/frc971/control_loops/hybrid_state_feedback_loop_test.cc b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
index 13f74fa..c5b0771 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop_test.cc
+++ b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
@@ -84,7 +84,7 @@
plants[0] = ::std::unique_ptr<StateFeedbackHybridPlantCoefficients<3, 1, 1>>(
new StateFeedbackHybridPlantCoefficients<3, 1, 1>(
MakeIntegralShooterPlantCoefficients()));
- return StateFeedbackHybridPlant<3, 1, 1>(&plants);
+ return StateFeedbackHybridPlant<3, 1, 1>(std::move(plants));
}
StateFeedbackController<3, 1, 1> MakeIntegralShooterController() {
@@ -94,7 +94,7 @@
::std::unique_ptr<StateFeedbackControllerCoefficients<3, 1, 1>>(
new StateFeedbackControllerCoefficients<3, 1, 1>(
MakeIntegralShooterControllerCoefficients()));
- return StateFeedbackController<3, 1, 1>(&controllers);
+ return StateFeedbackController<3, 1, 1>(std::move(controllers));
}
HybridKalman<3, 1, 1> MakeIntegralShooterObserver() {
@@ -103,7 +103,7 @@
observers[0] = ::std::unique_ptr<HybridKalmanCoefficients<3, 1, 1>>(
new HybridKalmanCoefficients<3, 1, 1>(
MakeIntegralShooterObserverCoefficients()));
- return HybridKalman<3, 1, 1>(&observers);
+ return HybridKalman<3, 1, 1>(std::move(observers));
}
StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
@@ -134,7 +134,7 @@
v_plant;
v_plant.emplace_back(
new StateFeedbackPlantCoefficients<2, 4, 7>(coefficients));
- StateFeedbackPlant<2, 4, 7> plant(&v_plant);
+ StateFeedbackPlant<2, 4, 7> plant(std::move(v_plant));
plant.Update(Eigen::Matrix<double, 4, 1>::Zero());
plant.Reset();
plant.CheckU(Eigen::Matrix<double, 4, 1>::Zero());
@@ -145,7 +145,7 @@
v_controller.emplace_back(new StateFeedbackControllerCoefficients<2, 4, 7>(
Eigen::Matrix<double, 4, 2>::Identity(),
Eigen::Matrix<double, 4, 2>::Identity()));
- StateFeedbackController<2, 4, 7> controller(&v_controller);
+ StateFeedbackController<2, 4, 7> controller(std::move(v_controller));
::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<2, 4, 7>>>
v_observer;
@@ -153,7 +153,7 @@
Eigen::Matrix<double, 2, 7>::Identity(),
Eigen::Matrix<double, 2, 2>::Identity(),
Eigen::Matrix<double, 7, 7>::Identity()));
- StateFeedbackObserver<2, 4, 7> observer(&v_observer);
+ StateFeedbackObserver<2, 4, 7> observer(std::move(v_observer));
StateFeedbackLoop<2, 4, 7> test_loop(
::std::move(plant), ::std::move(controller), ::std::move(observer));
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 74141c1..cea1fdd 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -216,7 +216,7 @@
fd.write(' plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n'
% (index, self._PlantCoeffType(),
self._PlantCoeffType(), loop.PlantFunction()))
- fd.write(' return %s(&plants);\n' % self._PlantType())
+ fd.write(' return %s(std::move(plants));\n' % self._PlantType())
fd.write('}\n\n')
fd.write('%s Make%sController() {\n' % (self._ControllerType(),
@@ -229,7 +229,8 @@
' controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n'
% (index, self._ControllerCoeffType(),
self._ControllerCoeffType(), loop.ControllerFunction()))
- fd.write(' return %s(&controllers);\n' % self._ControllerType())
+ fd.write(' return %s(std::move(controllers));\n' %
+ self._ControllerType())
fd.write('}\n\n')
fd.write('%s Make%sObserver() {\n' % (self._ObserverType(),
@@ -241,7 +242,8 @@
' observers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n'
% (index, self._ObserverCoeffType(),
self._ObserverCoeffType(), loop.ObserverFunction()))
- fd.write(' return %s(&observers);\n' % self._ObserverType())
+ fd.write(
+ ' return %s(std::move(observers));\n' % self._ObserverType())
fd.write('}\n\n')
fd.write('%s Make%sLoop() {\n' % (self._LoopType(),
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 4250df2..f591847 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -69,8 +69,8 @@
StateFeedbackPlant(
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
- *coefficients)
- : coefficients_(::std::move(*coefficients)), index_(0) {
+ &&coefficients)
+ : coefficients_(::std::move(coefficients)), index_(0) {
Reset();
}
@@ -222,8 +222,8 @@
explicit StateFeedbackController(
::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<
number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
- *controllers)
- : coefficients_(::std::move(*controllers)) {}
+ &&controllers)
+ : coefficients_(::std::move(controllers)) {}
StateFeedbackController(StateFeedbackController &&other)
: index_(other.index_) {
@@ -300,8 +300,8 @@
explicit StateFeedbackObserver(
::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
- *observers)
- : coefficients_(::std::move(*observers)) {}
+ &&observers)
+ : coefficients_(::std::move(observers)) {}
StateFeedbackObserver(StateFeedbackObserver &&other)
: X_hat_(other.X_hat_), index_(other.index_) {