-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
vector_system.h
342 lines (312 loc) · 14.6 KB
/
vector_system.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
#pragma once
#include <memory>
#include <optional>
#include <set>
#include <utility>
#include <vector>
#include "drake/common/default_scalars.h"
#include "drake/common/drake_assert.h"
#include "drake/common/drake_copyable.h"
#include "drake/common/drake_throw.h"
#include "drake/common/eigen_types.h"
#include "drake/common/never_destroyed.h"
#include "drake/common/unused.h"
#include "drake/systems/framework/basic_vector.h"
#include "drake/systems/framework/context.h"
#include "drake/systems/framework/leaf_system.h"
namespace drake {
namespace systems {
/// A base class that specializes LeafSystem for use with only zero or one
/// vector input ports, and only zero or one vector output ports.
///
/// @system
/// name: VectorSystem
/// input_ports:
/// - u0
/// output_ports:
/// - y0
/// @endsystem
///
/// By default, this base class does not declare any state; subclasses may
/// optionally declare continuous or discrete state, but not both; subclasses
/// may not declare abstract state.
///
/// @tparam_default_scalar
template <typename T>
class VectorSystem : public LeafSystem<T> {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(VectorSystem)
~VectorSystem() override = default;
protected:
/// Creates a system with one input port and one output port of the given
/// sizes, when the sizes are non-zero. Either size can be zero, in which
/// case no input (or output) port is created.
///
/// The `direct_feedthrough` specifies whether the input port direct feeds
/// through to the output port. (See SystemBase::GetDirectFeedthroughs().)
/// When not provided, assumes true (the output is direct feedthrough).
/// When false, the DoCalcVectorOutput `input` will be empty (zero-sized).
///
/// Does *not* declare scalar-type conversion support (AutoDiff, etc.). To
/// enable AutoDiff support, use the SystemScalarConverter-based constructor.
/// (For that, see @ref system_scalar_conversion at the example titled
/// "Example using drake::systems::VectorSystem as the base class".)
VectorSystem(int input_size, int output_size,
std::optional<bool> direct_feedthrough = std::nullopt)
: VectorSystem(SystemScalarConverter{}, input_size, output_size,
direct_feedthrough) {}
/// Creates a system with one input port and one output port of the given
/// sizes, when the sizes are non-zero. Either size can be zero, in which
/// case no input (or output) port is created. This constructor allows
/// subclasses to declare scalar-type conversion support (AutoDiff, etc.).
///
/// The `direct_feedthrough` specifies whether the input port direct feeds
/// through to the output port. (See SystemBase::GetDirectFeedthroughs().)
/// When not provided, infers feedthrough from the symbolic form if
/// available, or else assumes true (the output is direct feedthrough).
/// When false, the DoCalcVectorOutput `input` will be empty (zero-sized).
///
/// The scalar-type conversion support will use @p converter.
/// To enable scalar-type conversion support, pass a `SystemTypeTag<S>{}`
/// where `S` must be the exact class of `this` being constructed.
///
/// See @ref system_scalar_conversion for detailed background and examples
/// related to scalar-type conversion support, especially the example titled
/// "Example using drake::systems::VectorSystem as the base class".
VectorSystem(SystemScalarConverter converter, int input_size, int output_size,
std::optional<bool> direct_feedthrough = std::nullopt)
: LeafSystem<T>(std::move(converter)) {
if (input_size > 0) {
this->DeclareInputPort(kUseDefaultName, kVectorValued, input_size);
}
if (output_size > 0) {
std::set<DependencyTicket> prerequisites_of_calc;
if (direct_feedthrough.value_or(true)) {
// Depend on everything.
prerequisites_of_calc = {
this->all_sources_ticket()
};
} else {
// Depend on everything *except* for the inputs.
prerequisites_of_calc = {
this->time_ticket(),
this->accuracy_ticket(),
this->all_state_ticket(),
this->all_parameters_ticket(),
};
}
this->DeclareVectorOutputPort(
kUseDefaultName, output_size,
&VectorSystem::CalcVectorOutput, std::move(prerequisites_of_calc));
}
this->DeclareForcedDiscreteUpdateEvent(
&VectorSystem<T>::CalcDiscreteUpdate);
}
/// Causes the vector-valued input port to become up-to-date, and returns
/// the port's value as an %Eigen vector. If the system has zero inputs,
/// then returns an empty vector.
const VectorX<T>& EvalVectorInput(
const Context<T>& context) const {
// Obtain a reference to u (or the empty vector).
if (this->num_input_ports() > 0) {
return this->get_input_port().Eval(context);
}
static const never_destroyed<VectorX<T>> empty_vector(0);
return empty_vector.access();
}
/// Returns a reference to an %Eigen vector version of the state from within
/// the %Context.
const VectorX<T>& GetVectorState(
const Context<T>& context) const {
// Obtain the block form of xc or xd.
DRAKE_ASSERT(context.num_abstract_states() == 0);
const BasicVector<T>* state_vector{};
if (context.num_discrete_state_groups() == 0) {
const VectorBase<T>& vector_base = context.get_continuous_state_vector();
state_vector = dynamic_cast<const BasicVector<T>*>(&vector_base);
} else {
DRAKE_ASSERT(context.has_only_discrete_state());
state_vector = &context.get_discrete_state(0);
}
DRAKE_DEMAND(state_vector != nullptr);
return state_vector->value();
}
/// Converts the parameters to Eigen::VectorBlock form, then delegates to
/// DoCalcVectorTimeDerivatives().
void DoCalcTimeDerivatives(const Context<T>& context,
ContinuousState<T>* derivatives) const final {
// Short-circuit when there's no work to do.
if (derivatives->size() == 0) {
return;
}
const VectorX<T>& input_vector = EvalVectorInput(context);
const auto input_block = input_vector.head(input_vector.rows());
// Obtain the block form of xc.
DRAKE_ASSERT(context.has_only_continuous_state());
const VectorBase<T>& state_base = context.get_continuous_state_vector();
const VectorX<T>& state_vector =
dynamic_cast<const BasicVector<T>&>(state_base).value();
const Eigen::VectorBlock<const VectorX<T>> state_block =
state_vector.head(state_vector.rows());
// Obtain the block form of xcdot.
VectorBase<T>& derivatives_base = derivatives->get_mutable_vector();
Eigen::VectorBlock<VectorX<T>> derivatives_block =
dynamic_cast<BasicVector<T>&>(derivatives_base).get_mutable_value();
// Delegate to subclass.
DoCalcVectorTimeDerivatives(context, input_block, state_block,
&derivatives_block);
}
/// Converts the parameters to Eigen::VectorBlock form, then delegates to
/// DoCalcVectorOutput().
void CalcVectorOutput(const Context<T>& context,
BasicVector<T>* output) const {
// Should only get here if we've declared an output.
DRAKE_ASSERT(this->num_output_ports() > 0);
// Decide whether we should evaluate our input port and pass its value to
// our subclass's DoCalcVectorOutput method. When should_eval_input is
// false, we will pass an empty vector instead of pulling on our input.
bool should_eval_input = false;
if (this->num_input_ports() > 0) {
// We have an input port, but when our subclass's DoCalcVectorOutput is
// not direct-feedthrough, then evaluating the input port might cause a
// computational loop. We should only evaluate the input when this
// System is declared to be direct-feedthrough (i.e., by asking a System
// base class method such as HasAnyDirectFeedthrough).
//
// However, there is a Catch-22: our LeafSystem base class contains a
// default implementation of feedthrough reporting that uses the
// SystemSymbolicInspector to infer sparsity. The inspector fixes the
// input port to be a symbolic Variable, and then evaluates the output.
// If during that output calculation, this method *again* asked to
// compute the feedthrough, we would re-enter the inspection code and
// cause infinite recursion. We would have a Calc -> HasAny -> Calc ...
// infinite loop.
//
// To break that recursion, we avoid HasAnyDirectFeedthrough when our
// scalar type is a symbolic expression and the input port is fixed. We
// know the inspector must always used a fixed input port (it has no
// diagram that it could use), so this will always bottom out the
// recursion. Even if not being evaluated by the symbolic inspector, if
// the input port is fixed to a symbolic expression then it is no harm to
// evaluate the input, even if the system is not supposed to have
// feedthrough -- it is merely providing extra ignored data to the
// DoCalcVectorOutput helper.
constexpr bool is_symbolic = std::is_same_v<T, symbolic::Expression>;
const bool is_fixed_input =
(context.MaybeGetFixedInputPortValue(0) != nullptr);
if (is_symbolic && is_fixed_input) {
should_eval_input = true;
} else {
should_eval_input = this->HasAnyDirectFeedthrough();
}
}
// Only provide input when direct feedthrough occurs; otherwise, we might
// create a computational loop.
static const never_destroyed<VectorX<T>> empty_vector(0);
const VectorX<T>& input_vector =
should_eval_input ? EvalVectorInput(context) :
empty_vector.access();
const Eigen::VectorBlock<const VectorX<T>> input_block =
input_vector.head(input_vector.rows());
// Obtain the block form of xc or xd.
const VectorX<T>& state_vector = GetVectorState(context);
const Eigen::VectorBlock<const VectorX<T>> state_block =
state_vector.head(state_vector.rows());
// Obtain the block form of y.
Eigen::VectorBlock<VectorX<T>> output_block = output->get_mutable_value();
// Delegate to subclass.
DoCalcVectorOutput(context, input_block, state_block, &output_block);
}
/// Provides a convenience method for %VectorSystem subclasses. This
/// method performs the same logical operation as System::DoCalcOutput but
/// provides VectorBlocks to represent the input, state, and output.
/// Subclasses with outputs should override this method, and not the base
/// class method (which is `final`).
///
/// The @p state will be either empty, the continuous state, or the discrete
/// state, depending on which (or none) was declared at context-creation
/// time.
///
/// The @p input will be empty (zero-sized) when this System is declared to
/// be non-direct-feedthrough.
///
/// By default, this function does nothing if the @p output is empty,
/// and throws an exception otherwise.
virtual void DoCalcVectorOutput(
const Context<T>& context,
const Eigen::VectorBlock<const VectorX<T>>& input,
const Eigen::VectorBlock<const VectorX<T>>& state,
Eigen::VectorBlock<VectorX<T>>* output) const {
unused(context, input, state);
DRAKE_THROW_UNLESS(output->size() == 0);
}
/// Provides a convenience method for %VectorSystem subclasses. This
/// method performs the same logical operation as
/// System::DoCalcTimeDerivatives but provides VectorBlocks to represent the
/// input, continuous state, and derivatives. Subclasses should override
/// this method, and not the base class method (which is `final`).
/// The @p state will be either empty or the continuous state, depending on
/// whether continuous state was declared at context-creation time.
///
/// By default, this function does nothing if the @p derivatives are empty,
/// and throws an exception otherwise.
virtual void DoCalcVectorTimeDerivatives(
const Context<T>& context,
const Eigen::VectorBlock<const VectorX<T>>& input,
const Eigen::VectorBlock<const VectorX<T>>& state,
Eigen::VectorBlock<VectorX<T>>* derivatives) const {
unused(context, input, state);
DRAKE_THROW_UNLESS(derivatives->size() == 0);
}
/// Declares a discrete update rate. You must override
/// DoCalcVectorDiscreteVariableUpdates() to handle the update.
void DeclarePeriodicDiscreteUpdate(double period_sec, double offset_sec) {
this->DeclarePeriodicDiscreteUpdateEvent(
period_sec, offset_sec, &VectorSystem<T>::CalcDiscreteUpdate);
}
/// Provides a convenience method for %VectorSystem subclasses. This
/// method serves as the callback for DeclarePeriodicDiscreteUpdate(),
/// immediately above.
///
/// The @p state will be either empty or the discrete state, depending on
/// whether discrete state was declared at context-creation time.
///
/// By default, this function does nothing if the @p next_state is
/// empty, and throws an exception otherwise.
virtual void DoCalcVectorDiscreteVariableUpdates(
const Context<T>& context,
const Eigen::VectorBlock<const VectorX<T>>& input,
const Eigen::VectorBlock<const VectorX<T>>& state,
Eigen::VectorBlock<VectorX<T>>* next_state) const {
unused(context, input, state);
DRAKE_THROW_UNLESS(next_state->size() == 0);
}
private:
// Confirms the VectorSystem invariants when allocating the context.
void DoValidateAllocatedLeafContext(
const LeafContext<T>& context) const final {
// N.B. The DRAKE_THROW_UNLESS conditions can be triggered by subclass
// mistakes, so are part of our unit tests. The DRAKE_DEMAND conditions
// should be invariants guaranteed by the framework, so are asserted.
// Exactly one input and output.
DRAKE_THROW_UNLESS(this->num_input_ports() <= 1);
DRAKE_THROW_UNLESS(this->num_output_ports() <= 1);
DRAKE_DEMAND(context.num_input_ports() <= 1);
// At most one of either continuous or discrete state.
DRAKE_THROW_UNLESS(context.num_abstract_states() == 0);
const int continuous_size = context.num_continuous_states();
const int num_discrete_groups = context.num_discrete_state_groups();
DRAKE_DEMAND(continuous_size >= 0);
DRAKE_DEMAND(num_discrete_groups >= 0);
DRAKE_THROW_UNLESS(num_discrete_groups <= 1);
DRAKE_THROW_UNLESS((continuous_size == 0) || (num_discrete_groups == 0));
}
// Converts the parameters to Eigen::VectorBlock form, then delegates to
// DoCalcVectorDiscreteVariableUpdates().
EventStatus CalcDiscreteUpdate(const Context<T>& context,
DiscreteValues<T>* discrete_state) const;
};
} // namespace systems
} // namespace drake
DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
class ::drake::systems::VectorSystem)