-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
system.h
2066 lines (1715 loc) · 96.2 KB
/
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
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#pragma once
#include <cmath>
#include <functional>
#include <limits>
#include <map>
#include <memory>
#include <optional>
#include <stdexcept>
#include <string>
#include <type_traits>
#include <utility>
#include <variant>
#include <vector>
#include "drake/common/default_scalars.h"
#include "drake/common/drake_assert.h"
#include "drake/common/drake_bool.h"
#include "drake/common/drake_copyable.h"
#include "drake/common/drake_deprecated.h"
#include "drake/common/drake_throw.h"
#include "drake/common/nice_type_name.h"
#include "drake/common/pointer_cast.h"
#include "drake/common/random.h"
#include "drake/systems/framework/context.h"
#include "drake/systems/framework/event_collection.h"
#include "drake/systems/framework/input_port.h"
#include "drake/systems/framework/output_port.h"
#include "drake/systems/framework/system_base.h"
#include "drake/systems/framework/system_constraint.h"
#include "drake/systems/framework/system_output.h"
#include "drake/systems/framework/system_scalar_converter.h"
#include "drake/systems/framework/system_visitor.h"
#include "drake/systems/framework/witness_function.h"
// TODO(jwnimmer-tri) Remove on 2024-01-01 upon completion of deprecation.
#include <sstream>
namespace drake {
namespace systems {
/** Base class for all System functionality that is dependent on the templatized
scalar type T for input, state, parameters, and outputs.
@tparam_default_scalar */
template <typename T>
class System : public SystemBase {
public:
// System objects are neither copyable nor moveable.
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(System)
/// The scalar type with which this %System was instantiated.
using Scalar = T;
~System() override;
/// Implements a visitor pattern. @see SystemVisitor<T>.
virtual void Accept(SystemVisitor<T>* v) const;
//----------------------------------------------------------------------------
/** @name Cloning
These functions make a deep copy of a system. */
//@{
/** Creates a deep copy of this system.
Even though the cloned system is functionally identical, any contexts created
for this system are not compatible with the cloned system, and vice versa.
@see Context::SetTimeStateAndParametersFrom() for how to copy context data
between clones.
@warning This implementation is somewhat incomplete at the moment. Many
systems will not be able to be cloned, and will throw an exception instead.
To be cloned, at minimum a system must support scalar conversion.
See @ref system_scalar_conversion.
The result is never nullptr. */
std::unique_ptr<System<T>> Clone() const;
/** Creates a deep copy of this system.
In contrast with the instance member function `sys.Clone()`, this static
member function `Clone(sys)` is useful for C++ users to preserve the
<b>declared</b> type of the system being cloned in the returned pointer.
(For both clone overloads, the <b>runtime</b> type is always the same.)
Even though the cloned system is functionally identical, any contexts created
for this system are not compatible with the cloned system, and vice versa.
@warning This implementation is somewhat incomplete at the moment. Many
systems will not be able to be cloned, and will throw an exception instead.
To be cloned, at minimum a system must support scalar conversion.
See @ref system_scalar_conversion.
The result is never nullptr.
Usage: @code
MySystem<double> plant;
unique_ptr<MySystem<double>> copy = System<double>::Clone(plant);
@endcode
@tparam S The specific System type to accept and return. */
template <template <typename> class S = ::drake::systems::System>
static std::unique_ptr<S<T>> Clone(const S<T>& from) {
static_assert(std::is_base_of_v<System<T>, S<T>>);
return dynamic_pointer_cast_or_throw<S<T>>(from.Clone());
}
//@}
//----------------------------------------------------------------------------
/** @name Resource allocation and initialization
These methods are used to allocate and initialize Context resources. */
//@{
// This is just an intentional shadowing of the base class method to return
// a more convenient type.
/** Returns a Context<T> suitable for use with this System<T>. */
std::unique_ptr<Context<T>> AllocateContext() const;
/** Allocates a CompositeEventCollection for this system. The allocated
instance is used for populating collections of triggered events; for
example, Simulator passes this object to System::CalcNextUpdateTime() to
allow the system to identify and handle upcoming events. */
std::unique_ptr<CompositeEventCollection<T>>
AllocateCompositeEventCollection() const;
/** Given an input port, allocates the vector storage. The @p input_port
must match a port declared via DeclareInputPort. */
std::unique_ptr<BasicVector<T>> AllocateInputVector(
const InputPort<T>& input_port) const;
/** Given an input port, allocates the abstract storage. The @p input_port
must match a port declared via DeclareInputPort. */
std::unique_ptr<AbstractValue> AllocateInputAbstract(
const InputPort<T>& input_port) const;
/** Returns a container that can hold the values of all of this System's
output ports. It is sized with the number of output ports and uses each
output port's allocation method to provide an object of the right type
for that port. */
std::unique_ptr<SystemOutput<T>> AllocateOutput() const;
/** Returns a ContinuousState of the same size as the continuous_state
allocated in CreateDefaultContext. The simulator will provide this state
as the output argument to EvalTimeDerivatives. */
virtual std::unique_ptr<ContinuousState<T>> AllocateTimeDerivatives() const
= 0;
/** Returns an Eigen VectorX suitable for use as the output argument to
the CalcImplicitTimeDerivativesResidual() method. The returned VectorX
will have size implicit_time_derivatives_residual_size() with the
elements uninitialized. This is just a convenience method -- you are free
to use any properly-sized mutable Eigen object as the residual vector. */
VectorX<T> AllocateImplicitTimeDerivativesResidual() const {
return VectorX<T>(implicit_time_derivatives_residual_size());
}
/** Returns a DiscreteValues of the same dimensions as the discrete_state
allocated in CreateDefaultContext. The simulator will provide this state
as the output argument to Update. */
virtual std::unique_ptr<DiscreteValues<T>> AllocateDiscreteVariables() const
= 0;
/** This convenience method allocates a context using AllocateContext() and
sets its default values using SetDefaultContext(). */
std::unique_ptr<Context<T>> CreateDefaultContext() const;
/** Assigns default values to all elements of the state. Overrides must not
change the number of state variables. */
virtual void SetDefaultState(const Context<T>& context,
State<T>* state) const = 0;
/** Assigns default values to all parameters. Overrides must not
change the number of parameters. */
virtual void SetDefaultParameters(const Context<T>& context,
Parameters<T>* parameters) const = 0;
/** Sets Context fields to their default values. User code should not
override. */
void SetDefaultContext(Context<T>* context) const;
/** Assigns random values to all elements of the state.
This default implementation calls SetDefaultState; override this method to
provide random initial conditions using the stdc++ random library, e.g.:
@code
std::normal_distribution<T> gaussian();
state->get_mutable_continuous_state()->get_mutable_vector()
->SetAtIndex(0, gaussian(*generator));
@endcode
Overrides must not change the number of state variables.
@see @ref stochastic_systems */
virtual void SetRandomState(const Context<T>& context, State<T>* state,
RandomGenerator* generator) const;
/** Assigns random values to all parameters.
This default implementation calls SetDefaultParameters; override this
method to provide random parameters using the stdc++ random library, e.g.:
@code
std::uniform_real_distribution<T> uniform();
parameters->get_mutable_numeric_parameter(0)
->SetAtIndex(0, uniform(*generator));
@endcode
Overrides must not change the number of state variables.
@see @ref stochastic_systems */
virtual void SetRandomParameters(const Context<T>& context,
Parameters<T>* parameters,
RandomGenerator* generator) const;
/** Sets Context fields to random values. User code should not
override. */
void SetRandomContext(Context<T>* context, RandomGenerator* generator) const;
/** For each input port, allocates a fixed input of the concrete type
that this System requires, and binds it to the port, disconnecting any
prior input. Does not assign any values to the fixed inputs. */
void AllocateFixedInputs(Context<T>* context) const;
/** Returns `true` if any of the inputs to the system might be directly
fed through to any of its outputs and `false` otherwise. */
bool HasAnyDirectFeedthrough() const;
/** Returns true if there might be direct-feedthrough from any input port to
the given @p output_port, and false otherwise. */
bool HasDirectFeedthrough(int output_port) const;
/** Returns true if there might be direct-feedthrough from the given
@p input_port to the given @p output_port, and false otherwise. */
bool HasDirectFeedthrough(int input_port, int output_port) const;
using SystemBase::GetDirectFeedthroughs;
//@}
//----------------------------------------------------------------------------
/** @name Publishing
Publishing is the primary mechanism for a %System to communicate with
the world outside the %System abstraction during a simulation. Publishing
occurs at user-specified times or events and can generate side-effect
results such as terminal output, visualization, logging, plotting, and
network messages. Other than computational cost, publishing has no effect
on the progress of a simulation. */
//@{
/** This method is the public entry point for dispatching all publish event
handlers. It checks the validity of @p context, and directly calls
DispatchPublishHandler. @p events is a homogeneous collection of publish
events.
@note When publishing is triggered at particular times, those times likely
will not coincide with integrator step times. A Simulator may interpolate
to generate a suitable Context, or it may adjust the integrator step size
so that a step begins exactly at the next publication time. In the latter
case the change in step size may affect the numerical result somewhat
since a smaller integrator step produces a more accurate solution. */
[[nodiscard]] EventStatus Publish(
const Context<T>& context,
const EventCollection<PublishEvent<T>>& events) const;
/** (Advanced) Manually triggers any PublishEvent that has trigger
type kForced. Invokes the publish event dispatcher on this %System with the
given Context.
The default dispatcher will invoke the handlers (if any) associated with each
force-triggered event.
@note There will always be at least one force-triggered event, though with no
associated handler. By default that will do nothing when triggered, but that
behavior can be changed by overriding the dispatcher DoPublish()
(not recommended).
The Simulator can be configured to call this in Simulator::Initialize() and at
the start of each continuous integration step. See the Simulator API for more
details.
@throws std::exception if it invokes an event handler that returns status
indicating failure.
@see Publish(), CalcForcedDiscreteVariableUpdate(),
CalcForcedUnrestrictedUpdate() */
void ForcedPublish(const Context<T>& context) const;
//@}
//----------------------------------------------------------------------------
/** @name Cached evaluations
Given the values in a Context, a Drake %System must be able to provide
the results of particular computations needed for analysis and simulation
of the %System. These results are maintained in a mutable cache within
the Context so that a result need be computed only once, the first time
it is requested after a change to one of its prerequisite values.
The `Eval` methods in this group return a reference to the
already-computed result in the given Context's cache. If the current value
is out of date, they first update the cache entry using the corresponding
`Calc` method from the "Calculations" group. Evaluations of input ports
instead delegate to the containing Diagram, which arranges to have the
appropriate subsystem evaluate the source output port.
Methods in this group that specify preconditions operate as follows:
The preconditions will be checked in Debug builds but some or all might
not be checked in Release builds for performance reasons. If we do check
and a precondition is violated, an std::logic_error will be thrown with
a helpful message. */
//@{
/** Returns a reference to the cached value of the continuous state variable
time derivatives, evaluating first if necessary using CalcTimeDerivatives().
This method returns the time derivatives ẋ꜀ of the continuous state
x꜀. The referenced return object will correspond elementwise with the
continuous state in the given Context. Thus, if the state in the Context
has second-order structure `x꜀ = [q v z]`, that same structure applies to
the derivatives so we will have `ẋ꜀ = [q̇ ̇v̇ ż]`.
@param context The Context whose time, input port, parameter, state, and
accuracy values may be used to evaluate the derivatives.
@retval xcdot Time derivatives ẋ꜀ of x꜀ returned as a reference to an object
of the same type and size as `context`'s continuous state.
@see CalcTimeDerivatives(), CalcImplicitTimeDerivativesResidual(),
get_time_derivatives_cache_entry() */
const ContinuousState<T>& EvalTimeDerivatives(
const Context<T>& context) const {
ValidateContext(context);
const CacheEntry& entry = get_time_derivatives_cache_entry();
return entry.Eval<ContinuousState<T>>(context);
}
/** (Advanced) Returns the CacheEntry used to cache time derivatives for
EvalTimeDerivatives(). */
const CacheEntry& get_time_derivatives_cache_entry() const {
return this->get_cache_entry(time_derivatives_cache_index_);
}
/** Returns a reference to the cached value of the potential energy (PE),
evaluating first if necessary using CalcPotentialEnergy().
By definition here, potential energy depends only on "configuration"
(e.g. orientation and position), which includes a subset of the state
variables, and parameters that affect configuration or conservative
forces (such as lengths and masses). The calculated value may also be
affected by the accuracy value supplied in the Context. PE cannot depend
explicitly on time (∂PE/∂t = 0), velocities (∂PE/∂v = 0), or input port
values (∂PE/∂u = 0).
Non-physical systems where PE is not meaningful will return PE = 0.
@param context The Context whose configuration variables may be used to
evaluate potential energy.
@retval PE The potential energy in joules (J) represented by the
configuration given in `context`.
@see CalcPotentialEnergy() */
const T& EvalPotentialEnergy(const Context<T>& context) const;
/** Returns a reference to the cached value of the kinetic energy (KE),
evaluating first if necessary using CalcKineticEnergy().
By definition here, kinetic energy depends only on "configuration" and
"velocity" (e.g. angular and translational velocity) of moving masses
which includes a subset of the state variables, and parameters that affect
configuration, velocities, or mass properties. The calculated value may
also be affected by the accuracy value supplied in the Context. KE cannot
depend explicitly on time (∂KE/∂t = 0) or input port values (∂KE/∂u = 0).
Non-physical systems where KE is not meaningful will return KE = 0.
@param context The Context whose configuration and velocity variables may
be used to evaluate kinetic energy.
@retval KE The kinetic energy in joules (J) represented by the
configuration and velocity given in `context`.
@see CalcKineticEnergy() */
const T& EvalKineticEnergy(const Context<T>& context) const;
/** Returns a reference to the cached value of the conservative power (Pc),
evaluating first if necessary using CalcConservativePower().
The returned Pc represents the rate at which mechanical energy is being
converted _from_ potential energy (PE) _to_ kinetic energy (KE) by this
system in the given Context. This quantity will be _positive_ when PE
is _decreasing_. By definition here, conservative power may depend only
on quantities that explicitly contribute to PE and KE. See
EvalPotentialEnergy() and EvalKineticEnergy() for details.
Power due to non-conservative forces (e.g. dampers) can contribute to the
rate of change of KE. Therefore this method alone cannot be used to
determine whether KE is increasing or decreasing, only whether the
conservative power is adding or removing kinetic energy.
EvalNonConservativePower() can be used in conjunction with this method to
find the total rate of change of KE.
Non-physical systems where Pc is not meaningful will return Pc = 0.
@param context The Context whose contents may be used to evaluate
conservative power.
@retval Pc The conservative power in watts (W or J/s) represented by the
contents of the given `context`.
@see CalcConservativePower(), EvalNonConservativePower(),
EvalPotentialEnergy(), EvalKineticEnergy() */
const T& EvalConservativePower(const Context<T>& context) const;
/** Returns a reference to the cached value of the non-conservative power
(Pnc), evaluating first if necessary using CalcNonConservativePower().
The returned Pnc represents the rate at which work W is done on the system
by non-conservative forces. Pnc is _negative_ if the non-conservative
forces are _dissipative_, positive otherwise. Time integration of Pnc
yields work W, and the total mechanical energy `E = PE + KE − W` should be
conserved by any physically-correct model, to within integration accuracy
of W. Power is in watts (J/s). (Watts are abbreviated W but not to be
confused with work!) Any values in the supplied Context (including time
and input ports) may contribute to the computation of non-conservative
power.
Non-physical systems where Pnc is not meaningful will return Pnc = 0.
@param context The Context whose contents may be used to evaluate
non-conservative power.
@retval Pnc The non-conservative power in watts (W or J/s) represented by
the contents of the given `context`.
@see CalcNonConservativePower(), EvalConservativePower() */
const T& EvalNonConservativePower(const Context<T>& context) const;
// TODO(jwnimmer-tri) Deprecate me.
/** Returns the value of the vector-valued input port with the given
`port_index` as a BasicVector or a specific subclass `Vec` derived from
BasicVector. Causes the value to become up to date first if necessary. See
EvalAbstractInput() for more information.
The result is returned as a pointer to the input port's value of type
`Vec<T>` or nullptr if the port is not connected.
@pre `port_index` selects an existing input port of this System.
@pre the port must have been declared to be vector-valued.
@pre the port's value must be of type Vec<T>.
@tparam Vec The template type of the input vector, which must be a
subclass of BasicVector. */
template <template <typename> class Vec = BasicVector>
const Vec<T>* EvalVectorInput(const Context<T>& context,
int port_index) const {
static_assert(
std::is_base_of_v<BasicVector<T>, Vec<T>>,
"In EvalVectorInput<Vec>, Vec must be a subclass of BasicVector.");
ValidateContext(context);
// The API allows an int but we'll use InputPortIndex internally.
if (port_index < 0)
ThrowNegativePortIndex(__func__, port_index);
const InputPortIndex iport_index(port_index);
const BasicVector<T>* const basic_value =
EvalBasicVectorInputImpl(__func__, context, iport_index);
if (basic_value == nullptr)
return nullptr; // An unconnected port.
// It's a BasicVector, but we're fussy about the subtype here.
const Vec<T>* const value = dynamic_cast<const Vec<T>*>(basic_value);
if (value == nullptr) {
ThrowInputPortHasWrongType(__func__, iport_index,
NiceTypeName::Get<Vec<T>>(),
NiceTypeName::Get(*basic_value));
}
return value;
}
//----------------------------------------------------------------------------
/** @name Constraint-related functions */
//@{
/** Adds an "external" constraint to this System.
This method is intended for use by applications that are examining this
System to add additional constraints based on their particular situation
(e.g., that a velocity state element has an upper bound); it is not
intended for declaring intrinsic constraints that some particular System
subclass might always impose on itself (e.g., that a mass parameter is
non-negative). To that end, this method should not be called by
subclasses of `this` during their constructor.
The `constraint` will automatically persist across system scalar
conversion. */
SystemConstraintIndex AddExternalConstraint(
ExternalSystemConstraint constraint);
//@}
//----------------------------------------------------------------------------
/** @name Calculations
A Drake %System defines a set of common computations that are understood
by the framework. Most of these are embodied in a `Calc` method that
unconditionally performs the calculation into an output argument of the
appropriate type, using only values from the given Context. These are
paired with an `Eval` method that returns a reference to an
already-calculated result residing in the cache; if needed that result is
first obtained using the `Calc` method. See the "Evaluations" group for
more information.
This group also includes additional %System-specific operations that
depend on both Context and additional input arguments. */
//@{
/** Calculates the time derivatives ẋ꜀ of the continuous state x꜀ into
a given output argument. Prefer EvalTimeDerivatives() instead to avoid
unnecessary recomputation.
This method solves the %System equations in explicit form:
ẋ꜀ = fₑ(𝓒)
where `𝓒 = {a, p, t, x, u}` is the current value of the given Context from
which accuracy a, parameters p, time t, state x (`={x꜀ xd xₐ}`) and
input values u are obtained.
@param[in] context The source for time, state, inputs, etc. defining the
point at which the derivatives should be calculated.
@param[out] derivatives The time derivatives ẋ꜀. Must be the same size as
the continuous state vector in `context`.
@see EvalTimeDerivatives() for more information.
@see CalcImplicitTimeDerivativesResidual() for the implicit form of these
equations.*/
void CalcTimeDerivatives(const Context<T>& context,
ContinuousState<T>* derivatives) const;
/** Evaluates the implicit form of the %System equations and returns the
residual.
The explicit and implicit forms of the %System equations are
(1) ẋ꜀ = fₑ(𝓒) explicit
(2) 0 = fᵢ(𝓒; ẋ꜀) implicit
where `𝓒 = {a, p, t, x, u}` is the current value of the given Context from
which accuracy a, parameters p, time t, state x (`={x꜀ xd xₐ}`) and input
values u are obtained. Substituting (1) into (2) shows that the following
condition must always hold:
(3) fᵢ(𝓒; fₑ(𝓒)) = 0 always true
When `fᵢ(𝓒; ẋ꜀ₚ)` is evaluated with a proposed time derivative ẋ꜀ₚ that
differs from ẋ꜀ the result will be non-zero; we call that the _residual_ of
the implicit equation. Given a Context and proposed time derivative ẋ꜀ₚ, this
method returns the residual r such that
(4) r = fᵢ(𝓒; ẋ꜀ₚ).
The returned r will typically be the same length as x꜀ although that is not
required. And even if r and x꜀ are the same size, there will not necessarily
be any elementwise correspondence between them. (That is, you should not
assume that r[i] is the "residual" of ẋ꜀ₚ[i].) For a Diagram, r is the
concatenation of residuals from each of the subsystems, in order of subsystem
index within the Diagram.
A default implementation fᵢ⁽ᵈᵉᶠ⁾ for the implicit form is always provided and
makes use of the explicit form as follows:
(5) fᵢ⁽ᵈᵉᶠ⁾(𝓒; ẋ꜀ₚ) ≜ ẋ꜀ₚ − fₑ(𝓒)
which satisfies condition (3) by construction. (Note that the default
implementation requires the residual to have the same size as x꜀.) Substantial
efficiency gains can often be obtained by replacing the default function with
a customized implementation. Override DoCalcImplicitTimeDerivativesResidual()
to replace the default implementation with a better one.
@param[in] context The source for time, state, inputs, etc. to be used
in calculating the residual.
@param[in] proposed_derivatives The proposed value ẋ꜀ₚ for the time
derivatives of x꜀.
@param[out] residual The result r of evaluating the implicit function.
Can be any mutable Eigen vector object of size
implicit_time_derivatives_residual_size().
@pre `proposed_derivatives` is compatible with this System.
@pre `residual` is of size implicit_time_derivatives_residual_size().
@see SystemBase::implicit_time_derivatives_residual_size()
@see LeafSystem::DeclareImplicitTimeDerivativesResidualSize()
@see DoCalcImplicitTimeDerivativesResidual()
@see CalcTimeDerivatives() */
void CalcImplicitTimeDerivativesResidual(
const Context<T>& context, const ContinuousState<T>& proposed_derivatives,
EigenPtr<VectorX<T>> residual) const;
/** This method is the public entry point for dispatching all discrete
variable update event handlers. Using all the discrete update handlers in
@p events, the method calculates the update `xd(n+1)` to discrete
variables `xd(n)` in @p context and outputs the results to @p
discrete_state. See documentation for
DispatchDiscreteVariableUpdateHandler() for more details. */
[[nodiscard]] EventStatus CalcDiscreteVariableUpdate(
const Context<T>& context,
const EventCollection<DiscreteUpdateEvent<T>>& events,
DiscreteValues<T>* discrete_state) const;
/** Given the @p discrete_state results of a previous call to
CalcDiscreteVariableUpdate() that dispatched the given collection of
events, modifies the @p context to reflect the updated @p discrete_state.
@param[in] events
The Event collection that resulted in the given @p discrete_state.
@param[in,out] discrete_state
The updated discrete state from a CalcDiscreteVariableUpdate()
call. This is mutable to permit its contents to be swapped with the
corresponding @p context contents (rather than copied).
@param[in,out] context
The Context whose discrete state is modified to match
@p discrete_state. Note that swapping contents with @p discrete_state
may cause addresses of individual discrete state group vectors in
@p context to be different on return than they were on entry.
@pre @p discrete_state is the result of a previous
CalcDiscreteVariableUpdate() call that dispatched this @p events
collection. */
void ApplyDiscreteVariableUpdate(
const EventCollection<DiscreteUpdateEvent<T>>& events,
DiscreteValues<T>* discrete_state, Context<T>* context) const;
/** (Advanced) Manually triggers any DiscreteUpdateEvent that has trigger
type kForced. Invokes the discrete event dispatcher on this %System with the
given Context providing the initial values for the discrete variables. The
updated values of the discrete variables are written to the `discrete_state`
output argument; no change is made to the %Context.
The default dispatcher will invoke the handlers (if any) associated with each
force-triggered event.
@note There will always be at least one force-triggered event, though with no
associated handler. By default that will do nothing when triggered, but that
behavior can be changed by overriding the dispatcher (not recommended).
@throws std::exception if it invokes an event handler that returns status
indicating failure.
@see CalcDiscreteVariableUpdate(), CalcForcedUnrestrictedUpdate() */
void CalcForcedDiscreteVariableUpdate(
const Context<T>& context, DiscreteValues<T>* discrete_state) const;
/** This method is the public entry point for dispatching all unrestricted
update event handlers. Using all the unrestricted update handlers in
@p events, it updates *any* state variables in the @p context, and
outputs the results to @p state. It does not allow the dimensionality
of the state variables to change. See the documentation for
DispatchUnrestrictedUpdateHandler() for more details.
@throws std::exception if the dimensionality of the state variables
changes in the callback. */
[[nodiscard]] EventStatus CalcUnrestrictedUpdate(
const Context<T>& context,
const EventCollection<UnrestrictedUpdateEvent<T>>& events,
State<T>* state) const;
/** Given the @p state results of a previous call to CalcUnrestrictedUpdate()
that dispatched the given collection of events, modifies the @p context to
reflect the updated @p state.
@param[in] events
The Event collection that resulted in the given @p state.
@param[in,out] state
The updated State from a CalcUnrestrictedUpdate() call. This is
mutable to permit its contents to be swapped with the corresponding
@p context contents (rather than copied).
@param[in,out] context
The Context whose State is modified to match @p state. Note that
swapping contents with the @p state may cause addresses of
continuous, discrete, and abstract state containers in @p context
to be different on return than they were on entry.
@pre @p state is the result of a previous CalcUnrestrictedUpdate() call
that dispatched this @p events collection. */
void ApplyUnrestrictedUpdate(
const EventCollection<UnrestrictedUpdateEvent<T>>& events,
State<T>* state, Context<T>* context) const;
/** (Advanced) Manually triggers any UnrestrictedUpdateEvent that has trigger
type kForced. Invokes the unrestricted event dispatcher on this %System with
the given Context providing the initial values for the state variables. The
updated values of the state variables are written to the `state`
output argument; no change is made to the %Context.
The default dispatcher will invoke the handlers (if any) associated with each
force-triggered event.
@note There will always be at least one force-triggered event, though with no
associated handler. By default that will do nothing when triggered, but that
behavior can be changed by overriding the dispatcher (not recommended).
@throws std::exception if it invokes an event handler that returns status
indicating failure.
@see CalcUnrestrictedUpdate() */
void CalcForcedUnrestrictedUpdate(const Context<T>& context,
State<T>* state) const;
/** This method is called by a Simulator during its calculation of the size of
the next continuous step to attempt. The System returns the next time at
which some discrete action must be taken, and records what those actions
ought to be in @p events. Upon reaching that time, the simulator will
merge @p events with the other CompositeEventCollection instances
triggered through other mechanisms (e.g. GetPerStepEvents()), and the
merged CompositeEventCollection will be passed to all event handling
mechanisms.
Despite the name, the returned events includes both state-updating events
and publish events.
If there is no timed event coming, the return value is Infinity. If
a finite update time is returned, there will be at least one Event object
in the returned event collection.
@p events cannot be null. @p events will be cleared on entry. */
T CalcNextUpdateTime(const Context<T>& context,
CompositeEventCollection<T>* events) const;
/** Returns all periodic events in this %System. This includes publish,
discrete update, and unrestricted update events.
@p events cannot be null. @p events will be cleared on entry.
@see GetPerStepEvents(), GetInitializationEvents() */
void GetPeriodicEvents(const Context<T>& context,
CompositeEventCollection<T>* events) const;
/** This method is called by Simulator::Initialize() to gather all update
and publish events that are to be handled in AdvanceTo() at the point
before Simulator integrates continuous state. It is assumed that these
events remain constant throughout the simulation. The "step" here refers
to the major time step taken by the Simulator. During every simulation
step, the simulator will merge @p events with the event collections
populated by other types of event triggering mechanism (e.g.,
CalcNextUpdateTime()), and the merged CompositeEventCollection objects
will be passed to the appropriate handlers before Simulator integrates the
continuous state.
@p events cannot be null. @p events will be cleared on entry.
@see GetPeriodicEvents(), GetInitializationEvents() */
void GetPerStepEvents(const Context<T>& context,
CompositeEventCollection<T>* events) const;
/** This method is called by Simulator::Initialize() to gather all
update and publish events that need to be handled at initialization
before the simulator starts integration.
@p events cannot be null. @p events will be cleared on entry.
@see GetPeriodicEvents(), GetPerStepEvents() */
void GetInitializationEvents(const Context<T>& context,
CompositeEventCollection<T>* events) const;
/** This method triggers all of the initialization events returned by
GetInitializationEvents(). The method allocates temporary storage to perform
the updates, and is intended only as a convenience method for callers who do
not want to use the full Simulator workflow.
Note that this is not fully equivalent to Simulator::Initialize() because
_only_ initialization events are handled here, while Simulator::Initialize()
also processes other events associated with time zero. Also, "reached
termination" returns are ignored here.
@throws std::exception if it invokes an event handler that returns status
indicating failure. */
void ExecuteInitializationEvents(Context<T>* context) const;
/** Determines whether there exists a unique periodic timing (offset and
period) that triggers one or more discrete update events (and, if so, returns
that unique periodic timing). Thus, this method can be used (1) as a test to
determine whether a system's dynamics are at least partially governed by
difference equations, and (2) to obtain the difference equation update times.
Use EvalUniquePeriodicDiscreteUpdate() if you want to determine the actual
effects of triggering these events.
@warning Even if we find a unique discrete update timing as described above,
there may also be unrestricted updates performed with that timing or other
timings. (Unrestricted updates can modify any state variables _including_
discrete variables.) Also, there may be trigger types other than periodic that
can modify discrete variables. This function does not attempt to look for any
of those; they are simply ignored. If you are concerned with those, you can
use GetPerStepEvents(), GetInitializationEvents(), and GetPeriodicEvents() to
get a more comprehensive picture of the event landscape.
@returns optional<PeriodicEventData> Contains the unique periodic trigger
timing if it exists, otherwise `nullopt`.
@see EvalUniquePeriodicDiscreteUpdate(), IsDifferenceEquationSystem() */
std::optional<PeriodicEventData>
GetUniquePeriodicDiscreteUpdateAttribute() const;
/** If this %System contains a unique periodic timing for discrete update
events, this function executes the handlers for those periodic events to
determine what their effect would be. Returns a reference to the discrete
variable cache entry containing what values the discrete variables would have
if these periodic events were triggered.
Note that this function _does not_ change the value of the discrete variables
in the supplied Context. However, you can apply the result to the %Context
like this: @code
const DiscreteValue<T>& updated =
system.EvalUniquePeriodicDiscreteUpdate(context);
context.SetDiscreteState(updated);
@endcode
You can write the updated values to a different %Context than the one you
used to calculate the update; the requirement is only that the discrete
state in the destination has the same structure (number of groups and size of
each group).
You can use GetUniquePeriodicDiscreteUpdateAttribute() to check whether you
can call %EvalUniquePeriodicDiscreteUpdate() safely, and to find the unique
periodic timing information (offset and period).
@warning Even if we find a unique discrete update timing as described above,
there may also be unrestricted updates performed with that timing or other
timings. (Unrestricted updates can modify any state variables _including_
discrete variables.) Also, there may be trigger types other than periodic that
can modify discrete variables. This function does not attempt to look for any
of those; they are simply ignored. If you are concerned with those, you can
use GetPerStepEvents(), GetInitializationEvents(), and GetPeriodicEvents() to
get a more comprehensive picture of the event landscape.
@param[in] context
The Context containing the current %System state and the mutable cache
space into which the result is written. The current state is _not_
modified, though the cache entry may be updated.
@returns
A reference to the DiscreteValues cache space in `context` containing
the result of applying the discrete update event handlers to the current
discrete variable values.
@note The referenced cache entry is recalculated if anything in the
given Context has changed since last calculation. Subsequent calls just
return the already-calculated value.
@throws std::exception if there is not exactly one periodic timing in this
%System (which may be a Diagram) that triggers discrete update events.
@throws std::exception if it invokes an event handler that returns status
indicating failure.
@par Implementation
If recalculation is needed, copies the current discrete state values into
preallocated `context` cache space. Applies the discrete update event handlers
(in an unspecified order) to the cache copy, possibly updating it. Returns a
reference to the possibly-updated cache space.
@see GetUniquePeriodicDiscreteUpdateAttribute(), GetPeriodicEvents() */
const DiscreteValues<T>& EvalUniquePeriodicDiscreteUpdate(
const Context<T>& context) const;
/** Returns true iff the state dynamics of this system are governed
exclusively by a difference equation on a single discrete state group
and with a unique periodic update (having zero offset). E.g., it is
amenable to analysis of the form:
x[n+1] = f(x[n], u[n])
Note that we do NOT consider the number of input ports here, because
in practice many systems of interest (e.g. MultibodyPlant) have input
ports that are safely treated as constant during the analysis.
Consider using get_input_port_selection() to choose one.
@warning In determining whether this system is governed as above, we do not
consider unrestricted updates or any update events that have trigger types
other than periodic. See GetUniquePeriodicDiscreteUpdateAttribute() for more
information.
@param[out] time_period if non-null, then iff the function
returns `true`, then time_period is set to the period data
returned from GetUniquePeriodicDiscreteUpdateAttribute(). If the
function returns `false` (the system is not a difference equation
system), then `time_period` does not receive a value.
@see GetUniquePeriodicDiscreteUpdateAttribute()
@see EvalUniquePeriodicDiscreteUpdate() */
bool IsDifferenceEquationSystem(double* time_period = nullptr) const;
/** Maps all periodic triggered events for a %System, organized by timing.
Each unique periodic timing attribute (offset and period) is
mapped to the set of Event objects that are triggered with that timing.
Those may include a mix of Publish, DiscreteUpdate, and UnrestrictedUpdate
events.
@param context Optional Context to pass on to Event selection functions;
not commonly needed. */
std::map<PeriodicEventData, std::vector<const Event<T>*>,
PeriodicEventDataComparator>
MapPeriodicEventsByTiming(const Context<T>* context = nullptr) const;
/** Utility method that computes for _every_ output port i the value y(i) that
should result from the current contents of the given Context. Note that
individual output port values can be calculated using
`get_output_port(i).Calc()`; this method invokes that for each output port
in index order. The result may depend on time and the current values of
input ports, parameters, and state variables. The result is written to
`outputs` which must already have been allocated to have the right number
of entries of the right types. */
void CalcOutput(const Context<T>& context, SystemOutput<T>* outputs) const;
/** Calculates and returns the potential energy represented by the current
configuration provided in `context`. Prefer EvalPotentialEnergy() to
avoid unnecessary recalculation.
@see EvalPotentialEnergy() for more information. */
T CalcPotentialEnergy(const Context<T>& context) const;
/** Calculates and returns the kinetic energy represented by the current
configuration and velocity provided in `context`. Prefer
EvalKineticEnergy() to avoid unnecessary recalculation.
@see EvalKineticEnergy() for more information. */
T CalcKineticEnergy(const Context<T>& context) const;
/** Calculates and returns the conservative power represented by the current
contents of the given `context`. Prefer EvalConservativePower() to avoid
unnecessary recalculation.
@see EvalConservativePower() for more information. */
T CalcConservativePower(const Context<T>& context) const;
/** Calculates and returns the non-conservative power represented by the
current contents of the given `context`. Prefer EvalNonConservativePower()
to avoid unnecessary recalculation.
@see EvalNonConservativePower() for more information. */
T CalcNonConservativePower(const Context<T>& context) const;
/** Transforms a given generalized velocity `v` to the time derivative `qdot`
of the generalized configuration `q` taken from the supplied Context.
`v` and `qdot` are related linearly by `qdot = N(q) * v`, where `N` is a
block diagonal matrix. For example, in a multibody system there will be
one block of `N` per tree joint. This computation requires only `O(nq)`
time where `nq` is the size of `qdot`. Note that `v` is *not* taken from
the Context; it is given as an argument here.
See the alternate signature if you already have the generalized
velocity in an Eigen VectorX object; this signature will copy the
VectorBase into an Eigen object before performing the computation.
@see MapQDotToVelocity() */
void MapVelocityToQDot(const Context<T>& context,
const VectorBase<T>& generalized_velocity,
VectorBase<T>* qdot) const;
/** Transforms the given generalized velocity to the time derivative of
generalized configuration. See the other signature of MapVelocityToQDot()
for more information. */
void MapVelocityToQDot(
const Context<T>& context,
const Eigen::Ref<const VectorX<T>>& generalized_velocity,
VectorBase<T>* qdot) const;
/** Transforms the time derivative `qdot` of the generalized configuration `q`
to generalized velocities `v`. `v` and `qdot` are related linearly by
`qdot = N(q) * v`, where `N` is a block diagonal matrix. For example, in a
multibody system there will be one block of `N` per tree joint. Although
`N` is not necessarily square, its left pseudo-inverse `N+` can be used to
invert that relationship without residual error, provided that `qdot` is
in the range space of `N` (that is, if it *could* have been produced as
`qdot=N*v` for some `v`). Using the configuration `q` from the given
Context this method calculates `v = N+ * qdot` (where `N+=N+(q)`) for
a given `qdot`. This computation requires only `O(nq)` time where `nq` is
the size of `qdot`. Note that this method does not take `qdot` from the
Context.
See the alternate signature if you already have `qdot` in an %Eigen
VectorX object; this signature will copy the VectorBase into an %Eigen
object before performing the computation.
@see MapVelocityToQDot() */
void MapQDotToVelocity(const Context<T>& context, const VectorBase<T>& qdot,
VectorBase<T>* generalized_velocity) const;
/** Transforms the given time derivative `qdot` of generalized configuration
`q` to generalized velocity `v`. This signature takes `qdot` as an %Eigen
VectorX object for faster speed. See the other signature of
MapQDotToVelocity() for additional information. */
void MapQDotToVelocity(const Context<T>& context,
const Eigen::Ref<const VectorX<T>>& qdot,
VectorBase<T>* generalized_velocity) const;
//@}
//----------------------------------------------------------------------------
/** @name Subcontext access
Methods in this section locate the Context belonging to a particular
subsystem, from within the Context for a containing System (typically a
Diagram). There are two common circumstances where this is needed:
1. You are given a Diagram and its Context, and have a reference to a
particular subsystem contained somewhere in that Diagram (that is,
an immediate child or deeper descendent). You can ask the Diagram to
find the subcontext of that subsystem, using GetSubsystemContext()
or GetMutableSubsystemContext().
2. You are given the root Context for a complete Diagram (typically by
the Simulator as part of a generated trajectory). You don't have a
reference to the Diagram, but you do have a reference to a subsystem
of interest. You want to find its subcontext from within the root
Context. Use GetMyContextFromRoot() or GetMyMutableContextFromRoot().
The second case is particularly useful in monitor functions for the
Drake Simulator. */
//@{
/** Returns a const reference to the subcontext that corresponds to the
contained %System `subsystem`.
@throws std::exception if `subsystem` not contained in `this` %System.
@pre The given `context` is valid for use with `this` %System. */
const Context<T>& GetSubsystemContext(const System<T>& subsystem,
const Context<T>& context) const;