34template <
int States,
int Inputs,
int Outputs>
55 throw std::domain_error(
56 "Elements of A aren't finite. This is usually due to model "
57 "implementation errors.");
60 throw std::domain_error(
61 "Elements of B aren't finite. This is usually due to model "
62 "implementation errors.");
65 throw std::domain_error(
66 "Elements of C aren't finite. This is usually due to model "
67 "implementation errors.");
70 throw std::domain_error(
71 "Elements of D aren't finite. This is usually due to model "
72 "implementation errors.");
97 double A(
int i,
int j)
const {
return m_A(i, j); }
110 double B(
int i,
int j)
const {
return m_B(i, j); }
123 double C(
int i,
int j)
const {
return m_C(i, j); }
136 double D(
int i,
int j)
const {
return m_D(i, j); }
149 units::second_t dt)
const {
152 DiscretizeAB<States, Inputs>(m_A, m_B, dt, &discA, &discB);
154 return discA * x + discB * clampedU;
168 return m_C * x + m_D * clampedU;
186 template <std::same_as<
int>... OutputIndices>
188 OutputIndices... outputIndices) {
189 static_assert(
sizeof...(OutputIndices) <= Outputs,
190 "More outputs requested than available. This is usually due "
191 "to model implementation errors.");
194 [](
size_t i,
const auto& elem) {
195 if (elem < 0 || elem >= Outputs) {
196 throw std::domain_error(
197 "Slice indices out of range. This is usually due to model "
198 "implementation errors.");
205 std::sort(outputIndicesArray.begin(), outputIndicesArray.end());
207 std::unique(outputIndicesArray.begin(), outputIndicesArray.end());
208 outputIndicesArray.erase(last, outputIndicesArray.end());
210 if (outputIndicesArray.size() !=
sizeof...(outputIndices)) {
211 throw std::domain_error(
212 "Duplicate indices exist. This is usually due to model "
213 "implementation errors.");
216 return LinearSystem<States, Inputs,
sizeof...(OutputIndices)>{
217 m_A, m_B, m_C(outputIndicesArray, Eigen::placeholders::all),
218 m_D(outputIndicesArray, Eigen::placeholders::all)};
This file defines the SmallVector class.
A plant defined using state-space notation.
Definition: LinearSystem.h:35
const Matrixd< States, Inputs > & B() const
Returns the input matrix B.
Definition: LinearSystem.h:102
LinearSystem & operator=(const LinearSystem &)=default
LinearSystem< States, Inputs, sizeof...(OutputIndices)> Slice(OutputIndices... outputIndices)
Returns the LinearSystem with the outputs listed in outputIndices.
Definition: LinearSystem.h:187
double D(int i, int j) const
Returns an element of the feedthrough matrix D.
Definition: LinearSystem.h:136
double B(int i, int j) const
Returns an element of the input matrix B.
Definition: LinearSystem.h:110
double A(int i, int j) const
Returns an element of the system matrix A.
Definition: LinearSystem.h:97
StateVector CalculateX(const StateVector &x, const InputVector &clampedU, units::second_t dt) const
Computes the new x given the old x and the control input.
Definition: LinearSystem.h:148
const Matrixd< States, States > & A() const
Returns the system matrix A.
Definition: LinearSystem.h:89
Vectord< Outputs > OutputVector
Definition: LinearSystem.h:39
LinearSystem(LinearSystem &&)=default
LinearSystem & operator=(LinearSystem &&)=default
const Matrixd< Outputs, Inputs > & D() const
Returns the feedthrough matrix D.
Definition: LinearSystem.h:128
OutputVector CalculateY(const StateVector &x, const InputVector &clampedU) const
Computes the new y given the control input.
Definition: LinearSystem.h:166
Vectord< States > StateVector
Definition: LinearSystem.h:37
const Matrixd< Outputs, States > & C() const
Returns the output matrix C.
Definition: LinearSystem.h:115
double C(int i, int j) const
Returns an element of the output matrix C.
Definition: LinearSystem.h:123
LinearSystem(const LinearSystem &)=default
LinearSystem(const Matrixd< States, States > &A, const Matrixd< States, Inputs > &B, const Matrixd< Outputs, States > &C, const Matrixd< Outputs, Inputs > &D)
Constructs a discrete plant with the given continuous system coefficients.
Definition: LinearSystem.h:50
Vectord< Inputs > InputVector
Definition: LinearSystem.h:38
Definition: AprilTagDetector_cv.h:11
Eigen::Matrix< double, Rows, Cols, Options, MaxRows, MaxCols > Matrixd
Definition: EigenCore.h:21
Eigen::Vector< double, Size > Vectord
Definition: EigenCore.h:12
constexpr void for_each(F &&f, Ts &&... elems)
Calls f(i, elem) for each element of elems where i is the index of the element in elems and elem is t...
Definition: Algorithm.h:29