Drake
simulator.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <algorithm>
4 #include <chrono>
5 #include <limits>
6 #include <memory>
7 #include <tuple>
8 #include <utility>
9 #include <vector>
10 
11 #include "drake/common/autodiff.h"
12 #include "drake/common/drake_assert.h"
13 #include "drake/common/drake_copyable.h"
14 #include "drake/common/text_logging.h"
15 #include "drake/systems/analysis/integrator_base.h"
16 #include "drake/systems/analysis/runge_kutta3_integrator.h"
17 #include "drake/systems/framework/context.h"
18 #include "drake/systems/framework/system.h"
19 #include "drake/systems/framework/witness_function.h"
20 
21 namespace drake {
22 namespace systems {
23 
24 /// A forward dynamics solver for hybrid dynamic systems represented by
25 /// `System<T>` objects. Starting with an initial Context for a given System,
26 /// %Simulator advances time and produces a series of Context values that forms
27 /// a trajectory satisfying the system's dynamic equations to a specified
28 /// accuracy. Only the Context is modified by a %Simulator; the System is const.
29 ///
30 /// A Drake System is a continuous/discrete/hybrid dynamic system where the
31 /// continuous part is a DAE, that is, it is expected to consist of a set of
32 /// differential equations and bilateral algebraic constraints. The set of
33 /// active constraints may change as a result of particular events, such as
34 /// contact.
35 ///
36 /// Given a current Context, we expect a System to provide us with
37 /// - derivatives for the continuous differential equations that already satisfy
38 /// the differentiated form of the constraints (typically, acceleration
39 /// constraints),
40 /// - a projection method for least-squares correction of violated higher-level
41 /// constraints (position and velocity level),
42 /// - a time-of-next-update method that can be used to adjust the integrator
43 /// step size in preparation for a discrete update,
44 /// - a method that can update discrete variables when their update time is
45 /// reached,
46 /// - witness (guard) functions for event isolation,
47 /// - event handlers (reset functions) for making appropriate changes to state
48 /// and mode variables when an event has been isolated.
49 ///
50 /// The continuous parts of the trajectory are advanced using a numerical
51 /// integrator. Different integrators have different properties; if you know
52 /// about that you can choose the one that is most appropriate for your
53 /// application. Otherwise, a default is provided which is adequate for most
54 /// systems.
55 ///
56 /// @tparam T The vector element type, which must be a valid Eigen scalar.
57 ///
58 /// Instantiated templates for the following kinds of T's are provided and
59 /// available to link against in the containing library:
60 /// - double
61 /// - AutoDiffXd
62 ///
63 /// Other instantiations are permitted but take longer to compile.
64 // TODO(sherm1) When API stabilizes, should list the methods above in addition
65 // to describing them.
66 template <typename T>
67 class Simulator {
68  public:
70 
71  /// Create a %Simulator that can advance a given System through time to
72  /// produce a trajectory consisting of a sequence of Context values. The
73  /// System must not have unresolved input ports if the values of those ports
74  /// are necessary for computations performed during simulation (see class
75  /// documentation).
76  ///
77  /// The Simulator holds an internal, non-owned reference to the System
78  /// object so you must ensure that `system` has a longer lifetime than the
79  /// %Simulator. It also owns a compatible Context internally that takes on
80  /// each of the trajectory values. You may optionally provide a Context that
81  /// will be used as the initial condition for the simulation; otherwise the
82  /// %Simulator will obtain a default Context from `system`.
83  explicit Simulator(const System<T>& system,
84  std::unique_ptr<Context<T>> context = nullptr);
85 
86  /// Prepares the %Simulator for a simulation. If the initial Context does not
87  /// satisfy the System's constraints, an attempt is made to modify the values
88  /// of the continuous state variables to satisfy the constraints. This method
89  /// will throw `std::logic_error` if the combination of options doesn't make
90  /// sense, and `std::runtime_error` if it is unable to find a
91  /// constraint-satisfying initial condition.
92  void Initialize();
93 
94  // TODO(edrumwri): add ability to account for final time
95  /// Advance the System's trajectory until `boundary_time` is reached in
96  /// the context or some
97  /// other termination condition occurs. A variety of `std::runtime_error`
98  /// conditions are possible here, as well as error conditions that may be
99  /// thrown by the System when it is asked to perform computations. Be sure to
100  /// enclose your simulation in a `try-catch` block and display the
101  /// `what()` message.
102  ///
103  /// We recommend that you call `Initialize()` prior to making the first call
104  /// to `StepTo()`. However, if you don't it will be called for you the first
105  /// time you attempt a step, possibly resulting in unexpected error
106  /// conditions. See documentation for `Initialize()` for the error conditions
107  /// it might produce.
108  void StepTo(const T& boundary_time);
109 
110  /// Slow the simulation down to *approximately* synchronize with real time
111  /// when it would otherwise run too fast. Normally the %Simulator takes steps
112  /// as quickly as it can. You can request that it slow down to synchronize
113  /// with real time by providing a realtime rate greater than zero here.
114  ///
115  /// @warning No guarantees can be made about how accurately the simulation
116  /// can be made to track real time, even if computation is fast enough. That's
117  /// because the system utilities used to implement this do not themselves
118  /// provide such guarantees. So this is likely to work nicely for
119  /// visualization purposes where human perception is the only concern. For any
120  /// other uses you should consider whether approximate real time is adequate
121  /// for your purposes.
122  ///
123  /// @note If the full-speed simulation is already slower than real time you
124  /// can't speed it up with this call! Instead consider requesting less
125  /// integration accuracy, using a faster integration method or fixed time
126  /// step, or using a simpler model.
127  ///
128  /// @param realtime_rate
129  /// Desired rate relative to real time. Set to 1 to track real time, 2 to
130  /// run twice as fast as real time, 0.5 for half speed, etc. Zero or
131  /// negative restores the rate to its default of 0, meaning the simulation
132  /// will proceed as fast as possible.
133  // TODO(sherm1): Provide options for issuing a warning or aborting the
134  // simulation if the desired rate cannot be achieved.
135  void set_target_realtime_rate(double realtime_rate) {
136  target_realtime_rate_ = std::max(realtime_rate, 0.);
137  }
138 
139  /// Return the real time rate target currently in effect. The default is
140  /// zero, meaning the %Simulator runs as fast as possible. You can change the
141  /// target with set_target_realtime_rate().
142  double get_target_realtime_rate() const {
143  return target_realtime_rate_;
144  }
145 
146  /// Return the rate that simulated time has progressed relative to real time.
147  /// A return of 1 means the simulation just matched real
148  /// time, 2 means the simulation was twice as fast as real time, 0.5 means
149  /// it was running in 2X slow motion, etc.
150  ///
151  /// The value returned here is calculated as follows: <pre>
152  ///
153  /// simulated_time_now - initial_simulated_time
154  /// rate = -------------------------------------------
155  /// realtime_now - initial_realtime
156  /// </pre>
157  /// The `initial` times are recorded when Initialize() or ResetStatistics()
158  /// is called. The returned rate is undefined if Initialize() has not yet
159  /// been called.
160  ///
161  /// @returns The rate achieved since the last Initialize() or
162  /// ResetStatistics() call.
163  ///
164  /// @see set_target_realtime_rate()
165  double get_actual_realtime_rate() const;
166 
167  /// Sets whether the simulation should invoke Publish on the System under
168  /// simulation during every time step. If enabled, Publish will be invoked
169  /// after discrete updates and before continuous integration. Regardless of
170  /// whether publishing every time step is enabled, Publish will be invoked at
171  /// Simulator initialize time, and as System<T>::CalcNextUpdateTime requests.
172  void set_publish_every_time_step(bool publish) {
173  publish_every_time_step_ = publish;
174  }
175 
176  /// Sets whether the simulation should invoke Publish in Initialize().
177  void set_publish_at_initialization(bool publish) {
178  publish_at_initialization_ = publish;
179  }
180 
181  /// Returns true if the simulation should invoke Publish on the System under
182  /// simulation every time step. By default, returns true.
183  // TODO(sherm1, edrumwri): Consider making this false by default.
184  bool get_publish_every_time_step() const { return publish_every_time_step_; }
185 
186  /// Returns a const reference to the internally-maintained Context holding the
187  /// most recent step in the trajectory. This is suitable for publishing or
188  /// extracting information about this trajectory step. Do not call this method
189  /// if there is no Context.
190  const Context<T>& get_context() const {
191  DRAKE_ASSERT(context_ != nullptr);
192  return *context_;
193  }
194 
195  /// Returns a mutable reference to the internally-maintained Context holding
196  /// the most recent step in the trajectory. This is suitable for use in
197  /// updates, sampling operations, event handlers, and constraint projection.
198  /// You can also modify this prior to calling Initialize() to set initial
199  /// conditions. Do not call this method if there is no Context.
201  DRAKE_ASSERT(context_ != nullptr);
202  return *context_;
203  }
204 
205  /// Returns `true` if this Simulator has an internally-maintained Context.
206  /// This is always true unless `reset_context()` has been called.
207  bool has_context() const { return context_ != nullptr; }
208 
209  /// Replace the internally-maintained Context with a different one. The
210  /// current Context is deleted. This is useful for supplying a new set of
211  /// initial conditions. You should invoke Initialize() after replacing the
212  /// Context.
213  /// @param context The new context, which may be null. If the context is
214  /// null, a new context must be set before attempting to step
215  /// the system forward.
216  void reset_context(std::unique_ptr<Context<T>> context) {
217  context_ = std::move(context);
218  integrator_->reset_context(context_.get());
219  initialization_done_ = false;
220  }
221 
222  /// Transfer ownership of this %Simulator's internal Context to the caller.
223  /// The %Simulator will no longer contain a Context. The caller must not
224  /// attempt to advance the simulator in time after that point.
225  /// @sa reset_context()
226  std::unique_ptr<Context<T>> release_context() {
227  integrator_->reset_context(nullptr);
228  initialization_done_ = false;
229  return std::move(context_);
230  }
231 
232  /// Forget accumulated statistics. Statistics are reset to the values they
233  /// have post construction or immediately after `Initialize()`.
234  void ResetStatistics();
235 
236  /// Gets the number of publishes made since the last Initialize() or
237  /// ResetStatistics() call.
238  int64_t get_num_publishes() const { return num_publishes_; }
239 
240  /// Gets the number of integration steps since the last Initialize() call.
241  int64_t get_num_steps_taken() const { return num_steps_taken_; }
242 
243  /// Gets the number of discrete variable updates performed since the last
244  /// Initialize() call.
245  int64_t get_num_discrete_updates() const { return num_discrete_updates_; }
246 
247  /// Gets the number of "unrestricted" updates performed since the last
248  /// Initialize() call.
250  return num_unrestricted_updates_; }
251 
252  /// Gets a pointer to the integrator used to advance the continuous aspects
253  /// of the system.
254  const IntegratorBase<T>* get_integrator() const { return integrator_.get(); }
255 
256  /// Gets a pointer to the mutable integrator used to advance the continuous
257  /// aspects of the system.
258  IntegratorBase<T>* get_mutable_integrator() { return integrator_.get(); }
259 
260  /// Resets the integrator with a new one. An example usage is:
261  /// @code
262  /// simulator.reset_integrator<ExplicitEulerIntegrator<double>>
263  /// (sys, DT, context).
264  /// @endcode
265  /// The %Simulator must be reinitialized after resetting the integrator to
266  /// ensure the integrator is properly initialized. You can do that explicitly
267  /// with the Initialize() method or it will be done implicitly at the first
268  /// time step.
269  template <class U, typename... Args>
270  U* reset_integrator(Args&&... args) {
271  initialization_done_ = false;
272  integrator_ = std::make_unique<U>(std::forward<Args>(args)...);
273  return static_cast<U*>(integrator_.get());
274  }
275 
276  /// Gets the length of the interval used for witness function time isolation.
277  /// The length of the interval is computed differently, depending on context,
278  /// to support multiple applications, as described below:
279  ///
280  /// * **Simulations using error controlled integrators**: the isolation time
281  /// interval will be scaled by the product of the system's characteristic
282  /// time and the accuracy stored in the Context.
283  /// * **Simulations using integrators taking fixed steps**: the isolation time
284  /// interval will be determined differently depending on whether the
285  /// accuracy is set in the Context or not. If the accuracy *is* set in the
286  /// Context, the nominally fixed steps for integrating continuous state will
287  /// be subdivided until events have been isolated to the requisite interval
288  /// length, which is scaled by the step size times the accuracy in the
289  /// Context. If accuracy is not set in the Context, event isolation will
290  /// not be performed.
291  ///
292  /// The isolation window length will never be smaller than the integrator's
293  /// working minimum tolerance (see
294  /// IntegratorBase::get_working_minimum_step_size());
295  ///
296  /// @returns the isolation window if the Simulator should be isolating
297  /// witness-triggered events in time, or returns empty otherwise
298  /// (indicating that any witness-triggered events should trigger
299  /// at the end of a time interval over which continuous state is
300  /// integrated).
301  /// @throws std::logic_error if the accuracy is not set in the Context and
302  /// the integrator is not operating in fixed step mode (see
303  /// IntegratorBase::get_fixed_step_mode().
305 
306  /// Gets a constant reference to the system.
307  /// @note a mutable reference is not available.
308  const System<T>& get_system() const { return system_; }
309 
310  private:
311  void HandleUnrestrictedUpdate(
313 
314  void HandleDiscreteUpdate(
315  const EventCollection<DiscreteUpdateEvent<T>>& events);
316 
317  void HandlePublish(const EventCollection<PublishEvent<T>>& events);
318 
319  bool IntegrateContinuousState(const T& next_publish_dt,
320  const T& next_update_dt,
321  const T& next_sample_time,
322  const T& boundary_dt,
324 
325  void IsolateWitnessTriggers(
326  const std::vector<const WitnessFunction<T>*>& witnesses,
327  const VectorX<T>& w0,
328  const T& t0, const VectorX<T>& x0, const T& tf,
329  std::vector<const WitnessFunction<T>*>* triggered_witnesses);
330 
331  // The steady_clock is immune to system clock changes so increases
332  // monotonically. We'll work in fractional seconds.
333  using Clock = std::chrono::steady_clock;
334  using Duration = std::chrono::duration<double>;
335  using TimePoint = std::chrono::time_point<Clock, Duration>;
336 
337  // If the simulated time in the context is ahead of real time, pause long
338  // enough to let real time catch up (approximately).
339  void PauseIfTooFast() const;
340 
341  // A pointer to the integrator.
342  std::unique_ptr<IntegratorBase<T>> integrator_;
343 
344  // TODO(sherm1) This a workaround for an apparent bug in clang 3.8 in which
345  // defining this as a static constexpr member kNaN failed to instantiate
346  // properly for the AutoDiffXd instantiation (worked in gcc and MSVC).
347  // Restore to sanity when some later clang is current.
348  static constexpr double nan() {
349  return std::numeric_limits<double>::quiet_NaN();
350  }
351 
352  static constexpr double kDefaultAccuracy = 1e-3; // 1/10 of 1%.
353  static constexpr double kDefaultInitialStepSizeAttempt = 1e-3;
354 
355  const System<T>& system_; // Just a reference; not owned.
356  std::unique_ptr<Context<T>> context_; // The trajectory Context.
357 
358  // Temporaries used for witness function isolation.
359  std::vector<const WitnessFunction<T>*> triggered_witnesses_;
360  VectorX<T> w0_, wf_;
361 
362  // Slow down to this rate if possible (user settable).
363  double target_realtime_rate_{0.};
364 
365  bool publish_every_time_step_{true};
366 
367  bool publish_at_initialization_{true};
368 
369  // These are recorded at initialization or statistics reset.
370  double initial_simtime_{nan()}; // Simulated time at start of period.
371  TimePoint initial_realtime_; // Real time at start of period.
372 
373  // The number of discrete updates since the last call to Initialize().
374  int64_t num_discrete_updates_{0};
375 
376  // The number of unrestricted updates since the last call to Initialize().
377  int64_t num_unrestricted_updates_{0};
378 
379  // The number of publishes since the last call to Initialize().
380  int64_t num_publishes_{0};
381 
382  // The number of integration steps since the last call to Initialize().
383  int64_t num_steps_taken_{0};
384 
385  // Set by Initialize() and reset by various traumas.
386  bool initialization_done_{false};
387 
388  // Per step events that are to be handled on every "major time step" (i.e.,
389  // every successful completion of a step). This collection is set within
390  // Initialize().
391  std::unique_ptr<CompositeEventCollection<T>> per_step_events_;
392 
393  // Pre-allocated temporaries for updated discrete states.
394  std::unique_ptr<DiscreteValues<T>> discrete_updates_;
395 
396  // Pre-allocated temporaries for states from unrestricted updates.
397  std::unique_ptr<State<T>> unrestricted_updates_;
398 };
399 
400 template <typename T>
402  std::unique_ptr<Context<T>> context)
403  : system_(system), context_(std::move(context)) {
404  // Setup defaults that should be generally reasonable.
405  const double max_step_size = 0.1;
406  const double initial_step_size = 1e-4;
407  const double default_accuracy = 1e-4;
408 
409  // Create a context if necessary.
410  if (!context_) context_ = system_.CreateDefaultContext();
411 
412  // Create a default integrator and initialize it.
413  integrator_ = std::unique_ptr<IntegratorBase<T>>(
414  new RungeKutta3Integrator<T>(system_, context_.get()));
415  integrator_->request_initial_step_size_target(initial_step_size);
416  integrator_->set_maximum_step_size(max_step_size);
417  integrator_->set_target_accuracy(default_accuracy);
418  integrator_->Initialize();
419 
420  // Allocate the necessary temporaries for storing state in update calls
421  // (which will then be transferred back to system state).
422  discrete_updates_ = system_.AllocateDiscreteVariables();
423  unrestricted_updates_ = context_->CloneState();
424 }
425 
426 template <typename T>
428  // TODO(sherm1) Modify Context to satisfy constraints.
429  // TODO(sherm1) Invoke System's initial conditions computation.
430 
431  // Initialize the integrator.
432  integrator_->Initialize();
433 
434  // Process all the initialization events.
435  auto init_events = system_.AllocateCompositeEventCollection();
436  system_.GetInitializationEvents(*context_, init_events.get());
437 
438  // Do unrestricted updates first.
439  HandleUnrestrictedUpdate(init_events->get_unrestricted_update_events());
440  // Do restricted (discrete variable) updates next.
441  HandleDiscreteUpdate(init_events->get_discrete_update_events());
442  // Do any publishes last.
443  HandlePublish(init_events->get_publish_events());
444 
445  // Gets all per-step events to be handled.
446  per_step_events_ = system_.AllocateCompositeEventCollection();
447  DRAKE_DEMAND(per_step_events_ != nullptr);
448  system_.GetPerStepEvents(*context_, per_step_events_.get());
449 
450  // Restore default values.
451  ResetStatistics();
452 
453  // TODO(siyuan): transfer publish entirely to individual systems.
454  // Do a publish before the simulation starts.
455  if (publish_at_initialization_) {
456  system_.Publish(*context_);
457  ++num_publishes_;
458  }
459 
460  // Initialize runtime variables.
461  initialization_done_ = true;
462 }
463 
464 // Processes UnrestrictedUpdateEvent events.
465 template <typename T>
468  if (events.HasEvents()) {
469  // First, compute the unrestricted updates into a temporary buffer.
470  system_.CalcUnrestrictedUpdate(*context_, events,
471  unrestricted_updates_.get());
472  // TODO(edrumwri): simply swap the states for additional speed.
473  // Now write the update back into the context.
474  State<T>& x = context_->get_mutable_state();
475  x.CopyFrom(*unrestricted_updates_);
476  ++num_unrestricted_updates_;
477  }
478 }
479 
480 // Processes DiscreteEvent events.
481 template <typename T>
483  const EventCollection<DiscreteUpdateEvent<T>>& events) {
484  if (events.HasEvents()) {
485  // First, compute the discrete updates into a temporary buffer.
486  system_.CalcDiscreteVariableUpdates(*context_, events,
487  discrete_updates_.get());
488  // Then, write them back into the context.
489  DiscreteValues<T>& xd = context_->get_mutable_discrete_state();
490  xd.CopyFrom(*discrete_updates_);
491  ++num_discrete_updates_;
492  }
493 }
494 
495 // Processes Publish events.
496 template <typename T>
498  const EventCollection<PublishEvent<T>>& events) {
499  if (events.HasEvents()) {
500  system_.Publish(*context_, events);
501  ++num_publishes_;
502  }
503 }
504 
505 /// Steps the simulation to the specified time.
506 /// The simulation loop is as follows:
507 /// 1. Perform necessary discrete variable updates.
508 /// 2. Publish.
509 /// 3. Integrate the smooth system (the ODE or DAE)
510 /// 4. Perform post-step stabilization for DAEs (if desired).
511 /// @param boundary_time The time to advance the context to.
512 /// @pre The simulation state is valid (i.e., no discrete updates or state
513 /// projections are necessary) at the present time.
514 template <typename T>
515 void Simulator<T>::StepTo(const T& boundary_time) {
516  if (!initialization_done_) Initialize();
517 
518  DRAKE_THROW_UNLESS(boundary_time >= context_->get_time());
519 
520  // Updates/publishes can be triggered throughout the integration process,
521  // but are not active at the start of the step.
522  bool sample_time_hit = false;
523 
524  // Integrate until desired interval has completed.
525  auto timed_events = system_.AllocateCompositeEventCollection();
526  auto merged_events = system_.AllocateCompositeEventCollection();
527  auto witnessed_events = system_.AllocateCompositeEventCollection();
528  DRAKE_DEMAND(timed_events != nullptr);
529  DRAKE_DEMAND(merged_events != nullptr);
530  DRAKE_DEMAND(witnessed_events != nullptr);
531 
532  while (context_->get_time() < boundary_time || sample_time_hit) {
533  // Starting a new step on the trajectory.
534  const T step_start_time = context_->get_time();
535  SPDLOG_TRACE(log(), "Starting a simulation step at {}", step_start_time);
536 
537  // Delay to match target realtime rate if requested and possible.
538  PauseIfTooFast();
539 
540  // Merge events together.
541  merged_events->Clear();
542  merged_events->Merge(*per_step_events_);
543 
544  // Only merge timed / witnessed events in if the sample time was hit.
545  if (sample_time_hit) {
546  merged_events->Merge(*timed_events);
547  merged_events->Merge(*witnessed_events);
548  }
549 
550  // The general policy here is to do actions in decreasing order of
551  // "violence" to the state, i.e. unrestricted -> discrete -> publish.
552  // The "timed" actions happen before the "per step" ones.
553 
554  // Do unrestricted updates first.
555  HandleUnrestrictedUpdate(merged_events->get_unrestricted_update_events());
556  // Do restricted (discrete variable) updates next.
557  HandleDiscreteUpdate(merged_events->get_discrete_update_events());
558  // Do any publishes last.
559  HandlePublish(merged_events->get_publish_events());
560 
561  // TODO(siyuan): transfer per step publish entirely to individual systems.
562  // Allow System a chance to produce some output.
564  system_.Publish(*context_);
565  ++num_publishes_;
566  }
567 
568  // How far can we go before we have to take a sampling break?
569  const T next_sample_time =
570  system_.CalcNextUpdateTime(*context_, timed_events.get());
571 
572  DRAKE_DEMAND(next_sample_time >= step_start_time);
573 
574  // Determine whether the set of events requested by the System at
575  // next_sample_time includes an Update action, a Publish action, or both.
576  T next_update_dt = std::numeric_limits<double>::infinity();
577  T next_publish_dt = std::numeric_limits<double>::infinity();
578  if (timed_events->HasDiscreteUpdateEvents() ||
579  timed_events->HasUnrestrictedUpdateEvents()) {
580  next_update_dt = next_sample_time - step_start_time;
581  }
582  if (timed_events->HasPublishEvents()) {
583  next_publish_dt = next_sample_time - step_start_time;
584  }
585 
586  // Get the dt that gets to the boundary time.
587  const T boundary_dt = boundary_time - step_start_time;
588 
589  // Integrate the continuous state forward in time.
590  sample_time_hit = IntegrateContinuousState(next_publish_dt,
591  next_update_dt,
592  next_sample_time,
593  boundary_dt,
594  witnessed_events.get());
595 
596  // Update the number of simulation steps taken.
597  ++num_steps_taken_;
598 
599  // TODO(sherm1) Constraint projection goes here.
600  }
601 }
602 
603 template <class T>
605  using std::max;
606 
607  // TODO(edrumwri): Add ability to disable witness time isolation through
608  // a Simulator setting.
609 
610  // The scale factor for witness isolation.
611  // TODO(edrumwri): Consider making this user-settable.
612  const double iso_scale_factor = 1.0;
613 
614  // TODO(edrumwri): Acquire characteristic time properly from the system
615  // (i.e., modify the System to provide this value).
616  const double characteristic_time = 1.0;
617 
618  // Get the accuracy setting.
619  const optional<double>& accuracy = get_context().get_accuracy();
620 
621  // Determine the length of the isolation interval.
622  if (integrator_->get_fixed_step_mode()) {
623  // Look for accuracy information.
624  if (accuracy) {
625  return max(integrator_->get_working_minimum_step_size(),
626  T(iso_scale_factor * accuracy.value() *
627  integrator_->get_maximum_step_size()));
628  } else {
629  return optional<T>();
630  }
631  }
632 
633  // Integration with error control isolation window determination.
634  if (!accuracy) {
635  throw std::logic_error("Integrator is not operating in fixed step mode"
636  "and accuracy is not set in the context.");
637  }
638 
639  // Note: the max computation is used (here and above) because it is
640  // ineffectual to attempt to isolate intervals smaller than the current time
641  // in the context can allow.
642  return max(integrator_->get_working_minimum_step_size(),
643  iso_scale_factor * accuracy.value() * characteristic_time);
644 }
645 
646 // Isolates the first time at one or more witness functions triggered (in the
647 // interval [t0, tf]), to the requisite interval length.
648 // @param[in,out] on entry, the set of witness functions that triggered over
649 // [t0, tf]; on exit, the set of witness functions that triggered
650 // over [t0, tw], where tw is the first time that any witness
651 // function triggered.
652 // @pre The context and state are at tf and x(tf), respectively, and at least
653 // one witness function has triggered over [t0, tf].
654 // @post The context will be isolated to the first witness function trigger(s),
655 // to within the requisite interval length. It is guaranteed that all
656 // triggered witness functions change sign over [t0, tw].
657 // @note We assume that, if a witness function triggers over an interval
658 // [a, b], it also triggers over any larger interval [a, d], for d > b
659 // and d ≤ the maximum integrator step size (per WitnessFunction
660 // documentation, we assume that a witness function crosses zero at most
661 // once over an interval of size [t0, tf]).
662 template <class T>
664  const std::vector<const WitnessFunction<T>*>& witnesses,
665  const VectorX<T>& w0,
666  const T& t0, const VectorX<T>& x0, const T& tf,
667  std::vector<const WitnessFunction<T>*>* triggered_witnesses) {
668 
669  // Verify that the vector of triggered witnesses is non-null.
670  DRAKE_DEMAND(triggered_witnesses);
671 
672  // TODO(edrumwri): Speed this process using interpolation between states,
673  // more powerful root finding methods, and/or introducing the concept of
674  // a dead band.
675 
676  // Will need to alter the context repeatedly.
677  Context<T>& context = get_mutable_context();
678 
679  // Get the witness isolation interval length.
680  const optional<T> witness_iso_len = GetCurrentWitnessTimeIsolation();
681 
682  // Check whether witness functions *are* to be isolated. If not, the witnesses
683  // that were triggered on entry will be the set that is returned.
684  if (!witness_iso_len)
685  return;
686 
687  // Mini function for integrating the system forward in time from t0.
688  std::function<void(const T&)> integrate_forward =
689  [&t0, &x0, &context, this](const T& t_des) {
690  const T inf = std::numeric_limits<double>::infinity();
691  context.set_time(t0);
692  context.get_mutable_continuous_state().SetFromVector(x0);
693  T t_remaining = t_des - t0;
694  while (t_remaining > 0) {
695  integrator_->IntegrateAtMost(inf, inf, t_remaining);
696  t_remaining = t_des - context.get_time();
697  }
698  };
699 
700  // Loop until the isolation window is sufficiently small.
702  "Isolating witness functions using isolation window of {} over [{}, {}]",
703  witness_iso_len.value(), t0, tf);
704  VectorX<T> wc(witnesses.size());
705  T a = t0;
706  T b = tf;
707  do {
708  // Compute the midpoint and evaluate the witness functions at it.
709  T c = (a + b) / 2;
710  SPDLOG_DEBUG(drake::log(), "Integrating forward to time {}", c);
711  integrate_forward(c);
712 
713  // See whether any witness functions trigger.
714  bool trigger = false;
715  for (size_t i = 0; i < witnesses.size(); ++i) {
716  wc[i] = get_system().EvaluateWitness(context, *witnesses[i]);
717  if (witnesses[i]->should_trigger(w0[i], wc[i]))
718  trigger = true;
719  }
720 
721  // If no witness function triggered, we can continue integrating forward.
722  if (!trigger) {
723  // NOTE: Since we're always checking that the sign changes over [t0,c],
724  // it's also feasible to replace the two lines below with "a = c" without
725  // violating Simulator's contract to only integrate once over the interval
726  // [a, c], for some c <= b before per-step events are handled (i.e., it's
727  // unacceptable to take two steps of (c - a)/2 without processing per-step
728  // events first). That change would avoid handling unnecessary per-step
729  // events- we know no other events are to be handled between t0 and tf-
730  // but the current logic appears easier to follow.
731  SPDLOG_DEBUG(drake::log(), "No witness functions triggered up to {}", c);
732  triggered_witnesses->clear();
733  return;
734  } else {
735  b = c;
736  }
737  } while (b - a > witness_iso_len.value());
738 
739  // Determine the set of triggered witnesses.
740  triggered_witnesses->clear();
741  for (size_t i = 0; i < witnesses.size(); ++i) {
742  if (witnesses[i]->should_trigger(w0[i], wc[i]))
743  triggered_witnesses->push_back(witnesses[i]);
744  }
745 }
746 
747 // Integrates the continuous state forward in time while attempting to locate
748 // the first zero of any triggered witness functions.
749 // @param next_publish_dt the time step at which the next publish event occurs.
750 // @param next_update_dt the time step at which the next update event occurs.
751 // @param next_sample_time the time at which the next event occurs.
752 // @param boundary_dt the maximum time step to take.
753 // @param events a non-null collection of events, which the method will clear
754 // on entry.
755 // @returns `true` if integration terminated on a sample time, indicating that
756 // an event needs to be handled at the state/time on return.
757 template <class T>
758 bool Simulator<T>::IntegrateContinuousState(const T& next_publish_dt,
759  const T& next_update_dt,
760  const T& next_sample_time,
761  const T& boundary_dt,
762  CompositeEventCollection<T>* events) {
763  // Clear the composite event collection.
764  DRAKE_ASSERT(events);
765  events->Clear();
766 
767  // Save the time and current state.
768  const Context<T>& context = get_context();
769  const T t0 = context.get_time();
770  const VectorX<T> x0 = context.get_continuous_state().CopyToVector();
771 
772  // Get the set of witness functions active at the current time.
773  const System<T>& system = get_system();
774  std::vector<const WitnessFunction<T>*> witness_functions;
775  system.GetWitnessFunctions(context, &witness_functions);
776 
777  // Evaluate the witness functions.
778  w0_.resize(witness_functions.size());
779  for (size_t i = 0; i < witness_functions.size(); ++i)
780  w0_[i] = system.EvaluateWitness(context, *witness_functions[i]);
781 
782  // Attempt to integrate. Updates and boundary times are consciously
783  // distinguished between. See internal documentation for
784  // IntegratorBase::StepOnceAtMost() for more information.
786  integrator_->IntegrateAtMost(next_publish_dt, next_update_dt,
787  boundary_dt);
788  const T tf = context.get_time();
789 
790  // Evaluate the witness functions again.
791  wf_.resize(witness_functions.size());
792  for (size_t i =0; i < witness_functions.size(); ++i)
793  wf_[i] = system.EvaluateWitness(context, *witness_functions[i]);
794 
795  // See whether a witness function triggered.
796  triggered_witnesses_.clear();
797  bool witness_triggered = false;
798  for (size_t i =0; i < witness_functions.size() && !witness_triggered; ++i) {
799  if (witness_functions[i]->should_trigger(w0_[i], wf_[i])) {
800  witness_triggered = true;
801  triggered_witnesses_.push_back(witness_functions[i]);
802  }
803  }
804 
805  // Triggering requires isolating the witness function time.
806  if (witness_triggered) {
807  // Isolate the time that the witness function triggered.
808  IsolateWitnessTriggers(witness_functions, w0_, t0, x0, tf,
809  &triggered_witnesses_);
810 
811  // Store witness function(s) that triggered.
812  for (const WitnessFunction<T>* fn : triggered_witnesses_) {
813  SPDLOG_DEBUG(drake::log(), "Witness function {} crossed zero at time {}",
814  fn->get_name(), context.get_time());
816  *fn, events);
817  }
818 
819  // Indicate a "sample time was hit" if at least one witness function
820  // triggered (meaning that an event should be handled on the next simulation
821  // loop).
822  return !triggered_witnesses_.empty();
823  }
824 
825  // No witness function triggered; handle integration as usual.
826  // Updates and boundary times are consciously distinguished between. See
827  // internal documentation for IntegratorBase::StepOnceAtMost() for more
828  // information.
829  switch (result) {
832  // Next line sets the time to the exact sample time rather than
833  // introducing rounding error by summing the context time + dt.
834  context_->set_time(next_sample_time);
835  return true; // Sample time hit.
836  break;
837 
840  return false; // Did not hit a sample time.
841  break;
842 
843  default:DRAKE_ABORT_MSG("Unexpected integrator result.");
844  }
845  ++num_steps_taken_;
846 
847  // TODO(sherm1) Constraint projection goes here.
848 
849  // Should never get here.
850  DRAKE_ABORT();
851  return false;
852 }
853 
854 } // namespace systems
855 } // namespace drake
DiscreteValues is a container for numerical but non-continuous state and parameters.
Definition: discrete_values.h:32
int64_t get_num_discrete_updates() const
Gets the number of discrete variable updates performed since the last Initialize() call...
Definition: simulator.h:245
U * reset_integrator(Args &&...args)
Resets the integrator with a new one.
Definition: simulator.h:270
const IntegratorBase< T > * get_integrator() const
Gets a pointer to the integrator used to advance the continuous aspects of the system.
Definition: simulator.h:254
#define SPDLOG_DEBUG(logger,...)
Definition: text_logging.h:93
std::vector< Number > x
Definition: ipopt_solver.cc:150
A third-order Runge Kutta integrator with a third order error estimate.
Definition: runge_kutta3_integrator.h:58
Definition: automotive_demo.cc:89
const System< T > & get_system() const
Gets a constant reference to the system.
Definition: simulator.h:308
virtual void AddTriggeredWitnessFunctionToCompositeEventCollection(const WitnessFunction< T > &witness_func, CompositeEventCollection< T > *events) const =0
Add witness_func to events.
double get_actual_realtime_rate() const
Return the rate that simulated time has progressed relative to real time.
Definition: simulator.cc:27
STL namespace.
Context is an abstract base class template that represents all the inputs to a System: time...
Definition: query_handle.h:10
A forward dynamics solver for hybrid dynamic systems represented by System<T> objects.
Definition: simulator.h:67
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorX
A column vector of any size, templated on scalar type.
Definition: eigen_types.h:46
This class represents an unrestricted update event.
Definition: event.h:349
Simulator(const Simulator &)=delete
bool get_publish_every_time_step() const
Returns true if the simulation should invoke Publish on the System under simulation every time step...
Definition: simulator.h:184
#define DRAKE_ABORT_MSG(message)
Aborts the program (via ::abort) with a message showing at least the given message (macro argument)...
Definition: drake_assert.h:51
#define SPDLOG_TRACE(logger,...)
Definition: text_logging.h:92
void reset_context(std::unique_ptr< Context< T >> context)
Replace the internally-maintained Context with a different one.
Definition: simulator.h:216
#define DRAKE_THROW_UNLESS(condition)
Evaluates condition and iff the value is false will throw an exception with a message showing at leas...
Definition: drake_throw.h:23
logging::logger * log()
Retrieve an instance of a logger to use for logging; for example: drake::log()->info("potato!") ...
Definition: text_logging.cc:38
std::vector< double > vector
Definition: translator_test.cc:20
stx::optional< T > optional
Definition: drake_optional.h:14
std::vector< Number > result
Definition: ipopt_solver.cc:151
void CopyFrom(const DiscreteValues< T > &other)
Writes the values from other into this DiscreteValues, possibly writing through to unowned data...
Definition: discrete_values.h:135
void Initialize()
Prepares the Simulator for a simulation.
Definition: simulator.h:427
#define DRAKE_ASSERT(condition)
DRAKE_ASSERT(condition) is similar to the built-in assert(condition) from the C++ system header <cas...
Definition: drake_assert.h:37
StepResult
Status returned by StepOnceAtMost().
Definition: integrator_base.h:123
Context< T > & get_mutable_context()
Returns a mutable reference to the internally-maintained Context holding the most recent step in the ...
Definition: simulator.h:200
void set_publish_at_initialization(bool publish)
Sets whether the simulation should invoke Publish in Initialize().
Definition: simulator.h:177
int64_t get_num_steps_taken() const
Gets the number of integration steps since the last Initialize() call.
Definition: simulator.h:241
Expression max(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:696
#define DRAKE_DEMAND(condition)
Evaluates condition and iff the value is false will trigger an assertion failure with a message showi...
Definition: drake_assert.h:45
State is a container for all the data comprising the complete state of a particular System at a parti...
Definition: state.h:27
const T & get_time() const
Returns the current time in seconds.
Definition: context.h:49
A superclass template for systems that receive input, maintain state, and produce output of a given m...
Definition: input_port_descriptor.h:12
double get_target_realtime_rate() const
Return the real time rate target currently in effect.
Definition: simulator.h:142
const Context< T > & get_context() const
Returns a const reference to the internally-maintained Context holding the most recent step in the tr...
Definition: simulator.h:190
void Clear()
Clears all the events.
Definition: event_collection.h:405
This class bundles an instance of each EventCollection<EventType> into one object that stores the het...
Definition: event.h:22
T EvaluateWitness(const Context< T > &context, const WitnessFunction< T > &witness_func) const
Evaluates a witness function at the given context.
Definition: system.h:1342
std::unique_ptr< Context< T > > release_context()
Transfer ownership of this Simulator&#39;s internal Context to the caller.
Definition: simulator.h:226
void CopyFrom(const State< T > &other)
Copies the values from another State of the same scalar type into this State.
Definition: state.h:101
int64_t get_num_unrestricted_updates() const
Gets the number of "unrestricted" updates performed since the last Initialize() call.
Definition: simulator.h:249
void StepTo(const T &boundary_time)
Advance the System&#39;s trajectory until boundary_time is reached in the context or some other terminati...
Definition: simulator.h:515
void set_publish_every_time_step(bool publish)
Sets whether the simulation should invoke Publish on the System under simulation during every time st...
Definition: simulator.h:172
IntegratorBase< T > * get_mutable_integrator()
Gets a pointer to the mutable integrator used to advance the continuous aspects of the system...
Definition: simulator.h:258
#define DRAKE_ABORT()
Aborts the program (via ::abort) with a message showing at least the function name, file, and line.
Definition: drake_assert.h:48
int64_t get_num_publishes() const
Gets the number of publishes made since the last Initialize() or ResetStatistics() call...
Definition: simulator.h:238
This class represents a publish event.
Definition: event.h:215
An abstract class for an integrator for ODEs and DAEs as represented by a Drake System.
Definition: integrator_base.h:107
optional< T > GetCurrentWitnessTimeIsolation() const
Gets the length of the interval used for witness function time isolation.
Definition: simulator.h:604
void ResetStatistics()
Forget accumulated statistics.
Definition: simulator.cc:36
void GetWitnessFunctions(const Context< T > &context, std::vector< const WitnessFunction< T > * > *w) const
Gets the witness functions active at the beginning of a continuous time interval. ...
Definition: system.h:1333
bool has_context() const
Returns true if this Simulator has an internally-maintained Context.
Definition: simulator.h:207
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:33
const ContinuousState< T > & get_continuous_state() const
Returns a const reference to the continuous component of the state, which may be of size zero...
Definition: context.h:118
Abstract class that describes a function that is able to help determine the time and state at which a...
Definition: witness_function.h:89
void set_target_realtime_rate(double realtime_rate)
Slow the simulation down to approximately synchronize with real time when it would otherwise run too ...
Definition: simulator.h:135
There are three concrete event types for any System: publish, discrete state update, and unrestricted state update, listed in order of increasing ability to change the state (i.e., zero to all).
Definition: event.h:16
This class represents a discrete update event.
Definition: event.h:279