Split StatespaceLoop into a Plant, Controller, and Observer.
This doesn't yet move any of the logic out of the Loop.
Change-Id: I2cb0ea6d1a75c7011576ba752c50e512eeff5890
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index 5fc4b17..91c2fc9 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -163,7 +163,7 @@
::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
int min_FF_sum_index;
const double min_FF_sum = FF_sum.minCoeff(&min_FF_sum_index);
- const double min_K_sum = loop_->K().col(min_FF_sum_index).sum();
+ const double min_K_sum = loop_->controller().K().col(min_FF_sum_index).sum();
const double high_min_FF_sum = FF_high.col(0).sum();
const double adjusted_ff_voltage =
@@ -244,11 +244,13 @@
// Construct a constraint on R by manipulating the constraint on U
::aos::controls::HVPolytope<2, 4, 4> R_poly_hv(
- U_Poly_.static_H() * (loop_->K() + FF),
- U_Poly_.static_k() + U_Poly_.static_H() * loop_->K() * loop_->X_hat(),
- (loop_->K() + FF).inverse() *
- ::aos::controls::ShiftPoints<2, 4>(U_Poly_.StaticVertices(),
- loop_->K() * loop_->X_hat()));
+ U_Poly_.static_H() * (loop_->controller().K() + FF),
+ U_Poly_.static_k() +
+ U_Poly_.static_H() * loop_->controller().K() * loop_->X_hat(),
+ (loop_->controller().K() + FF).inverse() *
+ ::aos::controls::ShiftPoints<2, 4>(
+ U_Poly_.StaticVertices(),
+ loop_->controller().K() * loop_->X_hat()));
// Limit R back inside the box.
loop_->mutable_R() =
@@ -257,7 +259,7 @@
const Eigen::Matrix<double, 2, 1> FF_volts = FF * loop_->R();
const Eigen::Matrix<double, 2, 1> U_ideal =
- loop_->K() * (loop_->R() - loop_->X_hat()) + FF_volts;
+ loop_->controller().K() * (loop_->R() - loop_->X_hat()) + FF_volts;
for (int i = 0; i < 2; i++) {
loop_->mutable_U()[i] = ::aos::Clip(U_ideal[i], -12, 12);
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index b7a68a9..4aeb74b 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -35,9 +35,11 @@
LOG_MATRIX(DEBUG, "U_uncapped", *U);
Eigen::Matrix<double, 2, 2> position_K;
- position_K << kf_->K(0, 0), kf_->K(0, 2), kf_->K(1, 0), kf_->K(1, 2);
+ position_K << kf_->controller().K(0, 0), kf_->controller().K(0, 2),
+ kf_->controller().K(1, 0), kf_->controller().K(1, 2);
Eigen::Matrix<double, 2, 2> velocity_K;
- velocity_K << kf_->K(0, 1), kf_->K(0, 3), kf_->K(1, 1), kf_->K(1, 3);
+ velocity_K << kf_->controller().K(0, 1), kf_->controller().K(0, 3),
+ kf_->controller().K(1, 1), kf_->controller().K(1, 3);
Eigen::Matrix<double, 2, 1> position_error;
position_error << error(0, 0), error(2, 0);
@@ -144,7 +146,7 @@
goal.right_velocity_goal, 0.0, 0.0, 0.0;
use_profile_ =
- !kf_->Kff().isZero(0) &&
+ !kf_->controller().Kff().isZero(0) &&
(goal.linear.max_velocity != 0.0 && goal.linear.max_acceleration != 0.0 &&
goal.angular.max_velocity != 0.0 &&
goal.angular.max_acceleration != 0.0);
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 91e7ae7..4d09bbc 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -259,8 +259,10 @@
status->estimator_state = this->EstimatorState(0);
Eigen::Matrix<double, 3, 1> error = this->controller().error();
- status->position_power = this->controller().K(0, 0) * error(0, 0);
- status->velocity_power = this->controller().K(0, 1) * error(1, 0);
+ status->position_power =
+ this->controller().controller().K(0, 0) * error(0, 0);
+ status->velocity_power =
+ this->controller().controller().K(0, 1) * error(1, 0);
}
template <class ZeroingEstimator>
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index e8900af..77fd905 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -71,8 +71,12 @@
typename, num_states, num_inputs, num_outputs)
def _ControllerType(self):
- """Returns a template name for StateFeedbackControllerConstants."""
- return self._GenericType('StateFeedbackControllerConstants')
+ """Returns a template name for StateFeedbackController."""
+ return self._GenericType('StateFeedbackController')
+
+ def _ObserverType(self):
+ """Returns a template name for StateFeedbackObserver."""
+ return self._GenericType('StateFeedbackObserver')
def _LoopType(self):
"""Returns a template name for StateFeedbackLoop."""
@@ -82,10 +86,18 @@
"""Returns a template name for StateFeedbackPlant."""
return self._GenericType('StateFeedbackPlant')
- def _CoeffType(self):
+ def _PlantCoeffType(self):
"""Returns a template name for StateFeedbackPlantCoefficients."""
return self._GenericType('StateFeedbackPlantCoefficients')
+ def _ControllerCoeffType(self):
+ """Returns a template name for StateFeedbackControllerCoefficients."""
+ return self._GenericType('StateFeedbackControllerCoefficients')
+
+ def _ObserverCoeffType(self):
+ """Returns a template name for StateFeedbackObserverCoefficients."""
+ return self._GenericType('StateFeedbackObserverCoefficients')
+
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
@@ -108,10 +120,18 @@
fd.write('\n')
fd.write(loop.DumpControllerHeader())
fd.write('\n')
+ fd.write(loop.DumpObserverHeader())
+ fd.write('\n')
fd.write('%s Make%sPlant();\n\n' %
(self._PlantType(), self._gain_schedule_name))
+ fd.write('%s Make%sController();\n\n' %
+ (self._ControllerType(), self._gain_schedule_name))
+
+ fd.write('%s Make%sObserver();\n\n' %
+ (self._ObserverType(), self._gain_schedule_name))
+
fd.write('%s Make%sLoop();\n\n' %
(self._LoopType(), self._gain_schedule_name))
@@ -139,27 +159,48 @@
fd.write(loop.DumpController())
fd.write('\n')
+ for loop in self._loops:
+ fd.write(loop.DumpObserver())
+ fd.write('\n')
+
fd.write('%s Make%sPlant() {\n' %
(self._PlantType(), self._gain_schedule_name))
fd.write(' ::std::vector< ::std::unique_ptr<%s>> plants(%d);\n' % (
- self._CoeffType(), len(self._loops)))
+ self._PlantCoeffType(), len(self._loops)))
for index, loop in enumerate(self._loops):
fd.write(' plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
- (index, self._CoeffType(), self._CoeffType(),
+ (index, self._PlantCoeffType(), self._PlantCoeffType(),
loop.PlantFunction()))
fd.write(' return %s(&plants);\n' % self._PlantType())
fd.write('}\n\n')
- fd.write('%s Make%sLoop() {\n' %
- (self._LoopType(), self._gain_schedule_name))
+ fd.write('%s Make%sController() {\n' %
+ (self._ControllerType(), self._gain_schedule_name))
fd.write(' ::std::vector< ::std::unique_ptr<%s>> controllers(%d);\n' % (
- self._ControllerType(), len(self._loops)))
+ self._ControllerCoeffType(), len(self._loops)))
for index, loop in enumerate(self._loops):
fd.write(' controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
- (index, self._ControllerType(), self._ControllerType(),
+ (index, self._ControllerCoeffType(), self._ControllerCoeffType(),
loop.ControllerFunction()))
- fd.write(' return %s(Make%sPlant(), &controllers);\n' %
- (self._LoopType(), self._gain_schedule_name))
+ fd.write(' return %s(&controllers);\n' % self._ControllerType())
+ fd.write('}\n\n')
+
+ fd.write('%s Make%sObserver() {\n' %
+ (self._ObserverType(), self._gain_schedule_name))
+ fd.write(' ::std::vector< ::std::unique_ptr<%s>> observers(%d);\n' % (
+ self._ObserverCoeffType(), len(self._loops)))
+ for index, loop in enumerate(self._loops):
+ fd.write(' 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('}\n\n')
+
+ fd.write('%s Make%sLoop() {\n' %
+ (self._LoopType(), self._gain_schedule_name))
+ fd.write(' return %s(Make%sPlant(), Make%sController(), Make%sObserver());\n' %
+ (self._LoopType(), self._gain_schedule_name,
+ self._gain_schedule_name, self._gain_schedule_name))
fd.write('}\n\n')
fd.write(self._namespace_end)
@@ -298,7 +339,11 @@
def ControllerFunction(self):
"""Returns the name of the controller function."""
- return 'Make%sController()' % self._name
+ return 'Make%sControllerCoefficients()' % self._name
+
+ def ObserverFunction(self):
+ """Returns the name of the controller function."""
+ return 'Make%sObserverCoefficients()' % self._name
def DumpControllerHeader(self):
"""Writes out a c++ header declaration which will create a Controller object.
@@ -309,7 +354,7 @@
num_states = self.A.shape[0]
num_inputs = self.B.shape[1]
num_outputs = self.C.shape[0]
- return 'StateFeedbackControllerConstants<%d, %d, %d> %s;\n' % (
+ return 'StateFeedbackControllerCoefficients<%d, %d, %d> %s;\n' % (
num_states, num_inputs, num_outputs, self.ControllerFunction())
def DumpController(self):
@@ -321,18 +366,48 @@
num_states = self.A.shape[0]
num_inputs = self.B.shape[1]
num_outputs = self.C.shape[0]
- ans = ['StateFeedbackControllerConstants<%d, %d, %d> %s {\n' % (
+ ans = ['StateFeedbackControllerCoefficients<%d, %d, %d> %s {\n' % (
num_states, num_inputs, num_outputs, self.ControllerFunction())]
- ans.append(self._DumpMatrix('L', self.L))
ans.append(self._DumpMatrix('K', self.K))
if not hasattr(self, 'Kff'):
self.Kff = numpy.matrix(numpy.zeros(self.K.shape))
ans.append(self._DumpMatrix('Kff', self.Kff))
- ans.append(' return StateFeedbackControllerConstants<%d, %d, %d>'
- '(L, K, Kff);\n' % (
+ ans.append(' return StateFeedbackControllerCoefficients<%d, %d, %d>'
+ '(K, Kff);\n' % (
num_states, num_inputs, num_outputs))
ans.append('}\n')
return ''.join(ans)
+
+ def DumpObserverHeader(self):
+ """Writes out a c++ header declaration which will create a Observer object.
+
+ Returns:
+ string, The header declaration for the function.
+ """
+ num_states = self.A.shape[0]
+ num_inputs = self.B.shape[1]
+ num_outputs = self.C.shape[0]
+ return 'StateFeedbackObserverCoefficients<%d, %d, %d> %s;\n' % (
+ num_states, num_inputs, num_outputs, self.ObserverFunction())
+
+ def DumpObserver(self):
+ """Returns a c++ function which will create a Observer object.
+
+ Returns:
+ string, The function which will create the object.
+ """
+ num_states = self.A.shape[0]
+ num_inputs = self.B.shape[1]
+ num_outputs = self.C.shape[0]
+ ans = ['StateFeedbackObserverCoefficients<%d, %d, %d> %s {\n' % (
+ num_states, num_inputs, num_outputs, self.ObserverFunction())]
+
+ ans.append(self._DumpMatrix('L', self.L))
+
+ ans.append(' return StateFeedbackObserverCoefficients<%d, %d, %d>'
+ '(L);\n' % (num_states, num_inputs, num_outputs))
+ ans.append('}\n')
+ return ''.join(ans)
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 3ed1a9e..402585a 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -191,22 +191,143 @@
DISALLOW_COPY_AND_ASSIGN(StateFeedbackPlant);
};
-// A Controller is a structure which holds a plant and the K and L matrices.
-// This is designed such that multiple controllers can share one set of state to
-// support gain scheduling easily.
+// A container for all the controller coefficients.
template <int number_of_states, int number_of_inputs, int number_of_outputs>
-struct StateFeedbackControllerConstants final {
+struct StateFeedbackControllerCoefficients final {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
const Eigen::Matrix<double, number_of_inputs, number_of_states> K;
const Eigen::Matrix<double, number_of_inputs, number_of_states> Kff;
- StateFeedbackControllerConstants(
- const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
+ StateFeedbackControllerCoefficients(
const Eigen::Matrix<double, number_of_inputs, number_of_states> &K,
const Eigen::Matrix<double, number_of_inputs, number_of_states> &Kff)
- : L(L), K(K), Kff(Kff) {}
+ : K(K), Kff(Kff) {}
+};
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+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)
+ : coefficients_(::std::move(*controllers)) {}
+
+ StateFeedbackController(StateFeedbackController &&other)
+ : index_(other.index_) {
+ ::std::swap(coefficients_, other.coefficients_);
+ }
+
+ const Eigen::Matrix<double, 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 {
+ return coefficients().Kff;
+ }
+ double Kff(int i, int j) const { return Kff()(i, j); }
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> &L() const {
+ return coefficients().L;
+ }
+ double L(int i, int j) const { return L()(i, j); }
+
+ // Sets the current controller to be index, clamped to be within range.
+ void set_index(int index) {
+ if (index < 0) {
+ index_ = 0;
+ } else if (index >= static_cast<int>(coefficients_.size())) {
+ index_ = static_cast<int>(coefficients_.size()) - 1;
+ } else {
+ index_ = index;
+ }
+ }
+
+ int index() const { return index_; }
+
+ const StateFeedbackControllerCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients(int index) const {
+ return *coefficients_[index];
+ }
+
+ const StateFeedbackControllerCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients() const {
+ return *coefficients_[index_];
+ }
+
+ private:
+ int index_ = 0;
+ ::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ coefficients_;
+};
+
+
+// A container for all the observer coefficients.
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+struct StateFeedbackObserverCoefficients final {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
+
+ StateFeedbackObserverCoefficients(
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> &L)
+ : L(L) {}
+};
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+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)
+ : coefficients_(::std::move(*observers)) {}
+
+ StateFeedbackObserver(StateFeedbackObserver &&other)
+ : index_(other.index_) {
+ ::std::swap(coefficients_, other.coefficients_);
+ }
+
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> &L() const {
+ return coefficients().L;
+ }
+ double L(int i, int j) const { return L()(i, j); }
+
+ // Sets the current controller to be index, clamped to be within range.
+ void set_index(int index) {
+ if (index < 0) {
+ index_ = 0;
+ } else if (index >= static_cast<int>(coefficients_.size())) {
+ index_ = static_cast<int>(coefficients_.size()) - 1;
+ } else {
+ index_ = index;
+ }
+ }
+
+ int index() const { return index_; }
+
+ const StateFeedbackObserverCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients(int index) const {
+ return *coefficients_[index];
+ }
+
+ const StateFeedbackObserverCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients() const {
+ return *coefficients_[index_];
+ }
+
+ private:
+ int index_ = 0;
+ ::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ coefficients_;
};
template <int number_of_states, int number_of_inputs, int number_of_outputs>
@@ -214,41 +335,35 @@
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- StateFeedbackLoop(
+ explicit StateFeedbackLoop(
StateFeedbackPlant<number_of_states, number_of_inputs, number_of_outputs>
&&plant,
- ::std::vector<::std::unique_ptr<StateFeedbackControllerConstants<
- number_of_states, number_of_inputs, number_of_outputs>>> *controllers)
+ StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs> &&controller,
+ StateFeedbackObserver<number_of_states, number_of_inputs,
+ number_of_outputs> &&observer)
: plant_(::std::move(plant)),
- controllers_(::std::move(*controllers)),
- index_(0) {
+ controller_(::std::move(controller)),
+ observer_(::std::move(observer)) {
Reset();
}
StateFeedbackLoop(StateFeedbackLoop &&other)
- : plant_(::std::move(other.plant_)) {
+ : plant_(::std::move(other.plant_)),
+ controller_(::std::move(other.controller_)),
+ observer_(::std::move(other.observer_)) {
X_hat_.swap(other.X_hat_);
R_.swap(other.R_);
next_R_.swap(other.next_R_);
U_.swap(other.U_);
U_uncapped_.swap(other.U_uncapped_);
ff_U_.swap(other.ff_U_);
- ::std::swap(controllers_, other.controllers_);
- index_ = other.index_;
}
virtual ~StateFeedbackLoop() {}
- const Eigen::Matrix<double, number_of_inputs, number_of_states> &K() const {
- return controller().K;
- }
- double K(int i, int j) const { return K()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, number_of_states> &Kff() const {
- return controller().Kff;
- }
- double Kff(int i, int j) const { return Kff()(i, j); }
const Eigen::Matrix<double, number_of_states, number_of_outputs> &L() const {
- return controller().L;
+ return observer().L();
}
double L(int i, int j) const { return L()(i, j); }
@@ -296,16 +411,16 @@
return plant_;
}
- const StateFeedbackControllerConstants<number_of_states, number_of_inputs,
- number_of_outputs>
+ const StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs>
&controller() const {
- return *controllers_[index_];
+ return controller_;
}
- const StateFeedbackControllerConstants<number_of_states, number_of_inputs,
- number_of_outputs>
- &controller(int index) const {
- return *controllers_[index];
+ const StateFeedbackObserver<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &observer() const {
+ return observer_;
}
void Reset() {
@@ -341,13 +456,15 @@
// Returns the calculated controller power.
virtual const Eigen::Matrix<double, number_of_inputs, 1> ControllerOutput() {
+ // TODO(austin): Should this live in StateSpaceController?
ff_U_ = FeedForward();
- return K() * error() + ff_U_;
+ return controller().K() * error() + ff_U_;
}
// Calculates the feed forwards power.
virtual const Eigen::Matrix<double, number_of_inputs, 1> FeedForward() {
- return Kff() * (next_R() - plant().A() * R());
+ // TODO(austin): Should this live in StateSpaceController?
+ return controller().Kff() * (next_R() - plant().A() * R());
}
// stop_motors is whether or not to output all 0s.
@@ -369,36 +486,33 @@
// Updates R() after any CapU operations happen on U().
void UpdateFFReference() {
ff_U_ -= U_uncapped() - U();
- if (!Kff().isZero(0)) {
+ if (!controller().Kff().isZero(0)) {
R_ = plant().A() * R() + plant().B() * ff_U_;
}
}
void UpdateObserver(const Eigen::Matrix<double, number_of_inputs, 1> &new_u) {
+ // TODO(austin): Should this live in StateSpacePlant?
X_hat_ = plant().A() * X_hat() + plant().B() * new_u;
}
- // Sets the current controller to be index, clamped to be within range.
+ // Sets the current controller to be index.
void set_index(int index) {
- if (index < 0) {
- index_ = 0;
- } else if (index >= static_cast<int>(controllers_.size())) {
- index_ = static_cast<int>(controllers_.size()) - 1;
- } else {
- index_ = index;
- }
+ controller_.set_index(index);
plant_.set_index(index);
}
- int index() const { return index_; }
+ int index() const { return plant_.index(); }
protected:
StateFeedbackPlant<number_of_states, number_of_inputs, number_of_outputs>
plant_;
- ::std::vector<::std::unique_ptr<StateFeedbackControllerConstants<
- number_of_states, number_of_inputs, number_of_outputs>>>
- controllers_;
+ StateFeedbackController<number_of_states, number_of_inputs, number_of_outputs>
+ controller_;
+
+ StateFeedbackObserver<number_of_states, number_of_inputs, number_of_outputs>
+ observer_;
// These are accessible from non-templated subclasses.
static constexpr int kNumStates = number_of_states;
diff --git a/frc971/control_loops/state_feedback_loop_test.cc b/frc971/control_loops/state_feedback_loop_test.cc
index e81f6fd..ebe2871 100644
--- a/frc971/control_loops/state_feedback_loop_test.cc
+++ b/frc971/control_loops/state_feedback_loop_test.cc
@@ -34,13 +34,21 @@
plant.CheckU(Eigen::Matrix<double, 4, 1>::Zero());
// Now build a controller.
- ::std::vector<::std::unique_ptr<StateFeedbackControllerConstants<2, 4, 7>>>
- v_loop;
- v_loop.emplace_back(new StateFeedbackControllerConstants<2, 4, 7>(
- Eigen::Matrix<double, 2, 7>::Identity(),
+ ::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<2, 4, 7>>>
+ v_controller;
+ v_controller.emplace_back(new StateFeedbackControllerCoefficients<2, 4, 7>(
Eigen::Matrix<double, 4, 2>::Identity(),
Eigen::Matrix<double, 4, 2>::Identity()));
- StateFeedbackLoop<2, 4, 7> test_loop(::std::move(plant), &v_loop);
+ StateFeedbackController<2, 4, 7> controller(&v_controller);
+
+ ::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<2, 4, 7>>>
+ v_observer;
+ v_observer.emplace_back(new StateFeedbackObserverCoefficients<2, 4, 7>(
+ Eigen::Matrix<double, 2, 7>::Identity()));
+ StateFeedbackObserver<2, 4, 7> observer(&v_observer);
+
+ StateFeedbackLoop<2, 4, 7> test_loop(
+ ::std::move(plant), ::std::move(controller), ::std::move(observer));
test_loop.Correct(Eigen::Matrix<double, 7, 1>::Identity());
test_loop.Update(false);
test_loop.CapU();
diff --git a/y2014/actors/drivetrain_actor.cc b/y2014/actors/drivetrain_actor.cc
index 172ca13..418b591 100644
--- a/y2014/actors/drivetrain_actor.cc
+++ b/y2014/actors/drivetrain_actor.cc
@@ -28,7 +28,7 @@
}
bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams ¶ms) {
- static const auto K = loop_.K();
+ static const auto &K = loop_.controller().K();
const double yoffset = params.y_offset;
const double turn_offset =
diff --git a/y2014/control_loops/claw/claw.cc b/y2014/control_loops/claw/claw.cc
index 9320e6b..86ec084 100644
--- a/y2014/control_loops/claw/claw.cc
+++ b/y2014/control_loops/claw/claw.cc
@@ -106,11 +106,11 @@
// (H * position_K) * position_error <= k - H * UVel
Eigen::Matrix<double, 2, 2> position_K;
- position_K << K(0, 0), K(0, 1),
- K(1, 0), K(1, 1);
+ position_K << controller().K(0, 0), controller().K(0, 1),
+ controller().K(1, 0), controller().K(1, 1);
Eigen::Matrix<double, 2, 2> velocity_K;
- velocity_K << K(0, 2), K(0, 3),
- K(1, 2), K(1, 3);
+ velocity_K << controller().K(0, 2), controller().K(0, 3),
+ controller().K(1, 2), controller().K(1, 3);
Eigen::Matrix<double, 2, 1> position_error;
position_error << error(0, 0), error(1, 0);
@@ -924,19 +924,19 @@
case FINE_TUNE_TOP:
case UNKNOWN_LOCATION: {
if (claw_.uncapped_average_voltage() > values.claw.max_zeroing_voltage) {
- double dx_bot = (claw_.U_uncapped(0, 0) -
- values.claw.max_zeroing_voltage) /
- claw_.K(0, 0);
- double dx_top = (claw_.U_uncapped(1, 0) -
- values.claw.max_zeroing_voltage) /
- claw_.K(0, 0);
+ double dx_bot =
+ (claw_.U_uncapped(0, 0) - values.claw.max_zeroing_voltage) /
+ claw_.controller().K(0, 0);
+ double dx_top =
+ (claw_.U_uncapped(1, 0) - values.claw.max_zeroing_voltage) /
+ claw_.controller().K(0, 0);
double dx = ::std::max(dx_top, dx_bot);
bottom_claw_goal_ -= dx;
top_claw_goal_ -= dx;
Eigen::Matrix<double, 4, 1> R;
R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0),
claw_.R(3, 0);
- claw_.mutable_U() = claw_.K() * (R - claw_.X_hat());
+ claw_.mutable_U() = claw_.controller().K() * (R - claw_.X_hat());
capped_goal_ = true;
LOG(DEBUG, "Moving the goal by %f to prevent windup."
" Uncapped is %f, max is %f, difference is %f\n",
@@ -946,19 +946,19 @@
values.claw.max_zeroing_voltage));
} else if (claw_.uncapped_average_voltage() <
-values.claw.max_zeroing_voltage) {
- double dx_bot = (claw_.U_uncapped(0, 0) +
- values.claw.max_zeroing_voltage) /
- claw_.K(0, 0);
- double dx_top = (claw_.U_uncapped(1, 0) +
- values.claw.max_zeroing_voltage) /
- claw_.K(0, 0);
+ double dx_bot =
+ (claw_.U_uncapped(0, 0) + values.claw.max_zeroing_voltage) /
+ claw_.controller().K(0, 0);
+ double dx_top =
+ (claw_.U_uncapped(1, 0) + values.claw.max_zeroing_voltage) /
+ claw_.controller().K(0, 0);
double dx = ::std::min(dx_top, dx_bot);
bottom_claw_goal_ -= dx;
top_claw_goal_ -= dx;
Eigen::Matrix<double, 4, 1> R;
R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0),
claw_.R(3, 0);
- claw_.mutable_U() = claw_.K() * (R - claw_.X_hat());
+ claw_.mutable_U() = claw_.controller().K() * (R - claw_.X_hat());
capped_goal_ = true;
LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
}
diff --git a/y2014/control_loops/claw/claw_lib_test.cc b/y2014/control_loops/claw/claw_lib_test.cc
index d51049d..8444cea 100644
--- a/y2014/control_loops/claw/claw_lib_test.cc
+++ b/y2014/control_loops/claw/claw_lib_test.cc
@@ -542,7 +542,8 @@
claw_motor_.top_claw_goal_ - claw_motor_.bottom_claw_goal_, 0.0,
0.0;
Eigen::Matrix<double, 2, 1> uncapped_voltage =
- claw_motor_.claw_.K() * (R - claw_motor_.claw_.X_hat());
+ claw_motor_.claw_.controller().K() *
+ (R - claw_motor_.claw_.X_hat());
// Use a factor of 1.8 because so long as it isn't actually running
// away, the CapU function will deal with getting the actual output
// down.
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index 9c6b182..70a6cad 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -58,11 +58,12 @@
double dx;
if (index() == 0) {
dx = (uncapped_voltage() - max_voltage_) /
- (K(0, 0) - plant().A(1, 0) * K(0, 2) / plant().A(1, 2));
+ (controller().K(0, 0) -
+ plant().A(1, 0) * controller().K(0, 2) / plant().A(1, 2));
mutable_R(0, 0) -= dx;
mutable_R(2, 0) -= -plant().A(1, 0) / plant().A(1, 2) * dx;
} else {
- dx = (uncapped_voltage() - max_voltage_) / K(0, 0);
+ dx = (uncapped_voltage() - max_voltage_) / controller().K(0, 0);
mutable_R(0, 0) -= dx;
}
capped_goal_ = true;
@@ -72,11 +73,12 @@
double dx;
if (index() == 0) {
dx = (uncapped_voltage() + max_voltage_) /
- (K(0, 0) - plant().A(1, 0) * K(0, 2) / plant().A(1, 2));
+ (controller().K(0, 0) -
+ plant().A(1, 0) * controller().K(0, 2) / plant().A(1, 2));
mutable_R(0, 0) -= dx;
mutable_R(2, 0) -= -plant().A(1, 0) / plant().A(1, 2) * dx;
} else {
- dx = (uncapped_voltage() + max_voltage_) / K(0, 0);
+ dx = (uncapped_voltage() + max_voltage_) / controller().K(0, 0);
mutable_R(0, 0) -= dx;
}
capped_goal_ = true;
diff --git a/y2015/actors/drivetrain_actor.cc b/y2015/actors/drivetrain_actor.cc
index 8772af7..91873eb 100644
--- a/y2015/actors/drivetrain_actor.cc
+++ b/y2015/actors/drivetrain_actor.cc
@@ -25,7 +25,8 @@
: aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s) {}
bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams ¶ms) {
- static const auto K = constants::GetValues().make_drivetrain_loop().K();
+ static const auto K =
+ constants::GetValues().make_drivetrain_loop().controller().K();
const double yoffset = params.y_offset;
const double turn_offset =
diff --git a/y2015/control_loops/claw/claw.cc b/y2015/control_loops/claw/claw.cc
index 25b5b9d..6a56105 100644
--- a/y2015/control_loops/claw/claw.cc
+++ b/y2015/control_loops/claw/claw.cc
@@ -25,7 +25,7 @@
double ClawCappedStateFeedbackLoop::UnsaturateOutputGoalChange() {
// Compute K matrix to compensate for position errors.
- double Kp = K(0, 0);
+ double Kp = controller().K(0, 0);
// Compute how much we need to change R in order to achieve the change in U
// that was observed.
diff --git a/y2015/control_loops/fridge/fridge.cc b/y2015/control_loops/fridge/fridge.cc
index a6acae7..d34cd33 100644
--- a/y2015/control_loops/fridge/fridge.cc
+++ b/y2015/control_loops/fridge/fridge.cc
@@ -39,8 +39,8 @@
// Compute the K matrix used to compensate for position errors.
Eigen::Matrix<double, 2, 2> Kp;
Kp.setZero();
- Kp.col(0) = this->K().col(0);
- Kp.col(1) = this->K().col(2);
+ Kp.col(0) = this->controller().K().col(0);
+ Kp.col(1) = this->controller().K().col(2);
Eigen::Matrix<double, 2, 2> Kp_inv = Kp.inverse();
diff --git a/y2015_bot3/actors/drivetrain_actor.cc b/y2015_bot3/actors/drivetrain_actor.cc
index 313a818..f9587a0 100644
--- a/y2015_bot3/actors/drivetrain_actor.cc
+++ b/y2015_bot3/actors/drivetrain_actor.cc
@@ -30,7 +30,9 @@
bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams ¶ms) {
static const auto K =
- ::y2015_bot3::control_loops::drivetrain::MakeDrivetrainLoop().K();
+ ::y2015_bot3::control_loops::drivetrain::MakeDrivetrainLoop()
+ .controller()
+ .K();
const double yoffset = params.y_offset;
const double turn_offset =
diff --git a/y2015_bot3/control_loops/elevator/elevator.cc b/y2015_bot3/control_loops/elevator/elevator.cc
index d8b1368..eb3d870 100644
--- a/y2015_bot3/control_loops/elevator/elevator.cc
+++ b/y2015_bot3/control_loops/elevator/elevator.cc
@@ -20,7 +20,7 @@
double SimpleCappedStateFeedbackLoop::UnsaturateOutputGoalChange() {
// Compute K matrix to compensate for position errors.
- double Kp = K(0, 0);
+ double Kp = controller().K(0, 0);
// Compute how much we need to change R in order to achieve the change in U
// that was observed.
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 3bf0966..2366fb7 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -670,16 +670,22 @@
// Calculate the loops for a cycle.
{
Eigen::Matrix<double, 3, 1> error = intake_.controller().error();
- status->intake.position_power = intake_.controller().K(0, 0) * error(0, 0);
- status->intake.velocity_power = intake_.controller().K(0, 1) * error(1, 0);
+ status->intake.position_power =
+ intake_.controller().controller().K(0, 0) * error(0, 0);
+ status->intake.velocity_power =
+ intake_.controller().controller().K(0, 1) * error(1, 0);
}
{
Eigen::Matrix<double, 6, 1> error = arm_.controller().error();
- status->shoulder.position_power = arm_.controller().K(0, 0) * error(0, 0);
- status->shoulder.velocity_power = arm_.controller().K(0, 1) * error(1, 0);
- status->wrist.position_power = arm_.controller().K(0, 2) * error(2, 0);
- status->wrist.velocity_power = arm_.controller().K(0, 3) * error(3, 0);
+ status->shoulder.position_power =
+ arm_.controller().controller().K(0, 0) * error(0, 0);
+ status->shoulder.velocity_power =
+ arm_.controller().controller().K(0, 1) * error(1, 0);
+ status->wrist.position_power =
+ arm_.controller().controller().K(0, 2) * error(2, 0);
+ status->wrist.velocity_power =
+ arm_.controller().controller().K(0, 3) * error(3, 0);
}
arm_.Update(disable);
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index 95f04f8..5d8c85a 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -28,14 +28,16 @@
const Eigen::Matrix<double, 2, 1> ControllerOutput() override {
const Eigen::Matrix<double, 2, 1> accelerating_ff =
- controller(0).Kff * (next_R() - plant().coefficients(0).A * R());
+ controller().coefficients(0).Kff *
+ (next_R() - plant().coefficients(0).A * R());
const Eigen::Matrix<double, 2, 1> accelerating_controller =
- controller(0).K * error() + accelerating_ff;
+ controller().coefficients(0).K * error() + accelerating_ff;
const Eigen::Matrix<double, 2, 1> decelerating_ff =
- controller(1).Kff * (next_R() - plant().coefficients(1).A * R());
+ controller().coefficients(1).Kff *
+ (next_R() - plant().coefficients(1).A * R());
const Eigen::Matrix<double, 2, 1> decelerating_controller =
- controller(1).K * error() + decelerating_ff;
+ controller().coefficients(1).K * error() + decelerating_ff;
const double bemf_voltage = X_hat(1, 0) / kV_shoulder;
bool use_accelerating_controller = true;
@@ -66,18 +68,18 @@
if (U(0, 0) > max_voltage(0)) {
const double overage_amount = U(0, 0) - max_voltage(0);
mutable_U(0, 0) = max_voltage(0);
- const double coupled_amount =
- (Kff().block<1, 2>(1, 2) * plant().B().block<2, 1>(2, 0))(0, 0) *
- overage_amount;
+ const double coupled_amount = (controller().Kff().block<1, 2>(1, 2) *
+ plant().B().block<2, 1>(2, 0))(0, 0) *
+ overage_amount;
LOG(DEBUG, "Removing coupled amount %f\n", coupled_amount);
mutable_U(1, 0) += coupled_amount;
}
if (U(0, 0) < min_voltage(0)) {
const double under_amount = U(0, 0) - min_voltage(0);
mutable_U(0, 0) = min_voltage(0);
- const double coupled_amount =
- (Kff().block<1, 2>(1, 2) * plant().B().block<2, 1>(2, 0))(0, 0) *
- under_amount;
+ const double coupled_amount = (controller().Kff().block<1, 2>(1, 2) *
+ plant().B().block<2, 1>(2, 0))(0, 0) *
+ under_amount;
LOG(DEBUG, "Removing coupled amount %f\n", coupled_amount);
mutable_U(1, 0) += coupled_amount;
}