addressed reviews

RobotLocomotion · Feb 7, 2020 · ade360b · ade360b
1 parent 5b641fa
commit ade360b
Showing 1 changed file with 23 additions and 26 deletions.
diff --git a/systems/analysis/velocity_implicit_euler_integrator.h b/systems/analysis/velocity_implicit_euler_integrator.h
@@ -194,9 +194,8 @@ class VelocityImplicitEulerIntegrator final : public ImplicitIntegrator<T> {
   //        l(y).
   // @param qn refers to qⁿ, the initial position used in l(y)
   // @param [out] Jy is the Jacobian matrix, Jₗ(y).
-  // @post the context's time and continuous state will be temporarily set
-  //       during this call (and then reset to their original values) on
-  //       return.
+  // @post The context's time will be set to tf, and its continuous state will
+  //       be indeterminate on return.
   void CalcVelocityJacobian(const T& tf, const T& h, const VectorX<T>& y,
                             const VectorX<T>& qk, const VectorX<T>& qn,
                             MatrixX<T>* Jy);
@@ -222,7 +221,8 @@ class VelocityImplicitEulerIntegrator final : public ImplicitIntegrator<T> {
   //        l(y).
   // @param qn refers to qⁿ, the initial position used in l(y).
   // @param [out] Jy is the Jacobian matrix, Jₗ(y).
-  // @note The context's continuous state will be indeterminate on return.
+  // @note The context's time will be set to t, and its continuous state will
+  //       be indeterminate on return.
   void ComputeForwardDiffVelocityJacobian(const T& t, const T& h,
                                           const VectorX<T>& y,
                                           const VectorX<T>& qk,
@@ -273,8 +273,9 @@ class VelocityImplicitEulerIntegrator final : public ImplicitIntegrator<T> {
   // @returns `false` if the calling stepping method should indicate failure;
   //          `true` otherwise.
   // @pre 1 <= `trial` <= 4.
-  // @post the state in the internal context may or may not be altered on
-  //       return; if altered, it will be set to (t, qₖ, y).
+  // @post The state in the internal context may or may not be altered on
+  //       return; if altered, the time will be set to t and the continuous
+  //       state will be indeterminate.
   bool MaybeFreshenVelocityMatrices(
       const T& t, const VectorX<T>& y, const VectorX<T>& qk,
       const VectorX<T>& qn, const T& h, int trial,
@@ -300,7 +301,9 @@ class VelocityImplicitEulerIntegrator final : public ImplicitIntegrator<T> {
   // @param[out] iteration_matrix the updated and factored iteration matrix on
   //             return.
   // @param[out] Jy the updated Jacobian matrix Jₗ(y).
-  // @post the state in the internal context will be set to (t, xt).
+  // @post The state in the internal context may or may not be altered on
+  //       return; if altered, the time will be set to t and the continuous
+  //       state will be indeterminate.
   void FreshenVelocityMatricesIfFullNewton(
       const T& t, const VectorX<T>& y, const VectorX<T>& qk,
       const VectorX<T>& qn, const T& h,
@@ -438,16 +441,9 @@ template <class T>
 void VelocityImplicitEulerIntegrator<T>::CalcVelocityJacobian(const T& tf,
     const T& h, const VectorX<T>& y, const VectorX<T>& qk,
     const VectorX<T>& qn, MatrixX<T>* Jy) {
-  // Just like ImplicitIntegrator<T>::CalcJacobian, we change the context but
-  // will change it back. The user should assume all caches are dirty after it
-  // finishes.
-  Context<T>* context = this->get_mutable_context();
-
-  // Get the current time and state.
-  const T t_current = context->get_time();
-  const ContinuousState<T>& cstate = context->get_continuous_state();
-  const VectorX<T> x_current = cstate.CopyToVector();
-
+  // Note: Unlike ImplicitIntegrator<T>::CalcJacobian, we neither save the
+  // context or change it back, because our implementation of StepImplicitEuler
+  // does not require the context to be restored.
   this->increment_jacobian_evaluations();
 
   // Get the current number of ODE evaluations.
@@ -477,9 +473,6 @@ void VelocityImplicitEulerIntegrator<T>::CalcVelocityJacobian(const T& tf,
   // evaluations used in computing Jacobians.
   this->increment_jacobian_computation_derivative_evaluations(
       this->get_num_derivative_evaluations() - current_ODE_evals);
-
-  // Reset the time and state.
-  context->SetTimeAndContinuousState(t_current, x_current);
 }
 
 template <class T>
@@ -518,19 +511,23 @@ bool VelocityImplicitEulerIntegrator<T>::MaybeFreshenVelocityMatrices(
       return true;  // Indicate success.
 
     case 2: {
-      // For the second trial, we perform the (likely) next least expensive
-      // operation, re-constructing and factoring the iteration matrix.
+      // For the second trial, we know the first trial, which uses the last
+      // computed iteration matrix, has already failed. We perform the (likely)
+      // next least expensive operation, which is re-constructing and factoring
+      // the iteration matrix, using the last computed Jacobian.
       this->increment_num_iter_factorizations();
       compute_and_factor_iteration_matrix(*Jy, h, iteration_matrix);
       return true;
     }
 
     case 3: {
-      // For the third trial, we reform the Jacobian matrix and refactor the
-      // iteration matrix.
+      // For the third trial, we know that the first two trials, which
+      // exhausted all our options short of recomputing the Jacobian, have
+      // failed. We recompute the Jacobian matrix and refactor the iteration
+      // matrix.
 
-      // note: Based on a few simple experimental tests, we found that the
-      // optimization when matrices are already fresh in
+      // Note: Based on a few simple experimental tests, we found that the
+      // optimization to abort this trial when matrices are already fresh in
       // ImplicitIntegrator<T>::MaybeFreshenMatrices does not significantly help
       // here, especially because our Jacobian depends on step size h.
       CalcVelocityJacobian(t, h, y, qk, qn, Jy);