-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
simulator.h
934 lines (821 loc) · 42.5 KB
/
simulator.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
#pragma once
#include <algorithm>
#include <chrono>
#include <limits>
#include <memory>
#include <optional>
#include <unordered_map>
#include <utility>
#include <vector>
#include "drake/common/default_scalars.h"
#include "drake/common/drake_assert.h"
#include "drake/common/drake_copyable.h"
#include "drake/common/extract_double.h"
#include "drake/common/name_value.h"
#include "drake/systems/analysis/integrator_base.h"
#include "drake/systems/analysis/simulator_config.h"
#include "drake/systems/analysis/simulator_status.h"
#include "drake/systems/framework/context.h"
#include "drake/systems/framework/system.h"
#include "drake/systems/framework/witness_function.h"
namespace drake {
namespace systems {
/// @ingroup simulation
/// Parameters for fine control of simulator initialization.
/// @see Simulator<T>::Initialize().
struct InitializeParams {
/** Passes this object to an Archive.
Refer to @ref yaml_serialization "YAML Serialization" for background. */
template <typename Archive>
void Serialize(Archive* a) {
a->Visit(DRAKE_NVP(suppress_initialization_events));
}
/// Whether to trigger initialization events. Events are triggered by
/// default; it may be useful to suppress them when reusing a simulator.
bool suppress_initialization_events{false};
};
/** @ingroup simulation
A class for advancing the state of hybrid dynamic systems, represented by
`System<T>` objects, forward in time. Starting with an initial Context for a
given System, %Simulator advances time and produces a series of Context
values that forms a trajectory satisfying the system's dynamic equations to
a specified accuracy. Only the Context is modified by a %Simulator; the
System is const.
A Drake System is a continuous/discrete/hybrid dynamic system where the
continuous part is a DAE, that is, it is expected to consist of a set of
differential equations and bilateral algebraic constraints. The set of
active constraints may change as a result of particular events, such as
contact.
Given a current Context, we expect a System to provide us with
- derivatives for the continuous differential equations that already satisfy
the differentiated form of the constraints (typically, acceleration
constraints),
- a projection method for least-squares correction of violated higher-level
constraints (position and velocity level),
- a time-of-next-update method that can be used to adjust the integrator
step size in preparation for a discrete update,
- methods that can update discrete variables when their update time is
reached,
- witness (guard) functions for event isolation,
- event handlers (reset functions) for making appropriate changes to state
and mode variables when an event has been isolated.
The continuous parts of the trajectory are advanced using a numerical
integrator. Different integrators have different properties; you can choose
the one that is most appropriate for your application or use the default
which is adequate for most systems.
<h3>How the simulation is stepped: simulation mechanics for authors of discrete
and hybrid systems</h3>
This section is targeted toward users who have created a LeafSystem implementing
a discrete or hybrid system. For authors of such systems, it can be useful to
understand the simulation details in order to attain the desired state behavior
over time. This behavior is dependent on the ordering in which discrete events
and continuous updates are processed. (By "discrete events" we mean to include
any of Drake's event handlers.) The basic issues and terminology are
introduced in the @ref discrete_systems module; please look there first before
proceeding.
As pictured in @ref discrete_systems, when a continuous-time system has
discrete events, the state x can have two significant values at the event
time t. These are
- x⁻(t), the value of x _before_ the discrete update occurs (○ markers), and
- x⁺(t), the value of x _after_ the discrete update occurs (● markers).
Thus the value of the Context, which contains both time and state, advances
from {t, x⁻(t)} to {t, x⁺(t)} as a result of the update. While those Context
values are user-visible, the details of stepping here require an intermediate
value which we'll denote {t, x*(t)}.
Recall that Drake's state x is partitioned into continuous, discrete, and
abstract partitions xc, xd, and xa, so `x = { xc, xd, xa }`. Within a single
step, these are updated in three stages:
-# Unrestricted update (can change x)
-# Discrete update (can change only xd)
-# Continuous update (changes t and xc)
Where needed, we extend the above notation to xc⁻, xa⁺, etc. to indicate the
value of an individual partition at a particular stage of the stepping
algorithm.
The following pseudocode uses the above notation to describe the algorithm
"Step()" that the %Simulator uses to incrementally advance the system
trajectory (time t and state x). The Simulator's AdvanceTo() method will be
defined in terms of Step below. In general, the length of a step is not known a
priori and is determined by the Step() algorithm. Each step consists of zero or
more unrestricted updates, followed by zero or more discrete updates, followed
by (possibly zero-length) continuous time and state advancement, followed by
zero or more publishes, and then a call to the monitor() function if one has
been defined. Updates, publishes, and the monitor can report errors or detect a
termination condition; that is not shown in the pseudocode below.
The pseudocode will clarify the effects on time and state of each of the update
stages above. This algorithm is given a starting Context value `{tₛ, x⁻(tₛ)}`
and returns an end Context value `{tₑ, x⁻(tₑ)}`, where tₑ is _no later_ than a
given tₘₐₓ.
```
// Advance the trajectory (time and state) from start value {tₛ, x⁻(tₛ)} to an
// end value {tₑ, x⁻(tₑ)}, where tₛ ≤ tₑ ≤ tₘₐₓ.
procedure Step(tₛ, x⁻(tₛ), tₘₐₓ)
// Update any variables (no restrictions).
x*(tₛ) ← DoAnyUnrestrictedUpdates(tₛ, x⁻(tₛ))
// ----------------------------------
// Time and state are at {tₛ, x*(tₛ)}
// ----------------------------------
// Update discrete variables.
xd⁺(tₛ) ← DoAnyDiscreteUpdates(tₛ, x*(tₛ))
xc⁺(tₛ) ← xc*(tₛ) // These values carry over from x*(tₛ).
xa⁺(tₛ) ← xa*(tₛ)
// ----------------------------------
// Time and state are at {tₛ, x⁺(tₛ)}
// ----------------------------------
// See how far it is safe to integrate without missing any events.
tₑᵥₑₙₜ ← CalcNextEventTime(tₛ, x⁺(tₛ))
// Integrate continuous variables forward in time. Integration may terminate
// before reaching tₛₜₒₚ due to witnessed events.
tₛₜₒₚ ← min(tₑᵥₑₙₜ, tₘₐₓ)
tₑ, xc⁻(tₑ) ← Integrate(tₛ, x⁺(tₛ), tₛₜₒₚ)
xd⁻(tₑ) ← xd⁺(tₛ) // Discrete values are held from x⁺(tₛ).
xa⁻(tₑ) ← xa⁺(tₛ)
// ----------------------------------
// Time and state are at {tₑ, x⁻(tₑ)}
// ----------------------------------
DoAnyPublishes(tₑ, x⁻(tₑ))
CallMonitor(tₑ, x⁻(tₑ))
return {tₑ, x⁻(tₑ)}
```
We can use the notation and pseudocode to flesh out the AdvanceTo(),
AdvancePendingEvents(), and Initialize() functions. Termination and error
conditions detected by event handlers or the monitor are reported as status
returns from these methods.
```
// Advance the simulation until time tₘₐₓ.
procedure AdvanceTo(tₘₐₓ) → status
t ← current_time
while t < tₘₐₓ
{tₑ, x⁻(tₑ)} ← Step(t, x⁻(t), tₘₐₓ)
{t, x⁻(t)} ← {tₑ, x⁻(tₑ)}
endwhile
// AdvancePendingEvents() is an advanced method, not commonly used.
// Perform just the start-of-step update to advance from x⁻(t) to x⁺(t).
procedure AdvancePendingEvents() → status
t ≜ current_time, x⁻(t) ≜ current_state
x⁺(t) ← DoAnyPendingUpdates(t, x⁻(t)) as in Step()
x(t) ← x⁺(t) // No continuous update needed.
DoAnyPublishes(t, x(t))
CallMonitor(t, x(t))
// Update time and state to {t₀, x⁻(t₀)}, which is the starting value of the
// trajectory, and thus the value the Context should contain at the start of the
// first simulation step.
procedure Initialize(t₀, x₀) → status
// Initialization events can be optionally suppressed.
x⁺(t₀) ← DoAnyInitializationUpdates as in Step()
x⁻(t₀) ← x⁺(t₀) // No continuous update needed.
// ----------------------------------
// Time and state are at {t₀, x⁻(t₀)}
// ----------------------------------
DoAnyPublishes(t₀, x⁻(t₀))
CallMonitor(t₀, x⁻(t₀))
```
Initialize() can be viewed as a "0ᵗʰ step" that occurs before the first Step()
call as described above. Like Step(), Initialize() first performs pending
updates (in this case only initialization events can be "pending", and even
those may be optionally suppressed). Time doesn't advance so there is no
continuous update phase and witnesses cannot trigger. Finally, again like
Step(), the initial trajectory point `{t₀, x⁻(t₀)}` is provided to the handlers
for any triggered publish events. That includes initialization publish events
(if not suppressed), per-step publish events, and periodic or timed publish
events that trigger at t₀, followed by a call to the monitor() function if one
has been defined (a monitor is semantically identical to a per-step publish).
Optionally, initialization events can be suppressed. This can be useful when
reusing the simulator over the same system and time span.
@tparam_nonsymbolic_scalar
*/
template <typename T>
class Simulator {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Simulator)
/// Create a %Simulator that can advance a given System through time to
/// produce a trajectory consisting of a sequence of Context values. The
/// System must not have unresolved input ports if the values of those ports
/// are necessary for computations performed during simulation (see class
/// documentation).
///
/// The Simulator holds an internal, non-owned reference to the System
/// object so you must ensure that `system` has a longer lifetime than the
/// %Simulator. It also owns a compatible Context internally that takes on
/// each of the trajectory values. You may optionally provide a Context that
/// will be used as the initial condition for the simulation; otherwise the
/// %Simulator will obtain a default Context from `system`.
explicit Simulator(const System<T>& system,
std::unique_ptr<Context<T>> context = nullptr);
/// Create a %Simulator which additionally maintains ownership of the System.
///
/// @exclude_from_pydrake_mkdoc{The prior overload's docstring is better, and
/// we only need one of the two -- overloading on ownership doesn't make
/// sense for pydrake.}
Simulator(std::unique_ptr<const System<T>> system,
std::unique_ptr<Context<T>> context = nullptr);
// TODO(sherm1) Make Initialize() attempt to satisfy constraints.
/// Prepares the %Simulator for a simulation. In order, the sequence of
/// actions taken here are:
/// - The active integrator's Initialize() method is invoked.
/// - Statistics are reset.
/// - By default, initialization update events are triggered and handled to
/// produce the initial trajectory value `{t₀, x(t₀)}`. If initialization
/// events are suppressed, it is the caller's responsibility to ensure the
/// desired initial state.
/// - Then that initial value is provided to the handlers for any publish
/// events that have triggered, including initialization events if any, and
/// per-step publish events, periodic or other time-triggered publish
/// events that are scheduled for the initial time t₀, and finally a call
/// to the monitor() function if one has been defined.
///
/// See the class documentation for more information. We recommend calling
/// Initialize() explicitly prior to beginning a simulation so that error
/// conditions will be discovered early. However, Initialize() will be called
/// automatically by the first AdvanceTo() call if it hasn't already been
/// called.
///
/// @note If you make a change to the Context or to Simulator options between
/// AdvanceTo() calls you should consider whether to call Initialize() before
/// resuming; AdvanceTo() will not do that automatically for you. Whether to
/// do so depends on whether you want the above initialization operations
/// performed.
///
/// @note In particular, if you changed the time you must call Initialize().
/// The time-triggered events must be recalculated in case one is due at the
/// new starting time. The AdvanceTo() call will throw an exception if the
/// Initialize() call is missing.
///
/// @note The only way to suppress initialization events is by calling
/// Initialize() explicitly. The most common scenario for this is when
/// reusing a Simulator object. In this case, the caller is responsible for
/// ensuring the correctness of the initial state.
///
/// @warning Initialize() does not automatically attempt to satisfy System
/// constraints -- it is up to you to make sure that constraints are
/// satisfied by the initial conditions.
///
/// This method will throw `std::exception` if the combination of options
/// doesn't make sense. Other failures are possible from the System and
/// integrator in use.
///
/// @param params (optional) a parameter structure (@see InitializeParams).
///
/// @retval status A SimulatorStatus object indicating success, termination,
/// or an error condition as reported by event handlers or
/// the monitor function.
/// @see AdvanceTo(), AdvancePendingEvents(), SimulatorStatus
SimulatorStatus Initialize(const InitializeParams& params = {});
/// Advances the System's trajectory until `boundary_time` is reached in
/// the context or some other termination condition occurs. A variety of
/// `std::runtime_error` conditions are possible here, as well as error
/// conditions that may be thrown by the System when it is asked to perform
/// computations. Be sure to enclose your simulation in a `try-catch` block
/// and display the `what()` message.
///
/// We recommend that you call Initialize() prior to making the first call
/// to AdvanceTo(). However, if you don't it will be called for you the first
/// time that you attempt a step, possibly resulting in unexpected error
/// conditions. See documentation for `Initialize()` for the error conditions
/// it might produce.
///
/// @warning You should consider calling Initialize() if you alter the
/// the Context or Simulator options between successive AdvanceTo() calls. See
/// Initialize() for more information.
///
/// @note You can track simulation progress to terminate on arbitrary
/// conditions using a _monitor_ function; see set_monitor().
///
/// @param boundary_time The maximum time to which the trajectory will be
/// advanced by this call to %AdvanceTo(). The method may return earlier
/// if an event or the monitor function requests termination or reports
/// an error condition.
/// @retval status A SimulatorStatus object indicating success, termination,
/// or an error condition as reported by event handlers or the monitor
/// function. The time in the context will be set either to the
/// boundary_time or the time a termination or error was first detected.
///
/// @pre The internal Context satisfies all System constraints or will after
/// pending Context updates are performed.
/// @see Initialize(), AdvancePendingEvents(), SimulatorStatus, set_monitor()
SimulatorStatus AdvanceTo(const T& boundary_time);
/// (Advanced) Handles discrete and abstract state update events that are
/// pending from the previous AdvanceTo() call, without advancing time.
/// See the %Simulator class description for details about how %Simulator
/// advances time and handles events. In the terminology used there, this
/// method advances the internal Context from `{t, x⁻(t)}` to `{t, x⁺(t)}`.
///
/// Normally, these update events would be handled at the start of the next
/// AdvanceTo() call, so this method is rarely needed. It can be useful
/// at the end of a simulation or to get intermediate results when you are
/// specifically interested in the `x⁺(t)` result.
///
/// This method is equivalent to `AdvanceTo(current_time)`, where
/// `current_time=simulator.get_context().get_time())`. If there are no
/// pending events, nothing happens except possibly a final per-step publish
/// call (if enabled) followed by a call to the monitor() function (if one
/// has been provided).
///
/// @retval status A SimulatorStatus object indicating success, termination,
/// or an error condition as reported by event handlers or
/// the monitor function.
/// @see AdvanceTo(), Initialize(), SimulatorStatus
SimulatorStatus AdvancePendingEvents() {
return AdvanceTo(get_context().get_time());
}
/// Provides a monitoring function that will be invoked at the end of
/// every step. (See the Simulator class documentation for a precise
/// definition of "step".) A monitor() function can be used to capture the
/// trajectory, to terminate the simulation, or to detect error conditions.
/// The monitor() function is invoked by the %Simulator with a Context whose
/// value is a point along the simulated trajectory. The monitor can be any
/// functor and should capture any System references it needs to operate
/// correctly.
///
/// A monitor() function behaves the same as would a per-step Publish event
/// handler included in the top-level System or Diagram being simulated. As in
/// the case of Publish(), the monitor is called at the end of every step
/// taken internally by AdvanceTo(), and also at the end of Initialize() and
/// AdvancePendingEvents(). (See the Simulator class documentation for more
/// detail about what happens when in these methods.) The monitor receives the
/// top-level (root) Context, from which any sub-Context can be obtained using
/// `subsystem.GetMyContextFromRoot()`, provided the necessary subsystem
/// reference has been captured for use in the monitor.
///
/// #### Examples
/// Output time and continuous states whenever the trajectory is advanced:
/// @code
/// simulator.set_monitor([](const Context<T>& root_context) {
/// std::cout << root_context.get_time() << " "
/// << root_context.get_continuous_state_vector()
/// << std::endl;
/// return EventStatus::Succeeded();
/// });
/// @endcode
///
/// Terminate early but successfully on a condition in a subsystem of the
/// System diagram being simulated:
/// @code
/// simulator.set_monitor([&my_subsystem](const Context<T>& root_context) {
/// const Context<T>& subcontext =
/// my_subsystem.GetMyContextFromRoot(root_context);
/// if (my_subsystem.GoalReached(subcontext)) {
/// return EventStatus::ReachedTermination(my_subsystem,
/// "Simulation achieved the desired goal.");
/// }
/// return EventStatus::Succeeded();
/// });
/// @endcode
/// In the above case, the Simulator's AdvanceTo() method will return early
/// when the subsystem reports that it has reached its goal. The returned
/// status will indicate the termination reason, and a human-readable
/// termination message containing the message provided by the monitor can be
/// obtained with status.FormatMessage().
///
/// Failure due to plant center of mass falling below a threshold:
/// @code
/// simulator.set_monitor([&plant](const Context<T>& root_context) {
/// const Context<T>& plant_context =
/// plant.GetMyContextFromRoot(root_context);
/// const Vector3<T> com =
/// plant.CalcCenterOfMassPositionInWorld(plant_context);
/// if (com[2] < 0.1) { // Check z height of com.
/// return EventStatus::Failed(plant, "System fell over.");
/// }
/// return EventStatus::Succeeded();
/// });
/// @endcode
/// In the above case the Simulator's AdvanceTo() method will throw an
/// std::runtime_error containing a human-readable message including
/// the text provided in the monitor.
///
/// @note monitor() is called every time the trajectory is advanced by a step,
/// which can mean it is called many times during a single AdvanceTo() call.
///
/// @note The presence of a monitor has no effect on the step sizes taken,
/// so a termination or error condition will be discovered only when first
/// observed after a step is complete; it will not be further localized. Use
/// witness-triggered events instead if you need precise isolation.
void set_monitor(std::function<EventStatus(const Context<T>&)> monitor) {
monitor_ = std::move(monitor);
}
/// Removes the monitoring function if there is one.
/// @see set_monitor()
void clear_monitor() { monitor_ = nullptr; }
/// Obtains a reference to the monitoring function, which may be empty.
/// @see set_monitor()
const std::function<EventStatus(const Context<T>&)>& get_monitor() const {
return monitor_;
}
// TODO(sherm1): Provide options for issuing a warning or aborting the
// simulation if the desired rate cannot be achieved.
/// Slow the simulation down to *approximately* synchronize with real time
/// when it would otherwise run too fast. Normally the %Simulator takes steps
/// as quickly as it can. You can request that it slow down to synchronize
/// with real time by providing a realtime rate greater than zero here.
///
/// @warning No guarantees can be made about how accurately the simulation
/// can be made to track real time, even if computation is fast enough. That's
/// because the system utilities used to implement this do not themselves
/// provide such guarantees. So this is likely to work nicely for
/// visualization purposes where human perception is the only concern. For any
/// other uses you should consider whether approximate real time is adequate
/// for your purposes.
///
/// @note If the full-speed simulation is already slower than real time you
/// can't speed it up with this call! Instead consider requesting less
/// integration accuracy, using a faster integration method or fixed time
/// step, or using a simpler model.
///
/// @param realtime_rate
/// Desired rate relative to real time. Set to 1 to track real time, 2 to
/// run twice as fast as real time, 0.5 for half speed, etc. Zero or
/// negative restores the rate to its default of 0, meaning the simulation
/// will proceed as fast as possible.
void set_target_realtime_rate(double realtime_rate) {
target_realtime_rate_ = std::max(realtime_rate, 0.);
}
/// Return the real time rate target currently in effect. The default is
/// zero, meaning the %Simulator runs as fast as possible. You can change the
/// target with set_target_realtime_rate().
double get_target_realtime_rate() const {
return target_realtime_rate_;
}
/// Return the rate that simulated time has progressed relative to real time.
/// A return of 1 means the simulation just matched real
/// time, 2 means the simulation was twice as fast as real time, 0.5 means
/// it was running in 2X slow motion, etc.
///
/// The value returned here is calculated as follows: <pre>
///
/// simulated_time_now - initial_simulated_time
/// rate = -------------------------------------------
/// realtime_now - initial_realtime
/// </pre>
/// The `initial` times are recorded when Initialize() or ResetStatistics()
/// is called. The returned rate is undefined if Initialize() has not yet
/// been called.
///
/// @returns The rate achieved since the last Initialize() or
/// ResetStatistics() call.
///
/// @see set_target_realtime_rate()
double get_actual_realtime_rate() const;
/// Sets whether the simulation should trigger a forced-Publish event on the
/// System under simulation at the end of every trajectory-advancing step.
/// Specifically, that means the System::Publish() event dispatcher will be
/// invoked on each subsystem of the System and passed the current Context
/// and a forced-publish Event. If a subsystem has declared a forced-publish
/// event handler, that will be called. Otherwise, nothing will happen unless
/// the DoPublish() dispatcher has been overridden.
///
/// Enabling this option does not cause a forced-publish to be triggered at
/// initialization; if you want that you should also call
/// `set_publish_at_initialization(true)`. If you want a forced-publish at the
/// end of every step, you will usually also want one at the end of
/// initialization, requiring both options to be enabled.
///
/// @see LeafSystem::DeclareForcedPublishEvent()
void set_publish_every_time_step(bool publish) {
publish_every_time_step_ = publish;
}
/// Sets whether the simulation should trigger a forced-Publish at the end
/// of Initialize(). See set_publish_every_time_step() documentation for
/// more information.
void set_publish_at_initialization(bool publish) {
publish_at_initialization_ = publish;
}
/// Returns true if the set_publish_every_time_step() option has been
/// enabled. By default, returns false.
bool get_publish_every_time_step() const { return publish_every_time_step_; }
/// Returns a const reference to the internally-maintained Context holding the
/// most recent step in the trajectory. This is suitable for publishing or
/// extracting information about this trajectory step. Do not call this method
/// if there is no Context.
const Context<T>& get_context() const {
DRAKE_ASSERT(context_ != nullptr);
return *context_;
}
/// Returns a mutable reference to the internally-maintained Context holding
/// the most recent step in the trajectory. This is suitable for use in
/// updates, sampling operations, event handlers, and constraint projection.
/// You can also modify this prior to calling Initialize() to set initial
/// conditions. Do not call this method if there is no Context.
Context<T>& get_mutable_context() {
DRAKE_ASSERT(context_ != nullptr);
return *context_;
}
/// Returns `true` if this Simulator has an internally-maintained Context.
/// This is always true unless `reset_context()` has been called.
bool has_context() const { return context_ != nullptr; }
/// Replace the internally-maintained Context with a different one. The
/// current Context is deleted. This is useful for supplying a new set of
/// initial conditions. You should invoke Initialize() after replacing the
/// Context.
/// @param context The new context, which may be null. If the context is
/// null, a new context must be set before attempting to step
/// the system forward.
void reset_context(std::unique_ptr<Context<T>> context) {
context_ = std::move(context);
integrator_->reset_context(context_.get());
initialization_done_ = false;
}
/// Transfer ownership of this %Simulator's internal Context to the caller.
/// The %Simulator will no longer contain a Context. The caller must not
/// attempt to advance the simulator in time after that point.
/// @sa reset_context()
std::unique_ptr<Context<T>> release_context() {
integrator_->reset_context(nullptr);
initialization_done_ = false;
return std::move(context_);
}
/// Forget accumulated statistics. Statistics are reset to the values they
/// have post construction or immediately after `Initialize()`.
void ResetStatistics();
/// Gets the number of publishes made since the last Initialize() or
/// ResetStatistics() call.
int64_t get_num_publishes() const { return num_publishes_; }
/// Gets the number of steps since the last Initialize() call. (We're
/// not counting the Initialize() 0-length "step".) Note that every
/// AdvanceTo() call can potentially take many steps.
int64_t get_num_steps_taken() const { return num_steps_taken_; }
/// Gets the number of discrete variable updates performed since the last
/// Initialize() call.
int64_t get_num_discrete_updates() const { return num_discrete_updates_; }
/// Gets the number of "unrestricted" updates performed since the last
/// Initialize() call.
int64_t get_num_unrestricted_updates() const {
return num_unrestricted_updates_; }
/// Gets a reference to the integrator used to advance the continuous aspects
/// of the system.
const IntegratorBase<T>& get_integrator() const { return *integrator_.get(); }
/// Gets a reference to the mutable integrator used to advance the continuous
/// state of the system.
IntegratorBase<T>& get_mutable_integrator() { return *integrator_.get(); }
/// Resets the integrator with a new one using factory construction.
/// @code
/// simulator.reset_integrator<RungeKutta3Integrator<double>>().
/// @endcode
/// Resetting the integrator resets the %Simulator such that it needs to be
/// initialized again -- see Initialize() for details.
/// @note Integrator needs a constructor of the form
/// Integrator(const System&, Context*); this
/// constructor is usually associated with error-controlled integrators.
template <class Integrator>
Integrator& reset_integrator() {
static_assert(
std::is_constructible_v<Integrator, const System<T>&, Context<T>*>,
"Integrator needs a constructor of the form "
"Integrator::Integrator(const System&, Context*); this "
"constructor is usually associated with error-controlled integrators.");
integrator_ =
std::make_unique<Integrator>(get_system(), &get_mutable_context());
initialization_done_ = false;
return *static_cast<Integrator*>(integrator_.get());
}
/// Resets the integrator with a new one using factory construction and a
/// maximum step size argument (which is required for constructing fixed-step
/// integrators).
/// @code
/// simulator.reset_integrator<RungeKutta2Integrator<double>>(0.1).
/// @endcode
/// @see argument-less version of reset_integrator() for note about
/// initialization.
/// @note Integrator needs a constructor of the form
/// Integrator(const System&, const T&, Context*); this
/// constructor is usually associated with fixed-step integrators (i.e.,
/// integrators which do not support error estimation).
template <class Integrator>
Integrator& reset_integrator(const T max_step_size) {
static_assert(
std::is_constructible_v<Integrator, const System<T>&, double,
Context<T>*>,
"Integrator needs a constructor of the form "
"Integrator::Integrator(const System&, const T&, Context*); this "
"constructor is usually associated with fixed-step integrators.");
integrator_ = std::make_unique<Integrator>(get_system(), max_step_size,
&get_mutable_context());
initialization_done_ = false;
return *static_cast<Integrator*>(integrator_.get());
}
/// Gets the length of the interval used for witness function time isolation.
/// The length of the interval is computed differently, depending on context,
/// to support multiple applications, as described below:
///
/// * **Simulations using error controlled integrators**: the isolation time
/// interval will be scaled by the product of the system's characteristic
/// time and the accuracy stored in the Context.
/// * **Simulations using integrators taking fixed steps**: the isolation time
/// interval will be determined differently depending on whether the
/// accuracy is set in the Context or not. If the accuracy *is* set in the
/// Context, the nominally fixed steps for integrating continuous state will
/// be subdivided until events have been isolated to the requisite interval
/// length, which is scaled by the step size times the accuracy in the
/// Context. If accuracy is not set in the Context, event isolation will
/// not be performed.
///
/// The isolation window length will never be smaller than the integrator's
/// working minimum tolerance (see
/// IntegratorBase::get_working_minimum_step_size());
///
/// @returns the isolation window if the Simulator should be isolating
/// witness-triggered events in time, or returns empty otherwise
/// (indicating that any witness-triggered events should trigger
/// at the end of a time interval over which continuous state is
/// integrated).
/// @throws std::exception if the accuracy is not set in the Context and
/// the integrator is not operating in fixed step mode (see
/// IntegratorBase::get_fixed_step_mode().
std::optional<T> GetCurrentWitnessTimeIsolation() const;
/// Gets a constant reference to the system.
/// @note a mutable reference is not available.
const System<T>& get_system() const { return system_; }
private:
enum TimeOrWitnessTriggered {
kNothingTriggered = 0b00,
kTimeTriggered = 0b01,
kWitnessTriggered = 0b10,
kBothTriggered = 0b11
};
// All constructors delegate to here.
Simulator(
const System<T>* system,
std::unique_ptr<const System<T>> owned_system,
std::unique_ptr<Context<T>> context);
void HandleUnrestrictedUpdate(
const EventCollection<UnrestrictedUpdateEvent<T>>& events);
void HandleDiscreteUpdate(
const EventCollection<DiscreteUpdateEvent<T>>& events);
void HandlePublish(const EventCollection<PublishEvent<T>>& events);
// Invoke the monitor() if there is one. If it wants termination we'll
// update the Simulator status accordingly. If it reports failure,
// currently we just throw.
// TODO(sherm1) Add an option where the Simulator returns failed status
// rather than throwing.
void CallMonitorUpdateStatusAndMaybeThrow(SimulatorStatus* status) {
DRAKE_DEMAND(status != nullptr);
if (!get_monitor()) return;
const EventStatus monitor_status = get_monitor()(*context_);
if (monitor_status.severity() == EventStatus::kReachedTermination) {
status->SetReachedTermination(ExtractDoubleOrThrow(context_->get_time()),
monitor_status.system(),
monitor_status.message());
return;
}
if (monitor_status.severity() == EventStatus::kFailed) {
status->SetEventHandlerFailed(ExtractDoubleOrThrow(context_->get_time()),
monitor_status.system(),
monitor_status.message());
throw std::runtime_error(status->FormatMessage());
}
// For any other condition, leave the status unchanged.
}
TimeOrWitnessTriggered IntegrateContinuousState(
const T& next_publish_time,
const T& next_update_time,
const T& boundary_time,
CompositeEventCollection<T>* witnessed_events);
// Private methods related to witness functions.
void IsolateWitnessTriggers(
const std::vector<const WitnessFunction<T>*>& witnesses,
const VectorX<T>& w0,
const T& t0, const VectorX<T>& x0, const T& tf,
std::vector<const WitnessFunction<T>*>* triggered_witnesses);
void PopulateEventDataForTriggeredWitness(
const T& t0, const T& tf, const WitnessFunction<T>* witness,
Event<T>* event, CompositeEventCollection<T>* events) const;
static bool DidWitnessTrigger(
const std::vector<const WitnessFunction<T>*>& witness_functions,
const VectorX<T>& w0,
const VectorX<T>& wf,
std::vector<const WitnessFunction<T>*>* triggered_witnesses);
VectorX<T> EvaluateWitnessFunctions(
const std::vector<const WitnessFunction<T>*>& witness_functions,
const Context<T>& context) const;
void RedetermineActiveWitnessFunctionsIfNecessary();
// The steady_clock is immune to system clock changes so increases
// monotonically. We'll work in fractional seconds.
using Clock = std::chrono::steady_clock;
using Duration = std::chrono::duration<double>;
using TimePoint = std::chrono::time_point<Clock, Duration>;
// If the simulated time in the context is ahead of real time, pause long
// enough to let real time catch up (approximately).
void PauseIfTooFast() const;
// A pointer to the integrator.
std::unique_ptr<IntegratorBase<T>> integrator_;
// TODO(sherm1) This a workaround for an apparent bug in clang 3.8 in which
// defining this as a static constexpr member kNaN failed to instantiate
// properly for the AutoDiffXd instantiation (worked in gcc and MSVC).
// Restore to sanity when some later clang is current.
static constexpr double nan() {
return std::numeric_limits<double>::quiet_NaN();
}
// Do not use this. This is valid iff the constructor is passed a
// unique_ptr (allowing the Simulator to maintain ownership). Use the
// system_ variable instead, which is valid always.
const std::unique_ptr<const System<T>> owned_system_;
const System<T>& system_; // Just a reference; not owned.
std::unique_ptr<Context<T>> context_; // The trajectory Context.
// Temporaries used for witness function isolation.
std::vector<const WitnessFunction<T>*> triggered_witnesses_;
VectorX<T> w0_, wf_;
// Slow down to this rate if possible (user settable).
double target_realtime_rate_{SimulatorConfig{}.target_realtime_rate};
bool publish_every_time_step_{SimulatorConfig{}.publish_every_time_step};
bool publish_at_initialization_{SimulatorConfig{}.publish_every_time_step};
// These are recorded at initialization or statistics reset.
double initial_simtime_{nan()}; // Simulated time at start of period.
TimePoint initial_realtime_; // Real time at start of period.
// The number of discrete updates since the last statistics reset.
int64_t num_discrete_updates_{0};
// The number of unrestricted updates since the last statistics reset.
int64_t num_unrestricted_updates_{0};
// The number of publishes since the last statistics reset.
int64_t num_publishes_{0};
// The number of integration steps since the last statistics reset.
int64_t num_steps_taken_{0};
// Set by Initialize() and reset by various traumas.
bool initialization_done_{false};
// Set by Initialize() and AdvanceTo(). Used to detect unexpected jumps in
// time.
double last_known_simtime_{nan()};
// The vector of active witness functions.
std::unique_ptr<std::vector<const WitnessFunction<T>*>> witness_functions_;
// Indicator for whether the Simulator needs to redetermine the active witness
// functions.
bool redetermine_active_witnesses_{true};
// Per step events that are to be handled on every "major time step" (i.e.,
// every successful completion of a step). This collection is constructed
// within Initialize().
std::unique_ptr<CompositeEventCollection<T>> per_step_events_;
// Timed events can be triggered either at a particular time (like an alarm)
// or periodically. This collection is constructed within Initialize().
std::unique_ptr<CompositeEventCollection<T>> timed_events_;
// Witnessed events are triggered as a witness function crosses zero during
// AdvanceTo(). This collection is constructed within Initialize().
std::unique_ptr<CompositeEventCollection<T>> witnessed_events_;
// All events merged together. This collection is constructed within
// Initialize().
std::unique_ptr<CompositeEventCollection<T>> merged_events_;
// Indicates when a timed or witnessed event needs to be handled on the next
// call to AdvanceTo().
TimeOrWitnessTriggered time_or_witness_triggered_{
TimeOrWitnessTriggered::kNothingTriggered
};
// Pre-allocated temporaries for updated discrete states.
std::unique_ptr<DiscreteValues<T>> discrete_updates_;
// Pre-allocated temporaries for states from unrestricted updates.
std::unique_ptr<State<T>> unrestricted_updates_;
// Pre-allocated temporary for ContinuousState passed to event handlers after
// witness function triggering.
std::unique_ptr<ContinuousState<T>> event_handler_xc_;
// Mapping of witness functions to pre-allocated events.
std::unordered_map<const WitnessFunction<T>*, std::unique_ptr<Event<T>>>
witness_function_events_;
// Optional monitor() method to capture trajectory, terminate, or fail.
std::function<EventStatus(const Context<T>&)> monitor_;
};
#ifndef DRAKE_DOXYGEN_CXX
namespace internal {
// This function computes the previous (i.e., that which is one step closer to
// negative infinity) *non-denormalized* (i.e., either zero or normalized)
// floating-point number from `value`. nexttoward() provides very similar
// functionality except for its proclivity for producing denormalized numbers
// that typically only result from arithmetic underflow and are hence dangerous
// to use in further floating point operations. Thus,
// GetPreviousNormalizedValue() acts like nexttoward(value, -inf) but without
// producing denormalized numbers.
// @param value a floating point value that is not infinity or NaN. Denormalized
// inputs are treated as zero to attain consistent behavior regardless
// of the setting of the FPU "treat denormalized numbers as zero" mode
// (which can be activated through linking with shared libraries that
// use gcc's -ffast-math option).
template <class T>
T GetPreviousNormalizedValue(const T& value) {
using std::nexttoward;
using std::abs;
// There are three distinct cases to be handled:
// -∞ -10⁻³⁰⁸ 0 10⁻³⁰⁸ ∞
// |-----------|------|------|----------|
// (a) ^ ^ ^ ^ [-∞, 10⁻³⁰⁸] ∪ (10³⁰⁸, ∞]
// (b) ^ ^ (-10⁻³⁰⁸, 10⁻³⁰⁸)
// (c) ^ 10⁻³⁰⁸
// Treat denormalized numbers as zero. This code is designed to produce the
// same outputs for `value` regardless of the setting of the FPU's DAZ ("treat
// denormalized numbers as zero") mode.
const double min_normalized = std::numeric_limits<double>::min();
const T& value_mod = (abs(value) < min_normalized) ? 0.0 : value;
// Treat zero (b) and DBL_MIN (c) specially, since nexttoward(value, -inf)
// returns denormalized numbers for these two values.
if (value_mod == 0.0)
return -std::numeric_limits<double>::min();
if (value_mod == min_normalized)
return 0.0;
// Case (a) uses nexttoward(.).
const long double inf = std::numeric_limits<long double>::infinity();
const double prev_value = nexttoward(value, -inf);
DRAKE_DEMAND(
std::fpclassify(ExtractDoubleOrThrow(prev_value)) == FP_NORMAL ||
std::fpclassify(ExtractDoubleOrThrow(prev_value)) == FP_ZERO);
return prev_value;
}
} // namespace internal
#endif
} // namespace systems
} // namespace drake
DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
class drake::systems::Simulator)