-
Notifications
You must be signed in to change notification settings - Fork 1.3k
/
system_base.h
1427 lines (1251 loc) · 65.4 KB
/
system_base.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 <functional>
#include <map>
#include <memory>
#include <optional>
#include <set>
#include <string>
#include <utility>
#include <variant>
#include <vector>
#include "drake/common/drake_deprecated.h"
#include "drake/common/drake_throw.h"
#include "drake/common/unused.h"
#include "drake/systems/framework/abstract_value_cloner.h"
#include "drake/systems/framework/cache_entry.h"
#include "drake/systems/framework/framework_common.h"
#include "drake/systems/framework/input_port_base.h"
#include "drake/systems/framework/output_port_base.h"
#include "drake/systems/framework/value_producer.h"
namespace drake {
namespace systems {
/** Provides non-templatized functionality shared by the templatized System
classes.
Terminology: in general a Drake System is a tree structure composed of
"subsystems", which are themselves System objects. The corresponding Context is
a parallel tree structure composed of "subcontexts", which are themselves
Context objects. There is a one-to-one correspondence between subsystems and
subcontexts. Within a given System (Context), its child subsystems (subcontexts)
are indexed using a SubsystemIndex; there is no separate SubcontextIndex since
the numbering must be identical. */
class SystemBase : public internal::SystemMessageInterface {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SystemBase)
~SystemBase() override;
/** Sets the name of the system. Do not use the path delimiter character ':'
in the name. When creating a Diagram, names of sibling subsystems should be
unique. DiagramBuilder uses this method to assign a unique default name if
none is provided. */
// TODO(sherm1) Enforce reasonable naming policies.
void set_name(const std::string& name) { name_ = name; }
/** Returns the name last supplied to set_name(), if any. Diagrams built with
DiagramBuilder will always have a default name for every contained subsystem
for which no user-provided name is available. Systems created by copying with
a scalar type change have the same name as the source system. An empty string
is returned if no name has been set. */
// TODO(sherm1) This needs to be better distinguished from the human-readable
// name. Consider an api change like get_label() for this one, with the
// intent that the label could be used programmatically.
const std::string& get_name() const { return name_; }
/** Returns a human-readable name for this system, for use in messages and
logging. This will be the same as returned by get_name(), unless that would
be an empty string. In that case we return a non-unique placeholder name,
currently just "_" (a lone underscore). */
const std::string& GetSystemName() const final {
return name_.empty() ? internal::SystemMessageInterface::no_name() : name_;
}
/** Generates and returns a human-readable full path name of this subsystem,
for use in messages and logging. The name starts from the root System, with
"::" delimiters between parent and child subsystems, with the individual
subsystems represented by their names as returned by GetSystemName(). */
std::string GetSystemPathname() const final;
/** Returns the most-derived type of this concrete System object as a
human-readable string suitable for use in error messages. The format is as
generated by NiceTypeName and will include namespace qualification if
present.
@see NiceTypeName for more specifics. */
std::string GetSystemType() const final { return NiceTypeName::Get(*this); }
/** Returns a Context suitable for use with this System. Context resources
are allocated based on resource requests that were made during System
construction. */
std::unique_ptr<ContextBase> AllocateContext() const {
// Get a concrete Context of the right type, allocate internal resources
// like parameters, state, and cache entries, and set up intra- and
// inter-subcontext dependencies.
std::unique_ptr<ContextBase> context = DoAllocateContext();
// We depend on derived classes to call our InitializeContextBase() method
// after allocating the appropriate concrete Context.
DRAKE_DEMAND(
internal::SystemBaseContextBaseAttorney::is_context_base_initialized(
*context));
return context;
}
//----------------------------------------------------------------------------
/** @name Input port evaluation
These methods provide scalar type-independent evaluation of a System input
port in a particular Context. If necessary, they first cause the port's value
to become up to date, then they return a reference to the now-up-to-date value
in the given Context.
Specified preconditions for these methods 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.
@see System::EvalVectorInput(), System::EvalEigenVectorInput() for
scalar type-specific input port access. */
//@{
// TODO(jwnimmer-tri) Deprecate me.
/** Returns the value of the input port with the given `port_index` as an
AbstractValue, which is permitted for ports of any type. Causes the value to
become up to date first if necessary, delegating to our parent Diagram.
Returns a pointer to the port's value, or nullptr if the port is not
connected. If you know the actual type, use one of the more-specific
signatures.
@pre `port_index` selects an existing input port of this System.
@see EvalInputValue(), System::EvalVectorInput(),
System::EvalEigenVectorInput() */
const AbstractValue* EvalAbstractInput(const ContextBase& context,
int port_index) const {
ValidateContext(context);
if (port_index < 0)
ThrowNegativePortIndex(__func__, port_index);
const InputPortIndex port(port_index);
return EvalAbstractInputImpl(__func__, context, port);
}
// TODO(jwnimmer-tri) Deprecate me.
/** Returns the value of an abstract-valued input port with the given
`port_index` as a value of known type `V`. 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 `V`,
or nullptr if the port is not connected.
@pre `port_index` selects an existing input port of this System.
@pre the port's value must be retrievable from the stored abstract value
using `AbstractValue::get_value<V>`.
@tparam V The type of data expected. */
template <typename V>
const V* EvalInputValue(const ContextBase& context, int port_index) const {
ValidateContext(context);
if (port_index < 0)
ThrowNegativePortIndex(__func__, port_index);
const InputPortIndex port(port_index);
const AbstractValue* const abstract_value =
EvalAbstractInputImpl(__func__, context, port);
if (abstract_value == nullptr)
return nullptr; // An unconnected port.
// We have a value, is it a V?
const V* const value = abstract_value->maybe_get_value<V>();
if (value == nullptr) {
ThrowInputPortHasWrongType(__func__, port, NiceTypeName::Get<V>(),
abstract_value->GetNiceTypeName());
}
return value;
}
//@}
/** Returns the number of input ports currently allocated in this System.
These are indexed from 0 to %num_input_ports()-1. */
int num_input_ports() const {
return static_cast<int>(input_ports_.size());
}
/** Returns the number of output ports currently allocated in this System.
These are indexed from 0 to %num_output_ports()-1. */
int num_output_ports() const {
return static_cast<int>(output_ports_.size());
}
/** Returns a reference to an InputPort given its `port_index`.
@pre `port_index` selects an existing input port of this System. */
const InputPortBase& get_input_port_base(InputPortIndex port_index) const {
return GetInputPortBaseOrThrow(__func__, port_index);
}
/** Returns a reference to an OutputPort given its `port_index`.
@pre `port_index` selects an existing output port of this System. */
const OutputPortBase& get_output_port_base(OutputPortIndex port_index) const {
return GetOutputPortBaseOrThrow(__func__, port_index);
}
/** Returns the total dimension of all of the vector-valued input ports (as if
they were muxed). */
int num_total_inputs() const {
int count = 0;
for (const auto& in : input_ports_) count += in->size();
return count;
}
/** Returns the total dimension of all of the vector-valued output ports (as
if they were muxed). */
int num_total_outputs() const {
int count = 0;
for (const auto& out : output_ports_) count += out->size();
return count;
}
/** Reports all direct feedthroughs from input ports to output ports. For
a system with m input ports: `I = i₀, i₁, ..., iₘ₋₁`, and n output ports,
`O = o₀, o₁, ..., oₙ₋₁`, the return map will contain pairs (u, v) such that
- 0 ≤ u < m,
- 0 ≤ v < n,
- and there _might_ be a direct feedthrough from input iᵤ to each output oᵥ.
See @ref DeclareLeafOutputPort_feedthrough "DeclareLeafOutputPort"
documentation for how leaf systems can report their feedthrough.
*/
virtual std::multimap<int, int> GetDirectFeedthroughs() const = 0;
/** Returns the number nc of cache entries currently allocated in this System.
These are indexed from 0 to nc-1. */
int num_cache_entries() const {
return static_cast<int>(cache_entries_.size());
}
/** Returns a reference to a CacheEntry given its `index`. */
const CacheEntry& get_cache_entry(CacheIndex index) const {
DRAKE_ASSERT(0 <= index && index < num_cache_entries());
return *cache_entries_[index];
}
/** (Advanced) Returns a mutable reference to a CacheEntry given its `index`.
Note that you do not need mutable access to a CacheEntry to modify its value
in a Context, so most users should not use this method. */
CacheEntry& get_mutable_cache_entry(CacheIndex index) {
DRAKE_ASSERT(0 <= index && index < num_cache_entries());
return *cache_entries_[index];
}
//============================================================================
/** @name Declare cache entries
@anchor DeclareCacheEntry_documentation
Methods in this section are used by derived classes to declare cache entries
for their own internal computations. (Other cache entries are provided
automatically for well-known computations such as output ports and time
derivatives.) Cache entries may contain values of any type, however the type
for any particular cache entry is fixed after first allocation. Every cache
entry must have an _allocator_ function `Allocate()` and a _calculator_
function `Calc()`. `Allocate()` returns an object suitable for holding a value
of the cache entry. `Calc()` uses the contents of a given Context to produce
the cache entry's value, which is placed in an object of the type returned by
`Allocate()`.
@warning These methods are currently specified as `public` access, but will
be demoted to `protected` access on or after 2021-10-01.
<h4>Prerequisites</h4>
Correct runtime caching behavior depends critically on understanding the
dependencies of the cache entry's `Calc()` function (we call those
"prerequisites"). If none of the prerequisites has changed since the last
time `Calc()` was invoked to set the cache entry's value, then we don't need
to perform a potentially expensive recalculation. On the other hand, if any
of the prerequisites has changed then the current value is invalid and must
not be used without first recomputing.
Currently it is not possible for Drake to infer prerequisites accurately and
automatically from inspection of the `Calc()` implementation. Therefore,
if you don't say otherwise, Drake will assume `Calc()` is dependent
on all value sources in the Context, including time, state, input ports,
parameters, and accuracy. That means the cache entry's value will be
considered invalid if _any_ of those sources has changed since the last time
the value was calculated. That is safe, but can result in more computation
than necessary. If you know that your `Calc()` method has fewer prerequisites,
you may say so by providing an explicit list in the `prerequisites_of_calc`
parameter. Every possible prerequisite has a DependencyTicket ("ticket"), and
the list should consist of tickets. For example, if your calculator depends
only on time (e.g. `Calc(context)` is `sin(context.get_time())`) then you
would specify `prerequisites_of_calc={time_ticket()}` here. See
@ref DependencyTicket_documentation "Dependency tickets" for a list of the
possible tickets and what they mean.
@warning It is critical that the prerequisite list you supply be accurate, or
at least conservative, for correct functioning of the caching system. Drake
cannot currently detect that a `Calc()` function accesses an undeclared
prerequisite. Even assuming you have correctly listed the prerequisites, you
should include a prominent comment in every `Calc()` implementation noting
that if the implementation is changed then the prerequisite list must be
updated correspondingly.
A technique you can use to ensure that prerequisites have been properly
specified is to make use of the Context's
@ref drake::systems::ContextBase::DisableCaching "DisableCaching()"
method, which causes cache values to be recalculated unconditionally. You
should get identical results with caching enabled or disabled, with speed
being the only difference. You can also disable caching for individual
cache entries in a Context, or specify that individual cache entries should
be disabled by default when they are first allocated.
@see ContextBase::DisableCaching()
@see CacheEntry::disable_caching()
@see CacheEntry::disable_caching_by_default()
@see LeafOutputPort::disable_caching_by_default()
<h4>Which signature to use?</h4>
Although the allocator and calculator functions ultimately satisfy generic
function signatures defined by a ValueProducer, we provide a variety
of `DeclareCacheEntry()` signatures here for convenient specification,
with mapping to the generic form handled invisibly. In particular,
allocators are most easily defined by providing a model value that can be
used to construct an allocator that copies the model when a new value
object is needed. Alternatively a method can be provided that constructs
a value object when invoked (those methods are conventionally, but not
necessarily, named `MakeSomething()` where `Something` is replaced by the
cache entry value type).
Because cache entry values are ultimately stored in AbstractValue objects,
the underlying types must be suitable. That means the type must be copy
constructible or cloneable. For methods below that are not given an explicit
model value or construction ("make") method, the underlying type must also be
default constructible.
@see drake::Value for more about abstract values. */
//@{
// TODO(jwnimmer-tri) We are violating the style guide here by momentarily
// introducing a "protected:" section for just this one function and then
// going to back to "public:" afterwards. We need to do this so all of the
// DeclareCacheEntry overloads share a Doxygen section, but once the other
// overloads become protected after 2021-10-01 we should move this entire
// section down into the real "protected:" section lower in this class
// declaration and by doing so remove the style violation.
protected:
/// @anchor DeclareCacheEntry_primary
/** Declares a new %CacheEntry in this System using the most generic form
of the calculation function. Prefer one of the more convenient signatures
below if you can. The new cache entry is assigned a unique CacheIndex and
DependencyTicket, which can be obtained from the returned %CacheEntry.
@param[in] description
A human-readable description of this cache entry, most useful for debugging
and documentation. Not interpreted in any way by Drake; it is retained
by the cache entry and used to generate the description for the
corresponding CacheEntryValue in the Context.
@param[in] value_producer
Provides the computation that maps from a given Context to the current
value that this cache entry should have, as well as a way to allocate
storage prior to the computation.
@param[in] prerequisites_of_calc
Provides the DependencyTicket list containing a ticket for _every_ Context
value on which `calc_function` may depend when it computes its result.
Defaults to `{all_sources_ticket()}` if unspecified. If the cache value
is truly independent of the Context (rare!) say so explicitly by providing
the list `{nothing_ticket()}`; an explicitly empty list `{}` is forbidden.
@returns a reference to the newly-created %CacheEntry.
@throws std::exception if given an explicitly empty prerequisite list. */
CacheEntry& DeclareCacheEntry(
std::string description, ValueProducer value_producer,
std::set<DependencyTicket> prerequisites_of_calc = {
all_sources_ticket()});
// NOLINTNEXTLINE(whitespace/blank_line)
public:
// (Undo the momentary "protected:" from immediately above.)
// Declares a cache entry by specifying two callback functions.
DRAKE_DEPRECATED("2021-10-01", "Use the ValueProducer overload instead.")
CacheEntry& DeclareCacheEntry(
std::string description,
std::function<std::unique_ptr<AbstractValue>()> alloc_function,
std::function<void(const ContextBase&, AbstractValue*)> calc_function,
std::set<DependencyTicket> prerequisites_of_calc = {
all_sources_ticket()});
// Declares a cache entry via two member function pointers.
// This will be demoted to `protected` access on or after 2021-10-01, and then
// removed entirely on 2021-11-01.
template <class MySystem, class MyContext, typename ValueType>
DRAKE_DEPRECATED("2021-11-01",
"This overload for DeclareCacheEntry is rarely the best choice; it is"
" unusual for allocation to actually require a boutique callback rather"
" than just a Clone of a model_value. We found that most uses of this"
" overload hindered readability, because other overloads would often do"
" the job more directly. If no existing overload works, you may wrap a"
" ValueProducer around your existing make method and call the primary"
" DeclareCacheEntry overload that takes a ValueProducer, instead.")
CacheEntry& DeclareCacheEntry(
std::string description,
ValueType (MySystem::*make)() const,
void (MySystem::*calc)(const MyContext&, ValueType*) const,
std::set<DependencyTicket> prerequisites_of_calc = {
all_sources_ticket()});
/// @anchor DeclareCacheEntry_model_and_calc
/** Declares a cache entry by specifying a model value of concrete type
`ValueType` and a calculator function that is a class member function (method)
with signature: @code
void MySystem::CalcCacheValue(const MyContext&, ValueType*) const;
@endcode
where `MySystem` is a class derived from `SystemBase`, `MyContext` is a class
derived from `ContextBase`, and `ValueType` is any concrete type such that
`Value<ValueType>` is permitted. (The method names are arbitrary.) Template
arguments will be deduced and do not need to be specified. See the
@ref DeclareCacheEntry_primary "primary DeclareCacheEntry() signature"
above for more information about the parameters and behavior.
@see drake::Value
@warning This method is currently specified as `public` access, but will be
demoted to `protected` access on or after 2021-10-01. */
template <class MySystem, class MyContext, typename ValueType>
CacheEntry& DeclareCacheEntry(
std::string description, const ValueType& model_value,
void (MySystem::*calc)(const MyContext&, ValueType*) const,
std::set<DependencyTicket> prerequisites_of_calc = {
all_sources_ticket()});
// Declares a cache entry via a model value and calc member function pointer.
// This will be demoted to `protected` access on or after 2021-10-01, and then
// removed entirely on 2021-11-01.
template <class MySystem, class MyContext, typename ValueType>
DRAKE_DEPRECATED("2021-11-01",
"This overload for DeclareCacheEntry is dispreferred because it might"
" not reuse heap storage from one calculation to the next, and so is"
" typically less efficient than the other overloads. A better option"
" is to change the ValueType returned by-value to be an output pointer"
" instead, and return void. If that is not possible, you may wrap a"
" ValueProducer around your existing method and call the primary"
" DeclareCacheEntry overload that takes a ValueProducer, instead.")
CacheEntry& DeclareCacheEntry(
std::string description, const ValueType& model_value,
ValueType (MySystem::*calc)(const MyContext&) const,
std::set<DependencyTicket> prerequisites_of_calc = {
all_sources_ticket()});
/// @anchor DeclareCacheEntry_calc_only
/** Declares a cache entry by specifying only a calculator function that is a
class member function (method) with signature:
@code
void MySystem::CalcCacheValue(const MyContext&, ValueType*) const;
@endcode
where `MySystem` is a class derived from `SystemBase` and `MyContext` is a
class derived from `ContextBase`. `ValueType` is a concrete type such that
(a) `Value<ValueType>` is permitted, and (b) `ValueType` is default
constructible. That allows us to create a model value using
`Value<ValueType>{}` (value initialized so numerical types will be zeroed in
the model). (The method name is arbitrary.) Template arguments will be
deduced and do not need to be specified. See the first DeclareCacheEntry()
signature above for more information about the parameters and behavior.
@note The default constructor will be called once immediately to create a
model value, and subsequent allocations will just copy the model value without
invoking the constructor again. If you want the constructor invoked again at
each allocation (not common), use one of the other signatures to explicitly
provide a method for the allocator to call; that method can then invoke
the `ValueType` default constructor each time it is called.
@see drake::Value
@warning This method is currently specified as `public` access, but will be
demoted to `protected` access on or after 2021-10-01. */
template <class MySystem, class MyContext, typename ValueType>
CacheEntry& DeclareCacheEntry(
std::string description,
void (MySystem::*calc)(const MyContext&, ValueType*) const,
std::set<DependencyTicket> prerequisites_of_calc = {
all_sources_ticket()});
// Declares a cache entry via a calculator member function pointer only.
// This will be demoted to `protected` access on or after 2021-10-01, and then
// removed entirely on 2021-11-01.
template <class MySystem, class MyContext, typename ValueType>
DRAKE_DEPRECATED("2021-11-01",
"This overload for DeclareCacheEntry is dispreferred because it might"
" not reuse heap storage from one calculation to the next, and so is"
" typically less efficient than the other overloads. A better option"
" is to change the ValueType returned by-value to be an output pointer"
" instead, and return void. If that is not possible, you may wrap a"
" ValueProducer around your existing method and call the primary"
" DeclareCacheEntry overload that takes a ValueProducer, instead.")
CacheEntry& DeclareCacheEntry(
std::string description,
ValueType (MySystem::*calc)(const MyContext&) const,
std::set<DependencyTicket> prerequisites_of_calc = {
all_sources_ticket()});
//@}
//============================================================================
/** @name Dependency tickets
@anchor DependencyTicket_documentation
Use these tickets to declare well-known sources as prerequisites of a
downstream computation such as an output port, derivative, update, or cache
entry. The ticket numbers for the built-in sources are the same for all
systems. For time and accuracy they refer to the same global resource;
otherwise they refer to the specified sources within the referencing system.
A dependency ticket for a more specific resource (a particular input or
output port, a discrete variable group, abstract state variable, a parameter,
or a cache entry) is allocated and stored with the resource when it is
declared. Usually the tickets are obtained directly from the resource but
you can recover them with methods here knowing only the resource index. */
//@{
// The DependencyTrackers associated with these tickets are allocated
// in ContextBase::CreateBuiltInTrackers() and the implementation there must
// be kept up to date with the API contracts here.
// The ticket methods are promoted in the System<T> class so that users can
// invoke them in their constructors without prefixing with this->. If you
// add, remove, rename, or reorder any of these be sure to update the
// promotions in system.h.
// Keep the order here the same as they are defined in the internal enum
// BuiltInTicketNumbers, with the "particular resource" indexed methods
// inserted prior to the corresponding built-in "all such resources" ticket.
/** Returns a ticket indicating that a computation does not depend on *any*
source value; that is, it is a constant. If this appears in a prerequisite
list, it must be the only entry. */
static DependencyTicket nothing_ticket() {
return DependencyTicket(internal::kNothingTicket);
}
/** Returns a ticket indicating dependence on time. This is the same ticket
for all systems and refers to the same time value. */
static DependencyTicket time_ticket() {
return DependencyTicket(internal::kTimeTicket);
}
/** Returns a ticket indicating dependence on the accuracy setting in the
Context. This is the same ticket for all systems and refers to the same
accuracy value. */
static DependencyTicket accuracy_ticket() {
return DependencyTicket(internal::kAccuracyTicket);
}
/** Returns a ticket indicating that a computation depends on configuration
state variables q. There is no ticket representing just one of the state
variables qᵢ. */
static DependencyTicket q_ticket() {
return DependencyTicket(internal::kQTicket);
}
/** Returns a ticket indicating dependence on velocity state variables v. This
does _not_ also indicate a dependence on configuration variables q -- you must
list that explicitly or use kinematics_ticket() instead. There is no ticket
representing just one of the state variables vᵢ. */
static DependencyTicket v_ticket() {
return DependencyTicket(internal::kVTicket);
}
/** Returns a ticket indicating dependence on any or all of the miscellaneous
continuous state variables z. There is no ticket representing just one of
the state variables zᵢ. */
static DependencyTicket z_ticket() {
return DependencyTicket(internal::kZTicket);
}
/** Returns a ticket indicating dependence on _all_ of the continuous
state variables q, v, or z. */
static DependencyTicket xc_ticket() {
return DependencyTicket(internal::kXcTicket);
}
/** Returns a ticket indicating dependence on a particular discrete state
variable xdᵢ (may be a vector). (We sometimes refer to this as a "discrete
variable group".)
@see xd_ticket() to obtain a ticket for _all_ discrete variables. */
DependencyTicket discrete_state_ticket(DiscreteStateIndex index) const {
return discrete_state_tracker_info(index).ticket;
}
/** Returns a ticket indicating dependence on all of the numerical
discrete state variables, in any discrete variable group.
@see discrete_state_ticket() to obtain a ticket for just one discrete
state variable. */
static DependencyTicket xd_ticket() {
return DependencyTicket(internal::kXdTicket);
}
/** Returns a ticket indicating dependence on a particular abstract state
variable xaᵢ.
@see xa_ticket() to obtain a ticket for _all_ abstract variables. */
DependencyTicket abstract_state_ticket(AbstractStateIndex index) const {
return abstract_state_tracker_info(index).ticket;
}
/** Returns a ticket indicating dependence on all of the abstract
state variables in the current Context.
@see abstract_state_ticket() to obtain a ticket for just one abstract
state variable. */
static DependencyTicket xa_ticket() {
return DependencyTicket(internal::kXaTicket);
}
/** Returns a ticket indicating dependence on _all_ state variables x in this
system, including continuous variables xc, discrete (numeric) variables xd,
and abstract state variables xa. This does not imply dependence on time,
accuracy, parameters, or inputs; those must be specified separately. If you
mean to express dependence on all possible value sources, use
all_sources_ticket() instead. */
static DependencyTicket all_state_ticket() {
return DependencyTicket(internal::kXTicket);
}
/** Returns a ticket indicating dependence on a particular numeric parameter
pnᵢ (may be a vector).
@see pn_ticket() to obtain a ticket for _all_ numeric parameters. */
DependencyTicket numeric_parameter_ticket(NumericParameterIndex index) const {
return numeric_parameter_tracker_info(index).ticket;
}
/** Returns a ticket indicating dependence on all of the numerical
parameters in the current Context.
@see numeric_parameter_ticket() to obtain a ticket for just one numeric
parameter. */
static DependencyTicket pn_ticket() {
return DependencyTicket(internal::kPnTicket);
}
/** Returns a ticket indicating dependence on a particular abstract
parameter paᵢ.
@see pa_ticket() to obtain a ticket for _all_ abstract parameters. */
DependencyTicket abstract_parameter_ticket(
AbstractParameterIndex index) const {
return abstract_parameter_tracker_info(index).ticket;
}
/** Returns a ticket indicating dependence on all of the abstract
parameters pa in the current Context.
@see abstract_parameter_ticket() to obtain a ticket for just one abstract
parameter. */
static DependencyTicket pa_ticket() {
return DependencyTicket(internal::kPaTicket);
}
/** Returns a ticket indicating dependence on _all_ parameters p in this
system, including numeric parameters pn, and abstract parameters pa. */
static DependencyTicket all_parameters_ticket() {
return DependencyTicket(internal::kAllParametersTicket);
}
/** Returns a ticket indicating dependence on input port uᵢ indicated
by `index`.
@pre `index` selects an existing input port of this System. */
DependencyTicket input_port_ticket(InputPortIndex index) const {
DRAKE_DEMAND(0 <= index && index < num_input_ports());
return input_ports_[index]->ticket();
}
/** Returns a ticket indicating dependence on _all_ input ports u of this
system.
@see input_port_ticket() to obtain a ticket for just one input port. */
static DependencyTicket all_input_ports_ticket() {
return DependencyTicket(internal::kAllInputPortsTicket);
}
/** Returns a ticket indicating dependence on every possible independent
source value _except_ input ports. This can be helpful in avoiding the
incorrect appearance of algebraic loops in a Diagram (those always involve
apparent input port dependencies). For an output port, use this ticket plus
tickets for just the input ports on which the output computation _actually_
depends. The sources included in this ticket are: time, accuracy, state,
and parameters. Note that dependencies on cache entries are _not_ included
here. Usually that won't matter since cache entries typically depend on at
least one of time, accuracy, state, or parameters so will be invalidated for
the same reason the current computation is. However, for a computation that
depends on a cache entry that depends only on input ports, be sure that
you have included those input ports in the dependency list, or include a
direct dependency on the cache entry.
@see input_port_ticket() to obtain a ticket for an input port.
@see cache_entry_ticket() to obtain a ticket for a cache entry.
@see all_sources_ticket() to also include all input ports as dependencies. */
static DependencyTicket all_sources_except_input_ports_ticket() {
return DependencyTicket(internal::kAllSourcesExceptInputPortsTicket);
}
/** Returns a ticket indicating dependence on every possible independent
source value, including time, accuracy, state, input ports, and parameters
(but not cache entries). This is the default dependency for computations that
have not specified anything more refined. It is equivalent to the set
`{all_sources_except_input_ports_ticket(), all_input_ports_ticket()}`.
@see cache_entry_ticket() to obtain a ticket for a cache entry. */
static DependencyTicket all_sources_ticket() {
return DependencyTicket(internal::kAllSourcesTicket);
}
/** Returns a ticket indicating dependence on the cache entry indicated
by `index`. Note that cache entries are _not_ included in the `all_sources`
ticket so must be listed separately.
@pre `index` selects an existing cache entry in this System. */
DependencyTicket cache_entry_ticket(CacheIndex index) const {
DRAKE_DEMAND(0 <= index && index < num_cache_entries());
return cache_entries_[index]->ticket();
}
/** Returns a ticket indicating dependence on all source values that may
affect configuration-dependent computations. In particular, this category
_does not_ include time, generalized velocities v, miscellaneous continuous
state variables z, or input ports. Generalized coordinates q are included, as
well as any discrete state variables that have been declared as configuration
variables, and configuration-affecting parameters. Finally we assume that
the accuracy setting may affect some configuration-dependent computations.
Examples: a parameter that affects length may change the computation of an
end-effector location. A change in accuracy requirement may require
recomputation of an iterative approximation of contact forces.
@see kinematics_ticket()
@note Currently there is no way to declare specific variables and parameters
to be configuration-affecting so we include all state variables and
parameters except for state variables v and z. */
// TODO(sherm1) Remove the above note once #9171 is resolved.
// The configuration_tracker implementation in ContextBase must be kept
// up to date with the above API contract.
static DependencyTicket configuration_ticket() {
return DependencyTicket(internal::kConfigurationTicket);
}
/** Returns a ticket indicating dependence on all source values that may
affect configuration- or velocity-dependent computations. This ticket depends
on the configuration_ticket defined above, and adds in velocity-affecting
source values. This _does not_ include time or input ports.
@see configuration_ticket()
@note Currently there is no way to declare specific variables and parameters
to be configuration- or velocity-affecting so we include all state variables
and parameters except for state variables z. */
// TODO(sherm1) Remove the above note once #9171 is resolved.
static DependencyTicket kinematics_ticket() {
return DependencyTicket(internal::kKinematicsTicket);
}
/** Returns a ticket for the cache entry that holds time derivatives of
the continuous variables.
@see EvalTimeDerivatives() */
static DependencyTicket xcdot_ticket() {
return DependencyTicket(internal::kXcdotTicket);
}
/** Returns a ticket for the cache entry that holds the potential energy
calculation.
@see System::EvalPotentialEnergy() */
static DependencyTicket pe_ticket() {
return DependencyTicket(internal::kPeTicket);
}
/** Returns a ticket for the cache entry that holds the kinetic energy
calculation.
@see System::EvalKineticEnergy() */
static DependencyTicket ke_ticket() {
return DependencyTicket(internal::kKeTicket);
}
/** Returns a ticket for the cache entry that holds the conservative power
calculation.
@see System::EvalConservativePower() */
static DependencyTicket pc_ticket() {
return DependencyTicket(internal::kPcTicket);
}
/** Returns a ticket for the cache entry that holds the non-conservative
power calculation.
@see System::EvalNonConservativePower() */
static DependencyTicket pnc_ticket() {
return DependencyTicket(internal::kPncTicket);
}
/** (Internal use only) Returns a ticket indicating dependence on the output
port indicated by `index`. No user-definable quantities in a system can
meaningfully depend on that system's own output ports.
@pre `index` selects an existing output port of this System. */
DependencyTicket output_port_ticket(OutputPortIndex index) const {
DRAKE_DEMAND(0 <= index && index < num_output_ports());
return output_ports_[index]->ticket();
}
//@}
/** Returns the number of declared continuous state variables. */
int num_continuous_states() const {
return context_sizes_.num_generalized_positions +
context_sizes_.num_generalized_velocities +
context_sizes_.num_misc_continuous_states;
}
/** Returns the number of declared discrete state groups (each group is
a vector-valued discrete state variable). */
int num_discrete_state_groups() const {
return context_sizes_.num_discrete_state_groups;
}
/** Returns the number of declared abstract state variables. */
int num_abstract_states() const {
return context_sizes_.num_abstract_states;
}
/** Returns the number of declared numeric parameters (each of these is
a vector-valued parameter). */
int num_numeric_parameter_groups() const {
return context_sizes_.num_numeric_parameter_groups;
}
/** Returns the number of declared abstract parameters. */
int num_abstract_parameters() const {
return context_sizes_.num_abstract_parameters;
}
/** Returns the size of the implicit time derivatives residual vector.
By default this is the same as num_continuous_states() but a LeafSystem
can change it during construction via
LeafSystem::DeclareImplicitTimeDerivativesResidualSize(). */
int implicit_time_derivatives_residual_size() const {
return implicit_time_derivatives_residual_size_.has_value()
? *implicit_time_derivatives_residual_size_
: num_continuous_states();
}
// Note that it is extremely unlikely that a Context will have an invalid
// system id because it is near impossible for a user to create one. We'll
// just assume it's valid as a precondition on the ValidateContext() methods.
// In Debug builds this will be reported as an error but otherwise a
// readable but imperfect "Not created for this System" message will issue
// if there is no id.
/** Checks whether the given context was created for this system.
@note This method is sufficiently fast for performance sensitive code.
@throws std::exception if the System Ids don't match.
@pre both `context` and `this` have valid System Ids. */
void ValidateContext(const ContextBase& context) const final {
if (context.get_system_id() != system_id_) {
ThrowValidateContextMismatch(context);
}
}
/** Checks whether the given context was created for this system.
@note This method is sufficiently fast for performance sensitive code.
@throws std::exception if the System Ids don't match.
@throws std::exception if `context` is null.
@pre if `context` is non-null, then both `context` and `this` have valid
System Ids. */
void ValidateContext(const ContextBase* context) const {
DRAKE_THROW_UNLESS(context != nullptr);
ValidateContext(*context);
}
// In contrast to Contexts it is easier to create some System-related objects
// without assigning them a valid system id. So for checking arbitrary object
// types we'll permit the object to have an invalid system id. (We still
// require that this SystemBase has a valid system id.) If the object doesn't
// have a valid system id, we produce an accurate "Not created for this
// System" message rather than one that says something obscure about invalid
// Identifiers. We don't need to distinguish between "no system id" and "wrong
// system id" mismatches; the end result is the same and we can keep this
// method tinier (encouraging inlining) if we don't make a separate test.
/** Checks whether the given object was created for this system.
@note This method is sufficiently fast for performance sensitive code.
@throws std::exception if the System Ids don't match or if `object` doesn't
have an associated System Id.
@throws std::exception if the argument type is a pointer and it is null.
@pre `this` System has a valid system id. */
template <class Clazz>
void ValidateCreatedForThisSystem(const Clazz& object) const {
const internal::SystemId id = [&]() {
if constexpr (std::is_pointer_v<Clazz>) {
DRAKE_THROW_UNLESS(object != nullptr);
return object->get_system_id();
} else {
return object.get_system_id();
}
}();
if (!id.is_same_as_valid_id(system_id_))
ThrowNotCreatedForThisSystem(object);
}
protected:
/** (Internal use only). */
SystemBase() = default;
/** (Internal use only) Adds an already-constructed input port to this System.
Insists that the port already contains a reference to this System, and that
the port's index is already set to the next available input port index for
this System, that the port name is unique (just within this System), and that
the port name is non-empty. */
// TODO(sherm1) Add check on suitability of `size` parameter for the port's
// data type.
void AddInputPort(std::unique_ptr<InputPortBase> port) {
DRAKE_DEMAND(port != nullptr);
DRAKE_DEMAND(&port->get_system_interface() == this);
DRAKE_DEMAND(port->get_index() == num_input_ports());
DRAKE_DEMAND(!port->get_name().empty());
// Check that name is unique.
for (InputPortIndex i{0}; i < num_input_ports(); i++) {
if (port->get_name() == get_input_port_base(i).get_name()) {
throw std::logic_error("System " + GetSystemName() +
" already has an input port named " +
port->get_name());
}
}
input_ports_.push_back(std::move(port));
}
/** (Internal use only) Adds an already-constructed output port to this
System. Insists that the port already contains a reference to this System, and
that the port's index is already set to the next available output port index
for this System, and that the name of the port is unique.
@throws std::exception if the name of the output port is not unique. */
// TODO(sherm1) Add check on suitability of `size` parameter for the port's
// data type.
void AddOutputPort(std::unique_ptr<OutputPortBase> port) {
DRAKE_DEMAND(port != nullptr);
DRAKE_DEMAND(&port->get_system_interface() == this);
DRAKE_DEMAND(port->get_index() == num_output_ports());
DRAKE_DEMAND(!port->get_name().empty());
// Check that name is unique.
for (OutputPortIndex i{0}; i < num_output_ports(); i++) {
if (port->get_name() == get_output_port_base(i).get_name()) {
throw std::logic_error("System " + GetSystemName() +
" already has an output port named " +
port->get_name());
}
}
output_ports_.push_back(std::move(port));
}
/** (Internal use only) Returns a name for the next input port, using the
given name if it isn't kUseDefaultName, otherwise making up a name like "u3"
from the next available input port index.
@pre `given_name` must not be empty. */
std::string NextInputPortName(
std::variant<std::string, UseDefaultName> given_name) const {
const std::string result =
given_name == kUseDefaultName
? std::string("u") + std::to_string(num_input_ports())
: std::get<std::string>(std::move(given_name));
DRAKE_DEMAND(!result.empty());
return result;
}
/** (Internal use only) Returns a name for the next output port, using the
given name if it isn't kUseDefaultName, otherwise making up a name like "y3"
from the next available output port index.
@pre `given_name` must not be empty. */
std::string NextOutputPortName(
std::variant<std::string, UseDefaultName> given_name) const {
const std::string result =
given_name == kUseDefaultName
? std::string("y") + std::to_string(num_output_ports())
: std::get<std::string>(std::move(given_name));
DRAKE_DEMAND(!result.empty());
return result;
}
/** (Internal use only) Assigns a ticket to a new discrete variable group
with the given `index`.
@pre The supplied index must be the next available one; that is, indexes
must be assigned sequentially. */
void AddDiscreteStateGroup(DiscreteStateIndex index) {
DRAKE_DEMAND(index == discrete_state_tickets_.size());
DRAKE_DEMAND(index == context_sizes_.num_discrete_state_groups);
const DependencyTicket ticket(assign_next_dependency_ticket());
discrete_state_tickets_.push_back(
{ticket, "discrete state group " + std::to_string(index)});
++context_sizes_.num_discrete_state_groups;
}
/** (Internal use only) Assigns a ticket to a new abstract state variable with
the given `index`.
@pre The supplied index must be the next available one; that is, indexes
must be assigned sequentially. */
void AddAbstractState(AbstractStateIndex index) {
const DependencyTicket ticket(assign_next_dependency_ticket());
DRAKE_DEMAND(index == abstract_state_tickets_.size());
DRAKE_DEMAND(index == context_sizes_.num_abstract_states);
abstract_state_tickets_.push_back(
{ticket, "abstract state " + std::to_string(index)});
++context_sizes_.num_abstract_states;
}
/** (Internal use only) Assigns a ticket to a new numeric parameter with
the given `index`.
@pre The supplied index must be the next available one; that is, indexes
must be assigned sequentially. */
void AddNumericParameter(NumericParameterIndex index) {
DRAKE_DEMAND(index == numeric_parameter_tickets_.size());
DRAKE_DEMAND(index == context_sizes_.num_numeric_parameter_groups);
const DependencyTicket ticket(assign_next_dependency_ticket());
numeric_parameter_tickets_.push_back(
{ticket, "numeric parameter " + std::to_string(index)});
++context_sizes_.num_numeric_parameter_groups;
}
/** (Internal use only) Assigns a ticket to a new abstract parameter with
the given `index`.