addressed review comments

systems/analysis/velocity_implicit_euler_integrator.h

@@ -1,16 +1,17 @@
 #pragma once
 
-#include <algorithm>
+#include <cmath>
 #include <limits>
 #include <memory>
-#include <utility>
-
-#include <Eigen/LU>
+#include <stdexcept>
 
 #include "drake/common/default_scalars.h"
+#include "drake/common/drake_assert.h"
 #include "drake/common/drake_copyable.h"
+#include "drake/common/text_logging.h"
 #include "drake/math/compute_numerical_gradient.h"
 #include "drake/systems/analysis/implicit_integrator.h"
+#include "drake/systems/framework/basic_vector.h"
 
 namespace drake {
 namespace systems {
@@ -358,7 +359,7 @@ class VelocityImplicitEulerIntegrator final : public ImplicitIntegrator<T> {
                          const VectorX<T>& qn, const T& h,
                          BasicVector<T>* qdot);
 
-  // The last computed iteration matrix and factorization
+  // The last computed iteration matrix and factorization.
   typename ImplicitIntegrator<T>::IterationMatrix iteration_matrix_ie_;
 
   // The continuous state update vector used during Newton-Raphson.
@@ -411,7 +412,7 @@ void VelocityImplicitEulerIntegrator<T>::
   // TODO(edrumwri) Investigate using a move-type operation below.
   // We form the iteration matrix in this particular way to avoid an O(n^2)
   // subtraction as would be the case with:
-  // MatrixX<T>::Identity(n, n) - J * h.
+  // MatrixX<T>::Identity(n, n) - h * J.
   iteration_matrix->SetAndFactorIterationMatrix(-h * J  +
                                                 MatrixX<T>::Identity(n, n));
 }
@@ -612,8 +613,8 @@ VectorX<T> VelocityImplicitEulerIntegrator<T>::ComputeLOfY(const T& t,
   const VectorX<T> q = qn + h * qdot->get_value();
 
   // Evaluate l = f_y(t, q, v, z).
-  // TODO(antequ): Right now this may invalidate the entire cache that depends
-  // on any of the continuous state. Investigate invalidating less of the cache
+  // TODO(antequ): Right now this invalidates the entire cache that depends on
+  // any of the continuous state. Investigate invalidating less of the cache
   // once we have a Context method for modifying just the generalized position.
   context->get_mutable_continuous_state()
       .get_mutable_generalized_position()
@@ -695,7 +696,7 @@ bool VelocityImplicitEulerIntegrator<T>::StepVelocityImplicitEuler(
         ComputeAndFactorImplicitEulerIterationMatrix, iteration_matrix, Jy);
 
     // Update the number of Newton-Raphson iterations.
-    num_nr_iterations_++;
+    ++num_nr_iterations_;
 
     // Evaluate the residual error, which is defined above as R(yₖ):
     //     R(yₖ) = yₖ - yⁿ - h l(yₖ).
@@ -714,7 +715,7 @@ bool VelocityImplicitEulerIntegrator<T>::StepVelocityImplicitEuler(
     // TODO(antequ): Optimize this so that the context doesn't invalidate the
     // position state cache an unnecessary number of times, because evaluating
     // N(q) does not set any cache.
-    // TODO(antequ): Right now this may invalidate the entire cache that depends
+    // TODO(antequ): Right now this invalidates the entire cache that depends
     // on any of the continuous state. Investigate invalidating less of the
     // cache once we have a Context method for modifying just the generalized
     // position.
@@ -751,7 +752,8 @@ bool VelocityImplicitEulerIntegrator<T>::StepVelocityImplicitEuler(
     last_qtplus = qtplus;
   }
 
-  DRAKE_LOGGER_DEBUG("Velocity-Implicit Euler integrator convergence failed");
+  DRAKE_LOGGER_DEBUG("Velocity-Implicit Euler integrator convergence failed"
+                     "for t={}, h={}, trial={}", t0, h, trial);
 
   // If Jacobian and iteration matrix factorizations are not reused, there
   // is nothing else we can try; otherwise, the following code will recurse
@@ -781,8 +783,8 @@ bool VelocityImplicitEulerIntegrator<T>::DoImplicitIntegratorStep(const T& h) {
   // using an explicit Euler step.
   if (h < this->get_working_minimum_step_size()) {
     DRAKE_LOGGER_DEBUG(
-        "-- requested step too small, taking explicit "
-        "step instead");
+        "-- requested step too small, taking explicit step instead, at t={}, "
+        "h={}, minimum_h={}", t0, h, this->get_working_minimum_step_size());
 
     // Compute the explicit Euler step.
     xdot_ = this->EvalTimeDerivatives(*context).CopyToVector();
@@ -791,15 +793,14 @@ bool VelocityImplicitEulerIntegrator<T>::DoImplicitIntegratorStep(const T& h) {
     // Use the current state as the candidate value for the next state.
     // [Hairer 1996] validates this choice (p. 120).
     const VectorX<T>& xtplus_guess = xn_;
-    bool success = StepVelocityImplicitEuler(t0, h, xn_, xtplus_guess,
+    const bool success = StepVelocityImplicitEuler(t0, h, xn_, xtplus_guess,
         &xtplus_ie_, &iteration_matrix_ie_, &Jy_ie_);
 
     // If the step was not successful, reset the time and state.
     if (!success) {
       DRAKE_LOGGER_DEBUG(
           "Velocity-Implicit Euler approach did not converge for "
-          "step size {}",
-          h);
+          "time t={}, step size h={}", t0, h);
       context->SetTimeAndContinuousState(t0, xn_);
       return false;
     }