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"
20 
21 namespace drake {
22 namespace systems {
23 
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 
72  explicit Simulator(const System<T>& system,
84  std::unique_ptr<Context<T>> context = nullptr);
85 
92  void Initialize();
93 
94  // TODO(edrumwri): add ability to account for final time
108  void StepTo(const T& boundary_time);
109 
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 
142  double get_target_realtime_rate() const {
143  return target_realtime_rate_;
144  }
145 
165  double get_actual_realtime_rate() const;
166 
172  void set_publish_every_time_step(bool publish) {
173  publish_every_time_step_ = publish;
174  }
175 
177  void set_publish_at_initialization(bool publish) {
178  publish_at_initialization_ = publish;
179  }
180 
183  // TODO(sherm1, edrumwri): Consider making this false by default.
184  bool get_publish_every_time_step() const { return publish_every_time_step_; }
185 
189  const Context<T>& get_context() const { return *context_; }
190 
195  Context<T>* get_mutable_context() { return context_.get(); }
196 
204  void reset_context(std::unique_ptr<Context<T>> context) {
205  context_ = std::move(context);
206  integrator_->reset_context(context_.get());
207  initialization_done_ = false;
208  }
209 
214  std::unique_ptr<Context<T>> release_context() {
215  integrator_->reset_context(nullptr);
216  initialization_done_ = false;
217  return std::move(context_);
218  }
219 
222  void ResetStatistics();
223 
226  int64_t get_num_publishes() const { return num_publishes_; }
227 
229  int64_t get_num_steps_taken() const { return num_steps_taken_; }
230 
233  int64_t get_num_discrete_updates() const { return num_discrete_updates_; }
234 
238  return num_unrestricted_updates_; }
239 
242  const IntegratorBase<T>* get_integrator() const { return integrator_.get(); }
243 
246  IntegratorBase<T>* get_mutable_integrator() { return integrator_.get(); }
247 
257  template <class U, typename... Args>
258  U* reset_integrator(Args&&... args) {
259  initialization_done_ = false;
260  integrator_ = std::make_unique<U>(std::forward<Args>(args)...);
261  return static_cast<U*>(integrator_.get());
262  }
263 
293 
296  const System<T>& get_system() const { return system_; }
297 
298  private:
299  void HandleUnrestrictedUpdate(
301 
302  void HandleDiscreteUpdate(
303  const EventCollection<DiscreteUpdateEvent<T>>& events);
304 
305  void HandlePublish(const EventCollection<PublishEvent<T>>& events);
306 
307  bool IntegrateContinuousState(const T& next_publish_dt,
308  const T& next_update_dt,
309  const T& next_sample_time,
310  const T& boundary_dt,
312 
313  void IsolateWitnessTriggers(
314  const std::vector<const WitnessFunction<T>*>& witnesses,
315  const VectorX<T>& w0,
316  const T& t0, const VectorX<T>& x0, const T& tf,
317  std::vector<const WitnessFunction<T>*>* triggered_witnesses);
318 
319  // The steady_clock is immune to system clock changes so increases
320  // monotonically. We'll work in fractional seconds.
321  using Clock = std::chrono::steady_clock;
322  using Duration = std::chrono::duration<double>;
323  using TimePoint = std::chrono::time_point<Clock, Duration>;
324 
325  // If the simulated time in the context is ahead of real time, pause long
326  // enough to let real time catch up (approximately).
327  void PauseIfTooFast() const;
328 
329  // A pointer to the integrator.
330  std::unique_ptr<IntegratorBase<T>> integrator_;
331 
332  // TODO(sherm1) This a workaround for an apparent bug in clang 3.8 in which
333  // defining this as a static constexpr member kNaN failed to instantiate
334  // properly for the AutoDiffXd instantiation (worked in gcc and MSVC).
335  // Restore to sanity when some later clang is current.
336  static constexpr double nan() {
337  return std::numeric_limits<double>::quiet_NaN();
338  }
339 
340  static constexpr double kDefaultAccuracy = 1e-3; // 1/10 of 1%.
341  static constexpr double kDefaultInitialStepSizeAttempt = 1e-3;
342 
343  const System<T>& system_; // Just a reference; not owned.
344  std::unique_ptr<Context<T>> context_; // The trajectory Context.
345 
346  // Temporaries used for witness function isolation.
347  std::vector<const WitnessFunction<T>*> triggered_witnesses_;
348  VectorX<T> w0_, wf_;
349 
350  // Slow down to this rate if possible (user settable).
351  double target_realtime_rate_{0.};
352 
353  bool publish_every_time_step_{true};
354 
355  bool publish_at_initialization_{true};
356 
357  // These are recorded at initialization or statistics reset.
358  double initial_simtime_{nan()}; // Simulated time at start of period.
359  TimePoint initial_realtime_; // Real time at start of period.
360 
361  // The number of discrete updates since the last call to Initialize().
362  int64_t num_discrete_updates_{0};
363 
364  // The number of unrestricted updates since the last call to Initialize().
365  int64_t num_unrestricted_updates_{0};
366 
367  // The number of publishes since the last call to Initialize().
368  int64_t num_publishes_{0};
369 
370  // The number of integration steps since the last call to Initialize().
371  int64_t num_steps_taken_{0};
372 
373  // Set by Initialize() and reset by various traumas.
374  bool initialization_done_{false};
375 
376  // Per step events that are to be handled on every "major time step" (i.e.,
377  // every successful completion of a step). This collection is set within
378  // Initialize().
379  std::unique_ptr<CompositeEventCollection<T>> per_step_events_;
380 
381  // Pre-allocated temporaries for updated discrete states.
382  std::unique_ptr<DiscreteValues<T>> discrete_updates_;
383 
384  // Pre-allocated temporaries for states from unrestricted updates.
385  std::unique_ptr<State<T>> unrestricted_updates_;
386 };
387 
388 template <typename T>
390  std::unique_ptr<Context<T>> context)
391  : system_(system), context_(std::move(context)) {
392  // Setup defaults that should be generally reasonable.
393  const double max_step_size = 0.1;
394  const double initial_step_size = 1e-4;
395  const double default_accuracy = 1e-4;
396 
397  // Create a context if necessary.
398  if (!context_) context_ = system_.CreateDefaultContext();
399 
400  // Create a default integrator and initialize it.
401  integrator_ = std::unique_ptr<IntegratorBase<T>>(
402  new RungeKutta3Integrator<T>(system_, context_.get()));
403  integrator_->request_initial_step_size_target(initial_step_size);
404  integrator_->set_maximum_step_size(max_step_size);
405  integrator_->set_target_accuracy(default_accuracy);
406  integrator_->Initialize();
407 
408  // Allocate the necessary temporaries for storing state in update calls
409  // (which will then be transferred back to system state).
410  discrete_updates_ = system_.AllocateDiscreteVariables();
411  unrestricted_updates_ = context_->CloneState();
412 }
413 
414 template <typename T>
416  // TODO(sherm1) Modify Context to satisfy constraints.
417  // TODO(sherm1) Invoke System's initial conditions computation.
418 
419  // Initialize the integrator.
420  integrator_->Initialize();
421 
422  // Gets all per-step events to be handled.
423  per_step_events_ = system_.AllocateCompositeEventCollection();
424  DRAKE_DEMAND(per_step_events_ != nullptr);
425  system_.GetPerStepEvents(*context_, per_step_events_.get());
426 
427  // Restore default values.
428  ResetStatistics();
429 
430  // TODO(siyuan): transfer publish entirely to individual systems.
431  // Do a publish before the simulation starts.
432  if (publish_at_initialization_) {
433  system_.Publish(*context_);
434  ++num_publishes_;
435  }
436 
437  // Initialize runtime variables.
438  initialization_done_ = true;
439 }
440 
441 // Processes UnrestrictedUpdateEvent events.
442 template <typename T>
445  if (events.HasEvents()) {
446  // First, compute the unrestricted updates into a temporary buffer.
447  system_.CalcUnrestrictedUpdate(*context_, events,
448  unrestricted_updates_.get());
449  // TODO(edrumwri): simply swap the states for additional speed.
450  // Now write the update back into the context.
451  State<T>* x = context_->get_mutable_state();
452  DRAKE_DEMAND(x != nullptr);
453  x->CopyFrom(*unrestricted_updates_);
454  ++num_unrestricted_updates_;
455  }
456 }
457 
458 // Processes DiscreteEvent events.
459 template <typename T>
461  const EventCollection<DiscreteUpdateEvent<T>>& events) {
462  if (events.HasEvents()) {
463  // First, compute the discrete updates into a temporary buffer.
464  system_.CalcDiscreteVariableUpdates(*context_, events,
465  discrete_updates_.get());
466  DiscreteValues<T>* xd = context_->get_mutable_discrete_state();
467  // Systems with discrete update events must have discrete state.
468  DRAKE_DEMAND(xd != nullptr);
469  // Then, write them back into the context.
470  xd->CopyFrom(*discrete_updates_);
471  ++num_discrete_updates_;
472  }
473 }
474 
475 // Processes Publish events.
476 template <typename T>
478  const EventCollection<PublishEvent<T>>& events) {
479  if (events.HasEvents()) {
480  system_.Publish(*context_, events);
481  ++num_publishes_;
482  }
483 }
484 
494 template <typename T>
495 void Simulator<T>::StepTo(const T& boundary_time) {
496  if (!initialization_done_) Initialize();
497 
498  DRAKE_THROW_UNLESS(boundary_time >= context_->get_time());
499 
500  // Updates/publishes can be triggered throughout the integration process,
501  // but are not active at the start of the step.
502  bool sample_time_hit = false;
503 
504  // Integrate until desired interval has completed.
505  auto timed_events = system_.AllocateCompositeEventCollection();
506  auto merged_events = system_.AllocateCompositeEventCollection();
507  auto witnessed_events = system_.AllocateCompositeEventCollection();
508  DRAKE_DEMAND(timed_events != nullptr);
509  DRAKE_DEMAND(merged_events != nullptr);
510  DRAKE_DEMAND(witnessed_events != nullptr);
511 
512  while (context_->get_time() < boundary_time || sample_time_hit) {
513  // Starting a new step on the trajectory.
514  const T step_start_time = context_->get_time();
515  SPDLOG_TRACE(log(), "Starting a simulation step at {}", step_start_time);
516 
517  // Delay to match target realtime rate if requested and possible.
518  PauseIfTooFast();
519 
520  // Merge events together.
521  merged_events->Clear();
522  merged_events->Merge(*per_step_events_);
523 
524  // Only merge timed / witnessed events in if the sample time was hit.
525  if (sample_time_hit) {
526  merged_events->Merge(*timed_events);
527  merged_events->Merge(*witnessed_events);
528  }
529 
530  // The general policy here is to do actions in decreasing order of
531  // "violence" to the state, i.e. unrestricted -> discrete -> publish.
532  // The "timed" actions happen before the "per step" ones.
533 
534  // Do unrestricted updates first.
535  HandleUnrestrictedUpdate(merged_events->get_unrestricted_update_events());
536  // Do restricted (discrete variable) updates next.
537  HandleDiscreteUpdate(merged_events->get_discrete_update_events());
538  // Do any publishes last.
539  HandlePublish(merged_events->get_publish_events());
540 
541  // TODO(siyuan): transfer per step publish entirely to individual systems.
542  // Allow System a chance to produce some output.
544  system_.Publish(*context_);
545  ++num_publishes_;
546  }
547 
548  // How far can we go before we have to take a sampling break?
549  const T next_sample_time =
550  system_.CalcNextUpdateTime(*context_, timed_events.get());
551 
552  DRAKE_DEMAND(next_sample_time >= step_start_time);
553 
554  // Determine whether the set of events requested by the System at
555  // next_sample_time includes an Update action, a Publish action, or both.
556  T next_update_dt = std::numeric_limits<double>::infinity();
557  T next_publish_dt = std::numeric_limits<double>::infinity();
558  if (timed_events->HasDiscreteUpdateEvents() ||
559  timed_events->HasUnrestrictedUpdateEvents()) {
560  next_update_dt = next_sample_time - step_start_time;
561  }
562  if (timed_events->HasPublishEvents()) {
563  next_publish_dt = next_sample_time - step_start_time;
564  }
565 
566  // Get the dt that gets to the boundary time.
567  const T boundary_dt = boundary_time - step_start_time;
568 
569  // Integrate the continuous state forward in time.
570  sample_time_hit = IntegrateContinuousState(next_publish_dt,
571  next_update_dt,
572  next_sample_time,
573  boundary_dt,
574  witnessed_events.get());
575 
576  // Update the number of simulation steps taken.
577  ++num_steps_taken_;
578 
579  // TODO(sherm1) Constraint projection goes here.
580  }
581 }
582 
583 template <class T>
585  using std::max;
586 
587  // TODO(edrumwri): Add ability to disable witness time isolation through
588  // a Simulator setting.
589 
590  // The scale factor for witness isolation.
591  // TODO(edrumwri): Consider making this user-settable.
592  const double iso_scale_factor = 1.0;
593 
594  // TODO(edrumwri): Acquire characteristic time properly from the system
595  // (i.e., modify the System to provide this value).
596  const double characteristic_time = 1.0;
597 
598  // Get the accuracy setting.
599  const optional<double>& accuracy = get_context().get_accuracy();
600 
601  // Determine the length of the isolation interval.
602  if (integrator_->get_fixed_step_mode()) {
603  // Look for accuracy information.
604  if (accuracy) {
605  return max(integrator_->get_working_minimum_step_size(),
606  T(iso_scale_factor * accuracy.value() *
607  integrator_->get_maximum_step_size()));
608  } else {
609  return optional<T>();
610  }
611  }
612 
613  // Integration with error control isolation window determination.
614  if (!accuracy) {
615  throw std::logic_error("Integrator is not operating in fixed step mode"
616  "and accuracy is not set in the context.");
617  }
618 
619  // Note: the max computation is used (here and above) because it is
620  // ineffectual to attempt to isolate intervals smaller than the current time
621  // in the context can allow.
622  return max(integrator_->get_working_minimum_step_size(),
623  iso_scale_factor * accuracy.value() * characteristic_time);
624 }
625 
626 // Isolates the first time at one or more witness functions triggered (in the
627 // interval [t0, tf]), to the requisite interval length.
628 // @param[in,out] on entry, the set of witness functions that triggered over
629 // [t0, tf]; on exit, the set of witness functions that triggered
630 // over [t0, tw], where tw is the first time that any witness
631 // function triggered.
632 // @pre The context and state are at tf and x(tf), respectively, and at least
633 // one witness function has triggered over [t0, tf].
634 // @post The context will be isolated to the first witness function trigger(s),
635 // to within the requisite interval length. It is guaranteed that all
636 // triggered witness functions change sign over [t0, tw].
637 // @note We assume that, if a witness function triggers over an interval
638 // [a, b], it also triggers over any larger interval [a, d], for d > b
639 // and d ≤ the maximum integrator step size (per WitnessFunction
640 // documentation, we assume that a witness function crosses zero at most
641 // once over an interval of size [t0, tf]).
642 template <class T>
644  const std::vector<const WitnessFunction<T>*>& witnesses,
645  const VectorX<T>& w0,
646  const T& t0, const VectorX<T>& x0, const T& tf,
647  std::vector<const WitnessFunction<T>*>* triggered_witnesses) {
648 
649  // Verify that the vector of triggered witnesses is non-null.
650  DRAKE_DEMAND(triggered_witnesses);
651 
652  // TODO(edrumwri): Speed this process using interpolation between states,
653  // more powerful root finding methods, and/or introducing the concept of
654  // a dead band.
655 
656  // Will need to alter the context repeatedly.
657  Context<T>* context = get_mutable_context();
658 
659  // Get the witness isolation interval length.
660  const optional<T> witness_iso_len = GetCurrentWitnessTimeIsolation();
661 
662  // Check whether witness functions *are* to be isolated. If not, the witnesses
663  // that were triggered on entry will be the set that is returned.
664  if (!witness_iso_len)
665  return;
666 
667  // Mini function for integrating the system forward in time from t0.
668  std::function<void(const T&)> integrate_forward =
669  [&t0, &x0, context, this](const T& t_des) {
670  const T inf = std::numeric_limits<double>::infinity();
671  context->set_time(t0);
672  context->get_mutable_continuous_state()->SetFromVector(x0);
673  T t_remaining = t_des - t0;
674  while (t_remaining > 0) {
675  integrator_->IntegrateAtMost(inf, inf, t_remaining);
676  t_remaining = t_des - context->get_time();
677  }
678  };
679 
680  // Loop until the isolation window is sufficiently small.
681  VectorX<T> wc(witnesses.size());
682  T a = t0;
683  T b = tf;
684  do {
685  // Compute the midpoint and evaluate the witness functions at it.
686  T c = (a + b) / 2;
687  integrate_forward(c);
688 
689  // See whether any witness functions trigger.
690  bool trigger = false;
691  for (size_t i = 0; i < witnesses.size(); ++i) {
692  wc[i] = get_system().EvaluateWitness(*context, *witnesses[i]);
693  if (witnesses[i]->should_trigger(w0[i], wc[i]))
694  trigger = true;
695  }
696 
697  // If no witness function triggered, we can continue integrating forward.
698  if (!trigger) {
699  // NOTE: Since we're always checking that the sign changes over [t0,c],
700  // it's also feasible to replace the two lines below with "a = c" without
701  // violating Simulator's contract to only integrate once over the interval
702  // [a, c], for some c <= b before per-step events are handled (i.e., it's
703  // unacceptable to take two steps of (c - a)/2 without processing per-step
704  // events first). That change would avoid handling unnecessary per-step
705  // events- we know no other events are to be handled between t0 and tf-
706  // but the current logic appears easier to follow.
707  triggered_witnesses->clear();
708  return;
709  } else {
710  b = c;
711  }
712  } while (b - a > witness_iso_len.value());
713 
714  // Determine the set of triggered witnesses.
715  triggered_witnesses->clear();
716  for (size_t i = 0; i < witnesses.size(); ++i) {
717  if (witnesses[i]->should_trigger(w0[i], wc[i]))
718  triggered_witnesses->push_back(witnesses[i]);
719  }
720 }
721 
722 // Integrates the continuous state forward in time while attempting to locate
723 // the first zero of any triggered witness functions.
724 // @returns `true` if integration terminated on a sample time, indicating that
725 // an event needs to be handled at the state/time on return.
726 template <class T>
727 bool Simulator<T>::IntegrateContinuousState(const T& next_publish_dt,
728  const T& next_update_dt,
729  const T& next_sample_time,
730  const T& boundary_dt,
731  CompositeEventCollection<T>* events) {
732  // Save the time and current state.
733  const Context<T>& context = get_context();
734  const T t0 = context.get_time();
735  const VectorX<T> x0 = context.get_continuous_state()->CopyToVector();
736 
737  // Get the set of witness functions active at the current time.
738  const System<T>& system = get_system();
739  std::vector<const WitnessFunction<T>*> witness_functions;
740  system.GetWitnessFunctions(context, &witness_functions);
741 
742  // Evaluate the witness functions.
743  w0_.resize(witness_functions.size());
744  for (size_t i = 0; i < witness_functions.size(); ++i)
745  w0_[i] = system.EvaluateWitness(context, *witness_functions[i]);
746 
747  // Attempt to integrate. Updates and boundary times are consciously
748  // distinguished between. See internal documentation for
749  // IntegratorBase::StepOnceAtMost() for more information.
751  integrator_->IntegrateAtMost(next_publish_dt, next_update_dt,
752  boundary_dt);
753  const T tf = context.get_time();
754 
755  // Evaluate the witness functions again.
756  wf_.resize(witness_functions.size());
757  for (size_t i =0; i < witness_functions.size(); ++i)
758  wf_[i] = system.EvaluateWitness(context, *witness_functions[i]);
759 
760  // See whether a witness function triggered.
761  triggered_witnesses_.clear();
762  bool witness_triggered = false;
763  for (size_t i =0; i < witness_functions.size() && !witness_triggered; ++i) {
764  if (witness_functions[i]->should_trigger(w0_[i], wf_[i])) {
765  witness_triggered = true;
766  triggered_witnesses_.push_back(witness_functions[i]);
767  }
768  }
769 
770  // Triggering requires isolating the witness function time.
771  if (witness_triggered) {
772  // Isolate the time that the witness function triggered.
773  IsolateWitnessTriggers(witness_functions, w0_, t0, x0, tf,
774  &triggered_witnesses_);
775 
776  // Store witness function(s) that triggered.
777  for (const WitnessFunction<T>* fn : triggered_witnesses_) {
778  SPDLOG_DEBUG(drake::log(), "Witness function {} crossed zero at time {}",
779  fn->get_name(), context.get_time());
781  *fn, events);
782  }
783 
784  // Indicate a "sample time was hit" if at least one witness function
785  // triggered (meaning that an event should be handled on the next simulation
786  // loop).
787  return !triggered_witnesses_.empty();
788  }
789 
790  // No witness function triggered; handle integration as usual.
791  // Updates and boundary times are consciously distinguished between. See
792  // internal documentation for IntegratorBase::StepOnceAtMost() for more
793  // information.
794  switch (result) {
797  // Next line sets the time to the exact sample time rather than
798  // introducing rounding error by summing the context time + dt.
799  context_->set_time(next_sample_time);
800  return true; // Sample time hit.
801  break;
802 
805  return false; // Did not hit a sample time.
806  break;
807 
808  default:DRAKE_ABORT_MSG("Unexpected integrator result.");
809  }
810  ++num_steps_taken_;
811 
812  // TODO(sherm1) Constraint projection goes here.
813 
814  // Should never get here.
815  DRAKE_ABORT();
816  return false;
817 }
818 
819 } // namespace systems
820 } // namespace drake
The DiscreteValues is a container for numerical but non-continuous state and parameters.
Definition: discrete_values.h:28
int64_t get_num_discrete_updates() const
Gets the number of discrete variable updates performed since the last Initialize() call...
Definition: simulator.h:233
U * reset_integrator(Args &&...args)
Resets the integrator with a new one.
Definition: simulator.h:258
const IntegratorBase< T > * get_integrator() const
Gets a pointer to the integrator used to advance the continuous aspects of the system.
Definition: simulator.h:242
#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:88
const System< T > & get_system() const
Gets a constant reference to the system.
Definition: simulator.h:296
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
const ContinuousState< T > * get_continuous_state() const
Returns a const pointer to the continuous component of the state, which may be of size zero...
Definition: context.h:118
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:326
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:204
#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
int b
Definition: rgbd_camera.cc:91
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:108
void Initialize()
Prepares the Simulator for a simulation.
Definition: simulator.h:415
StepResult
Status returned by StepOnceAtMost().
Definition: integrator_base.h:123
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:229
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:697
#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:189
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:1289
std::unique_ptr< Context< T > > release_context()
Transfer ownership of this Simulator&#39;s internal Context to the caller.
Definition: simulator.h:214
void CopyFrom(const State< T > &other)
Copies the values from another State of the same scalar type into this State.
Definition: state.h:93
int64_t get_num_unrestricted_updates() const
Gets the number of "unrestricted" updates performed since the last Initialize() call.
Definition: simulator.h:237
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:495
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:246
#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:226
This class represents a publish event.
Definition: event.h:194
An abstract class for an integrator for ODEs and DAEs as represented by a Drake System.
Definition: integrator_base.h:107
Context< T > * get_mutable_context()
Returns a mutable pointer to the internally-maintained Context holding the most recent step in the tr...
Definition: simulator.h:195
optional< T > GetCurrentWitnessTimeIsolation() const
Gets the length of the interval used for witness function time isolation.
Definition: simulator.h:584
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:1280
#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
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:257
Provides careful macros to selectively enable or disable the special member functions for copy-constr...