34template <
int States,
int Inputs,
int Outputs>
54 auto allFinite = [](
const auto& mat) {
55 if (std::is_constant_evaluated()) {
56 for (
int row = 0; row < mat.rows(); ++row) {
57 for (
int col = 0; col < mat.cols(); ++col) {
65 return mat.allFinite();
70 throw std::domain_error(
71 "Elements of A aren't finite. This is usually due to model "
72 "implementation errors.");
75 throw std::domain_error(
76 "Elements of B aren't finite. This is usually due to model "
77 "implementation errors.");
80 throw std::domain_error(
81 "Elements of C aren't finite. This is usually due to model "
82 "implementation errors.");
85 throw std::domain_error(
86 "Elements of D aren't finite. This is usually due to model "
87 "implementation errors.");
112 constexpr double A(
int i,
int j)
const {
return m_A(i, j); }
125 constexpr double B(
int i,
int j)
const {
return m_B(i, j); }
138 constexpr double C(
int i,
int j)
const {
return m_C(i, j); }
151 constexpr double D(
int i,
int j)
const {
return m_D(i, j); }
164 units::second_t dt)
const {
169 return discA * x + discB * clampedU;
183 return m_C * x + m_D * clampedU;
201 template <std::same_as<
int>... OutputIndices>
203 OutputIndices... outputIndices) {
204 static_assert(
sizeof...(OutputIndices) <= Outputs,
205 "More outputs requested than available. This is usually due "
206 "to model implementation errors.");
209 [](
size_t i,
const auto& elem) {
210 if (elem < 0 || elem >= Outputs) {
211 throw std::domain_error(
212 "Slice indices out of range. This is usually due to model "
213 "implementation errors.");
220 std::sort(outputIndicesArray.begin(), outputIndicesArray.end());
222 std::unique(outputIndicesArray.begin(), outputIndicesArray.end());
223 outputIndicesArray.erase(last, outputIndicesArray.end());
225 if (outputIndicesArray.size() !=
sizeof...(outputIndices)) {
226 throw std::domain_error(
227 "Duplicate indices exist. This is usually due to model "
228 "implementation errors.");
231 return LinearSystem<States, Inputs,
sizeof...(OutputIndices)>{
232 m_A, m_B, m_C(outputIndicesArray, Eigen::placeholders::all),
233 m_D(outputIndicesArray, Eigen::placeholders::all)};
This file defines the SmallVector class.
A plant defined using state-space notation.
Definition LinearSystem.h:35
LinearSystem< States, Inputs, sizeof...(OutputIndices)> Slice(OutputIndices... outputIndices)
Returns the LinearSystem with the outputs listed in outputIndices.
Definition LinearSystem.h:202
constexpr double B(int i, int j) const
Returns an element of the input matrix B.
Definition LinearSystem.h:125
constexpr LinearSystem & operator=(const LinearSystem &)=default
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:163
Vectord< Outputs > OutputVector
Definition LinearSystem.h:39
constexpr const Matrixd< Outputs, Inputs > & D() const
Returns the feedthrough matrix D.
Definition LinearSystem.h:143
constexpr const Matrixd< Outputs, States > & C() const
Returns the output matrix C.
Definition LinearSystem.h:130
constexpr LinearSystem(LinearSystem &&)=default
constexpr LinearSystem(const LinearSystem &)=default
constexpr 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
OutputVector CalculateY(const StateVector &x, const InputVector &clampedU) const
Computes the new y given the control input.
Definition LinearSystem.h:181
constexpr double D(int i, int j) const
Returns an element of the feedthrough matrix D.
Definition LinearSystem.h:151
constexpr double C(int i, int j) const
Returns an element of the output matrix C.
Definition LinearSystem.h:138
Vectord< States > StateVector
Definition LinearSystem.h:37
constexpr const Matrixd< States, States > & A() const
Returns the system matrix A.
Definition LinearSystem.h:104
constexpr const Matrixd< States, Inputs > & B() const
Returns the input matrix B.
Definition LinearSystem.h:117
constexpr double A(int i, int j) const
Returns an element of the system matrix A.
Definition LinearSystem.h:112
constexpr LinearSystem & operator=(LinearSystem &&)=default
Vectord< Inputs > InputVector
Definition LinearSystem.h:38
This is a 'vector' (really, a variable-sized array), optimized for the case when the array is small.
Definition SmallVector.h:1212
void DiscretizeAB(const Matrixd< States, States > &contA, const Matrixd< States, Inputs > &contB, units::second_t dt, Matrixd< States, States > *discA, Matrixd< States, Inputs > *discB)
Discretizes the given continuous A and B matrices.
Definition Discretization.h:41
Eigen::Matrix< double, Rows, Cols, Options, MaxRows, MaxCols > Matrixd
Definition EigenCore.h:21
Eigen::Vector< double, Size > Vectord
Definition EigenCore.h:12
constexpr bool is_finite(const T x) noexcept
Definition is_finite.hpp:37
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