Drake
linear_system.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <utility>
5 
10 
11 namespace drake {
12 namespace systems {
13 
14 /// A discrete OR continuous linear system.
15 ///
16 /// If time_period>0.0, then the linear system will have the following discrete-
17 /// time state update:
18 /// @f[ x[n+1] = A x[n] + B u[n], @f]
19 ///
20 /// or if time_period==0.0, then the linear system will have the following
21 /// continuous-time state update:
22 /// @f[\dot{x} = A x + B u. @f]
23 ///
24 /// In both cases, the system will have the output:
25 /// @f[y = C x + D u, @f]
26 /// where `u` denotes the input vector, `x` denotes the state vector, and
27 /// `y` denotes the output vector.
28 ///
29 /// @tparam T The vector element type, which must be a valid Eigen scalar.
30 ///
31 /// Instantiated templates for the following kinds of T's are provided:
32 /// - double
33 /// - AutoDiffXd
34 /// - symbolic::Expression
35 ///
36 /// They are already available to link against in the containing library.
37 /// No other values for T are currently supported.
38 ///
39 /// @ingroup primitive_systems
40 ///
41 /// @see AffineSystem
42 /// @see MatrixGain
43 template <typename T>
44 class LinearSystem : public AffineSystem<T> {
45  public:
47 
48  /// Constructs a %LinearSystem with a fixed set of coefficient matrices `A`,
49  /// `B`,`C`, and `D`.
50  /// The coefficient matrices must obey the following dimensions:
51  /// | Matrix | Num Rows | Num Columns |
52  /// |:-------:|:-----------:|:-----------:|
53  /// | A | num states | num states |
54  /// | B | num states | num inputs |
55  /// | C | num outputs | num states |
56  /// | D | num outputs | num inputs |
57  ///
58  /// Subclasses must use the protected constructor, not this one.
59  LinearSystem(const Eigen::Ref<const Eigen::MatrixXd>& A,
60  const Eigen::Ref<const Eigen::MatrixXd>& B,
61  const Eigen::Ref<const Eigen::MatrixXd>& C,
62  const Eigen::Ref<const Eigen::MatrixXd>& D,
63  double time_period = 0.0);
64 
65  /// Scalar-converting copy constructor. See @ref system_scalar_conversion.
66  template <typename U>
67  explicit LinearSystem(const LinearSystem<U>&);
68 
69  /// Creates a unique pointer to LinearSystem<T> by decomposing @p dynamics and
70  /// @p outputs using @p state_vars and @p input_vars.
71  ///
72  /// @throws runtime_error if either @p dynamics or @p outputs is not linear in
73  /// @p state_vars and @p input_vars.
74  static std::unique_ptr<LinearSystem<T>> MakeLinearSystem(
75  const Eigen::Ref<const VectorX<symbolic::Expression>>& dynamics,
76  const Eigen::Ref<const VectorX<symbolic::Expression>>& output,
77  const Eigen::Ref<const VectorX<symbolic::Variable>>& state_vars,
78  const Eigen::Ref<const VectorX<symbolic::Variable>>& input_vars,
79  double time_period = 0.0);
80 
81  protected:
82  /// Constructor that specifies scalar-type conversion support.
83  /// @param converter scalar-type conversion support helper (i.e., AutoDiff,
84  /// etc.); pass a default-constructed object if such support is not desired.
85  /// See @ref system_scalar_conversion for detailed background and examples
86  /// related to scalar-type conversion support.
88  const Eigen::Ref<const Eigen::MatrixXd>& A,
89  const Eigen::Ref<const Eigen::MatrixXd>& B,
90  const Eigen::Ref<const Eigen::MatrixXd>& C,
91  const Eigen::Ref<const Eigen::MatrixXd>& D, double time_period);
92 };
93 
94 /// Base class for a discrete or continuous linear time-varying (LTV) system.
95 ///
96 /// If `time_period > 0.0`, the system will have the following discrete-time
97 /// state update:
98 /// @f[ x(t+h) = A(t) x(t) + B(t) u(t), @f]
99 /// where `h` is the time_period. If `time_period == 0.0`, the system will have
100 /// the following continuous-time state update:
101 /// @f[ \dot{x}(t) = A(t) x(t) + B(t) u(t), @f]
102 ///
103 /// both with the output:
104 /// @f[ y(t) = C(t) x(t) + D(t) u(t). @f]
105 ///
106 /// @tparam T The vector element type, which must be a valid Eigen scalar.
107 ///
108 /// Instantiated templates for the following kinds of T's are provided:
109 /// - double
110 /// - AutoDiffXd
111 /// - symbolic::Expression
112 ///
113 /// They are already available to link against in the containing library.
114 /// No other values for T are currently supported.
115 ///
116 /// @ingroup primitive_systems
117 ///
118 template <typename T>
120  public:
122 
123  protected:
124  /// Constructor.
125  ///
126  /// @param converter scalar-type conversion support helper (i.e., AutoDiff,
127  /// etc.); pass a default-constructed object if such support is not desired.
128  /// See @ref system_scalar_conversion for detailed background and examples
129  /// related to scalar-type conversion support.
130  /// @param num_states size of the system's state vector
131  /// @param num_inputs size of the system's input vector
132  /// @param num_outputs size of the system's output vector
133  /// @param time_period discrete update period, or 0.0 to use continuous time
135  int num_inputs, int num_outputs, double time_period)
136  : TimeVaryingAffineSystem<T>(std::move(converter), num_states, num_inputs,
137  num_outputs, time_period) {}
138 
139  private:
140  // N.B. A linear system is simply a restricted form of an affine system with
141  // the affine terms set to zero. The following adds this restriction.
142  VectorX<T> f0(const T&) const final {
143  return VectorX<T>::Zero(this->num_states());
144  }
145  VectorX<T> y0(const T&) const final {
146  return VectorX<T>::Zero(this->num_outputs());
147  }
148 };
149 
150 /// @defgroup Additional options for input/output port specification.
151 /// @{
152 // TODO(russt): Move these to a more central location if they are useful in
153 // other related methods.
154 const int kNoInput = -1;
156 
157 const int kNoOutput = -3;
159 /// @}
160 
161 /// Takes the first-order Taylor expansion of a System around a nominal
162 /// operating point (defined by the Context).
163 ///
164 /// This method currently supports linearizing around at most a single vector
165 /// input port and at most a single vector output port. For systems with
166 /// more ports, use @p input_port_index and @p output_port_index to select
167 /// the input for the newly constructed system. Any additional input ports
168 /// will be treated as constants (fixed at the value specified in @p context).
169 ///
170 /// @param system The system or subsystem to linearize.
171 /// @param context Defines the nominal operating point about which the system
172 /// should be linearized. See note below.
173 /// @param input_port_index A valid input port index for @p system or kNoInput
174 /// or (default) kUseFirstInputIfItExists.
175 /// @param output_port_index A valid output port index for @p system or
176 /// kNoOutput or (default) kUseFirstOutputIfItExists.
177 /// @param equilibrium_check_tolerance Specifies the tolerance on ensuring that
178 /// the derivative vector isZero at the nominal operating point. @default 1e-6.
179 /// @returns A LinearSystem that approximates the original system in the
180 /// vicinity of the operating point. See note below.
181 /// @throws std::runtime_error if the system the operating point is not an
182 /// equilibrium point of the system (within the specified tolerance)
183 /// @throws std::runtime_error if the system if the system is not (only)
184 /// continuous or (only) discrete time with a single periodic update.
185 ///
186 /// Note: All inputs in the Context must be connected, either to the
187 /// output of some upstream System within a Diagram (e.g., if system is a
188 /// reference to a subsystem in a Diagram), or to a constant value using, e.g.
189 /// context->FixInputPort(0,default_input);
190 ///
191 /// Note: The inputs, states, and outputs of the returned system are NOT the
192 /// same as the original system. Denote x0,u0 as the nominal state and input
193 /// defined by the Context, and y0 as the value of the output at (x0,u0),
194 /// then the created systems inputs are (u-u0), states are (x-x0), and
195 /// outputs are (y-y0).
196 ///
197 /// @ingroup primitive_systems
198 ///
199 std::unique_ptr<LinearSystem<double>> Linearize(
200  const System<double>& system, const Context<double>& context,
201  int input_port_index = kUseFirstInputIfItExists,
202  int output_port_index = kUseFirstOutputIfItExists,
203  double equilibrium_check_tolerance = 1e-6);
204 
205 /// A first-order Taylor series approximation to a @p system in the neighborhood
206 /// of an arbitrary point. When Taylor-expanding a system at a non-equilibrium
207 /// point, it may be represented either of the form:
208 /// @f[ \dot{x} - \dot{x0} = A (x - x0) + B (u - u0), @f]
209 /// for continuous time, or
210 /// @f[ x[n+1] - x0[n+1] = A (x[n] - x0[n]) + B (u[n] - u0[n]), @f]
211 /// for discrete time. As above, we denote x0, u0 to be the nominal state and
212 /// input at the provided @p context. The system description is affine when the
213 /// terms @f$ \dot{x0} - A x0 - B u0 @f$ and @f$ x0[n+1] - A x0[n] - B u0[n] @f$
214 /// are nonzero.
215 ///
216 /// More precisely, let x be a state and u be an input. This function returns
217 /// an AffineSystem of the form:
218 /// @f[ \dot{x} = A x + B u + f0, @f] (CT)
219 /// @f[ x[n+1] = A x[n] + B u[n] + f0, @f] (DT)
220 /// where @f$ f0 = \dot{x0} - A x0 - B u0 @f$ (CT) and
221 /// @f$ f0 = x0[n+1] - A x[n] - B u[n] @f$ (DT).
222 ///
223 /// This method currently supports approximating around at most a single vector
224 /// input port and at most a single vector output port. For systems with
225 /// more ports, use @p input_port_index and @p output_port_index to select
226 /// the input for the newly constructed system. Any additional input ports
227 /// will be treated as constants (fixed at the value specified in @p context).
228 ///
229 /// @param system The system or subsystem to linearize.
230 /// @param context Defines the nominal operating point about which the system
231 /// should be linearized.
232 /// @param input_port_index A valid input port index for @p system or kNoInput
233 /// or (default) kUseFirstInputIfItExists.
234 /// @param output_port_index A valid output port index for @p system or
235 /// kNoOutput or (default) kUseFirstOutputIfItExists.
236 /// @returns An AffineSystem at this linearization point.
237 /// @throws std::runtime_error if the system if the system is not (only)
238 /// continuous or (only) discrete time with a single periodic update.
239 ///
240 /// Note that x, u and y are in the same coordinate system as the original
241 /// @p system, since the terms involving x0, u0 reside in f0.
242 ///
243 /// @ingroup primitive_systems
244 ///
245 // Note: The TypeSafeIndices (InputPortIndex and OutputPortIndex) didn't let
246 // me handle the additional options without a lot of boilerplate.
247 std::unique_ptr<AffineSystem<double>> FirstOrderTaylorApproximation(
248  const System<double>& system, const Context<double>& context,
249  int input_port_index = kUseFirstInputIfItExists,
250  int output_port_index = kUseFirstOutputIfItExists);
251 
252 /// Returns the controllability matrix: R = [B, AB, ..., A^{n-1}B].
253 /// @ingroup control_systems
254 Eigen::MatrixXd ControllabilityMatrix(const LinearSystem<double>& sys);
255 
256 /// Returns true iff the controllability matrix is full row rank.
257 /// @ingroup control_systems
258 bool IsControllable(const LinearSystem<double>& sys,
259  optional<double> threshold = nullopt);
260 
261 /// Returns the observability matrix: O = [ C; CA; ...; CA^{n-1} ].
262 /// @ingroup estimator_systems
263 Eigen::MatrixXd ObservabilityMatrix(const LinearSystem<double>& sys);
264 
265 /// Returns true iff the observability matrix is full column rank.
266 /// @ingroup estimator_systems
267 bool IsObservable(const LinearSystem<double>& sys,
268  optional<double> threshold = nullopt);
269 
270 } // namespace systems
271 } // namespace drake
int num_states() const
Definition: affine_system.h:57
int num_outputs() const
Definition: affine_system.h:59
const int kNoOutput
Definition: linear_system.h:157
Definition: automotive_demo.cc:90
const Eigen::MatrixXd & C() const
Definition: affine_system.h:182
std::unique_ptr< LinearSystem< double > > Linearize(const System< double > &system, const Context< double > &context, int input_port_index, int output_port_index, double equilibrium_check_tolerance)
Takes the first-order Taylor expansion of a System around a nominal operating point (defined by the C...
Definition: linear_system.cc:266
STL namespace.
std::unique_ptr< AffineSystem< double > > FirstOrderTaylorApproximation(const System< double > &system, const Context< double > &context, int input_port_index, int output_port_index)
A first-order Taylor series approximation to a system in the neighborhood of an arbitrary point...
Definition: linear_system.cc:280
const int kUseFirstInputIfItExists
Definition: linear_system.h:155
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorX
A column vector of any size, templated on scalar type.
Definition: eigen_types.h:46
double time_period() const
Definition: affine_system.h:56
bool IsObservable(const LinearSystem< double > &sys, optional< double > threshold)
Returns true iff the observability matrix is full column rank.
Definition: linear_system.cc:329
const int kNoInput
Definition: linear_system.h:154
static std::unique_ptr< LinearSystem< T > > MakeLinearSystem(const Eigen::Ref< const VectorX< symbolic::Expression >> &dynamics, const Eigen::Ref< const VectorX< symbolic::Expression >> &output, const Eigen::Ref< const VectorX< symbolic::Variable >> &state_vars, const Eigen::Ref< const VectorX< symbolic::Variable >> &input_vars, double time_period=0.0)
Creates a unique pointer to LinearSystem<T> by decomposing dynamics and outputs using state_vars and ...
Definition: linear_system.cc:52
bool IsControllable(const LinearSystem< double > &sys, optional< double > threshold)
Returns true iff the controllability matrix is full row rank.
Definition: linear_system.cc:303
stx::optional< T > optional
Definition: drake_optional.h:22
A discrete OR continuous linear system.
Definition: linear_system.h:44
const Eigen::VectorXd & f0() const
Definition: affine_system.h:181
Base class for a discrete or continuous linear time-varying (LTV) system.
Definition: linear_system.h:119
const Eigen::VectorXd & y0() const
Definition: affine_system.h:184
const int kUseFirstOutputIfItExists
Definition: linear_system.h:158
Provides drake::optional as an alias for the appropriate implementation of std::optional or std::expe...
Base class for a discrete- or continuous-time, time-varying affine system, with potentially time-vary...
Definition: affine_system.h:32
const Eigen::MatrixXd & D() const
Definition: affine_system.h:183
const Eigen::MatrixXd & B() const
Definition: affine_system.h:180
Eigen::MatrixXd ObservabilityMatrix(const LinearSystem< double > &sys)
Returns the observability matrix: O = [ C; CA; ...; CA^{n-1} ].
Definition: linear_system.cc:314
int num_inputs() const
Definition: affine_system.h:58
Eigen::MatrixXd ControllabilityMatrix(const LinearSystem< double > &sys)
Returns the controllability matrix: R = [B, AB, ..., A^{n-1}B].
Definition: linear_system.cc:288
const Eigen::MatrixXd & A() const
Definition: affine_system.h:179
Provides public header files of Drake&#39;s symbolic library.
Helper class to convert a System<U> into a System<T>, intended for internal use by the System framewo...
Definition: system_scalar_converter.h:35
constexpr auto nullopt
Definition: drake_optional.h:24
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:33
LinearSystem(const LinearSystem &)=delete
A discrete OR continuous affine system (with constant coefficients).
Definition: affine_system.h:134
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
TimeVaryingLinearSystem(SystemScalarConverter converter, int num_states, int num_inputs, int num_outputs, double time_period)
Constructor.
Definition: linear_system.h:134