Drake
integrator_base.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <algorithm>
4 #include <limits>
5 #include <memory>
6 #include <utility>
7 
14 
15 namespace drake {
16 namespace systems {
17 
18 /*
19 TODO(edrumwri): comments below in preparation of upcoming functionality
20  Note: we ensure algorithmically that no report time, scheduled time, or
21 final time t can occur *within* an event window, that is, we will never have
22  t_low < t < t_high for any interesting t. Further, t_report, t_scheduled and
23  t_final can coincide with t_high but only t_report can be at t_low. The
24  interior of t_low:t_high is a "no man's land" where we don't understand the
25 solution, so must be avoided.
26 */
27 
106 template <class T>
108  public:
110 
111 
122  // TODO(edrumwri): incorporate kReachedZeroCrossing into the simulator.
123  enum StepResult {
147  };
148 
162  explicit IntegratorBase(const System<T>& system,
163  Context<T>* context = nullptr)
164  : system_(system), context_(context) {
165  initialization_done_ = false;
166  }
167 
169  virtual ~IntegratorBase() = default;
170 
175  virtual bool supports_error_estimation() const = 0;
176 
185  void set_fixed_step_mode(bool flag) {
186  if (!flag && !supports_error_estimation())
187  throw std::logic_error("Integrator does not support accuracy estimation");
188  fixed_step_mode_ = flag;
189  }
190 
200  bool get_fixed_step_mode() const {
201  return (!supports_error_estimation() || fixed_step_mode_);
202  }
203 
227  // TODO(edrumwri): complain if integrator with error estimation wants to drop
228  // below the minimum step size
229  void set_target_accuracy(double accuracy) {
231  throw std::logic_error(
232  "Integrator does not support accuracy estimation "
233  "and user has requested error control");
234  target_accuracy_ = accuracy;
235  accuracy_in_use_ = accuracy;
236  }
237 
242  double get_target_accuracy() const { return target_accuracy_; }
243 
249  double get_accuracy_in_use() const { return accuracy_in_use_; }
250 
257  // TODO(edrumwri): Update this comment when stretch size is configurable.
258  void set_maximum_step_size(const T& max_step_size) {
259  DRAKE_ASSERT(max_step_size >= 0.0);
260  max_step_size_ = max_step_size;
261  }
262 
269  // TODO(edrumwri): Update this comment when stretch size is configurable.
270  const T& get_maximum_step_size() const { return max_step_size_; }
271 
347  void set_requested_minimum_step_size(const T& min_step_size) {
348  DRAKE_ASSERT(min_step_size >= 0.0);
349  req_min_step_size_ = min_step_size;
350  }
351 
358  return req_min_step_size_; }
359 
369  min_step_exceeded_throws_ = throws;
370  }
371 
378  return min_step_exceeded_throws_;
379  }
380 
388  using std::max;
389  // Tolerance is just a number close to machine epsilon.
390  const double tol = 1e-14;
391  const T smart_minimum = max(tol, get_context().get_time()*tol);
392  return max(smart_minimum, req_min_step_size_);
393  }
395 
400  void Reset() {
401  // Kill the error estimate and weighting matrices.
402  err_est_.reset();
403  qbar_weight_.setZero(0);
404  z_weight_.setZero(0);
405  pinvN_dq_change_.reset();
406  unweighted_substate_change_.setZero(0);
407  weighted_q_change_.reset();
408 
409  // Integrator no longer operates in fixed step mode.
410  fixed_step_mode_ = false;
411 
412  // Statistics no longer valid.
413  ResetStatistics();
414 
415  // Wipe out settings.
416  req_min_step_size_ = 0;
417  max_step_size_ = nan();
418  accuracy_in_use_ = nan();
419 
420  // Indicate values used for error controlled integration no longer valid.
421  prev_step_size_ = nan();
422  ideal_next_step_size_ = nan();
423 
424  // Call the derived integrator reset routine.
425  DoReset();
426 
427  // Indicate that initialization is necessary.
428  initialization_done_ = false;
429  }
430 
446  void Initialize() {
447  if (!context_) throw std::logic_error("Context has not been set.");
448 
449  // Verify that user settings are reasonable.
450  if (max_step_size_ < req_min_step_size_) {
451  throw std::logic_error("Integrator maximum step size is less than the "
452  "minimum step size");
453  }
454  if (req_initial_step_size_ > max_step_size_) {
455  throw std::logic_error("Requested integrator initial step size is larger "
456  "than the maximum step size.");
457  }
458  if (req_initial_step_size_ < req_min_step_size_) {
459  throw std::logic_error("Requested integrator initial step size is smaller"
460  " than the minimum step size.");
461  }
462 
463  // TODO(edrumwri): Compute qbar_weight_, z_weight_ automatically.
464  // Set error weighting vectors if not already done.
466  // Allocate space for the error estimate.
467  err_est_ = system_.AllocateTimeDerivatives();
468 
469  const auto& xc = context_->get_state().get_continuous_state();
470  const int gv_size = xc->get_generalized_velocity().size();
471  const int misc_size = xc->get_misc_continuous_state().size();
472  if (qbar_weight_.size() != gv_size) qbar_weight_.setOnes(gv_size);
473  if (z_weight_.size() != misc_size) z_weight_.setOnes(misc_size);
474 
475  // Verify that minimum values of the weighting matrices are non-negative.
476  if ((qbar_weight_.size() && qbar_weight_.minCoeff() < 0) ||
477  (z_weight_.size() && z_weight_.minCoeff() < 0))
478  throw std::logic_error("Scaling coefficient is less than zero.");
479  }
480 
481  // Statistics no longer valid.
482  ResetStatistics();
483 
484  // Call the derived integrator initialization routine (if any)
485  DoInitialize();
486 
487  initialization_done_ = true;
488  }
489 
503  void request_initial_step_size_target(const T& step_size) {
504  using std::isnan;
506  throw std::logic_error(
507  "Integrator does not support error estimation and "
508  "user has initial step size target");
509  req_initial_step_size_ = step_size;
510  }
511 
518  const T& get_initial_step_size_target() const {
519  return req_initial_step_size_;
520  }
521 
551  // TODO(edrumwri): Make the stretch size configurable.
552  StepResult IntegrateAtMost(const T& publish_dt, const T& update_dt,
553  const T& boundary_dt);
554 
560  double get_stretch_factor() const { return 1.01; }
561 
585  using std::max;
586  using std::min;
587 
588  const Context<T>& context = get_context();
589  const T inf = std::numeric_limits<double>::infinity();
590  T t_remaining = dt;
591 
592  // Note: A concern below is that the while loop while run forever because
593  // t_remaining could be small, but not quite zero, if dt is relatively
594  // small compared to the context time. In such a case, t_final will be
595  // equal to context.get_time() in the expression immediately below,
596  // context.get_time() will not change during the call to IntegrateAtMost(),
597  // and t_remaining will be equal to zero (meaning that the loop will
598  // indeed terminate, as desired).
599  const T t_final = context.get_time() + t_remaining;
600  do {
601  IntegrateAtMost(inf, inf, min(t_remaining, get_maximum_step_size()));
602  t_remaining = t_final - context.get_time();
603  } while (t_remaining > 0);
604  }
605 
638  if (dt < 0) {
639  throw std::logic_error("IntegrateWithSingleFixedStep() called with a "
640  "negative step size.");
641  }
642  if (!this->get_fixed_step_mode())
643  throw std::logic_error("IntegrateWithSingleFixedStep() requires fixed "
644  "stepping.");
645  if (!Step(dt)) {
646  throw std::runtime_error("Integrator was unable to take a single fixed "
647  "step of the requested size.");
648  }
649 
650  UpdateStepStatistics(dt);
651  }
652 
668  actual_initial_step_size_taken_ = nan();
669  smallest_adapted_step_size_taken_ = nan();
670  largest_step_size_taken_ = nan();
671  num_steps_taken_ = 0;
672  num_ode_evals_ = 0;
673  num_shrinkages_from_error_control_ = 0;
674  num_shrinkages_from_substep_failures_ = 0;
675  num_substep_failures_ = 0;
677  }
678 
682  int64_t get_num_substep_failures() const {
683  return num_substep_failures_;
684  }
685 
690  return num_shrinkages_from_substep_failures_;
691  }
692 
696  return num_shrinkages_from_error_control_;
697  }
698 
708  int64_t get_num_derivative_evaluations() const { return num_ode_evals_; }
709 
714  return actual_initial_step_size_taken_;
715  }
716 
724  return smallest_adapted_step_size_taken_;
725  }
726 
731  const T& get_largest_step_size_taken() const {
732  return largest_step_size_taken_;
733  }
734 
739  int64_t get_num_steps_taken() const { return num_steps_taken_; }
740 
751  const T& get_ideal_next_step_size() const { return ideal_next_step_size_; }
752 
758  const Context<T>& get_context() const { return *context_; }
759 
764  Context<T>* get_mutable_context() { return context_; }
765 
775  void reset_context(Context<T>* context) {
776  context_ = context;
777  initialization_done_ = false;
778  }
779 
784  const System<T>& get_system() const { return system_; }
785 
787  bool is_initialized() const { return initialization_done_; }
788 
794  virtual int get_error_estimate_order() const = 0;
795 
801  return prev_step_size_;
802  }
803 
810  return err_est_.get();
811  }
812 
1010  const Eigen::VectorXd& get_generalized_state_weight_vector() const {
1011  return qbar_weight_;
1012  }
1013 
1026  Eigen::VectorBlock<Eigen::VectorXd>
1028  initialization_done_ = false;
1029  return qbar_weight_.head(qbar_weight_.rows());
1030  }
1031 
1037  const Eigen::VectorXd& get_misc_state_weight_vector() const {
1038  return z_weight_;
1039  }
1040 
1052  Eigen::VectorBlock<Eigen::VectorXd> get_mutable_misc_state_weight_vector() {
1053  initialization_done_ = false;
1054  return z_weight_.head(z_weight_.rows());
1055  }
1056 
1061  protected:
1066  virtual void DoResetStatistics() {}
1067 
1071  void CalcTimeDerivatives(const Context<T>& context,
1072  ContinuousState<T>* dxdt) {
1073  get_system().CalcTimeDerivatives(context, dxdt);
1074  ++num_ode_evals_;
1075  }
1076 
1082  template <typename U>
1083  void CalcTimeDerivatives(const System<U>& system,
1084  const Context<U>& context,
1085  ContinuousState<U>* dxdt) {
1086  system.CalcTimeDerivatives(context, dxdt);
1087  ++num_ode_evals_;
1088  }
1089 
1097  void set_accuracy_in_use(double accuracy) { accuracy_in_use_ = accuracy; }
1098 
1119  bool StepOnceErrorControlledAtMost(const T& dt_max);
1120 
1128  T CalcStateChangeNorm(const ContinuousState<T>& dx_state) const;
1129 
1149  std::pair<bool, T> CalcAdjustedStepSize(const T& err,
1150  bool dt_was_artificially_limited,
1151  const T& current_step_size) const;
1152 
1158  virtual void DoInitialize() {}
1159 
1164  virtual void DoReset() {}
1165 
1179  virtual bool DoStep(const T& dt) = 0;
1180 
1186  ContinuousState<T>* get_mutable_error_estimate() { return err_est_.get(); }
1187 
1188  // Sets the actual initial step size taken.
1190  actual_initial_step_size_taken_ = dt;
1191  }
1192 
1198  smallest_adapted_step_size_taken_ = dt;
1199  }
1200 
1201  // Sets the largest-step-size-taken statistic.
1202  void set_largest_step_size_taken(const T& dt) {
1203  largest_step_size_taken_ = dt;
1204  }
1205 
1206  // Sets the "ideal" next step size (typically done via error control).
1207  void set_ideal_next_step_size(const T& dt) { ideal_next_step_size_ = dt; }
1208 
1209  private:
1210  // Validates that a smaller step size does not fall below the working minimum
1211  // and throws an exception if desired.
1212  void ValidateSmallerStepSize(const T& current_step_size,
1213  const T& new_step_size) const {
1214  if (new_step_size < get_working_minimum_step_size() &&
1215  new_step_size < current_step_size && // Verify step adjusted downward.
1216  min_step_exceeded_throws_) {
1217  SPDLOG_DEBUG(drake::log(), "Integrator wants to select too small step "
1218  "size of {}; working minimum is ", new_step_size,
1220  std::ostringstream str;
1221  str << "Error control wants to select step smaller than minimum" <<
1222  " allowed (" << get_working_minimum_step_size() << ")";
1223  throw std::runtime_error(str.str());
1224  }
1225  }
1226 
1227  // Updates the integrator statistics, accounting for a step just taken of
1228  // size dt.
1229  void UpdateStepStatistics(const T& dt) {
1230  // Handle first step specially.
1231  if (++num_steps_taken_ == 1) {
1234  } else {
1236  }
1237 
1238  // Update the previous step size.
1239  prev_step_size_ = dt;
1240  }
1241 
1242  // Steps the system forward exactly by @p dt, if possible, by calling DoStep.
1243  // Does necessary pre-initialization and post-cleanup. This method does not
1244  // update general integrator statistics (which are updated in the calling
1245  // methods), because error control might decide that it does not like the
1246  // result of the step and might "rewind" and take a smaller one.
1247  // @returns `true` if successful, `false` otherwise (due to, e.g., integrator
1248  // convergence failure).
1249  // @sa DoStep()
1250  bool Step(const T& dt) {
1251  if (!DoStep(dt))
1252  return false;
1253  return true;
1254  }
1255 
1256  // Reference to the system being simulated.
1257  const System<T>& system_;
1258 
1259  // Pointer to the context.
1260  Context<T>* context_{nullptr}; // The trajectory Context.
1261 
1262  // Runtime variables.
1263  // For variable step integrators, this is set at the end of each step to guide
1264  // the next one.
1265  T ideal_next_step_size_{nan()}; // Indicates that the value is uninitialized.
1266 
1267  // The scaling factor to apply to an integration step size when an integrator
1268  // convergence failure occurs (to make convergence more likely on the next
1269  // attempt).
1270  // TODO(edrumwri): Allow subdivision factor to be user-tweakable.
1271  const double subdivision_factor_{0.5};
1272 
1273  // The accuracy being used.
1274  double accuracy_in_use_{nan()};
1275 
1276  // The maximum step size.
1277  T max_step_size_{nan()};
1278 
1279  // The minimum step size.
1280  T req_min_step_size_{0};
1281 
1282  // The last step taken by the integrator.
1283  T prev_step_size_{nan()};
1284 
1285  // Whether error-controlled integrator is running in fixed step mode. Value
1286  // is irrelevant for integrators without error estimation capabilities.
1287  bool fixed_step_mode_{false};
1288 
1289  // When the minimum step is exceeded, does the integrator throw an exception?
1290  bool min_step_exceeded_throws_{true};
1291 
1292  // Statistics.
1293  T actual_initial_step_size_taken_{nan()};
1294  T smallest_adapted_step_size_taken_{nan()};
1295  T largest_step_size_taken_{nan()};
1296  int64_t num_steps_taken_{0};
1297  int64_t num_ode_evals_{0};
1298  int64_t num_shrinkages_from_error_control_{0};
1299  int64_t num_shrinkages_from_substep_failures_{0};
1300  int64_t num_substep_failures_{0};
1301 
1302  // Applied as diagonal matrices to weight state change variables.
1303  Eigen::VectorXd qbar_weight_, z_weight_;
1304 
1305  // State copy for reversion during error-controlled integration.
1306  VectorX<T> xc0_save_;
1307 
1308  // The error estimate computed during integration with error control.
1309  std::unique_ptr<ContinuousState<T>> err_est_;
1310 
1311  // The pseudo-inverse of the matrix that converts time derivatives of
1312  // generalized coordinates to generalized velocities, multiplied by the
1313  // change in the generalized coordinates (used in state change norm
1314  // calculations).
1315  mutable std::unique_ptr<VectorBase<T>> pinvN_dq_change_;
1316 
1317  // Vectors used in state change norm calculations.
1318  mutable VectorX<T> unweighted_substate_change_;
1319  mutable std::unique_ptr<VectorBase<T>> weighted_q_change_;
1320 
1321  // Variable for indicating when an integrator has been initialized.
1322  bool initialization_done_{false};
1323 
1324  // This a workaround for an apparent bug in clang 3.8 in which
1325  // defining this as a static constexpr member kNaN failed to instantiate
1326  // properly for the AutoDiffXd instantiation (worked in gcc and MSVC).
1327  // Restore to sanity when some later clang is current.
1328  static constexpr double nan() {
1329  return std::numeric_limits<double>::quiet_NaN();
1330  }
1331 
1332  double target_accuracy_{nan()}; // means "unspecified, use default"
1333  T req_initial_step_size_{nan()}; // means "unspecified, use default"
1334 };
1335 
1336 template <class T>
1338  using std::isnan;
1339  using std::min;
1340 
1341  // Verify that the integrator supports error estimates.
1342  if (!supports_error_estimation()) {
1343  throw std::logic_error("StepOnceErrorControlledAtMost() requires error "
1344  "estimation.");
1345  }
1346 
1347  // Save time, continuous variables, and time derivative because we'll possibly
1348  // revert time and state.
1349  const Context<T>& context = get_context();
1350  const T current_time = context.get_time();
1351  VectorBase<T>* xc =
1352  get_mutable_context()->get_mutable_continuous_state_vector();
1353  xc0_save_ = xc->CopyToVector();
1354 
1355  // Set the "current" step size.
1356  T current_step_size = get_ideal_next_step_size();
1357  if (isnan(current_step_size)) {
1358  // Integrator has not taken a step. Set the current step size to the
1359  // initial step size.
1360  current_step_size = get_initial_step_size_target();
1361  DRAKE_DEMAND(!isnan(current_step_size));
1362  }
1363 
1364  bool step_succeeded = false;
1365  do {
1366  // Constants used to determine whether modifications to the step size are
1367  // close enough to the desired step size to use the unadjusted originals.
1368  const double near_enough_smaller = 0.95;
1369  const double near_enough_larger = 1.001;
1370 
1371  // If we lose more than a small fraction of the step size we wanted
1372  // to take due to a need to stop at dt_max, make a note of that so the
1373  // step size adjuster won't try to grow from the current step.
1374  bool dt_was_artificially_limited = false;
1375  if (dt_max < near_enough_smaller * current_step_size) {
1376  // dt_max much smaller than current step size.
1377  dt_was_artificially_limited = true;
1378  current_step_size = dt_max;
1379  } else {
1380  if (dt_max < near_enough_larger * current_step_size)
1381  current_step_size = dt_max; // dt_max is roughly current step.
1382  }
1383 
1384  // Limit the current step size.
1385  current_step_size = min(current_step_size, get_maximum_step_size());
1386 
1387  // Keep adjusting the integration step size until any integrator
1388  // convergence failures disappear.
1389  T adjusted_step_size = current_step_size;
1390  while (!Step(adjusted_step_size)) {
1391  SPDLOG_DEBUG(drake::log(), "Sub-step failed at {}", adjusted_step_size);
1392  adjusted_step_size *= subdivision_factor_;
1393  ValidateSmallerStepSize(current_step_size, adjusted_step_size);
1394  dt_was_artificially_limited = true;
1395  ++num_shrinkages_from_substep_failures_;
1396  ++num_substep_failures_;
1397  }
1398  current_step_size = adjusted_step_size;
1399 
1400  //--------------------------------------------------------------------
1401  T err_norm = CalcStateChangeNorm(*get_error_estimate());
1402  std::tie(step_succeeded, current_step_size) = CalcAdjustedStepSize(
1403  err_norm, dt_was_artificially_limited, current_step_size);
1404  SPDLOG_DEBUG(drake::log(), "Adjusted step size: {}", current_step_size);
1405 
1406  if (step_succeeded) {
1407  ideal_next_step_size_ = current_step_size;
1409  set_actual_initial_step_size_taken(current_step_size);
1410 
1411  // Record the adapted step size taken.
1413  (current_step_size < get_smallest_adapted_step_size_taken() &&
1414  current_step_size < dt_max))
1415  set_smallest_adapted_step_size_taken(current_step_size);
1416  } else {
1417  ++num_shrinkages_from_error_control_;
1418 
1419  // Reset the time, state, and time derivative at t0.
1420  get_mutable_context()->set_time(current_time);
1421  xc->SetFromVector(xc0_save_);
1422  }
1423  } while (!step_succeeded);
1424 
1425  return (current_step_size == dt_max);
1426 }
1427 
1428 template <class T>
1430  const ContinuousState<T>& dx_state) const {
1431  using std::max;
1432  const Context<T>& context = get_context();
1433  const auto& system = get_system();
1434 
1435  // Get weighting matrices.
1436  const auto& qbar_v_weight = this->get_generalized_state_weight_vector();
1437  const auto& z_weight = this->get_misc_state_weight_vector();
1438 
1439  // Get the differences in the generalized position, velocity, and
1440  // miscellaneous continuous state vectors.
1441  const VectorBase<T>& dgq = dx_state.get_generalized_position();
1442  const VectorBase<T>& dgv = dx_state.get_generalized_velocity();
1443  const VectorBase<T>& dgz = dx_state.get_misc_continuous_state();
1444 
1445  // (re-)Initialize pinvN_dq_change_ and weighted_q_change_, if necessary.
1446  // Reinitialization might be required if the system state variables can
1447  // change during the course of the simulation.
1448  if (pinvN_dq_change_ == nullptr) {
1449  pinvN_dq_change_ = std::make_unique<BasicVector<T>>(dgv.size());
1450  weighted_q_change_ = std::make_unique<BasicVector<T>>(dgq.size());
1451  }
1452  DRAKE_DEMAND(pinvN_dq_change_->size() == dgv.size());
1453  DRAKE_DEMAND(weighted_q_change_->size() == dgq.size());
1454 
1455  // TODO(edrumwri): Acquire characteristic time properly from the system
1456  // (i.e., modify the System to provide this value).
1457  const double characteristic_time = 1.0;
1458 
1459  // Computes the infinity norm of the weighted velocity variables.
1460  unweighted_substate_change_ = dgv.CopyToVector();
1461  T v_nrm = qbar_v_weight.cwiseProduct(unweighted_substate_change_).
1462  template lpNorm<Eigen::Infinity>() * characteristic_time;
1463 
1464  // Compute the infinity norm of the weighted auxiliary variables.
1465  unweighted_substate_change_ = dgz.CopyToVector();
1466  T z_nrm = (z_weight.cwiseProduct(unweighted_substate_change_))
1467  .template lpNorm<Eigen::Infinity>();
1468 
1469  // Compute N * Wq * dq = N * Wꝗ * N+ * dq.
1470  unweighted_substate_change_ = dgq.CopyToVector();
1471  system.MapQDotToVelocity(context, unweighted_substate_change_,
1472  pinvN_dq_change_.get());
1473  system.MapVelocityToQDot(
1474  context, qbar_v_weight.cwiseProduct(pinvN_dq_change_->CopyToVector()),
1475  weighted_q_change_.get());
1476  T q_nrm = weighted_q_change_->CopyToVector().
1477  template lpNorm<Eigen::Infinity>();
1478  SPDLOG_DEBUG(drake::log(), "dq norm: {}, dv norm: {}, dz norm: {}",
1479  q_nrm, v_nrm, z_nrm);
1480 
1481  // TODO(edrumwri): Record the worst offender (which of the norms resulted
1482  // in the largest value).
1483  // Infinity norm of the concatenation of multiple vectors is equal to the
1484  // maximum of the infinity norms of the individual vectors.
1485  return max(z_nrm, max(q_nrm, v_nrm));
1486 }
1487 
1488 template <class T>
1490  const T& err, bool dt_was_artificially_limited,
1491  const T& current_step_size) const {
1492  using std::pow;
1493  using std::min;
1494  using std::max;
1495  using std::isnan;
1496  using std::isinf;
1497 
1498  // Magic numbers come from Simbody.
1499  const double kSafety = 0.9;
1500  const double kMinShrink = 0.1;
1501  const double kMaxGrow = 5.0;
1502  const double kHysteresisLow = 0.9;
1503  const double kHysteresisHigh = 1.2;
1504 
1506  const int err_order = get_error_estimate_order();
1507 
1509  T new_step_size(-1);
1510 
1511  // First, make a first guess at the next step size to use based on
1512  // the supplied error norm. Watch out for NaN!
1513  if (isnan(err) || isinf(err)) // e.g., integrand returned NaN.
1514  new_step_size = kMinShrink * current_step_size;
1515  else if (err == 0) // A "perfect" step; can happen if no dofs for example.
1516  new_step_size = kMaxGrow * current_step_size;
1517  else // Choose best step for skating just below the desired accuracy.
1518  new_step_size = kSafety * current_step_size *
1519  pow(get_accuracy_in_use() / err, 1.0 / err_order);
1520 
1521  // If the new step is bigger than the old, don't make the change if the
1522  // old one was small for some unimportant reason (like reached a publishing
1523  // interval). Also, don't grow the step size if the change would be very
1524  // small; better to keep the step size stable in that case (maybe just
1525  // for aesthetic reasons).
1526  if (new_step_size > current_step_size) {
1527  if (dt_was_artificially_limited ||
1528  new_step_size < kHysteresisHigh * current_step_size)
1529  new_step_size = current_step_size;
1530  }
1531 
1532  // If we're supposed to shrink the step size but the one we have actually
1533  // achieved the desired accuracy last time, we won't change the step now.
1534  // Otherwise, if we are going to shrink the step, let's not be shy -- we'll
1535  // shrink it by at least a factor of kHysteresisLow.
1536  if (new_step_size < current_step_size) {
1537  if (err <= get_accuracy_in_use()) {
1538  new_step_size = current_step_size; // not this time
1539  } else {
1540  T test_value = kHysteresisLow * current_step_size;
1541  new_step_size = min(new_step_size, test_value);
1542  }
1543  }
1544 
1545  // Keep the size change within the allowable bounds.
1546  T max_grow_step = kMaxGrow * current_step_size;
1547  T min_shrink_step = kMinShrink * current_step_size;
1548  new_step_size = min(new_step_size, max_grow_step);
1549  new_step_size = max(new_step_size, min_shrink_step);
1550 
1551  // Apply user-requested limits on min and max step size.
1552  // TODO(edrumwri): Introduce some feedback to the user when integrator wants
1553  // to take a smaller step than user has selected as the minimum. Options for
1554  // this feedback could include throwing a special exception, logging, setting
1555  // a flag in the integrator that allows throwing an exception, or returning
1556  // a special status from IntegrateAtMost().
1557  if (!isnan(get_maximum_step_size()))
1558  new_step_size = min(new_step_size, get_maximum_step_size());
1559  ValidateSmallerStepSize(current_step_size, new_step_size);
1560  new_step_size = max(new_step_size, get_working_minimum_step_size());
1561 
1562  return std::make_pair(new_step_size >= current_step_size, new_step_size);
1563 }
1564 
1565 template <class T>
1567  const T& publish_dt, const T& update_dt, const T& boundary_dt) {
1569  throw std::logic_error("Integrator not initialized.");
1570 
1571  // Verify that update dt is positive.
1572  if (update_dt <= 0.0)
1573  throw std::logic_error("Update dt must be strictly positive.");
1574 
1575  // Verify that other dt's are non-negative.
1576  if (publish_dt < 0.0)
1577  throw std::logic_error("Publish dt is negative.");
1578  if (boundary_dt < 0.0)
1579  throw std::logic_error("Boundary dt is negative.");
1580 
1581  // The size of the integration step is the minimum of the time until the next
1582  // update event, the time until the next publish event, the boundary time
1583  // (i.e., the maximum time that the user wished to step to), and the maximum
1584  // step size (which may stretch slightly to hit a discrete event).
1585 
1586  // We report to the caller which event ultimately constrained the step size.
1587  // If multiple events constrained it equally, we prefer to report update
1588  // events over publish events, publish events over boundary step limits,
1589  // and boundary limits over maximum step size limits. The caller must
1590  // determine event simultaneity by inspecting the time.
1591 
1592  // The maintainer of this code is advised to consider that, while updates
1593  // and boundary times, may both conceptually be deemed events, the distinction
1594  // is made for a reason. If both an update and a boundary time occur
1595  // simultaneously, the following behavior should result:
1596  // (1) kReachedUpdateTime is returned, (2) Simulator::StepTo() performs the
1597  // necessary update, (3) IntegrateAtMost() is called with boundary_dt=0 and
1598  // returns kReachedBoundaryTime, and (4) the simulation terminates. This
1599  // sequence of operations will ensure that the simulation state is valid if
1600  // Simulator::StepTo() is called again to advance time further.
1601 
1602  // We now analyze the following simultaneous cases with respect to Simulator:
1603  //
1604  // { publish, update }
1605  // kReachedUpdateTime will be returned, an update will be followed by a
1606  // publish.
1607  //
1608  // { publish, update, max step }
1609  // kReachedUpdateTime will be returned, an update will be followed by a
1610  // publish.
1611  //
1612  // { publish, boundary time, max step }
1613  // kReachedPublishTime will be returned, a publish will be performed followed
1614  // by another call to this function, which should return kReachedBoundaryTime
1615  // (followed in rapid succession by StepTo(.) return).
1616  //
1617  // { publish, boundary time, max step }
1618  // kReachedPublishTime will be returned, a publish will be performed followed
1619  // by another call to this function, which should return kReachedBoundaryTime
1620  // (followed in rapid succession by StepTo(.) return).
1621  //
1622  // { publish, update, boundary time, maximum step size }
1623  // kUpdateTimeReached will be returned, an update followed by a publish
1624  // will then be performed followed by another call to this function, which
1625  // should return kReachedBoundaryTime (followed in rapid succession by
1626  // StepTo(.) return).
1627 
1628  // By default, the candidate dt is the next discrete update event.
1630  T dt = update_dt;
1631 
1632  // If the next discrete publish event is sooner than the next discrete update
1633  // event, the publish event becomes the candidate dt
1634  if (publish_dt < update_dt) {
1635  candidate_result = IntegratorBase<T>::kReachedPublishTime;
1636  dt = publish_dt;
1637  }
1638 
1639  // If the stop time (boundary time) is sooner than the candidate, use it
1640  // instead.
1641  if (boundary_dt < dt) {
1642  candidate_result = IntegratorBase<T>::kReachedBoundaryTime;
1643  dt = boundary_dt;
1644  }
1645 
1646  // If there is no continuous state, there will be no need to limit the
1647  // integration step size.
1648  if (get_context().get_continuous_state()->size() == 0) {
1649  Context<T>* context = get_mutable_context();
1650  context->set_time(context->get_time() + dt);
1651  return candidate_result;
1652  }
1653 
1654  // If all events are farther into the future than the maximum step
1655  // size times a stretch factor of 1.01, the maximum step size becomes the
1656  // candidate dt. Put another way, if the maximum step occurs right before
1657  // an update or a publish, the update or publish is done instead. In contrast,
1658  // we never step past boundary_dt, even if doing so would allow hitting a
1659  // publish or an update.
1660  const bool reached_boundary =
1661  (candidate_result == IntegratorBase<T>::kReachedBoundaryTime);
1662  const T& max_dt = IntegratorBase<T>::get_maximum_step_size();
1663  if ((reached_boundary && max_dt < dt) ||
1664  (!reached_boundary && max_dt * get_stretch_factor() < dt)) {
1665  candidate_result = IntegratorBase<T>::kTimeHasAdvanced;
1666  dt = max_dt;
1667  }
1668 
1669  if (dt < 0.0) throw std::logic_error("Negative dt.");
1670 
1671  // If error control is disabled, call the generic stepper. Otherwise, use
1672  // the error controlled method.
1673  bool full_step = true;
1674  if (this->get_fixed_step_mode()) {
1675  T adjusted_dt = dt;
1676  while (!Step(adjusted_dt)) {
1677  ++num_shrinkages_from_substep_failures_;
1678  ++num_substep_failures_;
1679  adjusted_dt *= subdivision_factor_;
1680  ValidateSmallerStepSize(dt, adjusted_dt);
1681  full_step = false;
1682  }
1683  } else {
1684  full_step = StepOnceErrorControlledAtMost(dt);
1685  }
1686 
1687  // Update generic statistics.
1688  UpdateStepStatistics(dt);
1689 
1690  if (full_step) {
1691  // If the integrator took the entire maximum step size we allowed above,
1692  // we report to the caller that a step constraint was hit, which may
1693  // indicate a discrete event has arrived.
1694  return candidate_result;
1695  } else {
1696  // Otherwise, we report to the caller that time has advanced, but no
1697  // discrete event has arrived.
1699  }
1700 }
1701 
1702 } // namespace systems
1703 } // 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:1083
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:387
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:242
void set_largest_step_size_taken(const T &dt)
Definition: integrator_base.h:1202
void CalcTimeDerivatives(const Context< T > &context, ContinuousState< T > *dxdt)
Evaluates the derivative function (and updates call statistics).
Definition: integrator_base.h:1071
bool isnan(const Eigen::AutoDiffScalar< DerType > &x)
Overloads isnan to mimic std::isnan from <cmath>.
Definition: autodiff_overloads.h:50
Localized an event; this is the before state (interpolated).
Definition: integrator_base.h:131
void set_ideal_next_step_size(const T &dt)
Definition: integrator_base.h:1207
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:1010
const T & get_actual_initial_step_size_taken() const
The actual size of the successful first step.
Definition: integrator_base.h:713
virtual void DoReset()
Derived classes can override this method to perform routines when Reset() is called.
Definition: integrator_base.h:1164
#define SPDLOG_DEBUG(logger,...)
Definition: text_logging.h:93
NOTE: The contents of this class are for the most part direct ports of drake/systems/plants//inverseK...
Definition: automotive_demo.cc:88
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:764
const T & get_previous_integration_step_size() const
Gets the size of the last (previous) integration step.
Definition: integrator_base.h:800
void Initialize()
An integrator must be initialized before being used.
Definition: integrator_base.h:446
double get_stretch_factor() const
Gets the stretch factor (> 1), which is multiplied by the maximum (typically user-designated) integra...
Definition: integrator_base.h:560
Context is an abstract base class template that represents all the inputs to a System: time...
Definition: context.h:38
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorX
A column vector of any size, templated on scalar type.
Definition: eigen_types.h:43
VectorBase is an abstract base class that real-valued signals between Systems and real-valued System ...
Definition: vector_base.h:27
Eigen::VectorBlock< Eigen::VectorXd > get_mutable_generalized_state_weight_vector()
Gets a mutable weighting vector (equivalent to a diagonal matrix) applied to weighting both generaliz...
Definition: integrator_base.h:1027
const VectorBase< T > & get_misc_continuous_state() const
Returns the subset of the continuous state vector that is other continuous state z.
Definition: continuous_state.h:123
void set_accuracy_in_use(double accuracy)
Sets the working ("in use") accuracy for this integrator.
Definition: integrator_base.h:1097
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:368
logging::logger * log()
Retrieve an instance of a logger to use for logging; for example: drake::log()->info("potato!") ...
Definition: text_logging.cc:38
void request_initial_step_size_target(const T &step_size)
Request that the first attempted integration step have a particular size.
Definition: integrator_base.h:503
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:258
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:758
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:637
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:1186
const T & get_initial_step_size_target() const
Gets the target size of the first integration step.
Definition: integrator_base.h:518
#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:39
bool get_fixed_step_mode() const
Gets whether an integrator is running in fixed step mode.
Definition: integrator_base.h:200
StepResult
Status returned by StepOnceAtMost().
Definition: integrator_base.h:123
const ContinuousState< T > * get_error_estimate() const
Gets the error estimate (used only for integrators that support error estimation).
Definition: integrator_base.h:809
virtual void set_time(const T &time_sec)
Set the current time in seconds.
Definition: context.h:52
User requested control whenever an internal step is successful.
Definition: integrator_base.h:139
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:1337
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:673
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:1566
Took maximum number of steps without finishing integrating over the interval.
Definition: integrator_base.h:146
Polynomial< CoefficientType > pow(const Polynomial< CoefficientType > &p, typename Polynomial< CoefficientType >::PowerType n)
Provides power function for Polynomial.
Definition: polynomial.h:478
#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:47
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:708
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:784
std::pair< bool, T > CalcAdjustedStepSize(const T &err, bool dt_was_artificially_limited, const T &current_step_size) const
Calculates adjusted integrator step sizes toward keeping state variables within error bounds on the n...
Definition: integrator_base.h:1489
Expression min(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:661
void set_fixed_step_mode(bool flag)
Sets an integrator with error control to fixed step mode.
Definition: integrator_base.h:185
Reached the desired integration time without reaching an update time.
Definition: integrator_base.h:143
void Reset()
Resets the integrator to initial values, i.e., default construction values.
Definition: integrator_base.h:400
void set_actual_initial_step_size_taken(const T &dt)
Definition: integrator_base.h:1189
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:751
const T & get_time() const
Returns the current time in seconds.
Definition: context.h:49
double get_accuracy_in_use() const
Gets the accuracy in use by the integrator.
Definition: integrator_base.h:249
const T & get_maximum_step_size() const
Gets the maximum step size that may be taken by this integrator.
Definition: integrator_base.h:270
const VectorBase< T > & get_generalized_position() const
Returns the subset of the state vector that is generalized position q.
Definition: continuous_state.h:99
A superclass template for systems that receive input, maintain state, and produce output of a given m...
Definition: input_port_descriptor.h:10
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:1037
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:347
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:739
const T & get_smallest_adapted_step_size_taken() const
The size of the smallest step taken as the result of a controlled integration step adjustment since t...
Definition: integrator_base.h:723
virtual void DoInitialize()
Derived classes can override this method to perform special initialization.
Definition: integrator_base.h:1158
void CalcTimeDerivatives(const Context< T > &context, ContinuousState< T > *derivatives) const
Calculates the time derivatives xcdot of the continuous state xc.
Definition: system.h:436
T CalcStateChangeNorm(const ContinuousState< T > &dx_state) const
Computes the infinity norm of a change in continuous state.
Definition: integrator_base.h:1429
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:689
void IntegrateWithMultipleSteps(const T &dt)
Stepping function for integrators operating outside of Simulator that advances the continuous state e...
Definition: integrator_base.h:584
Indicates that integration terminated at an update time.
Definition: integrator_base.h:135
void reset_context(Context< T > *context)
Replace the pointer to the internally-maintained Context with a different one.
Definition: integrator_base.h:775
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:162
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:695
const double dt
Definition: runge_kutta3_integrator_test.cc:38
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:377
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:1066
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:1052
const VectorBase< T > & get_generalized_velocity() const
Returns the subset of the continuous state vector that is generalized velocity v. ...
Definition: continuous_state.h:111
An abstract class for an integrator for ODEs and DAEs as represented by a Drake System.
Definition: integrator_base.h:107
bool is_initialized() const
Indicates whether the integrator has been initialized.
Definition: integrator_base.h:787
The ContinuousState is a container for all the State variables that are unique to continuous Systems...
Definition: continuous_state.h:25
bool isinf(const Eigen::AutoDiffScalar< DerType > &x)
Overloads isinf to mimic std::isinf from <cmath>.
Definition: autodiff_overloads.h:43
const T & get_requested_minimum_step_size() const
Gets the requested minimum step size h_min for this integrator.
Definition: integrator_base.h:357
#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:35
virtual int get_error_estimate_order() const =0
Derived classes must override this function to return the order of the integrator&#39;s error estimate...
void set_target_accuracy(double accuracy)
Request that the integrator attempt to achieve a particular accuracy for the continuous portions of t...
Definition: integrator_base.h:229
Indicates a publish time has been reached but not an update time.
Definition: integrator_base.h:127
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:682
void ResetStatistics()
Forget accumulated statistics.
Definition: integrator_base.h:667
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
void set_smallest_adapted_step_size_taken(const T &dt)
Sets the size of the smallest-step-taken statistic as the result of a controlled integration step adj...
Definition: integrator_base.h:1197
const T & get_largest_step_size_taken() const
The size of the largest step taken since the last Initialize() or ResetStatistics() call...
Definition: integrator_base.h:731