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