Make the internal control loop type a template argument.
We need this to switch the statespace loops over to floats for the
pistol grip controller. Also, plumb it up to the control loop writer.
Change-Id: I9e12d8d69ea7027861b488c06b45f791c71c4eb3
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index 27ad4fb..971f0b8 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -17,7 +17,8 @@
#include "aos/common/macros.h"
#include "frc971/control_loops/state_feedback_loop.h"
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double>
struct StateFeedbackHybridPlantCoefficients final {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
@@ -32,14 +33,14 @@
U_max(other.U_max) {}
StateFeedbackHybridPlantCoefficients(
- const Eigen::Matrix<double, number_of_states, number_of_states>
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states>
&A_continuous,
- const Eigen::Matrix<double, number_of_states, number_of_inputs>
+ const Eigen::Matrix<Scalar, number_of_states, number_of_inputs>
&B_continuous,
- const Eigen::Matrix<double, number_of_outputs, number_of_states> &C,
- const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D,
- const Eigen::Matrix<double, number_of_inputs, 1> &U_max,
- const Eigen::Matrix<double, number_of_inputs, 1> &U_min)
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> &C,
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> &D,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min)
: A_continuous(A_continuous),
B_continuous(B_continuous),
C(C),
@@ -47,15 +48,16 @@
U_min(U_min),
U_max(U_max) {}
- const Eigen::Matrix<double, number_of_states, number_of_states> A_continuous;
- const Eigen::Matrix<double, number_of_states, number_of_inputs> B_continuous;
- const Eigen::Matrix<double, number_of_outputs, number_of_states> C;
- const Eigen::Matrix<double, number_of_outputs, number_of_inputs> D;
- const Eigen::Matrix<double, number_of_inputs, 1> U_min;
- const Eigen::Matrix<double, number_of_inputs, 1> U_max;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> A_continuous;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> B_continuous;
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> C;
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> D;
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> U_min;
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> U_max;
};
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double>
class StateFeedbackHybridPlant {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
@@ -77,49 +79,49 @@
virtual ~StateFeedbackHybridPlant() {}
- const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A() const {
return A_;
}
- double A(int i, int j) const { return A()(i, j); }
- const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
+ Scalar A(int i, int j) const { return A()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> &B() const {
return B_;
}
- double B(int i, int j) const { return B()(i, j); }
- const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
+ Scalar B(int i, int j) const { return B()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> &C() const {
return coefficients().C;
}
- double C(int i, int j) const { return C()(i, j); }
- const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D() const {
+ Scalar C(int i, int j) const { return C()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> &D() const {
return coefficients().D;
}
- double D(int i, int j) const { return D()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
+ Scalar D(int i, int j) const { return D()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min() const {
return coefficients().U_min;
}
- double U_min(int i, int j) const { return U_min()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, 1> &U_max() const {
+ Scalar U_min(int i, int j) const { return U_min()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max() const {
return coefficients().U_max;
}
- double U_max(int i, int j) const { return U_max()(i, j); }
+ Scalar U_max(int i, int j) const { return U_max()(i, j); }
- const Eigen::Matrix<double, number_of_states, 1> &X() const { return X_; }
- double X(int i, int j) const { return X()(i, j); }
- const Eigen::Matrix<double, number_of_outputs, 1> &Y() const { return Y_; }
- double Y(int i, int j) const { return Y()(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); }
+ const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y() const { return Y_; }
+ Scalar Y(int i, int j) const { return Y()(i, j); }
- Eigen::Matrix<double, number_of_states, 1> &mutable_X() { return X_; }
- double &mutable_X(int i, int j) { return mutable_X()(i, j); }
- Eigen::Matrix<double, number_of_outputs, 1> &mutable_Y() { return Y_; }
- double &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
+ Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X() { return X_; }
+ Scalar &mutable_X(int i, int j) { return mutable_X()(i, j); }
+ Eigen::Matrix<Scalar, number_of_outputs, 1> &mutable_Y() { return Y_; }
+ Scalar &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
const StateFeedbackHybridPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
+ number_of_outputs, Scalar>
&coefficients(int index) const {
return *coefficients_[index];
}
const StateFeedbackHybridPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
+ number_of_outputs, Scalar>
&coefficients() const {
return *coefficients_[index_];
}
@@ -141,36 +143,38 @@
}
// Assert that U is within the hardware range.
- virtual void CheckU(const Eigen::Matrix<double, number_of_inputs, 1> &U) {
+ virtual void CheckU(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U) {
for (int i = 0; i < kNumInputs; ++i) {
- if (U(i, 0) > U_max(i, 0) + 0.00001 || U(i, 0) < U_min(i, 0) - 0.00001) {
+ if (U(i, 0) > U_max(i, 0) + static_cast<Scalar>(0.00001) ||
+ U(i, 0) < U_min(i, 0) - static_cast<Scalar>(0.00001)) {
LOG(FATAL, "U out of range\n");
}
}
}
// Computes the new X and Y given the control input.
- void Update(const Eigen::Matrix<double, number_of_inputs, 1> &U,
+ void Update(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
::std::chrono::nanoseconds dt) {
// Powers outside of the range are more likely controller bugs than things
// that the plant should deal with.
CheckU(U);
::aos::robot_state.FetchLatest();
- Eigen::Matrix<double, number_of_inputs, 1> current_U =
- DelayedU_ * (::aos::robot_state.get()
- ? ::aos::robot_state->voltage_battery / 12.0
- : 1.0);
+ Eigen::Matrix<Scalar, number_of_inputs, 1> current_U =
+ DelayedU_ *
+ (::aos::robot_state.get()
+ ? ::aos::robot_state->voltage_battery / static_cast<Scalar>(12.0)
+ : static_cast<Scalar>(1.0));
X_ = Update(X(), current_U, dt);
Y_ = C() * X() + D() * current_U;
DelayedU_ = U;
}
- Eigen::Matrix<double, number_of_inputs, 1> DelayedU_;
+ Eigen::Matrix<Scalar, number_of_inputs, 1> DelayedU_;
- Eigen::Matrix<double, number_of_states, 1> Update(
- const Eigen::Matrix<double, number_of_states, 1> X,
- const Eigen::Matrix<double, number_of_inputs, 1> &U,
+ Eigen::Matrix<Scalar, number_of_states, 1> Update(
+ const Eigen::Matrix<Scalar, number_of_states, 1> X,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
::std::chrono::nanoseconds dt) {
UpdateAB(dt);
return A() * X + B() * U;
@@ -184,22 +188,22 @@
private:
void UpdateAB(::std::chrono::nanoseconds dt) {
- Eigen::Matrix<double, number_of_states + number_of_inputs,
+ Eigen::Matrix<Scalar, number_of_states + number_of_inputs,
number_of_states + number_of_inputs>
M_state_continuous;
M_state_continuous.setZero();
M_state_continuous.template block<number_of_states, number_of_states>(0,
0) =
coefficients().A_continuous *
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+ ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
.count();
M_state_continuous.template block<number_of_states, number_of_inputs>(
0, number_of_states) =
coefficients().B_continuous *
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+ ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
.count();
- Eigen::Matrix<double, number_of_states + number_of_inputs,
+ Eigen::Matrix<Scalar, number_of_states + number_of_inputs,
number_of_states + number_of_inputs>
M_state = M_state_continuous.exp();
A_ = M_state.template block<number_of_states, number_of_states>(0, 0);
@@ -207,11 +211,11 @@
0, number_of_states);
}
- Eigen::Matrix<double, number_of_states, 1> X_;
- Eigen::Matrix<double, number_of_outputs, 1> Y_;
+ Eigen::Matrix<Scalar, number_of_states, 1> X_;
+ Eigen::Matrix<Scalar, number_of_outputs, 1> Y_;
- Eigen::Matrix<double, number_of_states, number_of_states> A_;
- Eigen::Matrix<double, number_of_states, number_of_inputs> B_;
+ Eigen::Matrix<Scalar, number_of_states, number_of_states> A_;
+ Eigen::Matrix<Scalar, number_of_states, number_of_inputs> B_;
::std::vector<::std::unique_ptr<StateFeedbackHybridPlantCoefficients<
@@ -225,34 +229,36 @@
// A container for all the observer coefficients.
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double>
struct HybridKalmanCoefficients final {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- const Eigen::Matrix<double, number_of_states, number_of_states> Q_continuous;
- const Eigen::Matrix<double, number_of_outputs, number_of_outputs> R_continuous;
- const Eigen::Matrix<double, number_of_states, number_of_states> P_steady_state;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> Q_continuous;
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> R_continuous;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> P_steady_state;
HybridKalmanCoefficients(
- const Eigen::Matrix<double, number_of_states, number_of_states>
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states>
&Q_continuous,
- const Eigen::Matrix<double, number_of_outputs, number_of_outputs>
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs>
&R_continuous,
- const Eigen::Matrix<double, number_of_states, number_of_states>
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states>
&P_steady_state)
: Q_continuous(Q_continuous),
R_continuous(R_continuous),
P_steady_state(P_steady_state) {}
};
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double>
class HybridKalman {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
explicit HybridKalman(
::std::vector<::std::unique_ptr<HybridKalmanCoefficients<
- number_of_states, number_of_inputs, number_of_outputs>>> *observers)
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>>> *observers)
: coefficients_(::std::move(*observers)) {}
HybridKalman(HybridKalman &&other)
@@ -261,27 +267,27 @@
}
// Getters for Q
- const Eigen::Matrix<double, number_of_states, number_of_states> &Q() const {
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> &Q() const {
return Q_;
}
- double Q(int i, int j) const { return Q()(i, j); }
+ Scalar Q(int i, int j) const { return Q()(i, j); }
// Getters for R
- const Eigen::Matrix<double, number_of_outputs, number_of_outputs> &R() const {
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> &R() const {
return R_;
}
- double R(int i, int j) const { return R()(i, j); }
+ Scalar R(int i, int j) const { return R()(i, j); }
// Getters for P
- const Eigen::Matrix<double, number_of_states, number_of_states> &P() const {
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> &P() const {
return P_;
}
- double P(int i, int j) const { return P()(i, j); }
+ Scalar P(int i, int j) const { return P()(i, j); }
// Getters for X_hat
- const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
+ const Eigen::Matrix<Scalar, number_of_states, 1> &X_hat() const {
return X_hat_;
}
- Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
+ Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
void Reset(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
number_of_outputs> *plant) {
@@ -291,8 +297,8 @@
}
void Predict(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
- number_of_outputs> *plant,
- const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
+ number_of_outputs, Scalar> *plant,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
::std::chrono::nanoseconds dt) {
// Trigger the predict step. This will update A() and B() in the plant.
mutable_X_hat() = plant->Update(X_hat(), new_u, dt);
@@ -303,14 +309,14 @@
void Correct(
const StateFeedbackHybridPlant<number_of_states, number_of_inputs,
- number_of_outputs> &plant,
- const Eigen::Matrix<double, number_of_inputs, 1> &U,
- const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
- Eigen::Matrix<double, number_of_outputs, 1> Y_bar =
+ number_of_outputs, Scalar> &plant,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
+ const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y) {
+ Eigen::Matrix<Scalar, number_of_outputs, 1> Y_bar =
Y - (plant.C() * X_hat_ + plant.D() * U);
- Eigen::Matrix<double, number_of_outputs, number_of_outputs> S =
+ Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> S =
plant.C() * P_ * plant.C().transpose() + R_;
- Eigen::Matrix<double, number_of_states, number_of_outputs> KalmanGain;
+ Eigen::Matrix<Scalar, number_of_states, number_of_outputs> KalmanGain;
KalmanGain =
(S.transpose().ldlt().solve((P() * plant.C().transpose()).transpose()))
.transpose();
@@ -350,16 +356,16 @@
number_of_outputs> *plant,
::std::chrono::nanoseconds dt) {
// Now, compute the discrete time Q and R coefficients.
- Eigen::Matrix<double, number_of_states, number_of_states> Qtemp =
+ Eigen::Matrix<Scalar, number_of_states, number_of_states> Qtemp =
(coefficients().Q_continuous +
coefficients().Q_continuous.transpose()) /
- 2.0;
- Eigen::Matrix<double, number_of_outputs, number_of_outputs> Rtemp =
+ static_cast<Scalar>(2.0);
+ Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> Rtemp =
(coefficients().R_continuous +
coefficients().R_continuous.transpose()) /
- 2.0;
+ static_cast<Scalar>(2.0);
- Eigen::Matrix<double, 2 * number_of_states, 2 * number_of_states> M_gain;
+ Eigen::Matrix<Scalar, 2 * number_of_states, 2 * number_of_states> M_gain;
M_gain.setZero();
// Set up the matrix M = [[-A, Q], [0, A.T]]
M_gain.template block<number_of_states, number_of_states>(0, 0) =
@@ -370,35 +376,35 @@
number_of_states, number_of_states) =
plant->coefficients().A_continuous.transpose();
- Eigen::Matrix<double, 2 * number_of_states, 2 *number_of_states> phi =
+ Eigen::Matrix<Scalar, 2 * number_of_states, 2 *number_of_states> phi =
(M_gain *
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+ ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
.count())
.exp();
// Phi12 = phi[0:number_of_states, number_of_states:2*number_of_states]
// Phi22 = phi[number_of_states:2*number_of_states,
// number_of_states:2*number_of_states]
- Eigen::Matrix<double, number_of_states, number_of_states> phi12 =
+ Eigen::Matrix<Scalar, number_of_states, number_of_states> phi12 =
phi.block(0, number_of_states, number_of_states, number_of_states);
- Eigen::Matrix<double, number_of_states, number_of_states> phi22 = phi.block(
+ Eigen::Matrix<Scalar, number_of_states, number_of_states> phi22 = phi.block(
number_of_states, number_of_states, number_of_states, number_of_states);
Q_ = phi22.transpose() * phi12;
- Q_ = (Q_ + Q_.transpose()) / 2.0;
+ Q_ = (Q_ + Q_.transpose()) / static_cast<Scalar>(2.0);
R_ = Rtemp /
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+ ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
.count();
}
// Internal state estimate.
- Eigen::Matrix<double, number_of_states, 1> X_hat_;
+ Eigen::Matrix<Scalar, number_of_states, 1> X_hat_;
// Internal covariance estimate.
- Eigen::Matrix<double, number_of_states, number_of_states> P_;
+ Eigen::Matrix<Scalar, number_of_states, number_of_states> P_;
// Discretized Q and R for the kalman filter.
- Eigen::Matrix<double, number_of_states, number_of_states> Q_;
- Eigen::Matrix<double, number_of_outputs, number_of_outputs> R_;
+ Eigen::Matrix<Scalar, number_of_states, number_of_states> Q_;
+ Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> R_;
int index_ = 0;
::std::vector<::std::unique_ptr<HybridKalmanCoefficients<
diff --git a/frc971/control_loops/hybrid_state_feedback_loop_test.cc b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
index 229bbad..401be44 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop_test.cc
+++ b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
@@ -106,10 +106,10 @@
return HybridKalman<3, 1, 1>(&observers);
}
-StateFeedbackLoop<3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>,
+StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
HybridKalman<3, 1, 1>>
MakeIntegralShooterLoop() {
- return StateFeedbackLoop<3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>,
+ return StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
HybridKalman<3, 1, 1>>(
MakeIntegralShooterPlant(), MakeIntegralShooterController(),
MakeIntegralShooterObserver());
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 50e4314..805e079 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -18,7 +18,8 @@
class ControlLoopWriter(object):
def __init__(self, gain_schedule_name, loops, namespaces=None,
write_constants=False, plant_type='StateFeedbackPlant',
- observer_type='StateFeedbackObserver'):
+ observer_type='StateFeedbackObserver',
+ scalar_type='double'):
"""Constructs a control loop writer.
Args:
@@ -29,6 +30,7 @@
order. If None, the default will be used.
plant_type: string, The C++ type of the plant.
observer_type: string, The C++ type of the observer.
+ scalar_type: string, The C++ type of the base scalar.
"""
self._gain_schedule_name = gain_schedule_name
self._loops = loops
@@ -46,6 +48,7 @@
self._constant_list = []
self._plant_type = plant_type
self._observer_type = observer_type
+ self._scalar_type = scalar_type
def AddConstant(self, constant):
"""Adds a constant to write.
@@ -77,6 +80,8 @@
extra_args = ', ' + extra_args
else:
extra_args = ''
+ if self._scalar_type != 'double':
+ extra_args += ', ' + self._scalar_type
return '%s<%d, %d, %d%s>' % (
typename, num_states, num_inputs, num_outputs, extra_args)
@@ -90,8 +95,16 @@
def _LoopType(self):
"""Returns a template name for StateFeedbackLoop."""
- extra_args = '%s, %s' % (self._PlantType(), self._ObserverType())
- return self._GenericType('StateFeedbackLoop', extra_args)
+ num_states = self._loops[0].A.shape[0]
+ num_inputs = self._loops[0].B.shape[1]
+ num_outputs = self._loops[0].C.shape[0]
+
+ return 'StateFeedbackLoop<%d, %d, %d, %s, %s, %s>' % (
+ num_states,
+ num_inputs,
+ num_outputs, self._scalar_type,
+ self._PlantType(), self._ObserverType())
+
def _PlantType(self):
"""Returns a template name for StateFeedbackPlant."""
@@ -109,10 +122,8 @@
"""Returns a template name for StateFeedbackObserverCoefficients."""
return self._GenericType(self._observer_type + 'Coefficients')
- def WriteHeader(self, header_file, double_appendage=False, MoI_ratio=0.0):
- """Writes the header file to the file named header_file.
- Set double_appendage to true in order to include a ratio of
- moments of inertia constant. Currently, only used for 2014 claw."""
+ def WriteHeader(self, header_file):
+ """Writes the header file to the file named header_file."""
with open(header_file, 'w') as fd:
header_guard = self._HeaderGuard(header_file)
fd.write('#ifndef %s\n'
@@ -133,7 +144,7 @@
for loop in self._loops:
fd.write(loop.DumpPlantHeader(self._PlantCoeffType()))
fd.write('\n')
- fd.write(loop.DumpControllerHeader())
+ fd.write(loop.DumpControllerHeader(self._scalar_type))
fd.write('\n')
fd.write(loop.DumpObserverHeader(self._ObserverCoeffType()))
fd.write('\n')
@@ -167,15 +178,15 @@
fd.write(self._namespace_start)
fd.write('\n\n')
for loop in self._loops:
- fd.write(loop.DumpPlant(self._PlantCoeffType()))
+ fd.write(loop.DumpPlant(self._PlantCoeffType(), self._scalar_type))
fd.write('\n')
for loop in self._loops:
- fd.write(loop.DumpController())
+ fd.write(loop.DumpController(self._scalar_type))
fd.write('\n')
for loop in self._loops:
- fd.write(loop.DumpObserver(self._ObserverCoeffType()))
+ fd.write(loop.DumpObserver(self._ObserverCoeffType(), self._scalar_type))
fd.write('\n')
fd.write('%s Make%sPlant() {\n' %
@@ -288,22 +299,26 @@
self.X_hat = (self.A * self.X_hat + self.B * U +
self.L * (self.Y - self.C * self.X_hat - self.D * U))
- def _DumpMatrix(self, matrix_name, matrix):
+ def _DumpMatrix(self, matrix_name, matrix, scalar_type):
"""Dumps the provided matrix into a variable called matrix_name.
Args:
matrix_name: string, The variable name to save the matrix to.
matrix: The matrix to dump.
+ scalar_type: The C++ type to use for the scalar in the matrix.
Returns:
string, The C++ commands required to populate a variable named matrix_name
with the contents of matrix.
"""
- ans = [' Eigen::Matrix<double, %d, %d> %s;\n' % (
- matrix.shape[0], matrix.shape[1], matrix_name)]
+ ans = [' Eigen::Matrix<%s, %d, %d> %s;\n' % (
+ scalar_type, matrix.shape[0], matrix.shape[1], matrix_name)]
for x in xrange(matrix.shape[0]):
for y in xrange(matrix.shape[1]):
- ans.append(' %s(%d, %d) = %s;\n' % (matrix_name, x, y, repr(matrix[x, y])))
+ write_type = repr(matrix[x, y])
+ if scalar_type == 'float':
+ write_type += 'f'
+ ans.append(' %s(%d, %d) = %s;\n' % (matrix_name, x, y, write_type))
return ''.join(ans)
@@ -316,7 +331,7 @@
return '%s Make%sPlantCoefficients();\n' % (
plant_coefficient_type, self._name)
- def DumpPlant(self, plant_coefficient_type):
+ def DumpPlant(self, plant_coefficient_type, scalar_type):
"""Writes out a c++ function which will create a PlantCoefficients object.
Returns:
@@ -325,21 +340,21 @@
ans = ['%s Make%sPlantCoefficients() {\n' % (
plant_coefficient_type, self._name)]
- ans.append(self._DumpMatrix('C', self.C))
- ans.append(self._DumpMatrix('D', self.D))
- ans.append(self._DumpMatrix('U_max', self.U_max))
- ans.append(self._DumpMatrix('U_min', self.U_min))
+ ans.append(self._DumpMatrix('C', self.C, scalar_type))
+ ans.append(self._DumpMatrix('D', self.D, scalar_type))
+ ans.append(self._DumpMatrix('U_max', self.U_max, scalar_type))
+ ans.append(self._DumpMatrix('U_min', self.U_min, scalar_type))
if plant_coefficient_type.startswith('StateFeedbackPlant'):
- ans.append(self._DumpMatrix('A', self.A))
- ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
- ans.append(self._DumpMatrix('B', self.B))
+ ans.append(self._DumpMatrix('A', self.A, scalar_type))
+ ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A), scalar_type))
+ ans.append(self._DumpMatrix('B', self.B, scalar_type))
ans.append(' return %s'
'(A, A_inv, B, C, D, U_max, U_min);\n' % (
plant_coefficient_type))
elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'):
- ans.append(self._DumpMatrix('A_continuous', self.A_continuous))
- ans.append(self._DumpMatrix('B_continuous', self.B_continuous))
+ ans.append(self._DumpMatrix('A_continuous', self.A_continuous, scalar_type))
+ ans.append(self._DumpMatrix('B_continuous', self.B_continuous, scalar_type))
ans.append(' return %s'
'(A_continuous, B_continuous, C, D, U_max, U_min);\n' % (
plant_coefficient_type))
@@ -361,7 +376,7 @@
"""Returns the name of the controller function."""
return 'Make%sObserverCoefficients()' % self._name
- def DumpControllerHeader(self):
+ def DumpControllerHeader(self, scalar_type):
"""Writes out a c++ header declaration which will create a Controller object.
Returns:
@@ -370,10 +385,11 @@
num_states = self.A.shape[0]
num_inputs = self.B.shape[1]
num_outputs = self.C.shape[0]
- return 'StateFeedbackControllerCoefficients<%d, %d, %d> %s;\n' % (
- num_states, num_inputs, num_outputs, self.ControllerFunction())
+ return 'StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s;\n' % (
+ num_states, num_inputs, num_outputs, scalar_type,
+ self.ControllerFunction())
- def DumpController(self):
+ def DumpController(self, scalar_type):
"""Returns a c++ function which will create a Controller object.
Returns:
@@ -382,18 +398,19 @@
num_states = self.A.shape[0]
num_inputs = self.B.shape[1]
num_outputs = self.C.shape[0]
- ans = ['StateFeedbackControllerCoefficients<%d, %d, %d> %s {\n' % (
- num_states, num_inputs, num_outputs, self.ControllerFunction())]
+ ans = ['StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s {\n' % (
+ num_states, num_inputs, num_outputs, scalar_type,
+ self.ControllerFunction())]
- ans.append(self._DumpMatrix('K', self.K))
+ ans.append(self._DumpMatrix('K', self.K, scalar_type))
if not hasattr(self, 'Kff'):
self.Kff = numpy.matrix(numpy.zeros(self.K.shape))
- ans.append(self._DumpMatrix('Kff', self.Kff))
+ ans.append(self._DumpMatrix('Kff', self.Kff, scalar_type))
- ans.append(' return StateFeedbackControllerCoefficients<%d, %d, %d>'
+ ans.append(' return StateFeedbackControllerCoefficients<%d, %d, %d, %s>'
'(K, Kff);\n' % (
- num_states, num_inputs, num_outputs))
+ num_states, num_inputs, num_outputs, scalar_type))
ans.append('}\n')
return ''.join(ans)
@@ -406,7 +423,7 @@
return '%s %s;\n' % (
observer_coefficient_type, self.ObserverFunction())
- def DumpObserver(self, observer_coefficient_type):
+ def DumpObserver(self, observer_coefficient_type, scalar_type):
"""Returns a c++ function which will create a Observer object.
Returns:
@@ -416,12 +433,12 @@
observer_coefficient_type, self.ObserverFunction())]
if observer_coefficient_type.startswith('StateFeedbackObserver'):
- ans.append(self._DumpMatrix('L', self.L))
+ ans.append(self._DumpMatrix('L', self.L, scalar_type))
ans.append(' return %s(L);\n' % (observer_coefficient_type,))
elif observer_coefficient_type.startswith('HybridKalman'):
- ans.append(self._DumpMatrix('Q_continuous', self.Q_continuous))
- ans.append(self._DumpMatrix('R_continuous', self.R_continuous))
- ans.append(self._DumpMatrix('P_steady_state', self.P_steady_state))
+ ans.append(self._DumpMatrix('Q_continuous', self.Q_continuous, scalar_type))
+ ans.append(self._DumpMatrix('R_continuous', self.R_continuous, scalar_type))
+ ans.append(self._DumpMatrix('P_steady_state', self.P_steady_state, scalar_type))
ans.append(' return %s(Q_continuous, R_continuous, P_steady_state);\n' % (
observer_coefficient_type,))
else:
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 34a0fcf..b9765fc 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -16,7 +16,7 @@
#include "aos/common/macros.h"
template <int number_of_states, int number_of_inputs, int number_of_outputs,
- typename PlantType, typename ObserverType>
+ typename PlantType, typename ObserverType, typename Scalar>
class StateFeedbackLoop;
// For everything in this file, "inputs" and "outputs" are defined from the
@@ -26,7 +26,8 @@
// controller (U is an output because that's what goes to the motors and Y is an
// input because that's what comes back from the sensors).
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double>
struct StateFeedbackPlantCoefficients final {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
@@ -41,32 +42,33 @@
U_max(other.U_max) {}
StateFeedbackPlantCoefficients(
- const Eigen::Matrix<double, number_of_states, number_of_states> &A,
- const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv,
- const Eigen::Matrix<double, number_of_states, number_of_inputs> &B,
- const Eigen::Matrix<double, number_of_outputs, number_of_states> &C,
- const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D,
- const Eigen::Matrix<double, number_of_inputs, 1> &U_max,
- const Eigen::Matrix<double, number_of_inputs, 1> &U_min)
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A,
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A_inv,
+ const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> &B,
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> &C,
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> &D,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min)
: A(A), A_inv(A_inv), B(B), C(C), D(D), U_min(U_min), U_max(U_max) {}
- const Eigen::Matrix<double, number_of_states, number_of_states> A;
- const Eigen::Matrix<double, number_of_states, number_of_states> A_inv;
- const Eigen::Matrix<double, number_of_states, number_of_inputs> B;
- const Eigen::Matrix<double, number_of_outputs, number_of_states> C;
- const Eigen::Matrix<double, number_of_outputs, number_of_inputs> D;
- const Eigen::Matrix<double, number_of_inputs, 1> U_min;
- const Eigen::Matrix<double, number_of_inputs, 1> U_max;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> A;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> A_inv;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> B;
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> C;
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> D;
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> U_min;
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> U_max;
};
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double>
class StateFeedbackPlant {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
StateFeedbackPlant(
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
- number_of_states, number_of_inputs, number_of_outputs>>>
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
*coefficients)
: coefficients_(::std::move(*coefficients)), index_(0) {
Reset();
@@ -81,53 +83,53 @@
virtual ~StateFeedbackPlant() {}
- const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A() const {
return coefficients().A;
}
- double A(int i, int j) const { return A()(i, j); }
- const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv() const {
+ Scalar A(int i, int j) const { return A()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A_inv() const {
return coefficients().A_inv;
}
- double A_inv(int i, int j) const { return A_inv()(i, j); }
- const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
+ Scalar A_inv(int i, int j) const { return A_inv()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> &B() const {
return coefficients().B;
}
- double B(int i, int j) const { return B()(i, j); }
- const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
+ Scalar B(int i, int j) const { return B()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> &C() const {
return coefficients().C;
}
- double C(int i, int j) const { return C()(i, j); }
- const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D() const {
+ Scalar C(int i, int j) const { return C()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> &D() const {
return coefficients().D;
}
- double D(int i, int j) const { return D()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
+ Scalar D(int i, int j) const { return D()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min() const {
return coefficients().U_min;
}
- double U_min(int i, int j) const { return U_min()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, 1> &U_max() const {
+ Scalar U_min(int i, int j) const { return U_min()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max() const {
return coefficients().U_max;
}
- double U_max(int i, int j) const { return U_max()(i, j); }
+ Scalar U_max(int i, int j) const { return U_max()(i, j); }
- const Eigen::Matrix<double, number_of_states, 1> &X() const { return X_; }
- double X(int i, int j) const { return X()(i, j); }
- const Eigen::Matrix<double, number_of_outputs, 1> &Y() const { return Y_; }
- double Y(int i, int j) const { return Y()(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); }
+ const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y() const { return Y_; }
+ Scalar Y(int i, int j) const { return Y()(i, j); }
- Eigen::Matrix<double, number_of_states, 1> &mutable_X() { return X_; }
- double &mutable_X(int i, int j) { return mutable_X()(i, j); }
- Eigen::Matrix<double, number_of_outputs, 1> &mutable_Y() { return Y_; }
- double &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
+ Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X() { return X_; }
+ Scalar &mutable_X(int i, int j) { return mutable_X()(i, j); }
+ Eigen::Matrix<Scalar, number_of_outputs, 1> &mutable_Y() { return Y_; }
+ Scalar &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
+ number_of_outputs, Scalar>
&coefficients(int index) const {
return *coefficients_[index];
}
const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
+ number_of_outputs, Scalar>
&coefficients() const {
return *coefficients_[index_];
}
@@ -145,16 +147,17 @@
}
// Assert that U is within the hardware range.
- virtual void CheckU(const Eigen::Matrix<double, number_of_inputs, 1> &U) {
+ virtual void CheckU(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U) {
for (int i = 0; i < kNumInputs; ++i) {
- if (U(i, 0) > U_max(i, 0) + 0.00001 || U(i, 0) < U_min(i, 0) - 0.00001) {
+ if (U(i, 0) > U_max(i, 0) + static_cast<Scalar>(0.00001) ||
+ U(i, 0) < U_min(i, 0) - static_cast<Scalar>(0.00001)) {
LOG(FATAL, "U out of range\n");
}
}
}
// Computes the new X and Y given the control input.
- void Update(const Eigen::Matrix<double, number_of_inputs, 1> &U) {
+ void Update(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U) {
// Powers outside of the range are more likely controller bugs than things
// that the plant should deal with.
CheckU(U);
@@ -163,13 +166,13 @@
}
// Computes the new Y given the control input.
- void UpdateY(const Eigen::Matrix<double, number_of_inputs, 1> &U) {
+ void UpdateY(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U) {
Y_ = C() * X() + D() * U;
}
- Eigen::Matrix<double, number_of_states, 1> Update(
- const Eigen::Matrix<double, number_of_states, 1> X,
- const Eigen::Matrix<double, number_of_inputs, 1> &U) const {
+ Eigen::Matrix<Scalar, number_of_states, 1> Update(
+ const Eigen::Matrix<Scalar, number_of_states, 1> X,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U) const {
return A() * X + B() * U;
}
@@ -180,11 +183,11 @@
static const int kNumInputs = number_of_inputs;
private:
- Eigen::Matrix<double, number_of_states, 1> X_;
- Eigen::Matrix<double, number_of_outputs, 1> Y_;
+ Eigen::Matrix<Scalar, number_of_states, 1> X_;
+ Eigen::Matrix<Scalar, number_of_outputs, 1> Y_;
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
- number_of_states, number_of_inputs, number_of_outputs>>>
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
coefficients_;
int index_;
@@ -193,27 +196,30 @@
};
// A container for all the controller coefficients.
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double>
struct StateFeedbackControllerCoefficients final {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- const Eigen::Matrix<double, number_of_inputs, number_of_states> K;
- const Eigen::Matrix<double, number_of_inputs, number_of_states> Kff;
+ const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> K;
+ const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> Kff;
StateFeedbackControllerCoefficients(
- const Eigen::Matrix<double, number_of_inputs, number_of_states> &K,
- const Eigen::Matrix<double, number_of_inputs, number_of_states> &Kff)
+ const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> &K,
+ const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> &Kff)
: K(K), Kff(Kff) {}
};
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double>
class StateFeedbackController {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
explicit StateFeedbackController(
::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<
- number_of_states, number_of_inputs, number_of_outputs>>> *controllers)
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
+ *controllers)
: coefficients_(::std::move(*controllers)) {}
StateFeedbackController(StateFeedbackController &&other)
@@ -221,14 +227,14 @@
::std::swap(coefficients_, other.coefficients_);
}
- const Eigen::Matrix<double, number_of_inputs, number_of_states> &K() const {
+ const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> &K() const {
return coefficients().K;
}
- double K(int i, int j) const { return K()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, number_of_states> &Kff() const {
+ Scalar K(int i, int j) const { return K()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> &Kff() const {
return coefficients().Kff;
}
- double Kff(int i, int j) const { return Kff()(i, j); }
+ Scalar Kff(int i, int j) const { return Kff()(i, j); }
void Reset() {}
@@ -246,13 +252,13 @@
int index() const { return index_; }
const StateFeedbackControllerCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
+ number_of_outputs, Scalar>
&coefficients(int index) const {
return *coefficients_[index];
}
const StateFeedbackControllerCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
+ number_of_outputs, Scalar>
&coefficients() const {
return *coefficients_[index_];
}
@@ -260,30 +266,32 @@
private:
int index_ = 0;
::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<
- number_of_states, number_of_inputs, number_of_outputs>>>
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
coefficients_;
};
// A container for all the observer coefficients.
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double>
struct StateFeedbackObserverCoefficients final {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> L;
StateFeedbackObserverCoefficients(
- const Eigen::Matrix<double, number_of_states, number_of_outputs> &L)
+ const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> &L)
: L(L) {}
};
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double>
class StateFeedbackObserver {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
explicit StateFeedbackObserver(
::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
- number_of_states, number_of_inputs, number_of_outputs>>> *observers)
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>>> *observers)
: coefficients_(::std::move(*observers)) {}
StateFeedbackObserver(StateFeedbackObserver &&other)
@@ -291,32 +299,32 @@
::std::swap(coefficients_, other.coefficients_);
}
- const Eigen::Matrix<double, number_of_states, number_of_outputs> &L() const {
+ const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> &L() const {
return coefficients().L;
}
- double L(int i, int j) const { return L()(i, j); }
+ Scalar L(int i, int j) const { return L()(i, j); }
- const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
+ const Eigen::Matrix<Scalar, number_of_states, 1> &X_hat() const {
return X_hat_;
}
- Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
+ Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
void Reset(StateFeedbackPlant<number_of_states, number_of_inputs,
- number_of_outputs> * /*loop*/) {
+ number_of_outputs, Scalar> * /*loop*/) {
X_hat_.setZero();
}
void Predict(StateFeedbackPlant<number_of_states, number_of_inputs,
- number_of_outputs> *plant,
- const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
+ number_of_outputs, Scalar> *plant,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
::std::chrono::nanoseconds /*dt*/) {
mutable_X_hat() = plant->Update(X_hat(), new_u);
}
void Correct(const StateFeedbackPlant<number_of_states, number_of_inputs,
- number_of_outputs> &plant,
- const Eigen::Matrix<double, number_of_inputs, 1> &U,
- const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
+ number_of_outputs, Scalar> &plant,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
+ const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y) {
mutable_X_hat() +=
plant.A_inv() * L() * (Y - plant.C() * X_hat() - plant.D() * U);
}
@@ -335,32 +343,33 @@
int index() const { return index_; }
const StateFeedbackObserverCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
+ number_of_outputs, Scalar>
&coefficients(int index) const {
return *coefficients_[index];
}
const StateFeedbackObserverCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
+ number_of_outputs, Scalar>
&coefficients() const {
return *coefficients_[index_];
}
private:
// Internal state estimate.
- Eigen::Matrix<double, number_of_states, 1> X_hat_;
+ Eigen::Matrix<Scalar, number_of_states, 1> X_hat_;
int index_ = 0;
::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
- number_of_states, number_of_inputs, number_of_outputs>>>
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
coefficients_;
};
template <int number_of_states, int number_of_inputs, int number_of_outputs,
+ typename Scalar = double,
typename PlantType = StateFeedbackPlant<
- number_of_states, number_of_inputs, number_of_outputs>,
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>,
typename ObserverType = StateFeedbackObserver<
- number_of_states, number_of_inputs, number_of_outputs>>
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>>
class StateFeedbackLoop {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
@@ -368,7 +377,7 @@
explicit StateFeedbackLoop(
PlantType &&plant,
StateFeedbackController<number_of_states, number_of_inputs,
- number_of_outputs> &&controller,
+ number_of_outputs, Scalar> &&controller,
ObserverType &&observer)
: plant_(::std::move(plant)),
controller_(::std::move(controller)),
@@ -389,43 +398,43 @@
virtual ~StateFeedbackLoop() {}
- const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
+ const Eigen::Matrix<Scalar, number_of_states, 1> &X_hat() const {
return observer().X_hat();
}
- double X_hat(int i, int j) const { return X_hat()(i, j); }
- const Eigen::Matrix<double, number_of_states, 1> &R() const { return R_; }
- double R(int i, int j) const { return R()(i, j); }
- const Eigen::Matrix<double, number_of_states, 1> &next_R() const {
+ Scalar X_hat(int i, int j) const { return X_hat()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_states, 1> &R() const { return R_; }
+ Scalar R(int i, int j) const { return R()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_states, 1> &next_R() const {
return next_R_;
}
- double next_R(int i, int j) const { return next_R()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, 1> &U() const { return U_; }
- double U(int i, int j) const { return U()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, 1> &U_uncapped() const {
+ Scalar next_R(int i, int j) const { return next_R()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U() const { return U_; }
+ Scalar U(int i, int j) const { return U()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_uncapped() const {
return U_uncapped_;
}
- double U_uncapped(int i, int j) const { return U_uncapped()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, 1> &ff_U() const {
+ Scalar U_uncapped(int i, int j) const { return U_uncapped()(i, j); }
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &ff_U() const {
return ff_U_;
}
- double ff_U(int i, int j) const { return ff_U()(i, j); }
+ Scalar ff_U(int i, int j) const { return ff_U()(i, j); }
- Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() {
+ Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X_hat() {
return observer_.mutable_X_hat();
}
- double &mutable_X_hat(int i, int j) { return mutable_X_hat()(i, j); }
- Eigen::Matrix<double, number_of_states, 1> &mutable_R() { return R_; }
- double &mutable_R(int i, int j) { return mutable_R()(i, j); }
- Eigen::Matrix<double, number_of_states, 1> &mutable_next_R() {
+ Scalar &mutable_X_hat(int i, int j) { return mutable_X_hat()(i, j); }
+ Eigen::Matrix<Scalar, number_of_states, 1> &mutable_R() { return R_; }
+ Scalar &mutable_R(int i, int j) { return mutable_R()(i, j); }
+ Eigen::Matrix<Scalar, number_of_states, 1> &mutable_next_R() {
return next_R_;
}
- double &mutable_next_R(int i, int j) { return mutable_next_R()(i, j); }
- Eigen::Matrix<double, number_of_inputs, 1> &mutable_U() { return U_; }
- double &mutable_U(int i, int j) { return mutable_U()(i, j); }
- Eigen::Matrix<double, number_of_inputs, 1> &mutable_U_uncapped() {
+ Scalar &mutable_next_R(int i, int j) { return mutable_next_R()(i, j); }
+ Eigen::Matrix<Scalar, number_of_inputs, 1> &mutable_U() { return U_; }
+ Scalar &mutable_U(int i, int j) { return mutable_U()(i, j); }
+ Eigen::Matrix<Scalar, number_of_inputs, 1> &mutable_U_uncapped() {
return U_uncapped_;
}
- double &mutable_U_uncapped(int i, int j) {
+ Scalar &mutable_U_uncapped(int i, int j) {
return mutable_U_uncapped()(i, j);
}
@@ -433,7 +442,7 @@
PlantType *mutable_plant() { return &plant_; }
const StateFeedbackController<number_of_states, number_of_inputs,
- number_of_outputs>
+ number_of_outputs, Scalar>
&controller() const {
return controller_;
}
@@ -465,23 +474,23 @@
}
// Corrects X_hat given the observation in Y.
- void Correct(const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
+ void Correct(const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y) {
observer_.Correct(this->plant(), U(), Y);
}
- const Eigen::Matrix<double, number_of_states, 1> error() const {
+ const Eigen::Matrix<Scalar, number_of_states, 1> error() const {
return R() - X_hat();
}
// Returns the calculated controller power.
- virtual const Eigen::Matrix<double, number_of_inputs, 1> ControllerOutput() {
+ virtual const Eigen::Matrix<Scalar, number_of_inputs, 1> ControllerOutput() {
// TODO(austin): Should this live in StateSpaceController?
ff_U_ = FeedForward();
return controller().K() * error() + ff_U_;
}
// Calculates the feed forwards power.
- virtual const Eigen::Matrix<double, number_of_inputs, 1> FeedForward() {
+ virtual const Eigen::Matrix<Scalar, number_of_inputs, 1> FeedForward() {
// TODO(austin): Should this live in StateSpaceController?
return controller().Kff() * (next_R() - plant().A() * R());
}
@@ -511,7 +520,7 @@
}
}
- void UpdateObserver(const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
+ void UpdateObserver(const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
::std::chrono::nanoseconds dt) {
observer_.Predict(this->mutable_plant(), new_u, dt);
}
@@ -528,7 +537,8 @@
protected:
PlantType plant_;
- StateFeedbackController<number_of_states, number_of_inputs, number_of_outputs>
+ StateFeedbackController<number_of_states, number_of_inputs, number_of_outputs,
+ Scalar>
controller_;
ObserverType observer_;
@@ -539,17 +549,17 @@
static constexpr int kNumInputs = number_of_inputs;
// Portion of U which is based on the feed-forwards.
- Eigen::Matrix<double, number_of_inputs, 1> ff_U_;
+ Eigen::Matrix<Scalar, number_of_inputs, 1> ff_U_;
private:
// Current goal (Used by the feed-back controller).
- Eigen::Matrix<double, number_of_states, 1> R_;
+ Eigen::Matrix<Scalar, number_of_states, 1> R_;
// Goal to go to in the next cycle (Used by Feed-Forward controller.)
- Eigen::Matrix<double, number_of_states, 1> next_R_;
+ Eigen::Matrix<Scalar, number_of_states, 1> next_R_;
// Computed output after being capped.
- Eigen::Matrix<double, number_of_inputs, 1> U_;
+ Eigen::Matrix<Scalar, number_of_inputs, 1> U_;
// Computed output before being capped.
- Eigen::Matrix<double, number_of_inputs, 1> U_uncapped_;
+ Eigen::Matrix<Scalar, number_of_inputs, 1> U_uncapped_;
DISALLOW_COPY_AND_ASSIGN(StateFeedbackLoop);
};