Drake
Simulator< T > Class Template Reference

A class for advancing the state of hybrid dynamic systems, represented by System<T> objects, forward in time. More...

#include <drake/systems/analysis/simulator.h>

Public Member Functions

 Simulator (const System< T > &system, std::unique_ptr< Context< T >> context=nullptr)
 Create a Simulator that can advance a given System through time to produce a trajectory consisting of a sequence of Context values. More...
 
 Simulator (std::unique_ptr< const System< T >> system, std::unique_ptr< Context< T >> context=nullptr)
 Create a Simulator which additionally maintains ownership of the System. More...
 
void Initialize ()
 Prepares the Simulator for a simulation. More...
 
void AdvanceTo (const T &boundary_time)
 Advances the System's trajectory until boundary_time is reached in the context or some other termination condition occurs. More...
 
void AdvancePendingEvents ()
 (Advanced) Handles discrete and abstract state update events that are pending from the previous AdvanceTo() call, without advancing time. More...
 
void set_target_realtime_rate (double realtime_rate)
 Slow the simulation down to approximately synchronize with real time when it would otherwise run too fast. More...
 
double get_target_realtime_rate () const
 Return the real time rate target currently in effect. More...
 
double get_actual_realtime_rate () const
 Return the rate that simulated time has progressed relative to real time. More...
 
void set_publish_every_time_step (bool publish)
 Sets whether the simulation should trigger a forced-Publish event on the System under simulation at the end of every trajectory-advancing step. More...
 
void set_publish_at_initialization (bool publish)
 Sets whether the simulation should trigger a forced-Publish at the end of Initialize(). More...
 
bool get_publish_every_time_step () const
 Returns true if the set_publish_every_time_step() option has been enabled. More...
 
const Context< T > & get_context () const
 Returns a const reference to the internally-maintained Context holding the most recent step in the trajectory. More...
 
Context< T > & get_mutable_context ()
 Returns a mutable reference to the internally-maintained Context holding the most recent step in the trajectory. More...
 
bool has_context () const
 Returns true if this Simulator has an internally-maintained Context. More...
 
void reset_context (std::unique_ptr< Context< T >> context)
 Replace the internally-maintained Context with a different one. More...
 
std::unique_ptr< Context< T > > release_context ()
 Transfer ownership of this Simulator's internal Context to the caller. More...
 
void ResetStatistics ()
 Forget accumulated statistics. More...
 
int64_t get_num_publishes () const
 Gets the number of publishes made since the last Initialize() or ResetStatistics() call. More...
 
int64_t get_num_steps_taken () const
 Gets the number of steps since the last Initialize() call. More...
 
int64_t get_num_discrete_updates () const
 Gets the number of discrete variable updates performed since the last Initialize() call. More...
 
int64_t get_num_unrestricted_updates () const
 Gets the number of "unrestricted" updates performed since the last Initialize() call. More...
 
const IntegratorBase< T > & get_integrator () const
 Gets a reference to the integrator used to advance the continuous aspects of the system. More...
 
IntegratorBase< T > & get_mutable_integrator ()
 Gets a reference to the mutable integrator used to advance the continuous state of the system. More...
 
template<class U >
U * reset_integrator (std::unique_ptr< U > integrator)
 Resets the integrator with a new one. More...
 
template<class U , typename... Args>
U * reset_integrator (Args &&... args)
 Resets the integrator with a new one using factory construction. More...
 
optional< T > GetCurrentWitnessTimeIsolation () const
 Gets the length of the interval used for witness function time isolation. More...
 
const System< T > & get_system () const
 Gets a constant reference to the system. More...
 
Does not allow copy, move, or assignment
 Simulator (const Simulator &)=delete
 
Simulatoroperator= (const Simulator &)=delete
 
 Simulator (Simulator &&)=delete
 
Simulatoroperator= (Simulator &&)=delete
 

Detailed Description

template<typename T>
class drake::systems::Simulator< T >

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.

How the simulation is stepped: simulation mechanics for authors of discrete and hybrid systems

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 Discrete Systems module; please look there first before proceeding.

As pictured in 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:

  1. Unrestricted update (can change x)
  2. Discrete update (can change only xd)
  3. 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.

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ₑ))
return {tₑ, x⁻(tₑ)}

We can use the notation and pseudocode to flesh out the AdvanceTo(), AdvancePendingEvents(), and Initialize() functions:

// Advance the simulation until time tₘₐₓ.
procedure AdvanceTo(tₘₐₓ)
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).
t ≜ current_time, x⁻(t) ≜ current_state
x⁺(t) ← DoAnyPendingUpdates(t, x⁻(t)) as in Step()
x(t) ← x⁺(t) // No continuous update needed.
// 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₀)
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₀))

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"). 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, per-step publish events, and periodic or timed publish events that trigger at t₀.

Template Parameters
TThe vector element type, which must be a valid Eigen scalar.

Instantiated templates for the following kinds of T's are provided and available to link against in the containing library:

  • double
  • AutoDiffXd

Other instantiations are permitted but take longer to compile.

Constructor & Destructor Documentation

◆ Simulator() [1/4]

Simulator ( const Simulator< T > &  )
delete

◆ Simulator() [2/4]

Simulator ( Simulator< T > &&  )
delete

