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