Skip to content

Commit

Permalink
Refactor the convergence check of Newton-Raphson iterations (RobotLoc…
Browse files Browse the repository at this point in the history 
…omotion#12599)

With the new Velocity-Implicit Integrator Euler integrator RobotLocomotion#12528, we will have at least four implementations of Newton-Raphson iterations in our implicit integrators. This PR:

    Moves the convergence check in each iteration, which was written based on Hairer 1996, from each implicit integrator into the parent class ImplicitIntegrator.

In other words, it refactors the easiest component, the convergence check, and makes the convergence behavior consistent between the integrators. It will also simplify PR RobotLocomotion#12543 by moving the logic for this convergence check to a common location.
  • Loading branch information
@antequ @amcastro-tri
antequ authored and amcastro-tri committed Jan 25, 2020
1 parent 696be03 commit ba63f37
Show file tree
Hide file tree
Showing 4 changed files with 121 additions and 111 deletions.
52 changes: 12 additions & 40 deletions systems/analysis/implicit_euler_integrator-inl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -200,50 +200,22 @@ bool ImplicitEulerIntegrator<T>::StepAbstract(
dx_state_->get_mutable_vector().SetFromVector(dx);
T dx_norm = this->CalcStateChangeNorm(*dx_state_);

// The check below looks for convergence by identifying cases where the
// update to the state results in no change. We do this check only after
// at least one Newton-Raphson update has been applied to ensure that there
// is at least some change to the state, no matter how small, on a
// non-stationary system.
if (i > 0 && this->IsUpdateZero(*xtplus, dx)) {
DRAKE_LOGGER_DEBUG("Converged with zero update. xt+: {}",
xtplus->transpose());
return true;
}

// Update the state vector.
*xtplus += dx;
context->SetTimeAndContinuousState(tf, *xtplus);

// Compute the convergence rate and check convergence.
// [Hairer, 1996] notes that this convergence strategy should only be
// applied after *at least* two iterations (p. 121).
if (i > 1) {
const T theta = dx_norm / last_dx_norm;
const T eta = theta / (1 - theta);
DRAKE_LOGGER_DEBUG("Newton-Raphson loop {} theta: {}, eta: {}",
i, theta, eta);

// Look for divergence.
if (theta > 1) {
DRAKE_LOGGER_DEBUG("Newton-Raphson divergence detected for "
"h={}", h);
break;
}

// Look for convergence using Equation 8.10 from [Hairer, 1996].
// [Hairer, 1996] determined values of kappa in [0.01, 0.1] work most
// efficiently on a number of test problems with Radau5 (a fifth order
// implicit integrator), p. 121. We select a value halfway in-between.
const double kappa = 0.05;
const double k_dot_tol = kappa * this->get_accuracy_in_use();
if (eta * dx_norm < k_dot_tol) {
DRAKE_LOGGER_DEBUG(
"Newton-Raphson converged; η = {}, h = {}, xt+ = {}",
eta, h, xtplus->transpose());
return true;
}
}
// Check for Newton-Raphson convergence.
typename ImplicitIntegrator<T>::ConvergenceStatus status =
this->CheckNewtonConvergence(i, *xtplus, dx, dx_norm, last_dx_norm);
// If it converged, we're done.
if (status == ImplicitIntegrator<T>::ConvergenceStatus::kConverged)
return true;
// If it diverged, we have to abort and try again.
if (status == ImplicitIntegrator<T>::ConvergenceStatus::kDiverged)
break;
// Otherwise, continue to the next Newton-Raphson iteration.
DRAKE_DEMAND(status ==
ImplicitIntegrator<T>::ConvergenceStatus::kNotConverged);

// Update the norm of the state update.
last_dx_norm = dx_norm;
Expand Down
83 changes: 83 additions & 0 deletions systems/analysis/implicit_integrator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -329,6 +329,35 @@ class ImplicitIntegrator : public IntegratorBase<T> {
return true;
}

enum class ConvergenceStatus {
kDiverged,
kConverged,
kNotConverged,
};

/// Checks a Newton-Raphson iteration process for convergence. The logic
/// is based on the description on p. 121 from
/// [Hairer, 1996] E. Hairer and G. Wanner. Solving Ordinary Differential
/// Equations II (Stiff and Differential-Algebraic Problems).
/// Springer, 1996.
/// This function is called after the dx is computed in an iteration, to
/// determine if the Newton process converged, diverged, or needs further
/// iterations.
/// @param iteration the iteration index, starting at 0 for the first
/// iteration.
/// @param xtplus the state x at the current iteration.
/// @param dx the state change dx the difference between xtplus at the
/// current and the previous iteration.
/// @param dx_norm the weighted norm of dx
/// @param last_dx_norm the weighted norm of dx from the previous iteration.
/// This parameter is ignored during the first iteration.
/// @return `kConverged` for convergence, `kDiverged` for divergence,
/// otherwise `kNotConverged` if Newton-Raphson should simply
/// continue.
ConvergenceStatus CheckNewtonConvergence(int iteration,
const VectorX<T>& xtplus, const VectorX<T>& dx, const T& dx_norm,
const T& last_dx_norm) const;

/// Resets any statistics particular to a specific implicit integrator. The
/// default implementation of this function does nothing. If your integrator
/// collects its own statistics, you should re-implement this method and
Expand Down Expand Up @@ -665,6 +694,60 @@ ImplicitIntegrator<AutoDiffXd>::IterationMatrix::Solve(
return QR_.solve(b);
}

template <typename T>
typename ImplicitIntegrator<T>::ConvergenceStatus
ImplicitIntegrator<T>::CheckNewtonConvergence(
int iteration, const VectorX<T>& xtplus, const VectorX<T>& dx,
const T& dx_norm, const T& last_dx_norm) const {
// The check below looks for convergence by identifying cases where the
// update to the state results in no change.
// Note: Since we are performing this check at the end of the iteration,
// after xtplus has been updated, we also know that there is at least some
// change to the state, no matter how small, on a non-stationary system.
// Future maintainers should make sure this check only occurs after a change
// has been made to the state.
if (this->IsUpdateZero(xtplus, dx)) {
DRAKE_LOGGER_DEBUG("magnitude of state update indicates convergence");
return ConvergenceStatus::kConverged;
}

// Compute the convergence rate and check convergence.
// [Hairer, 1996] notes that this convergence strategy should only be applied
// after *at least* two iterations (p. 121). In practice, we find that it
// needs to run at least three iterations otherwise some error-controlled runs
// may choke, hence we check if iteration > 1.
if (iteration > 1) {
// TODO(edrumwri) Hairer's RADAU5 implementation (allegedly) uses
// theta = sqrt(dx[k] / dx[k-2]) while DASSL uses
// theta = pow(dx[k] / dx[0], 1/k), so investigate setting
// theta to these alternative values for minimizing convergence failures.
const T theta = dx_norm / last_dx_norm;
const T eta = theta / (1 - theta);
DRAKE_LOGGER_DEBUG("Newton-Raphson loop {} theta: {}, eta: {}",
iteration, theta, eta);

// Look for divergence.
if (theta > 1) {
DRAKE_LOGGER_DEBUG("Newton-Raphson divergence detected");
return ConvergenceStatus::kDiverged;
}

// Look for convergence using Equation IV.8.10 from [Hairer, 1996].
// [Hairer, 1996] determined values of kappa in [0.01, 0.1] work most
// efficiently on a number of test problems with *Radau5* (a fifth order
// implicit integrator), p. 121. We select a value halfway in-between.
const double kappa = 0.05;
const double k_dot_tol = kappa * this->get_accuracy_in_use();
if (eta * dx_norm < k_dot_tol) {
DRAKE_LOGGER_DEBUG("Newton-Raphson converged; η = {}", eta);
return ConvergenceStatus::kConverged;
}
}

return ConvergenceStatus::kNotConverged;
}


template <class T>
bool ImplicitIntegrator<T>::IsBadJacobian(const MatrixX<T>& J) const {
return !J.allFinite();
Expand Down
93 changes: 24 additions & 69 deletions systems/analysis/radau_integrator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,13 +116,6 @@ class RadauIntegrator final : public ImplicitIntegrator<T> {
return num_err_est_iter_factorizations_;
}

enum class ConvergenceStatus {
kDiverged,
kConverged,
kNotConverged,
};
ConvergenceStatus CheckConvergence(int iteration, const VectorX<T>& xtplus,
const VectorX<T>& dx, const T& dx_norm, const T& last_dx_norm) const;
void ComputeSolutionFromIterate(
const VectorX<T>& xt0, const VectorX<T>& Z, VectorX<T>* xtplus) const;
void ComputeAndSetErrorEstimate(
Expand Down Expand Up @@ -383,56 +376,6 @@ void RadauIntegrator<T, num_stages>::ComputeSolutionFromIterate(
*xtplus += xt0;
}

// Checks the Newton-Raphson iteration process for convergence.
template <typename T, int num_stages>
typename RadauIntegrator<T, num_stages>::ConvergenceStatus
RadauIntegrator<T, num_stages>::CheckConvergence(
int iteration, const VectorX<T>& xtplus, const VectorX<T>& dx,
const T& dx_norm, const T& last_dx_norm) const {
// The check below looks for convergence by identifying cases where the
// update to the state results in no change. We do this check only after
// at least one Newton-Raphson update has been applied to ensure that there
// is at least some change to the state, no matter how small, on a
// non-stationary system.
if (iteration > 0 && this->IsUpdateZero(xtplus, dx)) {
DRAKE_LOGGER_DEBUG("magnitude of state update indicates convergence");
return ConvergenceStatus::kConverged;
}

// Compute the convergence rate and check convergence.
// [Hairer, 1996] notes that this convergence strategy should only be
// applied after *at least* two iterations (p. 121).
if (iteration > 1) {
// TODO(edrumwri) Hairer's RADAU5 implementation (allegedly) uses
// theta = sqrt(dx[k] / dx[k-2]) while DASSL uses
// theta = pow(dx[k] / dx[0], 1/k), so investigate setting
// theta to these alternative values for minimizing convergence failures.
const T theta = dx_norm / last_dx_norm;
const T eta = theta / (1 - theta);
DRAKE_LOGGER_DEBUG("Newton-Raphson loop {} theta: {}, eta: {}",
iteration, theta, eta);

// Look for divergence.
if (theta > 1) {
DRAKE_LOGGER_DEBUG("Newton-Raphson divergence detected");
return ConvergenceStatus::kDiverged;
}

// Look for convergence using Equation IV.8.10 from [Hairer, 1996].
// [Hairer, 1996] determined values of kappa in [0.01, 0.1] work most
// efficiently on a number of test problems with *Radau5* (a fifth order
// implicit integrator), p. 121. We select a value halfway in-between.
const double kappa = 0.05;
const double k_dot_tol = kappa * this->get_accuracy_in_use();
if (eta * dx_norm < k_dot_tol) {
DRAKE_LOGGER_DEBUG("Newton-Raphson converged; η = {}", eta);
return ConvergenceStatus::kConverged;
}
}

return ConvergenceStatus::kNotConverged;
}

// Computes the next continuous state (at t0 + h) using the Radau method,
// assuming that the method is able to converge at that step size.
// @param t0 the initial time.
Expand Down Expand Up @@ -551,12 +494,18 @@ bool RadauIntegrator<T, num_stages>::StepRadau(const T& t0, const T& h,
// Compute the update.
ComputeSolutionFromIterate(xt0, Z_, &(*xtplus));

// Check for convergence.
ConvergenceStatus status =
CheckConvergence(iter, *xtplus, dx, dx_norm, last_dx_norm);
if (status == ConvergenceStatus::kConverged) return true; // We win.
if (status == ConvergenceStatus::kDiverged) break; // Try something else.
DRAKE_DEMAND(status == ConvergenceStatus::kNotConverged);
// Check for Newton-Raphson convergence.
typename ImplicitIntegrator<T>::ConvergenceStatus status =
this->CheckNewtonConvergence(iter, *xtplus, dx, dx_norm, last_dx_norm);
// If it converged, we're done.
if (status == ImplicitIntegrator<T>::ConvergenceStatus::kConverged)
return true;
// If it diverged, we have to abort and try again.
if (status == ImplicitIntegrator<T>::ConvergenceStatus::kDiverged)
break;
// Otherwise, continue to the next Newton-Raphson iteration.
DRAKE_DEMAND(status ==
ImplicitIntegrator<T>::ConvergenceStatus::kNotConverged);

// Update the norm of the state update.
last_dx_norm = dx_norm;
Expand Down Expand Up @@ -715,12 +664,18 @@ bool RadauIntegrator<T, num_stages>::StepImplicitTrapezoidDetail(
*xtplus += dx;
context->SetTimeAndContinuousState(tf, *xtplus);

// Check for convergence.
ConvergenceStatus status =
CheckConvergence(iter, *xtplus, dx, dx_norm, last_dx_norm);
if (status == ConvergenceStatus::kConverged) return true; // We win.
if (status == ConvergenceStatus::kDiverged) break; // Try something else.
DRAKE_DEMAND(status == ConvergenceStatus::kNotConverged);
// Check for Newton-Raphson convergence.
typename ImplicitIntegrator<T>::ConvergenceStatus status =
this->CheckNewtonConvergence(iter, *xtplus, dx, dx_norm, last_dx_norm);
// If it converged, we're done.
if (status == ImplicitIntegrator<T>::ConvergenceStatus::kConverged)
return true;
// If it diverged, we have to abort and try again.
if (status == ImplicitIntegrator<T>::ConvergenceStatus::kDiverged)
break;
// Otherwise, continue to the next Newton-Raphson iteration.
DRAKE_DEMAND(status ==
ImplicitIntegrator<T>::ConvergenceStatus::kNotConverged);

// Update the norm of the state update.
last_dx_norm = dx_norm;
Expand Down
4 changes: 2 additions & 2 deletions systems/analysis/test/radau_integrator_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,7 @@ GTEST_TEST(RadauIntegratorTest, QuadraticTest) {
// Ensure that Radau, and not BS3, was used by counting the number of
// function evaluations. Note that this number is somewhat brittle and might
// need to be revised in the future.
EXPECT_EQ(radau.get_num_derivative_evaluations(), 9);
EXPECT_EQ(radau.get_num_derivative_evaluations(), 8);

// Per the description in IntegratorBase::get_error_estimate_order(), this
// should return "3", in accordance with the order of the polynomial in the
Expand Down Expand Up @@ -171,7 +171,7 @@ GTEST_TEST(RadauIntegratorTest, LinearRadauTest) {
// Ensure that Radau, and not Euler+RK2, was used by counting the number of
// function evaluations. Note that this number is somewhat brittle and might
// need to be revised in the future.
EXPECT_EQ(radau.get_num_derivative_evaluations(), 7);
EXPECT_EQ(radau.get_num_derivative_evaluations(), 6);

// Per the description in IntegratorBase::get_error_estimate_order(), this
// should return "2", in accordance with the order of the polynomial in the
Expand Down

0 comments on commit ba63f37

Please sign in to comment.