◆ Simulator() [3/4]

Simulator ( const System< T > &  system,
std::unique_ptr< Context< T >>  context = nullptr 
)
explicit

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.

◆ Simulator() [4/4]

Simulator ( std::unique_ptr< const System< T >>  system,
std::unique_ptr< Context< T >>  context = nullptr 
)

Create a Simulator which additionally maintains ownership of the System.

Member Function Documentation

◆ AdvancePendingEvents()

void AdvancePendingEvents ( )

(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.

See also
AdvanceTo(), Initialize()

◆ AdvanceTo()

void AdvanceTo ( const T &  boundary_time)

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.
Parameters
boundary_timeThe time to advance the context to.
Precondition
The internal Context satisfies all System constraints or will after pending Context updates are performed.
See also
Initialize(), AdvancePendingEvents()

◆ get_actual_realtime_rate()

double get_actual_realtime_rate ( ) const

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:

         simulated_time_now - initial_simulated_time
  rate = -------------------------------------------
               realtime_now - initial_realtime

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 also
set_target_realtime_rate()

◆ get_context()

const Context<T>& get_context ( ) const

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.

◆ get_integrator()

const IntegratorBase<T>& get_integrator ( ) const

Gets a reference to the integrator used to advance the continuous aspects of the system.

◆ get_mutable_context()

Context<T>& get_mutable_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.

◆ get_mutable_integrator()

IntegratorBase<T>& get_mutable_integrator ( )

Gets a reference to the mutable integrator used to advance the continuous state of the system.

◆ get_num_discrete_updates()

int64_t get_num_discrete_updates ( ) const

Gets the number of discrete variable updates performed since the last Initialize() call.

◆ get_num_publishes()

int64_t get_num_publishes ( ) const

Gets the number of publishes made since the last Initialize() or ResetStatistics() call.

◆ get_num_steps_taken()

int64_t get_num_steps_taken ( ) const

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.

◆ get_num_unrestricted_updates()

int64_t get_num_unrestricted_updates ( ) const

Gets the number of "unrestricted" updates performed since the last Initialize() call.

◆ get_publish_every_time_step()

bool get_publish_every_time_step ( ) const

Returns true if the set_publish_every_time_step() option has been enabled.

By default, returns false.

◆ get_system()

const System<T>& get_system ( ) const

Gets a constant reference to the system.

Note
a mutable reference is not available.

◆ get_target_realtime_rate()

double get_target_realtime_rate ( ) const

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().

◆ GetCurrentWitnessTimeIsolation()

optional< T > GetCurrentWitnessTimeIsolation ( ) const

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).
Exceptions
std::logic_errorif the accuracy is not set in the Context and the integrator is not operating in fixed step mode (see IntegratorBase::get_fixed_step_mode().

◆ has_context()

bool has_context ( ) const

Returns true if this Simulator has an internally-maintained Context.

This is always true unless reset_context() has been called.

◆ Initialize()

void Initialize ( )

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.
  • Initialization update events are triggered and handled to produce the initial trajectory value {t₀, x(t₀)}.
  • Then that initial value is provided to the handlers for any publish events that have triggered, including initialization and per-step publish events, and periodic or other time-triggered publish events that are scheduled for the initial time t₀.

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. For example, if you changed the time you will likely want the time-triggered events to be recalculated in case one is due at the new starting time.
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::logic_error if the combination of options doesn't make sense. Other failures are possible from the System and integrator in use.

See also
AdvanceTo(), AdvancePendingEvents()

◆ operator=() [1/2]

Simulator& operator= ( Simulator< T > &&  )
delete

◆ operator=() [2/2]

Simulator& operator= ( const Simulator< T > &  )
delete

◆ release_context()

std::unique_ptr<Context<T> > release_context ( )

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.

See also
reset_context()

◆ reset_context()

void reset_context ( std::unique_ptr< Context< T >>  context)

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.

Parameters
contextThe new context, which may be null. If the context is null, a new context must be set before attempting to step the system forward.

◆ reset_integrator() [1/2]

U* reset_integrator ( std::unique_ptr< U >  integrator)

Resets the integrator with a new one.

An example usage is:

simulator.reset_integrator(std::move(integrator));

The Simulator must be reinitialized after resetting the integrator to ensure the integrator is properly initialized. You can do that explicitly with the Initialize() method or it will be done implicitly at the first time step.

Exceptions
std::logic_errorif integrator is nullptr.

◆ reset_integrator() [2/2]

U* reset_integrator ( Args &&...  args)

Resets the integrator with a new one using factory construction.

An example usage is:

simulator.reset_integrator<ExplicitEulerIntegrator<double>>
(sys, DT, context).

See the base overload for reset_integrator for more details.

◆ ResetStatistics()

void ResetStatistics ( )

Forget accumulated statistics.

Statistics are reset to the values they have post construction or immediately after Initialize().

◆ set_publish_at_initialization()

void set_publish_at_initialization ( bool  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.

◆ set_publish_every_time_step()

void set_publish_every_time_step ( bool  publish)

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 also
LeafSystem::DeclareForcedPublishEvent()

◆ set_target_realtime_rate()

void set_target_realtime_rate ( double  realtime_rate)

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.
Parameters
realtime_rateDesired 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.

The documentation for this class was generated from the following files: