WPILibC++ 2025.2.1
Loading...
Searching...
No Matches
LinearSystem.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <algorithm>
8#include <concepts>
9#include <stdexcept>
10#include <type_traits>
11
12#include <gcem.hpp>
13#include <wpi/Algorithm.h>
14#include <wpi/SmallVector.h>
15
16#include "frc/EigenCore.h"
18#include "units/time.h"
19
20namespace frc {
21
22/**
23 * A plant defined using state-space notation.
24 *
25 * A plant is a mathematical model of a system's dynamics.
26 *
27 * For more on the underlying math, read
28 * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
29 *
30 * @tparam States Number of states.
31 * @tparam Inputs Number of inputs.
32 * @tparam Outputs Number of outputs.
33 */
34template <int States, int Inputs, int Outputs>
36 public:
40
41 /**
42 * Constructs a discrete plant with the given continuous system coefficients.
43 *
44 * @param A System matrix.
45 * @param B Input matrix.
46 * @param C Output matrix.
47 * @param D Feedthrough matrix.
48 * @throws std::domain_error if any matrix element isn't finite.
49 */
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) {
58 if (!gcem::internal::is_finite(mat(row, col))) {
59 return false;
60 }
61 }
62 }
63 return true;
64 } else {
65 return mat.allFinite();
66 }
67 };
68
69 if (!allFinite(A)) {
70 throw std::domain_error(
71 "Elements of A aren't finite. This is usually due to model "
72 "implementation errors.");
73 }
74 if (!allFinite(B)) {
75 throw std::domain_error(
76 "Elements of B aren't finite. This is usually due to model "
77 "implementation errors.");
78 }
79 if (!allFinite(C)) {
80 throw std::domain_error(
81 "Elements of C aren't finite. This is usually due to model "
82 "implementation errors.");
83 }
84 if (!allFinite(D)) {
85 throw std::domain_error(
86 "Elements of D aren't finite. This is usually due to model "
87 "implementation errors.");
88 }
89
90 m_A = A;
91 m_B = B;
92 m_C = C;
93 m_D = D;
94 }
95
96 constexpr LinearSystem(const LinearSystem&) = default;
97 constexpr LinearSystem& operator=(const LinearSystem&) = default;
98 constexpr LinearSystem(LinearSystem&&) = default;
99 constexpr LinearSystem& operator=(LinearSystem&&) = default;
100
101 /**
102 * Returns the system matrix A.
103 */
104 constexpr const Matrixd<States, States>& A() const { return m_A; }
105
106 /**
107 * Returns an element of the system matrix A.
108 *
109 * @param i Row of A.
110 * @param j Column of A.
111 */
112 constexpr double A(int i, int j) const { return m_A(i, j); }
113
114 /**
115 * Returns the input matrix B.
116 */
117 constexpr const Matrixd<States, Inputs>& B() const { return m_B; }
118
119 /**
120 * Returns an element of the input matrix B.
121 *
122 * @param i Row of B.
123 * @param j Column of B.
124 */
125 constexpr double B(int i, int j) const { return m_B(i, j); }
126
127 /**
128 * Returns the output matrix C.
129 */
130 constexpr const Matrixd<Outputs, States>& C() const { return m_C; }
131
132 /**
133 * Returns an element of the output matrix C.
134 *
135 * @param i Row of C.
136 * @param j Column of C.
137 */
138 constexpr double C(int i, int j) const { return m_C(i, j); }
139
140 /**
141 * Returns the feedthrough matrix D.
142 */
143 constexpr const Matrixd<Outputs, Inputs>& D() const { return m_D; }
144
145 /**
146 * Returns an element of the feedthrough matrix D.
147 *
148 * @param i Row of D.
149 * @param j Column of D.
150 */
151 constexpr double D(int i, int j) const { return m_D(i, j); }
152
153 /**
154 * Computes the new x given the old x and the control input.
155 *
156 * This is used by state observers directly to run updates based on state
157 * estimate.
158 *
159 * @param x The current state.
160 * @param clampedU The control input.
161 * @param dt Timestep for model update.
162 */
164 units::second_t dt) const {
167 DiscretizeAB<States, Inputs>(m_A, m_B, dt, &discA, &discB);
168
169 return discA * x + discB * clampedU;
170 }
171
172 /**
173 * Computes the new y given the control input.
174 *
175 * This is used by state observers directly to run updates based on state
176 * estimate.
177 *
178 * @param x The current state.
179 * @param clampedU The control input.
180 */
182 const InputVector& clampedU) const {
183 return m_C * x + m_D * clampedU;
184 }
185
186 /**
187 * Returns the LinearSystem with the outputs listed in outputIndices.
188 *
189 * <p>This is used by state observers such as the Kalman Filter.
190 *
191 * @param outputIndices the list of output indices to include in the sliced
192 * system.
193 * @return the sliced LinearSystem with outputs set to row vectors of
194 * LinearSystem.
195 * @throws std::domain_error if any outputIndices are outside the range of
196 * system outputs.
197 * @throws std::domain_error if number of outputIndices exceeds the system
198 * outputs.
199 * @throws std::domain_error if duplication exists in outputIndices.
200 */
201 template <std::same_as<int>... OutputIndices>
202 LinearSystem<States, Inputs, sizeof...(OutputIndices)> Slice(
203 OutputIndices... outputIndices) {
204 static_assert(sizeof...(OutputIndices) <= Outputs,
205 "More outputs requested than available. This is usually due "
206 "to model implementation errors.");
207
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.");
214 }
215 },
216 outputIndices...);
217
218 // Sort and deduplicate output indices
219 wpi::SmallVector<int> outputIndicesArray{outputIndices...};
220 std::sort(outputIndicesArray.begin(), outputIndicesArray.end());
221 auto last =
222 std::unique(outputIndicesArray.begin(), outputIndicesArray.end());
223 outputIndicesArray.erase(last, outputIndicesArray.end());
224
225 if (outputIndicesArray.size() != sizeof...(outputIndices)) {
226 throw std::domain_error(
227 "Duplicate indices exist. This is usually due to model "
228 "implementation errors.");
229 }
230
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)};
234 }
235
236 private:
237 /**
238 * Continuous system matrix.
239 */
241
242 /**
243 * Continuous input matrix.
244 */
246
247 /**
248 * Output matrix.
249 */
251
252 /**
253 * Feedthrough matrix.
254 */
256};
257
258} // namespace frc
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
Definition CAN.h:11
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