Drake
integrator_base.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <algorithm>
4 #include <limits>
5 #include <memory>
6 #include <utility>
7 
14 
15 namespace drake {
16 namespace systems {
17 
18 /*
19 TODO(edrumwri): comments below in preparation of upcoming functionality
20  Note: we ensure algorithmically that no report time, scheduled time, or
21 final time t can occur *within* an event window, that is, we will never have
22  t_low < t < t_high for any interesting t. Further, t_report, t_scheduled and
23  t_final can coincide with t_high but only t_report can be at t_low. The
24  interior of t_low:t_high is a "no man's land" where we don't understand the
25 solution, so must be avoided.
26 */
27 
28 /**
29  * An abstract class for an integrator for ODEs and DAEs as represented by a
30  * Drake System. Integrators solve initial value problems of the form:<pre>
31  * ẋ(t) = f(t, x(t)) with f : ℝ × ℝⁿ → ℝⁿ
32  * </pre>
33  * (i.e., `f()` is an ordinary differential equation) given initial conditions
34  * (t₀, x₀). Thus, integrators advance the continuous state of a dynamical
35  * system forward in time.
36  *
37  * Apart from solving initial value problems, for which the integrator is a
38  * key component of a simulator, integrators can also be used to solve
39  * boundary value problems (via numerical methods like the Multiple Shooting
40  * Method) and trajectory optimization problems (via numerical methods like
41  * direct transcription). This class and its derivatives were developed
42  * primarily toward the former application (through IntegrateAtMost() and
43  * the Simulator class). However, the IntegratorBase architecture was developed
44  * to support these ancillary applications as well using the
45  * IntegrateWithMultipleSteps() and IntegrateWithSingleFixedStep() methods;
46  * the latter permits the caller to advance time using fixed steps in
47  * applications where variable stepping would be deleterious (e.g., direct
48  * transcription).
49  *
50  * A natural question for a user to ask of an integrator is: Which scheme
51  * (method) should be applied to a particular problem? The answer is whichever
52  * one most quickly computes the solution to the desired accuracy! Selecting
53  * an integration scheme for a particular problem is presently an artform. As
54  * examples of some selection criteria: multistep methods (none of which are
55  * currently implemented in Drake) generally work poorly when events (that
56  * require state reinitializations) are common, symplectic methods generally
57  * work well at maintaining stability for large integration steps, and stiff
58  * integrators are often best for computationally stiff systems. If ignorant
59  * as to the characteristics of a particular problem, it is often best to start
60  * with an explicit, Runge-Kutta type method. Statistics collected by the
61  * integrator can help diagnose performance issues and possibly point to the
62  * use of a different integration scheme.
63  *
64  * Some systems are known to exhibit "computational stiffness", by which it is
65  * meant that (excessively) small integration steps are necessary for purposes
66  * of stability: in other words, steps must be taken smaller than that
67  * required to achieve a desired accuracy *over a particular interval*.
68  * Thus, the nature of computationally stiff problems is that the solution to
69  * the ODE is *smooth* in the interval of stiffness (in contrast, some problems
70  * possess such high frequency dynamics that very small steps are simply
71  * necessary to capture the solution accurately). Implicit integrators are the
72  * go-to approach for solving computationally stiff problems, but careful
73  * consideration is warranted. Implicit integrators typically require much more
74  * computation than non-implicit (explicit) integrators, stiffness might be an
75  * issue on only a very small time interval, and some problems might be only
76  * "moderately stiff". Put another way, applying an implicit integrator to a
77  * potentially stiff problem might not yield faster computation. The first
78  * chapter of [Hairer, 1996] illustrates the issues broached in this paragraph
79  * using various examples.
80  *
81  * Established methods for integrating ordinary differential equations
82  * invariably make provisions for estimating the "local error" (i.e., the
83  * error over a small time interval) of a solution. Although the relationship
84  * between local error and global error (i.e., the accumulated error over
85  * multiple time steps) can be tenuous, such error estimates can allow
86  * integrators to work adaptively, subdividing time intervals as necessary
87  * (if, e.g., the system is particularly dynamic or stationary in an interval).
88  * Even for applications that do not recommend such adaptive integration- like
89  * direct transcription methods for trajectory optimization- error estimation
90  * allows the user to assess the accuracy of the solution.
91  *
92  * IntegratorBase provides numerous settings and flags that can leverage
93  * problem-specific information to speed integration and/or improve integration
94  * accuracy. As an example, set_maximum_step_size() allows the user to prevent
95  * overly large integration steps (that integration error control alone might
96  * be insufficient to detect). As noted previously, IntegratorBase also collects
97  * a plethora of statistics that can be used to diagnose poor integration
98  * performance. For example, a large number of shrinkages due to error control
99  * could indicate that a system is computationally stiff.
100  *
101  * - [Hairer, 1996] E. Hairer and G. Wanner. Solving Ordinary Differential
102  * Equations II (Stiff and Differential-Algebraic Problems).
103  * Springer, 1996.
104 
105  * @tparam T The vector element type, which must be a valid Eigen scalar.
106  */
107 template <class T>
109  public:
111 
112  /**
113  * Status returned by StepOnceAtMost().
114  * When a step is successful, it will return an indication of what caused it
115  * to stop where it did. When unsuccessful it will throw an exception so you
116  * won't see any return value. When return of control is due ONLY to reaching
117  * a publish time, (status is kReachedPublishTime) the context may return an
118  * interpolated value at an earlier time.
119  *
120  * Note: the simulation step must always end at an update time but can end
121  * after a publish time.
122  */
123  // TODO(edrumwri): incorporate kReachedZeroCrossing into the simulator.
124  enum StepResult {
125  /**
126  * Indicates a publish time has been reached but not an update time.
127  */
129  /**
130  * Localized an event; this is the *before* state (interpolated).
131  */
133  /**
134  * Indicates that integration terminated at an update time.
135  */
137  /**
138  * User requested control whenever an internal step is successful.
139  */
141  /**
142  * Reached the desired integration time without reaching an update time.
143  */
145  /** Took maximum number of steps without finishing integrating over the
146  * interval. */
148  };
149 
150  /**
151  * Maintains references to the system being integrated and the context used
152  * to specify the initial conditions for that system (if any).
153  * @param system A reference to the system to be integrated; the integrator
154  * will maintain a reference to the system in perpetuity, so
155  * the integrator must not outlive the system.
156  * @param context A pointer to a writeable context (nullptr is ok, but a
157  * non-null pointer must be set before Initialize() is
158  * called). The integrator will advance the system state using
159  * the pointer to this context. The pointer to the context will
160  * be maintained internally. The integrator must not outlive
161  * the context.
162  */
163  explicit IntegratorBase(const System<T>& system,
164  Context<T>* context = nullptr)
165  : system_(system), context_(context) {
166  initialization_done_ = false;
167  }
168 
169  /** Destructor. */
170  virtual ~IntegratorBase() = default;
171 
172  /**
173  * Indicates whether an integrator supports error estimation.
174  * Without error estimation, target accuracy will be unused.
175  */
176  virtual bool supports_error_estimation() const = 0;
177 
178  /**
179  * Sets an integrator with error control to fixed step mode. If the integrator
180  * runs in fixed step mode, it will always take the maximum step size
181  * directed (which may be that determined by get_maximum_step_size(), or may
182  * be smaller, as directed by, e.g., Simulator for event handling purposes).
183  * @throws std::logic_error if integrator does not support error
184  * estimation and @p flag is set to `false`.
185  */
186  void set_fixed_step_mode(bool flag) {
187  if (!flag && !supports_error_estimation())
188  throw std::logic_error("Integrator does not support accuracy estimation");
189  fixed_step_mode_ = flag;
190  }
191 
192  /**
193  * Gets whether an integrator is running in fixed step mode. If the integrator
194  * does not support error estimation, this function will always return `true`.
195  * If the integrator runs in fixed step mode, it will always take the maximum
196  * step size directed (which may be that determined by get_maximum_step_size()
197  * or may be smaller, as directed by, e.g., Simulator for event handling
198  * purposes).
199  * @sa set_fixed_step_mode()
200  */
201  bool get_fixed_step_mode() const {
202  return (!supports_error_estimation() || fixed_step_mode_);
203  }
204 
205  /** Request that the integrator attempt to achieve a particular accuracy for
206  * the continuous portions of the simulation. Otherwise a default accuracy is
207  * chosen for you. This may be ignored for fixed-step integration since
208  * accuracy control requires variable step sizes. You should call
209  * supports_error_estimation() to ensure that the
210  * integrator supports this capability before calling this function; if
211  * the integrator does not support it, this method will throw an exception.
212  *
213  * Integrators vary in the range of accuracy (loosest to tightest) that they
214  * can support. If you request accuracy outside the supported range for the
215  * chosen integrator it will be quietly adjusted to be in range. You can find
216  * out the accuracy setting actually being used using `get_accuracy_in_use()`.
217  *
218  * The precise meaning of *accuracy* is a complicated discussion, but
219  * translates roughly to the number of significant digits you want in the
220  * results. By convention it is supplied as `10^-digits`, meaning that an
221  * accuracy of 1e-3 provides about three significant digits. For more
222  * information, see [Sherman 2011].
223  * - M. Sherman, et al. Procedia IUTAM 2:241-261 (2011), Section 3.3.
224  * http://dx.doi.org/10.1016/j.piutam.2011.04.023
225  * @throws std::logic_error if integrator does not support error
226  * estimation.
227  */
228  // TODO(edrumwri): complain if integrator with error estimation wants to drop
229  // below the minimum step size
230  void set_target_accuracy(double accuracy) {
232  throw std::logic_error(
233  "Integrator does not support accuracy estimation "
234  "and user has requested error control");
235  target_accuracy_ = accuracy;
236  accuracy_in_use_ = accuracy;
237  }
238 
239  /**
240  * Gets the target accuracy.
241  * @sa get_accuracy_in_use()
242  */
243  double get_target_accuracy() const { return target_accuracy_; }
244 
245  /**
246  * Gets the accuracy in use by the integrator. This number may differ from
247  * the target accuracy if, for example, the user has requested an accuracy
248  * not attainable or not recommended for the particular integrator.
249  */
250  double get_accuracy_in_use() const { return accuracy_in_use_; }
251 
252  /**
253  * Sets the maximum step size that may be taken by this integrator. The
254  * integrator may stretch the maximum step size by as much as 1% to reach a
255  * discrete event. For fixed step integrators, all steps will be taken at the
256  * maximum step size *unless* an event would be missed.
257  */
258  // TODO(edrumwri): Update this comment when stretch size is configurable.
259  void set_maximum_step_size(const T& max_step_size) {
260  DRAKE_ASSERT(max_step_size >= 0.0);
261  max_step_size_ = max_step_size;
262  }
263 
264  /**
265  * Gets the maximum step size that may be taken by this integrator. This is
266  * a soft maximum: the integrator may stretch it by as much as 1% to hit a
267  * discrete event.
268  * @sa set_requested_minimum_step_size()
269  */
270  // TODO(edrumwri): Update this comment when stretch size is configurable.
271  const T& get_maximum_step_size() const { return max_step_size_; }
272 
273  /**
274  * @anchor Minstep
275  * @name Methods for minimum integration step size selection and behavior
276  *
277  * Variable step integrators reduce their step sizes as needed to achieve
278  * requirements such as specified accuracy or step convergence. However, it is
279  * not possible to take an arbitrarily small step. Normally integrators choose
280  * an appropriate minimum step and throw an exception if the requirements
281  * can't be achieved without going below that. Methods in this section allow
282  * you to influence two aspects of this procedure:
283  * - you can increase the minimum step size, and
284  * - you can control whether an exception is thrown if a smaller step would
285  * have been needed to achieve the aforementioned integrator requirements.
286  *
287  * By default, integrators allow a very small minimum step which can
288  * result in long run times. Setting a larger minimum can be helpful as a
289  * diagnostic to figure out what aspect of your simulation is requiring small
290  * steps. You can set the minimum to what should be a "reasonable" minimum
291  * based on what you know about the physical system. You will then get an
292  * std::runtime_error exception thrown at any point in time where your model
293  * behaves unexpectedly (due to, e.g., a discontinuity in the derivative
294  * evaluation function).
295  *
296  * If you disable the exception (via
297  * `set_throw_on_minimum_step_size_violation(false)`), the integrator will
298  * simply proceed with a step of the minimum size: accuracy is guaranteed
299  * only when the minimum step size is not violated. Beware that there can be
300  * no guarantee about the magnitude of any errors introduced by violating the
301  * accuracy "requirements" in this manner, so disabling the exception should
302  * be done warily.
303  *
304  * #### Details
305  * Because time is maintained to finite precision, there is an absolute
306  * minimum step size `h_floor` required to avoid roundoff error. The
307  * integrator will never take a step smaller than `h_floor`. We calculate
308  * `h_floor=max(ε,ε⋅t)`, where t is the current time and ε is a small multiple
309  * of machine precision, typically a number like 1e-14. Note that `h_floor`
310  * necessarily grows with time; if that is a concern you should limit how
311  * long your simulations are allowed to run without resetting time.
312  *
313  * You may request a larger minimum step size `h_min`. Then at every time t,
314  * the integrator determines a "working" minimum `h_work=max(h_min,h_floor)`.
315  * If the step size selection algorithm determines that a step smaller than
316  * `h_work` is needed to meet accuracy or other needs, then a
317  * std::runtime_error exception will be thrown and the simulation halted. On
318  * the other hand, if you have suppressed the exception (again, via
319  * `set_throw_on_minimum_step_size_violation(false)`), the integration
320  * will continue, taking a step of size `h_work`.
321  *
322  * Under some circumstances the integrator may legitimately take a step of
323  * size `h` smaller than your specified `h_min`, although never smaller than
324  * `h_floor`. For example, occasionally the integrator may reach an event or
325  * time limit that occurs a very short time after the end of a previous step,
326  * necessitating that a tiny "sliver" of a step be taken to complete the
327  * interval. That does not indicate an error, and required accuracy and
328  * convergence goals are achieved. Larger steps can resume immediately
329  * afterwards. Another circumstance is when one of the integrator's stepping
330  * methods is called directly requesting a very small step, for example
331  * `IntegrateWithMultipleSteps(h)`. No exception will be thrown in either of
332  * these cases.
333  */
334 
335  //@{
336  /**
337  * Sets the requested minimum step size `h_min` that may be taken by this
338  * integrator. No step smaller than this will be taken except under
339  * circumstances as described @link Minstep above. @endlink This setting will
340  * be ignored if it is smaller than the absolute minimum `h_floor` also
341  * described above. Default value is zero.
342  * @param min_step_size a non-negative value. Setting this value to zero
343  * will cause the integrator to use a reasonable value
344  * instead (see get_working_minimum_step_size()).
345  * @sa get_requested_minimum_step_size()
346  * @sa get_working_minimum_step_size()
347  */
348  void set_requested_minimum_step_size(const T& min_step_size) {
349  DRAKE_ASSERT(min_step_size >= 0.0);
350  req_min_step_size_ = min_step_size;
351  }
352 
353  /**
354  * Gets the requested minimum step size `h_min` for this integrator.
355  * @sa set_requested_minimum_step_size()
356  * @sa get_working_minimum_step_size(T)
357  */
359  return req_min_step_size_; }
360 
361  /**
362  * Sets whether the integrator should throw a std::runtime_error exception
363  * when the integrator's step size selection algorithm determines that it
364  * must take a step smaller than the minimum step size (for, e.g., purposes
365  * of error control). Default is `true`. If `false`, the integrator will
366  * advance time and state using the minimum specified step size in such
367  * situations. See @link Minstep this section @endlink for more detail.
368  */
370  min_step_exceeded_throws_ = throws;
371  }
372 
373  /**
374  * Reports the current setting of the throw_on_minimum_step_size_violation
375  * flag.
376  * @sa set_throw_on_minimum_step_size_violation().
377  */
379  return min_step_exceeded_throws_;
380  }
381 
382  /**
383  * Gets the current value of the working minimum step size `h_work(t)` for
384  * this integrator, which may vary with the current time t as stored in the
385  * integrator's context.
386  * See @link Minstep this section @endlink for more detail.
387  */
389  using std::max;
390  // Tolerance is just a number close to machine epsilon.
391  const double tol = 1e-14;
392  const T smart_minimum = max(tol, get_context().get_time()*tol);
393  return max(smart_minimum, req_min_step_size_);
394  }
395  //@}
396 
397  /**
398  * Resets the integrator to initial values, i.e., default construction
399  * values.
400  */
401  void Reset() {
402  // Kill the error estimate and weighting matrices.
403  err_est_.reset();
404  qbar_weight_.setZero(0);
405  z_weight_.setZero(0);
406  pinvN_dq_change_.reset();
407  unweighted_substate_change_.setZero(0);
408  weighted_q_change_.reset();
409 
410  // Integrator no longer operates in fixed step mode.
411  fixed_step_mode_ = false;
412 
413  // Statistics no longer valid.
414  ResetStatistics();
415 
416  // Wipe out settings.
417  req_min_step_size_ = 0;
418  max_step_size_ = nan();
419  accuracy_in_use_ = nan();
420 
421  // Indicate values used for error controlled integration no longer valid.
422  prev_step_size_ = nan();
423  ideal_next_step_size_ = nan();
424 
425  // Call the derived integrator reset routine.
426  DoReset();
427 
428  // Indicate that initialization is necessary.
429  initialization_done_ = false;
430  }
431 
432  /**
433  * An integrator must be initialized before being used. The pointer to the
434  * context must be set before Initialize() is called (or an std::logic_error
435  * will be thrown). If Initialize() is not called, an exception will be
436  * thrown when attempting to call StepOnceAtMost(). To reinitialize the
437  * integrator, Reset() should be called followed by Initialize().
438  * @throws std::logic_error If the context has not been set or a user-set
439  * parameter has been set illogically (i.e., one of the
440  * weighting matrix coefficients is set to a negative value- this
441  * check is only performed for integrators that support error
442  * estimation; the maximum step size is smaller than the minimum
443  * step size; the requested initial step size is outside of the
444  * interval [minimum step size, maximum step size]).
445  * @sa Reset()
446  */
447  void Initialize() {
448  if (!context_) throw std::logic_error("Context has not been set.");
449 
450  // Verify that user settings are reasonable.
451  if (max_step_size_ < req_min_step_size_) {
452  throw std::logic_error("Integrator maximum step size is less than the "
453  "minimum step size");
454  }
455  if (req_initial_step_size_ > max_step_size_) {
456  throw std::logic_error("Requested integrator initial step size is larger "
457  "than the maximum step size.");
458  }
459  if (req_initial_step_size_ < req_min_step_size_) {
460  throw std::logic_error("Requested integrator initial step size is smaller"
461  " than the minimum step size.");
462  }
463 
464  // TODO(edrumwri): Compute qbar_weight_, z_weight_ automatically.
465  // Set error weighting vectors if not already done.
467  // Allocate space for the error estimate.
468  err_est_ = system_.AllocateTimeDerivatives();
469 
470  const auto& xc = context_->get_state().get_continuous_state();
471  const int gv_size = xc.get_generalized_velocity().size();
472  const int misc_size = xc.get_misc_continuous_state().size();
473  if (qbar_weight_.size() != gv_size) qbar_weight_.setOnes(gv_size);
474  if (z_weight_.size() != misc_size) z_weight_.setOnes(misc_size);
475 
476  // Verify that minimum values of the weighting matrices are non-negative.
477  if ((qbar_weight_.size() && qbar_weight_.minCoeff() < 0) ||
478  (z_weight_.size() && z_weight_.minCoeff() < 0))
479  throw std::logic_error("Scaling coefficient is less than zero.");
480  }
481 
482  // Statistics no longer valid.
483  ResetStatistics();
484 
485  // Call the derived integrator initialization routine (if any)
486  DoInitialize();
487 
488  initialization_done_ = true;
489  }
490 
491  /**
492  * Request that the first attempted integration step have a particular size.
493  * If no request is made, the integrator will estimate a suitable size
494  * for the initial step attempt. *If the integrator does not support error
495  * control*, this method will throw a std::logic_error (call
496  * supports_error_estimation() to verify before calling this method). For
497  * variable-step integration, the initial target will be treated as a maximum
498  * step size subject to accuracy requirements and event occurrences. You can
499  * find out what size *actually* worked with
500  * `get_actual_initial_step_size_taken()`.
501  * @throws std::logic_error If the integrator does not support error
502  * estimation.
503  */
504  void request_initial_step_size_target(const T& step_size) {
505  using std::isnan;
507  throw std::logic_error(
508  "Integrator does not support error estimation and "
509  "user has initial step size target");
510  req_initial_step_size_ = step_size;
511  }
512 
513  /**
514  * Gets the target size of the first integration step. You can find out what
515  * step size was *actually* used for the first integration step with
516  * `get_actual_initial_step_size_taken()`.
517  * @see request_initial_step_size_target()
518  */
519  const T& get_initial_step_size_target() const {
520  return req_initial_step_size_;
521  }
522 
523  /**
524  * Integrates the system forward in time by a single step with step size
525  * subject to integration error tolerances (assuming that the integrator
526  * supports error estimation). The integrator must already have
527  * been initialized or an exception will be thrown. The context will be
528  * integrated forward by an amount that will never exceed the minimum of
529  * `publish_dt`, `update_dt`, and `1.01 * get_maximum_step_size()`.
530  *
531  * @param publish_dt The step size, >= 0.0 (exception will be thrown
532  * if this is not the case) at which the next publish will occur.
533  * @param update_dt The step size, >= 0.0 (exception will be thrown
534  * if this is not the case) at which the next update will occur.
535  * @param boundary_dt The step size, >= 0.0 (exception will be thrown
536  * if this is not the case) marking the end of the user-designated
537  * simulated interval.
538  * @throws std::logic_error If the integrator has not been initialized or one
539  * of publish_dt, update_dt, or boundary_dt is
540  * negative.
541  * @return The reason for the integration step ending.
542  * @warning Users should generally not call this function directly; within
543  * simulation circumstances, users will typically call
544  * `Simulator::StepTo()`. In other circumstances, users will
545  * typically call `IntegratorBase::IntegrateWithMultipleSteps()`.
546  *
547  * This method at a glance:
548  * - For integrating ODEs/DAEs via Simulator
549  * - Supports fixed step and variable step integration schemes
550  * - Takes only a single step forward.
551  */
552  // TODO(edrumwri): Make the stretch size configurable.
553  StepResult IntegrateAtMost(const T& publish_dt, const T& update_dt,
554  const T& boundary_dt);
555 
556  /// Gets the stretch factor (> 1), which is multiplied by the maximum
557  /// (typically user-designated) integration step size to obtain the amount
558  /// that the integrator is able to stretch the maximum time step toward
559  /// hitting an upcoming publish or update event in IntegrateAtMost().
560  /// @sa IntegrateAtMost()
561  double get_stretch_factor() const { return 1.01; }
562 
563  /// Stepping function for integrators operating outside of Simulator that
564  /// advances the continuous state exactly by @p dt. This method is designed
565  /// for integrator users that do not wish to consider publishing or
566  /// discontinuous, mid-interval updates. This method will step the integrator
567  /// multiple times, as necessary, to attain requested error tolerances and
568  /// to ensure the integrator converges.
569  /// @warning Users should simulate systems using `Simulator::StepTo()` in
570  /// place of this function (which was created for off-simulation
571  /// purposes), generally.
572  /// @param dt The non-negative integration step to take.
573  /// @throws std::logic_error If the integrator has not been initialized or
574  /// dt is negative.
575  /// @sa IntegrateAtMost(), which is designed to be operated by Simulator and
576  /// accounts for publishing and state reinitialization.
577  /// @sa IntegrateWithSingleStep(), which is also designed to be operated
578  /// *outside of* Simulator, but throws an exception if the integrator
579  /// cannot advance time by @p dt in a single step.
580  ///
581  /// This method at a glance:
582  /// - For integrating ODEs/DAEs not using Simulator
583  /// - Supports fixed step and variable step integration schemes
584  /// - Takes as many steps as necessary until time has advanced by @p dt
585  void IntegrateWithMultipleSteps(const T& dt) {
586  using std::max;
587  using std::min;
588 
589  const Context<T>& context = get_context();
590  const T inf = std::numeric_limits<double>::infinity();
591  T t_remaining = dt;
592 
593  // Note: A concern below is that the while loop while run forever because
594  // t_remaining could be small, but not quite zero, if dt is relatively
595  // small compared to the context time. In such a case, t_final will be
596  // equal to context.get_time() in the expression immediately below,
597  // context.get_time() will not change during the call to IntegrateAtMost(),
598  // and t_remaining will be equal to zero (meaning that the loop will
599  // indeed terminate, as desired).
600  const T t_final = context.get_time() + t_remaining;
601  do {
602  IntegrateAtMost(inf, inf, min(t_remaining, get_maximum_step_size()));
603  t_remaining = t_final - context.get_time();
604  } while (t_remaining > 0);
605  }
606 
607  /// Stepping function for integrators operating outside of Simulator that
608  /// advances the continuous state exactly by @p dt *and using a single fixed
609  /// step*. This method is designed for integrator users that do not wish to
610  /// consider publishing or discontinuous, mid-interval updates. One such
611  /// example application is that of direct transcription for trajectory
612  /// optimization, for which the integration process should be _consistent_: it
613  /// should execute the same sequence of arithmetic operations for all values
614  /// of the nonlinear programming variables. In keeping with the naming
615  /// semantics of this function, error controlled integration is not supported
616  /// (though error estimates will be computed for integrators that support that
617  /// feature), which is a minimal requirement for "consistency".
618  /// @warning Users should simulate systems using `Simulator::StepTo()` in
619  /// place of this function (which was created for off-simulation
620  /// purposes), generally.
621  /// @param dt The non-negative integration step to take.
622  /// @throws std::logic_error If the integrator has not been initialized or
623  /// dt is negative **or** if the integrator
624  /// is not operating in fixed step mode.
625  /// @throws std::runtime_error If the integrator was unable to take a step
626  /// of the requested size.
627  /// @sa IntegrateAtMost(), which is designed to be operated by Simulator and
628  /// accounts for publishing and state reinitialization.
629  /// @sa IntegrateWithMultipleSteps(), which is also designed to be operated
630  /// *outside of* Simulator, but will take as many integration steps as
631  /// necessary until time has been stepped forward by @p dt.
632  ///
633  /// This method at a glance:
634  /// - For integrating ODEs/DAEs not using Simulator
635  /// - Fixed step integration (no step size reductions for error control or
636  /// integrator convergence)
637  /// - Takes only a single step forward.
638  void IntegrateWithSingleFixedStep(const T& dt) {
639  if (dt < 0) {
640  throw std::logic_error("IntegrateWithSingleFixedStep() called with a "
641  "negative step size.");
642  }
643  if (!this->get_fixed_step_mode())
644  throw std::logic_error("IntegrateWithSingleFixedStep() requires fixed "
645  "stepping.");
646  if (!Step(dt)) {
647  throw std::runtime_error("Integrator was unable to take a single fixed "
648  "step of the requested size.");
649  }
650 
651  UpdateStepStatistics(dt);
652  }
653 
654  /**
655  * @name Integrator statistics methods.
656  *
657  * @{
658  * These methods allow the caller to manipulate and query integrator
659  * statistics. Generally speaking, the larger the integration step taken, the
660  * faster a simulation will run. These methods allow querying (and resetting)
661  * the integrator statistics as one means of determining how to make
662  * a simulation run faster.
663  */
664  /**
665  * Forget accumulated statistics. These are reset to the values they have
666  * post construction or immediately after `Initialize()`.
667  */
669  actual_initial_step_size_taken_ = nan();
670  smallest_adapted_step_size_taken_ = nan();
671  largest_step_size_taken_ = nan();
672  num_steps_taken_ = 0;
673  num_ode_evals_ = 0;
674  num_shrinkages_from_error_control_ = 0;
675  num_shrinkages_from_substep_failures_ = 0;
676  num_substep_failures_ = 0;
678  }
679 
680  /// Gets the number of failed sub-steps (implying one or more step size
681  /// reductions was required to permit solving the necessary nonlinear system
682  /// of equations).
683  int64_t get_num_substep_failures() const {
684  return num_substep_failures_;
685  }
686 
687  /// Gets the number of step size shrinkages due to sub-step failures (e.g.,
688  /// integrator convergence failures) since the last call to ResetStatistics()
689  /// or Initialize().
691  return num_shrinkages_from_substep_failures_;
692  }
693 
694  /// Gets the number of step size shrinkages due to failure to meet targeted
695  /// error tolerances, since the last call to ResetStatistics or Initialize().
697  return num_shrinkages_from_error_control_;
698  }
699 
700 /**
701  * Returns the number of ODE function evaluations (calls to
702  * CalcTimeDerivatives()) since the last call to ResetStatistics() or
703  * Initialize(). This count includes *all* such calls including (1)
704  * those necessary to compute Jacobian matrices; (2) those used in rejected
705  * integrated steps (for, e.g., purposes of error control); (3) those used
706  * strictly for integrator error estimation; and (4) calls that exhibit little
707  * cost (due to results being cached).
708  */
709  int64_t get_num_derivative_evaluations() const { return num_ode_evals_; }
710 
711  /**
712  * The actual size of the successful first step.
713  */
715  return actual_initial_step_size_taken_;
716  }
717 
718  /**
719  * The size of the smallest step taken *as the result of a controlled
720  * integration step adjustment* since the last Initialize() or
721  * ResetStatistics() call. This value will be NaN for integrators without
722  * error estimation.
723  */
725  return smallest_adapted_step_size_taken_;
726  }
727 
728  /**
729  * The size of the largest step taken since the last Initialize() or
730  * ResetStatistics() call.
731  */
732  const T& get_largest_step_size_taken() const {
733  return largest_step_size_taken_;
734  }
735 
736  /**
737  * The number of integration steps taken since the last Initialize()
738  * or ResetStatistics() call.
739  */
740  int64_t get_num_steps_taken() const { return num_steps_taken_; }
741 
742  /**
743  * @}
744  */
745 
746  /**
747  * Return the step size the integrator would like to take next, based
748  * primarily on the integrator's accuracy prediction. This value will not
749  * be computed for integrators that do not support error estimation and
750  * NaN will be returned.
751  */
752  const T& get_ideal_next_step_size() const { return ideal_next_step_size_; }
753 
754  /**
755  * Returns a const reference to the internally-maintained Context holding
756  * the most recent state in the trajectory. This is suitable for publishing or
757  * extracting information about this trajectory step.
758  */
759  const Context<T>& get_context() const { return *context_; }
760 
761  /**
762  * Returns a mutable pointer to the internally-maintained Context holding
763  * the most recent state in the trajectory.
764  */
765  Context<T>* get_mutable_context() { return context_; }
766 
767  /**
768  * Replace the pointer to the internally-maintained Context with a different
769  * one. This is useful for supplying a new set of initial conditions or
770  * wiping out the current context (by passing in a null pointer). You
771  * should invoke Initialize() after replacing the Context unless the
772  * context is null.
773  * @param context The pointer to the new context or nullptr to wipe out
774  * the current context without replacing it with another.
775  */
776  void reset_context(Context<T>* context) {
777  context_ = context;
778  initialization_done_ = false;
779  }
780 
781  /**
782  * Gets a constant reference to the system that is being integrated (and
783  * was provided to the constructor of the integrator).
784  */
785  const System<T>& get_system() const { return system_; }
786 
787  /// Indicates whether the integrator has been initialized.
788  bool is_initialized() const { return initialization_done_; }
789 
790  /**
791  * Derived classes must override this function to return the order of
792  * the integrator's error estimate. If the integrator does not provide an
793  * error estimate, the derived class implementation should return 0.
794  */
795  virtual int get_error_estimate_order() const = 0;
796 
797  /**
798  * Gets the size of the last (previous) integration step. If no integration
799  * steps have been taken, value will be NaN.
800  */
802  return prev_step_size_;
803  }
804 
805  /**
806  * Gets the error estimate (used only for integrators that support error
807  * estimation). If the integrator does not support error estimation, nullptr
808  * is returned.
809  */
811  return err_est_.get();
812  }
813 
814  /**
815  * @name Methods for weighting state variable errors
816  * @{
817  * This group of methods describes how errors for state variables with
818  * heterogeneous units are weighted in the context of error-controlled
819  * integration. This is an advanced topic and most users can simply specify
820  * desired accuracy and accept the default state variable weights.
821  *
822  * A collection of state variables is generally defined in heterogenous units
823  * (e.g. length, angles, velocities, energy). Some of the state
824  * variables cannot even be expressed in meaningful units, like
825  * quaternions. Certain integrators provide an estimate of the absolute error
826  * made in each state variable during an integration step. These errors must
827  * be properly weighted to obtain an "accuracy" _with respect to each
828  * particular variable_. These per-variable accuracy determinations can be
829  * compared against the user's requirements and used to select an appropriate
830  * size for the next step [Sherman 2011]. The weights are
831  * normally determined automatically using the system's characteristic
832  * dimensions, so *most users can stop reading now!* Custom weighting is
833  * primarily useful for performance improvement; an optimal weighting would
834  * allow an error-controlled integrator to provide the desired level of
835  * accuracy across all state variables without wasting computation
836  * achieving superfluous accuracy for some of those variables.
837  *
838  * Users interested in more precise control over state variable weighting may
839  * use the methods in this group to access and modify weighting factors for
840  * individual state variables. Changes to these weights can only be made prior
841  * to integrator initialization or as a result of an event being triggered
842  * and then followed by re-initialization.
843  *
844  * <h4>Relative versus absolute accuracy</h4>
845  *
846  * %State variable integration error, as estimated by an integrator, is an
847  * absolute quantity with the same
848  * units as the variable. At each time step we therefore need to determine
849  * an absolute error that would be deemed "good enough", i.e. satisfies
850  * the user's accuracy requirement. If a variable is maintained to a
851  * *relative* accuracy then that "good enough" value is defined to be the
852  * required accuracy `a` (a fraction like 0.001) times the current value of
853  * the variable, as long as that value
854  * is far from zero. For variables maintained to an *absolute* accuracy, or
855  * relative variables that are at or near zero (where relative accuracy would
856  * be undefined or too strict, respectively), we need a different way to
857  * determine the "good enough" absolute error. The methods in this section
858  * control how that absolute error value is calculated.
859  *
860  * <h4>How to choose weights</h4>
861  *
862  * The weight `wᵢ` for a state variable `xᵢ` should be
863  * chosen so that the product `wᵢ * dxᵢ` is unitless, and in particular is 1
864  * when `dxᵢ` represents a "unit effect" of state variable `xᵢ`; that is, the
865  * change in `xᵢ` that produces a unit change in some quantity of interest in
866  * the system being simulated. Why unity (1)? Aside from normalizing the
867  * values, unity "grounds" the weighted error to the user-specified accuracy.
868  * A weighting can be applied individually to each state variable, but
869  * typically it is done approximately by combining the known type of the
870  * variable (e.g. length, angle) with a "characteristic scale" for that
871  * quantity. For example, if a "characteristic length" for the system being
872  * simulated is 0.1 meters, and `x₀` is a length variable measured in meters,
873  * then `w₀` should be 10 so that `w₀*dx₀=1` when `dx₀=0.1`. For angles
874  * representing pointing accuracy (say a camera direction) we typically assume
875  * a "characteristic angle" is one radian (about 60 degrees), so if x₁ is a
876  * pointing direction then w₁=1 is an appropriate weight. We can now scale an
877  * error vector `e=[dx₀ dx₁]` to a unitless fractional error vector
878  * `f=[w₀*dx₀ w₁*dx₁]`. Now to achieve a given accuracy `a`, say `a=.0001`,
879  * we need only check that `|fᵢ|<=a` for each element `i` of `f`. Further,
880  * this gives us a quantitative measure of "worst accuracy" that we can use
881  * to increase or reduce size of the next attempted step, so that we will just
882  * achieve the required accuracy but not much more. We'll be more precise
883  * about this below.
884  *
885  * <h4>Some subtleties for second-order dynamic systems</h4>
886  *
887  * Systems governed by 2nd-order differential equations are typically split
888  * into second order (configuration) variables q, and rate (velocity)
889  * variables v, where the time derivatives qdot of q are linearly related to
890  * v by the kinematic differential equation `qdot = dq/dt = N(q)*v`.
891  * Velocity variables are
892  * chosen to be physically significant, but configuration variables
893  * may be chosen for convenience and do not necessarily have direct physical
894  * interpretation. For examples, quaternions are chosen as a numerically
895  * stable orientation representation. This is problematic for choosing weights
896  * which must be done by physical reasoning
897  * as sketched above. We resolve this by introducing
898  * the notion of "quasi-coordinates" ꝗ (pronounced "qbar") which are defined
899  * by the equation `ꝗdot = dꝗ/dt = v`. Other than time scaling,
900  * quasi-coordinates have the same units as their corresponding velocity
901  * variables. That is, for weighting we need to think
902  * of the configuration coordinates in the same physical space as the velocity
903  * variables; weight those by their physical significance; and then map back
904  * to an instantaneous weighting
905  * on the actual configuration variables q. This mapping is performed
906  * automatically; you need only to be concerned about physical weightings.
907  *
908  * Note that generalized quasi-coordinates `ꝗ` can only be defined locally for
909  * a particular configuration `q`. There is in general no meaningful set of
910  * `n` generalized
911  * coordinates which can be differentiated with respect to time to yield `v`.
912  * For example, the Hairy Ball Theorem implies that it is not possible for
913  * three orientation variables to represent all 3D rotations without
914  * singularities, yet three velocity variables can represent angular velocity
915  * in 3D without singularities.
916  *
917  * To summarize, separate weights can be provided for each of
918  * - `n` generalized quasi-coordinates `ꝗ` (configuration variables in the
919  * velocity variable space), and
920  * - `nz` miscellaneous continuous state variables `z`.
921  *
922  * Weights on the generalized velocity variables `v (= dꝗ/dt)` are derived
923  * directly from the weights on `ꝗ`, weighted by a characteristic time.
924  * Weights on the actual `nq` generalized coordinates can
925  * be calculated efficiently from weights on the quasi-coordinates (details
926  * below).
927  *
928  * <h4>How the weights are used</h4>
929  *
930  * The errors in the `ꝗ` and `z` variables are weighted by the diagonal
931  * elements
932  * of diagonal weighting matrices Wꝗ and Wz, respectively. (The block-diagonal
933  * weighting matrix `Wq` on the original generalized coordinates `q` is
934  * calculated from `N` and `Wꝗ`; see below.) In the absence of
935  * other information, the default for all weighting values is one, so `Wꝗ` and
936  * `Wz` are `n × n` and `nz × nz` identity matrices. The weighting matrix `Wv`
937  * for the velocity variables is just `Wv = τ*Wꝗ` where `τ` is a
938  * "characteristic time" for the system, that is, a quantity in time units
939  * that represents a significant evolution of the trajectory. This serves to
940  * control the accuracy with which velocity is determined relative to
941  * configuration. Note that larger values of `τ` are more conservative since
942  * they increase the velocity weights. Typically we use `τ=1.0` or `0.1`
943  * seconds for human-scale mechanical systems.
944  * <!-- TODO(sherm1): provide more guidance for velocity weighting. -->
945  *
946  * The weighting matrices `Wq`, `Wv`, and `Wz` are used to compute a weighted
947  * infinity norm as follows. Although `Wv` and `Wz` are constant, the actual
948  * weightings may be state dependent for relative-error calculations.
949  * Define block diagonal error weighting matrix `E=diag(Eq,Ev,Ez)` as follows:
950  * <pre>
951  * Eq = Wq
952  * Ev: Ev(i,i) = { min(Wv(i,i), 1/|vᵢ|) if vᵢ is relative
953  * { Wv(i,i) if vᵢ is absolute
954  * Ez: Ez(i,i) = { min(Wz(i,i), 1/|zᵢ|) if zᵢ is relative
955  * { Wz(i,i) if zᵢ is absolute
956  * </pre>
957  * (`Ev` and `Ez` are diagonal.) A `v` or `z` will be maintained to relative
958  * accuracy unless (a) it is "close" to zero (less than 1), or (b) the
959  * variable has been defined as requiring absolute accuracy. Position
960  * variables `q` are always maintained to absolute accuracy (see
961  * [Sherman 2011] for rationale).
962  *
963  * Now given an error estimate vector `e=[eq ev ez]`, the vector `f=E*e`
964  * can be considered to provide a unitless fractional error for each of the
965  * state variables. To achieve a given user-specified accuracy `a`, we require
966  * that norm_inf(`f`) <= `a`. That is, no element of `f` can have absolute
967  * value larger than `a`. We also use `f` to determine an ideal next step
968  * size using an appropriate integrator-specific computation.
969  *
970  * <h4>Determining weights for q</h4>
971  *
972  * The kinematic differential equations `qdot=N(q)*v` employ an `nq × n`
973  * matrix `N`. By construction, this relationship is invertible using `N`'s
974  * left pseudo-inverse `N⁺` so that `v=N⁺ qdot` and `N⁺ N = I` (the identity
975  * matrix); however, `N N⁺ != I`, as `N` has more rows than columns generally.
976  * [Nikravesh 1988] shows how such a matrix `N` can be determined and provides
977  * more information. Given this relationship between `N` and `N⁺`, we can
978  * relate weighted errors in configuration coordinates `q` to weighted errors
979  * in generalized quasi-coordinates `ꝗ`, as the following derivation shows:
980  * <pre>
981  * v = N⁺ qdot Inverse kinematic differential equation
982  * dꝗ/dt = N⁺ dq/dt Use synonyms for v and qdot
983  * dꝗ = N⁺ dq Change time derivatives to differentials
984  * Wꝗ dꝗ = Wꝗ N⁺ dq Pre-multiply both sides by Wꝗ
985  * N Wꝗ dꝗ = N Wꝗ N⁺ dq Pre-multiply both sides by N
986  * N Wꝗ dꝗ = Wq dq Define Wq := N Wꝗ N⁺
987  * N Wꝗ v = Wq qdot Back to time derivatives.
988  * </pre>
989  * The last two equations show that `Wq` as defined above provides the
990  * expected relationship between the weighted `ꝗ` or `v` variables in velocity
991  * space and the weighted `q` or `qdot` (resp.) variables in configuration
992  * space.
993  *
994  * Finally, note that a diagonal entry of one of the weighting matrices can
995  * be set to zero to disable error estimation for that state variable
996  * (i.e., auxiliary variable or configuration/velocity variable pair), but
997  * that setting an entry to a negative value will cause an exception to be
998  * thrown when the integrator is initialized.
999  *
1000  * - [Nikravesh 1988] P. Nikravesh. Computer-Aided Analysis of Mechanical
1001  * Systems. Prentice Hall, 1988. Sec. 6.3.
1002  * - [Sherman 2011] M. Sherman, et al. Procedia IUTAM 2:241-261 (2011),
1003  * Section 3.3. http://dx.doi.org/10.1016/j.piutam.2011.04.023
1004  *
1005  * @sa CalcStateChangeNorm()
1006  */
1007  /**
1008  * Gets the weighting vector (equivalent to a diagonal matrix) applied to
1009  * weighting both generalized coordinate and velocity state variable errors,
1010  * as described in the group documentation. Only used for integrators that
1011  * support error estimation.
1012  */
1013  const Eigen::VectorXd& get_generalized_state_weight_vector() const {
1014  return qbar_weight_;
1015  }
1016 
1017  /**
1018  * Gets a mutable weighting vector (equivalent to a diagonal matrix) applied
1019  * to weighting both generalized coordinate and velocity state variable
1020  * errors, as described in the group documentation. Only used for
1021  * integrators that support error estimation. Returns a VectorBlock
1022  * to make the values mutable without permitting changing the size of
1023  * the vector. Requires re-initializing the integrator after calling
1024  * this method; if Initialize() is not called afterward, an exception will be
1025  * thrown when attempting to call StepOnceAtMost(). If the caller sets
1026  * one of the entries to a negative value, an exception will be thrown
1027  * when the integrator is initialized.
1028  */
1029  Eigen::VectorBlock<Eigen::VectorXd>
1031  initialization_done_ = false;
1032  return qbar_weight_.head(qbar_weight_.rows());
1033  }
1034 
1035  /**
1036  * Gets the weighting vector (equivalent to a diagonal matrix) for
1037  * weighting errors in miscellaneous continuous state variables `z`. Only used
1038  * for integrators that support error estimation.
1039  */
1040  const Eigen::VectorXd& get_misc_state_weight_vector() const {
1041  return z_weight_;
1042  }
1043 
1044  /**
1045  * Gets a mutable weighting vector (equivalent to a diagonal matrix) for
1046  * weighting errors in miscellaneous continuous state variables `z`. Only used
1047  * for integrators that support error estimation. Returns a VectorBlock
1048  * to make the values mutable without permitting changing the size of
1049  * the vector. Requires re-initializing the integrator after calling this
1050  * method. If Initialize() is not called afterward, an exception will be
1051  * thrown when attempting to call StepOnceAtMost(). If the caller sets
1052  * one of the entries to a negative value, an exception will be thrown
1053  * when the integrator is initialized.
1054  */
1055  Eigen::VectorBlock<Eigen::VectorXd> get_mutable_misc_state_weight_vector() {
1056  initialization_done_ = false;
1057  return z_weight_.head(z_weight_.rows());
1058  }
1059 
1060  /**
1061  * @}
1062  */
1063 
1064  protected:
1065  /// Resets any statistics particular to a specific integrator. The default
1066  /// implementation of this function does nothing. If your integrator
1067  /// collects its own statistics, you should re-implement this method and
1068  /// reset them there.
1069  virtual void DoResetStatistics() {}
1070 
1071  /// Evaluates the derivative function (and updates call statistics).
1072  /// Subclasses should call this function rather than calling
1073  /// system.CalcTimeDerivatives() directly.
1074  void CalcTimeDerivatives(const Context<T>& context,
1075  ContinuousState<T>* dxdt) {
1076  get_system().CalcTimeDerivatives(context, dxdt);
1077  ++num_ode_evals_;
1078  }
1079 
1080  /// Evaluates the derivative function (and updates call statistics).
1081  /// Subclasses should call this function rather than calling
1082  /// system.CalcTimeDerivatives() directly. This version of this function
1083  /// exists to allow integrators to count AutoDiff'd systems in derivative
1084  /// function evaluations.
1085  template <typename U>
1086  void CalcTimeDerivatives(const System<U>& system,
1087  const Context<U>& context,
1088  ContinuousState<U>* dxdt) {
1089  system.CalcTimeDerivatives(context, dxdt);
1090  ++num_ode_evals_;
1091  }
1092 
1093  /**
1094  * Sets the working ("in use") accuracy for this integrator. The working
1095  * accuracy may not be equivalent to the target accuracy when the latter is
1096  * too loose or tight for an integrator's capabilities.
1097  * @sa get_accuracy_in_use()
1098  * @sa get_target_accuracy()
1099  */
1100  void set_accuracy_in_use(double accuracy) { accuracy_in_use_ = accuracy; }
1101 
1102  /// Generic code for validating (and resetting, if need be) the integrator
1103  /// working accuracy for error controlled integrators. This method is
1104  /// intended to be called from an integrator's DoInitialize() method.
1105  /// @param default_accuracy a reasonable default accuracy setting for this
1106  /// integrator.
1107  /// @param loosest_accuracy the loosest accuracy that this integrator should
1108  /// support.
1109  /// @param max_step_fraction a fraction of the maximum step size to use when
1110  /// setting the integrator accuracy and the user has not specified
1111  /// accuracy directly.
1112  /// @throws std::logic_error if neither the initial step size target nor
1113  /// the maximum step size has been set.
1114  void InitializeAccuracy(double default_accuracy, double loosest_accuracy,
1115  double max_step_fraction) {
1116  using std::isnan;
1117 
1118  // Set an artificial step size target, if not set already.
1119  if (isnan(this->get_initial_step_size_target())) {
1120  // Verify that maximum step size has been set.
1121  if (isnan(this->get_maximum_step_size()))
1122  throw std::logic_error("Neither initial step size target nor maximum "
1123  "step size has been set!");
1124 
1126  this->get_maximum_step_size() * max_step_fraction);
1127  }
1128 
1129  // Sets the working accuracy to a good value.
1130  double working_accuracy = this->get_target_accuracy();
1131 
1132  // If the user asks for accuracy that is looser than the loosest this
1133  // integrator can provide, use the integrator's loosest accuracy setting
1134  // instead.
1135  if (isnan(working_accuracy)) {
1136  working_accuracy = default_accuracy;
1137  } else {
1138  if (working_accuracy > loosest_accuracy)
1139  working_accuracy = loosest_accuracy;
1140  }
1141  this->set_accuracy_in_use(working_accuracy);
1142  }
1143 
1144  /**
1145  * Default code for advancing the continuous state of the system by a single
1146  * step of @p dt_max (or smaller, depending on error control). This particular
1147  * function is designed to be called directly by an error estimating
1148  * integrator's DoStep() method to effect error-controlled integration.
1149  * The integrator can effect error controlled integration without calling this
1150  * method, if the implementer so chooses, but this default method is expected
1151  * to function well in most circumstances.
1152  * @param[in] dt_max The maximum step size to be taken. The integrator may
1153  * take a smaller step than specified to satisfy accuracy
1154  * requirements, to resolve integrator convergence problems, or
1155  * to respect the integrator's maximum step size.
1156  * @throws std::logic_error if integrator does not support error
1157  * estimation.
1158  * @note This function will shrink the integration step as necessary whenever
1159  * the integrator's DoStep() fails to take the requested step
1160  * e.g., due to integrator convergence failure.
1161  * @returns `true` if the full step of size @p dt_max is taken and `false`
1162  * otherwise (i.e., a smaller step than @p dt_max was taken).
1163  */
1164  bool StepOnceErrorControlledAtMost(const T& dt_max);
1165 
1166  /**
1167  * Computes the infinity norm of a change in continuous state. We use the
1168  * infinity norm to capture the idea that, by providing accuracy requirements,
1169  * the user can indirectly specify error tolerances that act to limit the
1170  * largest error in any state vector component.
1171  * @returns the norm (a non-negative value)
1172  */
1173  T CalcStateChangeNorm(const ContinuousState<T>& dx_state) const;
1174 
1175  /**
1176  * Calculates adjusted integrator step sizes toward keeping state variables
1177  * within error bounds on the next integration step. Note that it is not
1178  * guaranteed that the (possibly) reduced step size will keep state variables
1179  * within error bounds; however, the process of (1) taking a trial
1180  * integration step, (2) calculating the error, and (3) adjusting the step
1181  * size can be repeated until convergence.
1182  * @param err
1183  * The norm of the integrator error that was computed using
1184  * @p attempted_step_size.
1185  * @param attempted_step_size
1186  * The step size that was attempted.
1187  * @param[in,out] at_minimum_step_size
1188  * If `true` on entry, the error control mechanism is not allowed to
1189  * shrink the step because the integrator is stepping at the minimum
1190  * step size (note that this condition will only occur if
1191  * `get_throw_on_minimum_step_size_violation() == false`- an exception
1192  * would be thrown otherwise). If `true` on entry and `false` on exit,
1193  * the error control mechanism has managed to increase the step size
1194  * above the working minimum; if `true` on entry and `true` on exit,
1195  * error control would like to shrink the step size but cannot. If
1196  * `false` on entry and `true` on exit, error control shrank the step
1197  * to the working minimum step size.
1198  * @returns a pair of types bool and T; the bool will be set to `true` if
1199  * the integration step was to be considered successful and `false`
1200  * otherwise. The value of the T type will be set to the recommended next
1201  * step size.
1202  */
1203  std::pair<bool, T> CalcAdjustedStepSize(
1204  const T& err,
1205  const T& attempted_step_size,
1206  bool* at_minimum_step_size) const;
1207 
1208  /**
1209  * Derived classes can override this method to perform special
1210  * initialization. This method is called during the Initialize() method. This
1211  * default method does nothing.
1212  */
1213  virtual void DoInitialize() {}
1214 
1215  /**
1216  * Derived classes can override this method to perform routines when
1217  * Reset() is called. This default method does nothing.
1218  */
1219  virtual void DoReset() {}
1220 
1221  /**
1222  * Derived classes must implement this method to (1) integrate the continuous
1223  * portion of this system forward by a single step of size @p dt and
1224  * (2) set the error estimate (via get_mutable_error_estimate()). This
1225  * method is called during the default Step() method.
1226  * @param dt The integration step to take.
1227  * @returns `true` if successful, `false` if the integrator was unable to take
1228  * a single step of size @p dt (due to, e.g., an integrator
1229  * convergence failure).
1230  * @post If the time on entry is denoted `t`, the time and state will be
1231  * advanced to `t+dt` if the method returns `true`; otherwise, the
1232  * time and state should be reset to those at `t`.
1233  * @warning It is expected that DoStep() will return `true` for some, albeit
1234  * possibly very small, positive value of @p dt. The derived
1235  * integrator's stepping algorithm can make this guarantee, for
1236  * example, by switching to an algorithm not subject to convergence
1237  * failures (e.g., explicit Euler) for very small step sizes.
1238  */
1239  virtual bool DoStep(const T& dt) = 0;
1240 
1241  /**
1242  * Gets an error estimate of the state variables recorded by the last call
1243  * to StepOnceFixedSize(). If the integrator does not support error
1244  * estimation, this function will return nullptr.
1245  */
1246  ContinuousState<T>* get_mutable_error_estimate() { return err_est_.get(); }
1247 
1248  // Sets the actual initial step size taken.
1250  actual_initial_step_size_taken_ = dt;
1251  }
1252 
1253  /**
1254  * Sets the size of the smallest-step-taken statistic as the result of a
1255  * controlled integration step adjustment.
1256  */
1258  smallest_adapted_step_size_taken_ = dt;
1259  }
1260 
1261  // Sets the largest-step-size-taken statistic.
1262  void set_largest_step_size_taken(const T& dt) {
1263  largest_step_size_taken_ = dt;
1264  }
1265 
1266  // Sets the "ideal" next step size (typically done via error control).
1267  void set_ideal_next_step_size(const T& dt) { ideal_next_step_size_ = dt; }
1268 
1269  private:
1270  // Validates that a smaller step size does not fall below the working minimum
1271  // and throws an exception if desired.
1272  void ValidateSmallerStepSize(const T& current_step_size,
1273  const T& new_step_size) const {
1274  if (new_step_size < get_working_minimum_step_size() &&
1275  new_step_size < current_step_size && // Verify step adjusted downward.
1276  min_step_exceeded_throws_) {
1277  SPDLOG_DEBUG(drake::log(), "Integrator wants to select too small step "
1278  "size of {}; working minimum is ", new_step_size,
1280  std::ostringstream str;
1281  str << "Error control wants to select step smaller than minimum" <<
1282  " allowed (" << get_working_minimum_step_size() << ")";
1283  throw std::runtime_error(str.str());
1284  }
1285  }
1286 
1287  // Updates the integrator statistics, accounting for a step just taken of
1288  // size dt.
1289  void UpdateStepStatistics(const T& dt) {
1290  // Handle first step specially.
1291  if (++num_steps_taken_ == 1) {
1294  } else {
1296  }
1297 
1298  // Update the previous step size.
1299  prev_step_size_ = dt;
1300  }
1301 
1302  // Steps the system forward exactly by @p dt, if possible, by calling DoStep.
1303  // Does necessary pre-initialization and post-cleanup. This method does not
1304  // update general integrator statistics (which are updated in the calling
1305  // methods), because error control might decide that it does not like the
1306  // result of the step and might "rewind" and take a smaller one.
1307  // @returns `true` if successful, `false` otherwise (due to, e.g., integrator
1308  // convergence failure).
1309  // @sa DoStep()
1310  bool Step(const T& dt) {
1311  if (!DoStep(dt))
1312  return false;
1313  return true;
1314  }
1315 
1316  // Reference to the system being simulated.
1317  const System<T>& system_;
1318 
1319  // Pointer to the context.
1320  Context<T>* context_{nullptr}; // The trajectory Context.
1321 
1322  // Runtime variables.
1323  // For variable step integrators, this is set at the end of each step to guide
1324  // the next one.
1325  T ideal_next_step_size_{nan()}; // Indicates that the value is uninitialized.
1326 
1327  // The scaling factor to apply to an integration step size when an integrator
1328  // convergence failure occurs (to make convergence more likely on the next
1329  // attempt).
1330  // TODO(edrumwri): Allow subdivision factor to be user-tweakable.
1331  const double subdivision_factor_{0.5};
1332 
1333  // The accuracy being used.
1334  double accuracy_in_use_{nan()};
1335 
1336  // The maximum step size.
1337  T max_step_size_{nan()};
1338 
1339  // The minimum step size.
1340  T req_min_step_size_{0};
1341 
1342  // The last step taken by the integrator.
1343  T prev_step_size_{nan()};
1344 
1345  // Whether error-controlled integrator is running in fixed step mode. Value
1346  // is irrelevant for integrators without error estimation capabilities.
1347  bool fixed_step_mode_{false};
1348 
1349  // When the minimum step is exceeded, does the integrator throw an exception?
1350  bool min_step_exceeded_throws_{true};
1351 
1352  // Statistics.
1353  T actual_initial_step_size_taken_{nan()};
1354  T smallest_adapted_step_size_taken_{nan()};
1355  T largest_step_size_taken_{nan()};
1356  int64_t num_steps_taken_{0};
1357  int64_t num_ode_evals_{0};
1358  int64_t num_shrinkages_from_error_control_{0};
1359  int64_t num_shrinkages_from_substep_failures_{0};
1360  int64_t num_substep_failures_{0};
1361 
1362  // Applied as diagonal matrices to weight state change variables.
1363  Eigen::VectorXd qbar_weight_, z_weight_;
1364 
1365  // State copy for reversion during error-controlled integration.
1366  VectorX<T> xc0_save_;
1367 
1368  // The error estimate computed during integration with error control.
1369  std::unique_ptr<ContinuousState<T>> err_est_;
1370 
1371  // The pseudo-inverse of the matrix that converts time derivatives of
1372  // generalized coordinates to generalized velocities, multiplied by the
1373  // change in the generalized coordinates (used in state change norm
1374  // calculations).
1375  mutable std::unique_ptr<VectorBase<T>> pinvN_dq_change_;
1376 
1377  // Vectors used in state change norm calculations.
1378  mutable VectorX<T> unweighted_substate_change_;
1379  mutable std::unique_ptr<VectorBase<T>> weighted_q_change_;
1380 
1381  // Variable for indicating when an integrator has been initialized.
1382  bool initialization_done_{false};
1383 
1384  // This a workaround for an apparent bug in clang 3.8 in which
1385  // defining this as a static constexpr member kNaN failed to instantiate
1386  // properly for the AutoDiffXd instantiation (worked in gcc and MSVC).
1387  // Restore to sanity when some later clang is current.
1388  static constexpr double nan() {
1389  return std::numeric_limits<double>::quiet_NaN();
1390  }
1391 
1392  double target_accuracy_{nan()}; // means "unspecified, use default"
1393  T req_initial_step_size_{nan()}; // means "unspecified, use default"
1394 };
1395 
1396 template <class T>
1398  using std::isnan;
1399  using std::min;
1400 
1401  // Verify that the integrator supports error estimates.
1402  if (!supports_error_estimation()) {
1403  throw std::logic_error("StepOnceErrorControlledAtMost() requires error "
1404  "estimation.");
1405  }
1406 
1407  // Save time, continuous variables, and time derivative because we'll possibly
1408  // revert time and state.
1409  const Context<T>& context = get_context();
1410  const T current_time = context.get_time();
1411  VectorBase<T>& xc =
1412  get_mutable_context()->get_mutable_continuous_state_vector();
1413  xc0_save_ = xc.CopyToVector();
1414 
1415  // Set the step size to attempt.
1416  T step_size_to_attempt = get_ideal_next_step_size();
1417  if (isnan(step_size_to_attempt)) {
1418  // Integrator has not taken a step. Set the current step size to the
1419  // initial step size.
1420  step_size_to_attempt = get_initial_step_size_target();
1421  DRAKE_DEMAND(!isnan(step_size_to_attempt));
1422  }
1423 
1424  // This variable indicates when the integrator has been pushed to its minimum
1425  // step limit. It can only be "true" if minimum step exceptions have been
1426  // suppressed by the user via set_throw_on_minimum_step_size_violation(false),
1427  // and the error control mechanism determines that the step is as low as it
1428  // can go.
1429  bool at_minimum_step_size = false;
1430 
1431  bool step_succeeded = false;
1432  do {
1433  // Constants used to determine whether modifications to the step size are
1434  // close enough to the attempted step size to use the unadjusted originals,
1435  // or (1) whether the step size to be attempted is so small that we should
1436  // consider it to be artifically limited or (2) whether the step size to
1437  // be attempted is sufficiently close to that requested such that the step
1438  // size should be stretched slightly.
1439  const double near_enough_smaller = 0.95;
1440  const double near_enough_larger = 1.001;
1441 
1442  // If we lose more than a small fraction of the step size we wanted
1443  // to take due to a need to stop at dt_max, make a note of that so the
1444  // step size adjuster won't try to grow from the current step.
1445  bool dt_was_artificially_limited = false;
1446  if (dt_max < near_enough_smaller * step_size_to_attempt) {
1447  // dt_max much smaller than current step size.
1448  dt_was_artificially_limited = true;
1449  step_size_to_attempt = dt_max;
1450  } else {
1451  if (dt_max < near_enough_larger * step_size_to_attempt) {
1452  // dt_max is roughly current step. Make it the step size to prevent
1453  // creating a small sliver (the remaining step).
1454  step_size_to_attempt = dt_max;
1455  }
1456  }
1457 
1458  // Limit the current step size.
1459  step_size_to_attempt = min(step_size_to_attempt, get_maximum_step_size());
1460 
1461  // Keep adjusting the integration step size until any integrator
1462  // convergence failures disappear. Note: this loop's correctness is
1463  // predicated on the assumption that an integrator will always converge for
1464  // a sufficiently small, yet nonzero step size.
1465  T adjusted_step_size = step_size_to_attempt;
1466  while (!Step(adjusted_step_size)) {
1467  SPDLOG_DEBUG(drake::log(), "Sub-step failed at {}", adjusted_step_size);
1468  adjusted_step_size *= subdivision_factor_;
1469 
1470  // Note: we could give the user more rope to hang themselves by looking
1471  // for zero rather than machine epsilon, which might be advantageous if
1472  // the user were modeling systems over extremely small time scales.
1473  // However, that issue could be addressed instead by scaling units, and
1474  // using machine epsilon allows failure to be detected much more rapidly.
1475  if (adjusted_step_size < std::numeric_limits<double>::epsilon()) {
1476  throw std::runtime_error("Integrator has been directed to a near zero-"
1477  "length step in order to obtain convergence.");
1478  }
1479  ValidateSmallerStepSize(step_size_to_attempt, adjusted_step_size);
1480  ++num_shrinkages_from_substep_failures_;
1481  ++num_substep_failures_;
1482  }
1483  step_size_to_attempt = adjusted_step_size;
1484 
1485  //--------------------------------------------------------------------
1486  T err_norm = CalcStateChangeNorm(*get_error_estimate());
1487  T next_step_size;
1488  std::tie(step_succeeded, next_step_size) = CalcAdjustedStepSize(
1489  err_norm, step_size_to_attempt, &at_minimum_step_size);
1490  SPDLOG_DEBUG(drake::log(), "Next step size: {}", next_step_size);
1491 
1492  if (step_succeeded) {
1493  // Only update the next step size (retain the previous one) if the
1494  // step size was not artificially limited.
1495  if (!dt_was_artificially_limited)
1496  ideal_next_step_size_ = next_step_size;
1497 
1499  set_actual_initial_step_size_taken(step_size_to_attempt);
1500 
1501  // Record the adapted step size taken.
1503  (step_size_to_attempt < get_smallest_adapted_step_size_taken() &&
1504  step_size_to_attempt < dt_max))
1505  set_smallest_adapted_step_size_taken(step_size_to_attempt);
1506  } else {
1507  ++num_shrinkages_from_error_control_;
1508 
1509  // Set the next step size to attempt.
1510  step_size_to_attempt = next_step_size;
1511 
1512  // Reset the time, state, and time derivative at t0.
1513  get_mutable_context()->set_time(current_time);
1514  xc.SetFromVector(xc0_save_);
1515  }
1516  } while (!step_succeeded);
1517 
1518  return (step_size_to_attempt == dt_max);
1519 }
1520 
1521 template <class T>
1523  const ContinuousState<T>& dx_state) const {
1524  using std::max;
1525  const Context<T>& context = get_context();
1526  const auto& system = get_system();
1527 
1528  // Get weighting matrices.
1529  const auto& qbar_v_weight = this->get_generalized_state_weight_vector();
1530  const auto& z_weight = this->get_misc_state_weight_vector();
1531 
1532  // Get the differences in the generalized position, velocity, and
1533  // miscellaneous continuous state vectors.
1534  const VectorBase<T>& dgq = dx_state.get_generalized_position();
1535  const VectorBase<T>& dgv = dx_state.get_generalized_velocity();
1536  const VectorBase<T>& dgz = dx_state.get_misc_continuous_state();
1537 
1538  // (re-)Initialize pinvN_dq_change_ and weighted_q_change_, if necessary.
1539  // Reinitialization might be required if the system state variables can
1540  // change during the course of the simulation.
1541  if (pinvN_dq_change_ == nullptr) {
1542  pinvN_dq_change_ = std::make_unique<BasicVector<T>>(dgv.size());
1543  weighted_q_change_ = std::make_unique<BasicVector<T>>(dgq.size());
1544  }
1545  DRAKE_DEMAND(pinvN_dq_change_->size() == dgv.size());
1546  DRAKE_DEMAND(weighted_q_change_->size() == dgq.size());
1547 
1548  // TODO(edrumwri): Acquire characteristic time properly from the system
1549  // (i.e., modify the System to provide this value).
1550  const double characteristic_time = 1.0;
1551 
1552  // Computes the infinity norm of the weighted velocity variables.
1553  unweighted_substate_change_ = dgv.CopyToVector();
1554  T v_nrm = qbar_v_weight.cwiseProduct(unweighted_substate_change_).
1555  template lpNorm<Eigen::Infinity>() * characteristic_time;
1556 
1557  // Compute the infinity norm of the weighted auxiliary variables.
1558  unweighted_substate_change_ = dgz.CopyToVector();
1559  T z_nrm = (z_weight.cwiseProduct(unweighted_substate_change_))
1560  .template lpNorm<Eigen::Infinity>();
1561 
1562  // Compute N * Wq * dq = N * Wꝗ * N+ * dq.
1563  unweighted_substate_change_ = dgq.CopyToVector();
1564  system.MapQDotToVelocity(context, unweighted_substate_change_,
1565  pinvN_dq_change_.get());
1566  system.MapVelocityToQDot(
1567  context, qbar_v_weight.cwiseProduct(pinvN_dq_change_->CopyToVector()),
1568  weighted_q_change_.get());
1569  T q_nrm = weighted_q_change_->CopyToVector().
1570  template lpNorm<Eigen::Infinity>();
1571  SPDLOG_DEBUG(drake::log(), "dq norm: {}, dv norm: {}, dz norm: {}",
1572  q_nrm, v_nrm, z_nrm);
1573 
1574  // TODO(edrumwri): Record the worst offender (which of the norms resulted
1575  // in the largest value).
1576  // Infinity norm of the concatenation of multiple vectors is equal to the
1577  // maximum of the infinity norms of the individual vectors.
1578  return max(z_nrm, max(q_nrm, v_nrm));
1579 }
1580 
1581 template <class T>
1583  const T& err,
1584  const T& step_taken,
1585  bool* at_minimum_step_size) const {
1586  using std::pow;
1587  using std::min;
1588  using std::max;
1589  using std::isnan;
1590  using std::isinf;
1591 
1592  // Magic numbers come from Simbody.
1593  const double kSafety = 0.9;
1594  const double kMinShrink = 0.1;
1595  const double kMaxGrow = 5.0;
1596  const double kHysteresisLow = 0.9;
1597  const double kHysteresisHigh = 1.2;
1598 
1599  // Get the order for the integrator's error estimate.
1600  const int err_order = get_error_estimate_order();
1601 
1602  // Set value for new step size to invalid value initially.
1603  T new_step_size(-1);
1604 
1605  // First, make a guess at the next step size to use based on
1606  // the supplied error norm. Watch out for NaN. Further adjustments will be
1607  // made in blocks of code that follow.
1608  if (isnan(err) || isinf(err)) // e.g., integrand returned NaN.
1609  new_step_size = kMinShrink * step_taken;
1610  else if (err == 0) // A "perfect" step; can happen if no dofs for example.
1611  new_step_size = kMaxGrow * step_taken;
1612  else // Choose best step for skating just below the desired accuracy.
1613  new_step_size = kSafety * step_taken *
1614  pow(get_accuracy_in_use() / err, 1.0 / err_order);
1615 
1616  // Error indicates that the step size can be increased.
1617  if (new_step_size > step_taken) {
1618  // If the integrator has been directed down to the minimum step size, but
1619  // now error indicates that the step size can be increased, de-activate
1620  // at_minimum_step_size.
1621  *at_minimum_step_size = false;
1622 
1623  // If the new step is bigger than the old, don't make the change if the
1624  // old one was small for some unimportant reason (like reached a publishing
1625  // interval). Also, don't grow the step size if the change would be very
1626  // small; better to keep the step size stable in that case (maybe just
1627  // for aesthetic reasons).
1628  if (new_step_size < kHysteresisHigh * step_taken)
1629  new_step_size = step_taken;
1630  }
1631 
1632  // If error indicates that we should shrink the step size but are not allowed
1633  // to, quit and indicate that the step was successful.
1634  if (new_step_size < step_taken && *at_minimum_step_size) {
1635  return std::make_pair(true, step_taken);
1636  }
1637 
1638  // If we're supposed to shrink the step size but the one we have actually
1639  // achieved the desired accuracy last time, we won't change the step now.
1640  // Otherwise, if we are going to shrink the step, let's not be shy -- we'll
1641  // shrink it by at least a factor of kHysteresisLow.
1642  if (new_step_size < step_taken) {
1643  if (err <= get_accuracy_in_use()) {
1644  new_step_size = step_taken; // not this time
1645  } else {
1646  T test_value = kHysteresisLow * step_taken;
1647  new_step_size = min(new_step_size, test_value);
1648  }
1649  }
1650 
1651  // Keep the size change within the allowable bounds.
1652  T max_grow_step = kMaxGrow * step_taken;
1653  T min_shrink_step = kMinShrink * step_taken;
1654  new_step_size = min(new_step_size, max_grow_step);
1655  new_step_size = max(new_step_size, min_shrink_step);
1656 
1657  // Apply user-requested limits on min and max step size.
1658  // TODO(edrumwri): Introduce some feedback to the user when integrator wants
1659  // to take a smaller step than user has selected as the minimum. Options for
1660  // this feedback could include throwing a special exception, logging, setting
1661  // a flag in the integrator that allows throwing an exception, or returning
1662  // a special status from IntegrateAtMost().
1663  if (!isnan(get_maximum_step_size()))
1664  new_step_size = min(new_step_size, get_maximum_step_size());
1665  ValidateSmallerStepSize(step_taken, new_step_size);
1666 
1667  // Increase the next step size, as necessary.
1668  new_step_size = max(new_step_size, get_working_minimum_step_size());
1669  if (new_step_size == get_working_minimum_step_size()) {
1670  // Indicate that the step is integrator is now trying the minimum step
1671  // size.
1672  *at_minimum_step_size = true;
1673 
1674  // If the integrator wants to shrink the step size below the
1675  // minimum allowed and exceptions are suppressed, indicate that status.
1676  if (new_step_size < step_taken)
1677  return std::make_pair(false, new_step_size);
1678  }
1679 
1680  return std::make_pair(new_step_size >= step_taken, new_step_size);
1681 }
1682 
1683 template <class T>
1685  const T& publish_dt, const T& update_dt, const T& boundary_dt) {
1686 
1688  throw std::logic_error("Integrator not initialized.");
1689 
1690  // Now that integrator has been checked for initialization, get the current
1691  // time.
1692  const T t0 = context_->get_time();
1693 
1694  // Verify that dt's are non-negative.
1695  if (update_dt < 0.0)
1696  throw std::logic_error("Update dt is negative.");
1697  if (publish_dt < 0.0)
1698  throw std::logic_error("Publish dt is negative.");
1699  if (boundary_dt < 0.0)
1700  throw std::logic_error("Boundary dt is negative.");
1701 
1702  // The size of the integration step is the minimum of the time until the next
1703  // update event, the time until the next publish event, the boundary time
1704  // (i.e., the maximum time that the user wished to step to), and the maximum
1705  // step size (which may stretch slightly to hit a discrete event).
1706 
1707  // We report to the caller which event ultimately constrained the step size.
1708  // If multiple events constrained it equally, we prefer to report update
1709  // events over publish events, publish events over boundary step limits,
1710  // and boundary limits over maximum step size limits. The caller must
1711  // determine event simultaneity by inspecting the time.
1712 
1713  // The maintainer of this code is advised to consider that, while updates
1714  // and boundary times, may both conceptually be deemed events, the distinction
1715  // is made for a reason. If both an update and a boundary time occur
1716  // simultaneously, the following behavior should result:
1717  // (1) kReachedUpdateTime is returned, (2) Simulator::StepTo() performs the
1718  // necessary update, (3) IntegrateAtMost() is called with boundary_dt=0 and
1719  // returns kReachedBoundaryTime, and (4) the simulation terminates. This
1720  // sequence of operations will ensure that the simulation state is valid if
1721  // Simulator::StepTo() is called again to advance time further.
1722 
1723  // We now analyze the following simultaneous cases with respect to Simulator:
1724  //
1725  // { publish, update }
1726  // kReachedUpdateTime will be returned, an update will be followed by a
1727  // publish.
1728  //
1729  // { publish, update, max step }
1730  // kReachedUpdateTime will be returned, an update will be followed by a
1731  // publish.
1732  //
1733  // { publish, boundary time, max step }
1734  // kReachedPublishTime will be returned, a publish will be performed followed
1735  // by another call to this function, which should return kReachedBoundaryTime
1736  // (followed in rapid succession by StepTo(.) return).
1737  //
1738  // { publish, boundary time, max step }
1739  // kReachedPublishTime will be returned, a publish will be performed followed
1740  // by another call to this function, which should return kReachedBoundaryTime
1741  // (followed in rapid succession by StepTo(.) return).
1742  //
1743  // { publish, update, boundary time, maximum step size }
1744  // kUpdateTimeReached will be returned, an update followed by a publish
1745  // will then be performed followed by another call to this function, which
1746  // should return kReachedBoundaryTime (followed in rapid succession by
1747  // StepTo(.) return).
1748 
1749  // By default, the candidate dt is the next discrete update event.
1751  T dt = update_dt;
1752 
1753  // If the next discrete publish event is sooner than the next discrete update
1754  // event, the publish event becomes the candidate dt
1755  if (publish_dt < update_dt) {
1756  candidate_result = IntegratorBase<T>::kReachedPublishTime;
1757  dt = publish_dt;
1758  }
1759 
1760  // If the stop time (boundary time) is sooner than the candidate, use it
1761  // instead.
1762  if (boundary_dt < dt) {
1763  candidate_result = IntegratorBase<T>::kReachedBoundaryTime;
1764  dt = boundary_dt;
1765  }
1766 
1767  // If there is no continuous state, there will be no need to limit the
1768  // integration step size.
1769  if (get_context().get_continuous_state().size() == 0) {
1770  Context<T>* context = get_mutable_context();
1771  context->set_time(context->get_time() + dt);
1772  return candidate_result;
1773  }
1774 
1775  // If all events are farther into the future than the maximum step
1776  // size times a stretch factor of 1.01, the maximum step size becomes the
1777  // candidate dt. Put another way, if the maximum step occurs right before
1778  // an update or a publish, the update or publish is done instead. In contrast,
1779  // we never step past boundary_dt, even if doing so would allow hitting a
1780  // publish or an update.
1781  const bool reached_boundary =
1782  (candidate_result == IntegratorBase<T>::kReachedBoundaryTime);
1783  const T& max_dt = IntegratorBase<T>::get_maximum_step_size();
1784  if ((reached_boundary && max_dt < dt) ||
1785  (!reached_boundary && max_dt * get_stretch_factor() < dt)) {
1786  candidate_result = IntegratorBase<T>::kTimeHasAdvanced;
1787  dt = max_dt;
1788  }
1789 
1790  if (dt < 0.0) throw std::logic_error("Negative dt.");
1791 
1792  // If error control is disabled, call the generic stepper. Otherwise, use
1793  // the error controlled method.
1794  bool full_step = true;
1795  if (this->get_fixed_step_mode()) {
1796  T adjusted_dt = dt;
1797  while (!Step(adjusted_dt)) {
1798  ++num_shrinkages_from_substep_failures_;
1799  ++num_substep_failures_;
1800  adjusted_dt *= subdivision_factor_;
1801  ValidateSmallerStepSize(dt, adjusted_dt);
1802  full_step = false;
1803  }
1804  } else {
1805  full_step = StepOnceErrorControlledAtMost(dt);
1806  }
1807 
1808  // Update generic statistics.
1809  const T actual_dt = context_->get_time() - t0;
1810  UpdateStepStatistics(actual_dt);
1811 
1812  if (full_step) {
1813  // If the integrator took the entire maximum step size we allowed above,
1814  // we report to the caller that a step constraint was hit, which may
1815  // indicate a discrete event has arrived.
1816  return candidate_result;
1817  } else {
1818  // Otherwise, we report to the caller that time has advanced, but no
1819  // discrete event has arrived.
1821  }
1822 }
1823 
1824 } // namespace systems
1825 } // namespace drake
void CalcTimeDerivatives(const System< U > &system, const Context< U > &context, ContinuousState< U > *dxdt)
Evaluates the derivative function (and updates call statistics).
Definition: integrator_base.h:1086
T get_working_minimum_step_size() const
Gets the current value of the working minimum step size h_work(t) for this integrator, which may vary with the current time t as stored in the integrator&#39;s context.
Definition: integrator_base.h:388
virtual int size() const =0
Returns the number of elements in the vector.
double get_target_accuracy() const
Gets the target accuracy.
Definition: integrator_base.h:243
void set_largest_step_size_taken(const T &dt)
Definition: integrator_base.h:1262
void CalcTimeDerivatives(const Context< T > &context, ContinuousState< T > *dxdt)
Evaluates the derivative function (and updates call statistics).
Definition: integrator_base.h:1074
bool isnan(const Eigen::AutoDiffScalar< DerType > &x)
Overloads isnan to mimic std::isnan from <cmath>.
Definition: autodiff_overloads.h:52
Localized an event; this is the before state (interpolated).
Definition: integrator_base.h:132
void set_ideal_next_step_size(const T &dt)
Definition: integrator_base.h:1267
const Eigen::VectorXd & get_generalized_state_weight_vector() const
Gets the weighting vector (equivalent to a diagonal matrix) applied to weighting both generalized coo...
Definition: integrator_base.h:1013
const T & get_actual_initial_step_size_taken() const
The actual size of the successful first step.
Definition: integrator_base.h:714
virtual void DoReset()
Derived classes can override this method to perform routines when Reset() is called.
Definition: integrator_base.h:1219
#define SPDLOG_DEBUG(logger,...)
Definition: text_logging.h:125
Definition: automotive_demo.cc:90
Context< T > * get_mutable_context()
Returns a mutable pointer to the internally-maintained Context holding the most recent state in the t...
Definition: integrator_base.h:765
const T & get_previous_integration_step_size() const
Gets the size of the last (previous) integration step.
Definition: integrator_base.h:801
void Initialize()
An integrator must be initialized before being used.
Definition: integrator_base.h:447
double get_stretch_factor() const
Gets the stretch factor (> 1), which is multiplied by the maximum (typically user-designated) integra...
Definition: integrator_base.h:561
void InitializeAccuracy(double default_accuracy, double loosest_accuracy, double max_step_fraction)
Generic code for validating (and resetting, if need be) the integrator working accuracy for error con...
Definition: integrator_base.h:1114
Context is an abstract class template that represents all the typed values that are used in a System&#39;...
Definition: context.h:40
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorX
A column vector of any size, templated on scalar type.
Definition: eigen_types.h:46
VectorBase is an abstract base class that real-valued signals between Systems and real-valued System ...
Definition: vector_base.h:27
Eigen::VectorBlock< Eigen::VectorXd > get_mutable_generalized_state_weight_vector()
Gets a mutable weighting vector (equivalent to a diagonal matrix) applied to weighting both generaliz...
Definition: integrator_base.h:1030
const VectorBase< T > & get_misc_continuous_state() const
Returns a const reference to the subset of the continuous state vector that is other continuous state...
Definition: continuous_state.h:197
void set_accuracy_in_use(double accuracy)
Sets the working ("in use") accuracy for this integrator.
Definition: integrator_base.h:1100
void set_throw_on_minimum_step_size_violation(bool throws)
Sets whether the integrator should throw a std::runtime_error exception when the integrator&#39;s step si...
Definition: integrator_base.h:369
logging::logger * log()
Retrieve an instance of a logger to use for logging; for example: drake::log()->info("potato!") ...
Definition: text_logging.cc:38
void request_initial_step_size_target(const T &step_size)
Request that the first attempted integration step have a particular size.
Definition: integrator_base.h:504
void set_maximum_step_size(const T &max_step_size)
Sets the maximum step size that may be taken by this integrator.
Definition: integrator_base.h:259
const Context< T > & get_context() const
Returns a const reference to the internally-maintained Context holding the most recent state in the t...
Definition: integrator_base.h:759
virtual VectorX< T > CopyToVector() const
Copies the entire state to a vector with no semantics.
Definition: vector_base.h:98
void IntegrateWithSingleFixedStep(const T &dt)
Stepping function for integrators operating outside of Simulator that advances the continuous state e...
Definition: integrator_base.h:638
ContinuousState< T > * get_mutable_error_estimate()
Gets an error estimate of the state variables recorded by the last call to StepOnceFixedSize().
Definition: integrator_base.h:1246
const T & get_initial_step_size_target() const
Gets the target size of the first integration step.
Definition: integrator_base.h:519
#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
bool get_fixed_step_mode() const
Gets whether an integrator is running in fixed step mode.
Definition: integrator_base.h:201
StepResult
Status returned by StepOnceAtMost().
Definition: integrator_base.h:124
const ContinuousState< T > * get_error_estimate() const
Gets the error estimate (used only for integrators that support error estimation).
Definition: integrator_base.h:810
virtual void set_time(const T &time_sec)
Set the current time in seconds.
Definition: context.h:69
User requested control whenever an internal step is successful.
Definition: integrator_base.h:140
bool StepOnceErrorControlledAtMost(const T &dt_max)
Default code for advancing the continuous state of the system by a single step of dt_max (or smaller...
Definition: integrator_base.h:1397
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:711
StepResult IntegrateAtMost(const T &publish_dt, const T &update_dt, const T &boundary_dt)
Integrates the system forward in time by a single step with step size subject to integration error to...
Definition: integrator_base.h:1684
Took maximum number of steps without finishing integrating over the interval.
Definition: integrator_base.h:147
#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
int64_t get_num_derivative_evaluations() const
Returns the number of ODE function evaluations (calls to CalcTimeDerivatives()) since the last call t...
Definition: integrator_base.h:709
const System< T > & get_system() const
Gets a constant reference to the system that is being integrated (and was provided to the constructor...
Definition: integrator_base.h:785
Expression min(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:699
void set_fixed_step_mode(bool flag)
Sets an integrator with error control to fixed step mode.
Definition: integrator_base.h:186
Reached the desired integration time without reaching an update time.
Definition: integrator_base.h:144
void Reset()
Resets the integrator to initial values, i.e., default construction values.
Definition: integrator_base.h:401
void set_actual_initial_step_size_taken(const T &dt)
Definition: integrator_base.h:1249
const T & get_ideal_next_step_size() const
Return the step size the integrator would like to take next, based primarily on the integrator&#39;s accu...
Definition: integrator_base.h:752
const T & get_time() const
Returns the current time in seconds.
Definition: context.h:66
double get_accuracy_in_use() const
Gets the accuracy in use by the integrator.
Definition: integrator_base.h:250
const T & get_maximum_step_size() const
Gets the maximum step size that may be taken by this integrator.
Definition: integrator_base.h:271
const VectorBase< T > & get_generalized_position() const
Returns a const reference to the subset of the state vector that is generalized position q...
Definition: continuous_state.h:173
Base class for all System functionality that is dependent on the templatized scalar type T for input...
Definition: input_port_descriptor.h:12
const Eigen::VectorXd & get_misc_state_weight_vector() const
Gets the weighting vector (equivalent to a diagonal matrix) for weighting errors in miscellaneous con...
Definition: integrator_base.h:1040
void set_requested_minimum_step_size(const T &min_step_size)
Sets the requested minimum step size h_min that may be taken by this integrator.
Definition: integrator_base.h:348
virtual void SetFromVector(const Eigen::Ref< const VectorX< T >> &value)
Replaces the entire vector with the contents of value.
Definition: vector_base.h:80
virtual ~IntegratorBase()=default
Destructor.
int64_t get_num_steps_taken() const
The number of integration steps taken since the last Initialize() or ResetStatistics() call...
Definition: integrator_base.h:740
const T & get_smallest_adapted_step_size_taken() const
The size of the smallest step taken as the result of a controlled integration step adjustment since t...
Definition: integrator_base.h:724
virtual void DoInitialize()
Derived classes can override this method to perform special initialization.
Definition: integrator_base.h:1213
void CalcTimeDerivatives(const Context< T > &context, ContinuousState< T > *derivatives) const
Calculates the time derivatives xcdot of the continuous state xc.
Definition: system.h:566
T CalcStateChangeNorm(const ContinuousState< T > &dx_state) const
Computes the infinity norm of a change in continuous state.
Definition: integrator_base.h:1522
int64_t get_num_step_shrinkages_from_substep_failures() const
Gets the number of step size shrinkages due to sub-step failures (e.g., integrator convergence failur...
Definition: integrator_base.h:690
void IntegrateWithMultipleSteps(const T &dt)
Stepping function for integrators operating outside of Simulator that advances the continuous state e...
Definition: integrator_base.h:585
Indicates that integration terminated at an update time.
Definition: integrator_base.h:136
void reset_context(Context< T > *context)
Replace the pointer to the internally-maintained Context with a different one.
Definition: integrator_base.h:776
const double epsilon
Definition: unrevised_lemke_solver_test.cc:13
virtual bool supports_error_estimation() const =0
Indicates whether an integrator supports error estimation.
IntegratorBase(const System< T > &system, Context< T > *context=nullptr)
Maintains references to the system being integrated and the context used to specify the initial condi...
Definition: integrator_base.h:163
int64_t get_num_step_shrinkages_from_error_control() const
Gets the number of step size shrinkages due to failure to meet targeted error tolerances, since the last call to ResetStatistics or Initialize().
Definition: integrator_base.h:696
bool get_throw_on_minimum_step_size_violation() const
Reports the current setting of the throw_on_minimum_step_size_violation flag.
Definition: integrator_base.h:378
virtual bool DoStep(const T &dt)=0
Derived classes must implement this method to (1) integrate the continuous portion of this system for...
virtual void DoResetStatistics()
Resets any statistics particular to a specific integrator.
Definition: integrator_base.h:1069
Eigen::VectorBlock< Eigen::VectorXd > get_mutable_misc_state_weight_vector()
Gets a mutable weighting vector (equivalent to a diagonal matrix) for weighting errors in miscellaneo...
Definition: integrator_base.h:1055
const VectorBase< T > & get_generalized_velocity() const
Returns a const reference to the subset of the continuous state vector that is generalized velocity v...
Definition: continuous_state.h:185
An abstract class for an integrator for ODEs and DAEs as represented by a Drake System.
Definition: integrator_base.h:108
bool is_initialized() const
Indicates whether the integrator has been initialized.
Definition: integrator_base.h:788
ContinuousState is a view of, and optionally a container for, all the continuous state variables xc o...
Definition: continuous_state.h:76
bool isinf(const Eigen::AutoDiffScalar< DerType > &x)
Overloads isinf to mimic std::isinf from <cmath>.
Definition: autodiff_overloads.h:45
Polynomial< CoefficientType > pow(const Polynomial< CoefficientType > &base, typename Polynomial< CoefficientType >::PowerType exponent)
Provides power function for Polynomial.
Definition: polynomial.h:449
const T & get_requested_minimum_step_size() const
Gets the requested minimum step size h_min for this integrator.
Definition: integrator_base.h:358
#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
virtual int get_error_estimate_order() const =0
Derived classes must override this function to return the order of the integrator&#39;s error estimate...
void set_target_accuracy(double accuracy)
Request that the integrator attempt to achieve a particular accuracy for the continuous portions of t...
Definition: integrator_base.h:230
std::pair< bool, T > CalcAdjustedStepSize(const T &err, const T &attempted_step_size, bool *at_minimum_step_size) const
Calculates adjusted integrator step sizes toward keeping state variables within error bounds on the n...
Definition: integrator_base.h:1582
Indicates a publish time has been reached but not an update time.
Definition: integrator_base.h:128
int64_t get_num_substep_failures() const
Gets the number of failed sub-steps (implying one or more step size reductions was required to permit...
Definition: integrator_base.h:683
void ResetStatistics()
Forget accumulated statistics.
Definition: integrator_base.h:668
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
void set_smallest_adapted_step_size_taken(const T &dt)
Sets the size of the smallest-step-taken statistic as the result of a controlled integration step adj...
Definition: integrator_base.h:1257
const T & get_largest_step_size_taken() const
The size of the largest step taken since the last Initialize() or ResetStatistics() call...
Definition: integrator_base.h:732