Velocity-Implicit Euler Fixed Step - Second PR of #12528 #12543
Conversation
There was a problem hiding this comment.
Reviewed 3 of 5 files at r1, 1 of 1 files at r2.
Reviewable status: 6 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @edrumwri and @sherm1)
systems/analysis/implicit_integrator.h, line 112 at r3 (raw file):
/// and central differencing. This should be true unless the derived /// integrator defines its own Jacobian computation schemes. bool supports_autodiff_and_central_differencing() const {
Do not review this change (from Previous PR)
systems/analysis/implicit_integrator.h, line 210 at r3 (raw file):
/// Jacobian computation schemes to indicate whether Autodiff and Central /// Differencing are available yet. By default they are supported. virtual bool do_supports_autodiff_and_central_differencing() const {
Do not review this change (from Previous PR)
systems/analysis/test/implicit_euler_integrator_test.cc, line 1 at r3 (raw file):
#include "drake/systems/analysis/implicit_euler_integrator.h"
Do not review this file (from Previous PR)
systems/analysis/test/radau_integrator_test.cc, line 1 at r3 (raw file):
#include "drake/systems/analysis/radau_integrator.h"
Do not review this file (from Previous PR)
systems/analysis/test_utilities/implicit_integrator_test.h, line 25 at r3 (raw file):
template <typename T> class ImplicitIntegratorTest : public ::testing::Test {
Do not review this file (previous PR)
systems/analysis/test_utilities/stiff_double_mass_spring_system.h, line 1 at r3 (raw file):
#pragma once
Do not review this file! (Previous PR)
There was a problem hiding this comment.
Reviewed 1 of 5 files at r1.
Reviewable status: 6 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @antequ, @edrumwri, and @sherm1)
There was a problem hiding this comment.
Reviewable status: 8 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @antequ, @edrumwri, and @sherm1)
systems/analysis/velocity_implicit_euler_integrator.h, line 400 at r4 (raw file):
Eigen::Ref
I'll change these to Eigen::VectorBlock<VectorX>
systems/analysis/velocity_implicit_euler_integrator.h, line 626 at r4 (raw file):
const auto& qt0
I'll change this to Eigen::VectorBlock<const VectorX> or const Eigen::VectorBlock<VectorX>, whichever one doesn't force a copy.
There was a problem hiding this comment.
Reviewed 3 of 5 files at r3.
Reviewable status: 21 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform), labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @antequ, @edrumwri, and @sherm1)
systems/analysis/velocity_implicit_euler_integrator.h, line 38 at r4 (raw file):
* for solving the position states. * * In particular, it solves a system of ordinary differential equations in
Could you harden this statement a little? It's not quite evident that the equations must be in the form below.
systems/analysis/velocity_implicit_euler_integrator.h, line 40 at r4 (raw file):
* In particular, it solves a system of ordinary differential equations in * state x = (q,v,z), such that<pre> * q̇ = N(q) v; (1)
BTW if you indent each of these lines by four spaces, you don't need the
.
systems/analysis/velocity_implicit_euler_integrator.h, line 40 at r4 (raw file):
* In particular, it solves a system of ordinary differential equations in * state x = (q,v,z), such that<pre> * q̇ = N(q) v; (1)N is not defined. I suggest taking a look at System::MapVelocityToQDot() to see how it is defined there.
systems/analysis/velocity_implicit_euler_integrator.h, line 41 at r4 (raw file):
* state x = (q,v,z), such that<pre> * q̇ = N(q) v; (1) * ẏ = fᵥ(t,q,y), (2)fᵥ is not defined.
systems/analysis/velocity_implicit_euler_integrator.h, line 51 at r4 (raw file):
* * To solve the nonlinear system for (qⁿ⁺¹,yⁿ⁺¹), the velocity-implicit Euler * integrator iterates the following with Newton's method: At iteration k, findSuggest "iteratively solves for q and y with Newton's method".
systems/analysis/velocity_implicit_euler_integrator.h, line 54 at r4 (raw file):
* (qₖ₊₁,yₖ₊₁) that satisfies<pre> * yₖ₊₁ = yⁿ + h f(tⁿ⁺¹,qₖ₊₁,yₖ₊₁); (5) * qₖ₊₁ = qⁿ + h N(qₖ) vₖ₊₁. (6)N(q) is not just one iteration behind qₖ₊₁, is it? In other words, should this be N(qⁿ)?
systems/analysis/velocity_implicit_euler_integrator.h, line 57 at r4 (raw file):
* </pre> * * For the unfamiliar reader, in this notation, n's index timesteps, while k'sSuggest striking "For the unfamiliar reader" (I think all readers will be unfamiliar, as the notation you are using is not standardized, at least according to a quick look at [Hairer 1996]).
systems/analysis/velocity_implicit_euler_integrator.h, line 61 at r4 (raw file):
* * To solve (5-6), first define<pre> * lₖ(y) = f(tⁿ⁺¹,qⁿ + h N(qₖ) v,y), (7)BTW If this is N(qⁿ) then this should not be labeled lₖ.
systems/analysis/velocity_implicit_euler_integrator.h, line 65 at r4 (raw file):
* </pre> * * (CalcVelocityJacobian() computes Jₗₖ(y) by automatic differentiation orBTW This is a private method and should not be described in the public documentation.
systems/analysis/velocity_implicit_euler_integrator.h, line 74 at r4 (raw file):
* in (6) to get qₖ₊₁. * * TODO(antequ): support Error Control, AutodiffXdTODOs should not be in the public documentation.
systems/analysis/velocity_implicit_euler_integrator.h, line 77 at r4 (raw file):
* * This implementation uses Newton-Raphson (NR) and relies upon the obvious * convergence to a solution for `R(y) = 0` whereBTW suggest "for y in
R(y) = 0"
systems/analysis/velocity_implicit_euler_integrator.h, line 153 at r4 (raw file):
bool DoImplicitIntegratorStep(const T& h) final; /// Steps the system forward by a single step of at most h using theYou're commenting this as if it will show up in Doxygen, but it's private. Solution: change triple slashes to double slashes.
systems/analysis/velocity_implicit_euler_integrator.h, line 157 at r4 (raw file):
/// @param t0 the time at the left end of the integration interval. /// @param h the maximum time increment to step forward. /// @param xt0 the continuous state at t0.You would call this xn in the class documentation. Can you reconcile your notation?
systems/analysis/velocity_implicit_euler_integrator.h, line 159 at r4 (raw file):
/// @param xt0 the continuous state at t0. /// @param xtplus_guess the starting guess for x(t0+h). /// @param [out] xtplus the computed value for `x(t0+h)` on successful return.You would call this xn+1 in the class documentation. Can you reconcile your notation?
systems/analysis/velocity_implicit_euler_integrator.h, line 160 at r4 (raw file):
/// @param xtplus_guess the starting guess for x(t0+h). /// @param [out] xtplus the computed value for `x(t0+h)` on successful return. /// @param [in, out] iteration_matrix the cached iteration matrixWhy is this quantity and the next referred to as "cached"?
systems/analysis/velocity_implicit_euler_integrator.h, line 178 at r4 (raw file):
/// Compute the partial derivative of the ordinary differential equations withNit derivative -> derivatives
systems/analysis/velocity_implicit_euler_integrator.h, line 179 at r4 (raw file):
/// Compute the partial derivative of the ordinary differential equations with /// respect to the y variables of a given x(t). In particular, we compute theCheckpoint.
There was a problem hiding this comment.
Reviewable status: 26 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform), labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @antequ and @sherm1)
a discussion (no related file):
Before I move forward with the review. I believe we finally hit the "rule of three" between ImplicitEulerIntegrator, VelocityImplicitEulerIntegrator, and RadauIntegrator (@antequ, with the "rule of three" we mean, if we copy/pasted code three or more times, it deserves the refactoring into its own separate unit).
Here are the blocks of code I propose we refactor out:
- We can compute Jacobians numerically with
drake::math::ComputeNumericalGradient(). - We can compute Jacobians with autodiff using
drake::math::jacobian()(I might be wrong on this one sincejacobian()might not work as of today forAutoDiffXd. But we could possibly extend it?). - we definitely need a Newton-Raphson solver in Drake. With this PR now we'd have three versions sprinkled just across the integrators. Having a stand-alone Newton-Raphson solver would greatly simplify the code bits in this PR, the logic across the integrators and it would provide a much desired solver within Drake.
thoughts?
systems/analysis/velocity_implicit_euler_integrator.h, line 125 at r4 (raw file):
} int64_t do_get_num_error_estimator_derivative_evaluations_for_jacobian()
I don't understand why these general statistic methods have to overriden by each implicit integrator. Couldn't ImplicitIntegrator track these values and provide final implementation for these? @edrumwri, what's the purpose of making these NVIs?
systems/analysis/velocity_implicit_euler_integrator.h, line 166 at r4 (raw file):
/// increase. /// @returns `true` if the step of size `h` was successful, `false` otherwise. /// @note The time and continuous state in the context are indeterminate upon
why this @note? if all the computed quantities are outputs as arguments, why isn't this method marked const? I'd assume statistics are mutable?
systems/analysis/velocity_implicit_euler_integrator.h, line 312 at r4 (raw file):
// Second order Runge-Kutta method for estimating the integration error when // the requested step size lies below the working step size. std::unique_ptr<RungeKutta2Integrator<T>> rk2_;
not used in this PR. Remove and push in the follow up PR for simplicity.
systems/analysis/velocity_implicit_euler_integrator.h, line 337 at r4 (raw file):
// step size will be set to infinity because we will explicitly request the // step sizes to be taken. rk2_ = std::make_unique<RungeKutta2Integrator<T>>(
not used in this PR, remove.
antequ
force-pushed
the
dev_vie_fixed_step_pr2
branch
from
5c0c323
to
8f1606f
Compare
There was a problem hiding this comment.
Reviewable status: 26 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform), labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @antequ, @edrumwri, and @sherm1)
systems/analysis/velocity_implicit_euler_integrator.h, line 54 at r4 (raw file):
Previously, edrumwri (Evan Drumwright) wrote…
N(q) is not just one iteration behind qₖ₊₁, is it? In other words, should this be N(qⁿ)?
No, N(q) is updated every iteration; that is part of why we believe it converges to the actual implicit Euler solution.
systems/analysis/velocity_implicit_euler_integrator.h, line 57 at r4 (raw file):
Previously, edrumwri (Evan Drumwright) wrote…
Suggest striking "For the unfamiliar reader" (I think all readers will be unfamiliar, as the notation you are using is not standardized, at least according to a quick look at [Hairer 1996]).
I'll also consider changing the notation to make it match Hairer's -- I'll take a look
systems/analysis/velocity_implicit_euler_integrator.h, line 61 at r4 (raw file):
Previously, edrumwri (Evan Drumwright) wrote…
BTW If this is N(qⁿ) then this should not be labeled lₖ.
It is N(q_k). But I will consider removing the k subscript.
There was a problem hiding this comment.
Reviewable status: 26 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform), labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri, @antequ, @edrumwri, and @sherm1)
a discussion (no related file):
Previously, amcastro-tri (Alejandro Castro) wrote…
Before I move forward with the review. I believe we finally hit the between
ImplicitEulerIntegrator,VelocityImplicitEulerIntegrator, andRadauIntegrator(@antequ, with the "rule of three" we mean, if we copy/pasted code three or more times, it deserves the refactoring into its own separate unit).Here are the blocks of code I propose we refactor out:
- We can compute Jacobians numerically with
drake::math::ComputeNumericalGradient().- We can compute Jacobians with autodiff using
drake::math::jacobian()(I might be wrong on this one sincejacobian()might not work as of today forAutoDiffXd. But we could possibly extend it?).- we definitely need a Newton-Raphson solver in Drake. With this PR now we'd have three versions sprinkled just across the integrators. Having a stand-alone Newton-Raphson solver would greatly simplify the code bits in this PR, the logic across the integrators and it would provide a much desired solver within Drake.
thoughts?
I think jacobian() only works for Autodiff, based on the description; I don't think it supports forward differencing
There was a problem hiding this comment.
Reviewable status: 26 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform), labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri, @antequ, @edrumwri, and @sherm1)
a discussion (no related file):
Previously, antequ (Ante Qu) wrote…
I think jacobian() only works for Autodiff, based on the description; I don't think it supports forward differencing
Ah, I misread your comment. I'll try ComputeNumericalGradient.
There was a problem hiding this comment.
Reviewable status: 26 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform), labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri, @antequ, @edrumwri, and @sherm1)
a discussion (no related file):
Previously, antequ (Ante Qu) wrote…
Ah, I misread your comment. I'll try ComputeNumericalGradient.
The main problem with the drake::math refactoring is that ComputeNumericalGradient() does not support AutoDiffXd as a scalar, while my current implementation (and all the other implicit integrators) do. This would require me to add a lot more complexity or disable the integrator for AutoDiffXd. Instead, I'm going to add a TODO to use ComputeNumericalGradient() once it supports the AutoDiffXd scalar type.
For the N-R logic refactoring, I'll create a separate branch and investigate how easy it'll be to create a separate Newton-Raphson evaluator in ImplicitIntegrator and move the common code. If it's simple or easy, I'll include it in this PR.
There was a problem hiding this comment.
Reviewed 1 of 4 files at r5.
Reviewable status: 23 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform), labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri, @antequ, @edrumwri, @note, and @sherm1)
systems/analysis/velocity_implicit_euler_integrator.h, line 38 at r4 (raw file):
Previously, edrumwri (Evan Drumwright) wrote…
Could you harden this statement a little? It's not quite evident that the equations must be in the form below.
Reworded
systems/analysis/velocity_implicit_euler_integrator.h, line 40 at r4 (raw file):
Previously, edrumwri (Evan Drumwright) wrote…
BTW if you indent each of these lines by four spaces, you don't need the
.
indented
systems/analysis/velocity_implicit_euler_integrator.h, line 40 at r4 (raw file):
Previously, edrumwri (Evan Drumwright) wrote…
N is not defined. I suggest taking a look at System::MapVelocityToQDot() to see how it is defined there.
Reworded.
systems/analysis/velocity_implicit_euler_integrator.h, line 41 at r4 (raw file):
Previously, edrumwri (Evan Drumwright) wrote…
fᵥ is not defined.
Defined.
systems/analysis/velocity_implicit_euler_integrator.h, line 51 at r4 (raw file):
Previously, edrumwri (Evan Drumwright) wrote…
Suggest "iteratively solves for q and y with Newton's method".
Fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 57 at r4 (raw file):
Previously, antequ (Ante Qu) wrote…
I'll also consider changing the notation to make it match Hairer's -- I'll take a look
For now removed "For the unfamiliar reader"
systems/analysis/velocity_implicit_euler_integrator.h, line 65 at r4 (raw file):
Previously, edrumwri (Evan Drumwright) wrote…
BTW This is a private method and should not be described in the public documentation.
fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 74 at r4 (raw file):
Previously, edrumwri (Evan Drumwright) wrote…
TODOs should not be in the public documentation.
fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 77 at r4 (raw file):
Previously, edrumwri (Evan Drumwright) wrote…
BTW suggest "for y in
R(y) = 0"
fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 125 at r4 (raw file):
Previously, amcastro-tri (Alejandro Castro) wrote…
I don't understand why these general statistic methods have to overriden by each implicit integrator. Couldn't
ImplicitIntegratortrack these values and provide final implementation for these? @edrumwri, what's the purpose of making these NVIs?
I based these on Evan's first Radau PR, which was fixed only.
We can keep these because we will be supporting Error Control in all integrators.
It's hard for ImplicitIntegrator to track these values because the integrator itself implements the error control. However, if we want to, we could refactor all the integrators to use something similar to what I added to ImplicitIntegrator if we like that better: helper functions like "increment_jacobian_computation_derivative_evaluations()" in ImplicitIntegrator. It makes it so that each derived integrator doesn't have to keep track of these values themselves. But I'll need to make quite a few changes in ImplicitEulerIntegrator and RadauIntegrator if we want to do that.
systems/analysis/velocity_implicit_euler_integrator.h, line 153 at r4 (raw file):
Previously, edrumwri (Evan Drumwright) wrote…
You're commenting this as if it will show up in Doxygen, but it's private. Solution: change triple slashes to double slashes.
Good point. I'll make this change for all private functions
systems/analysis/velocity_implicit_euler_integrator.h, line 157 at r4 (raw file):
Previously, edrumwri (Evan Drumwright) wrote…
You would call this xn in the class documentation. Can you reconcile your notation?
fixed for now -- it uses xn here in the documentation. I am keeping the name xt0 to be consistent with the other integrators. I may change the entire documentation to use x(t0) and x(t0+h), depending on how that looks.
systems/analysis/velocity_implicit_euler_integrator.h, line 159 at r4 (raw file):
Previously, edrumwri (Evan Drumwright) wrote…
You would call this xn+1 in the class documentation. Can you reconcile your notation?
fixed for now, it uses xn+1 here.
systems/analysis/velocity_implicit_euler_integrator.h, line 160 at r4 (raw file):
Previously, edrumwri (Evan Drumwright) wrote…
Why is this quantity and the next referred to as "cached"?
In our quasi-newton implementation, the velocity Jacobian and the iteration matrix are not necessarily updated every time step.
systems/analysis/velocity_implicit_euler_integrator.h, line 166 at r4 (raw file):
Previously, amcastro-tri (Alejandro Castro) wrote…
why this @note? if all the computed quantities are outputs as arguments, why isn't this method marked
const? I'd assume statistics aremutable?
The context is changed during this, in several possible ways:
- MaybeRefreshenMatrices may recalculate the Jacobian, which changes the context.
- computing residuals and right-hand-sides may change the context.
This is a note saying this function does not purposefully set the context to anything when it finishes; it's up to the caller to set the context to the solution xtplus.
So the method modifies the system's context as well as statistics.
systems/analysis/velocity_implicit_euler_integrator.h, line 178 at r4 (raw file):
Previously, edrumwri (Evan Drumwright) wrote…
Nit derivative -> derivatives
done
systems/analysis/velocity_implicit_euler_integrator.h, line 179 at r4 (raw file):
Previously, edrumwri (Evan Drumwright) wrote…
Checkpoint.
Done.
systems/analysis/velocity_implicit_euler_integrator.h, line 312 at r4 (raw file):
Previously, amcastro-tri (Alejandro Castro) wrote…
not used in this PR. Remove and push in the follow up PR for simplicity.
Removed. Good catch!
systems/analysis/velocity_implicit_euler_integrator.h, line 337 at r4 (raw file):
Previously, amcastro-tri (Alejandro Castro) wrote…
not used in this PR, remove.
Removed. Good catch!
There was a problem hiding this comment.
Reviewed 1 of 4 files at r5, 2 of 2 files at r7.
Reviewable status: 23 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform), labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri, @edrumwri, @note, and @sherm1)
systems/analysis/velocity_implicit_euler_integrator.h, line 400 at r4 (raw file):
Previously, antequ (Ante Qu) wrote…
Eigen::RefI'll change these to Eigen::VectorBlock<VectorX>
fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 626 at r4 (raw file):
Previously, antequ (Ante Qu) wrote…
const auto& qt0I'll change this to Eigen::VectorBlock<const VectorX> or const Eigen::VectorBlock<VectorX>, whichever one doesn't force a copy.
fixed
antequ
force-pushed
the
dev_vie_fixed_step_pr2
branch
from
901ca65
to
7278d8b
Compare
There was a problem hiding this comment.
Reviewed 9 of 9 files at r10.
Reviewable status: 23 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform), labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri, @edrumwri, @note, and @sherm1)
There was a problem hiding this comment.
Reviewed 1 of 2 files at r11.
Reviewable status: 23 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform), labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri, @edrumwri, @note, and @sherm1)
There was a problem hiding this comment.
Reviewable status: 22 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform), labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri, @antequ, @edrumwri, and @note)
a discussion (no related file):
Previously, antequ (Ante Qu) wrote…
The main problem with the drake::math refactoring is that
ComputeNumericalGradient()does not support AutoDiffXd as a scalar, while my current implementation (and all the other implicit integrators) do. This would require me to add a lot more complexity or disable the integrator for AutoDiffXd. Instead, I'm going to add a TODO to use ComputeNumericalGradient() once it supports the AutoDiffXd scalar type.For the N-R logic refactoring, I'll create a separate branch and investigate how easy it'll be to create a separate Newton-Raphson evaluator in ImplicitIntegrator and move the common code. If it's simple or easy, I'll include it in this PR.
@antequ is this PR ready for review?
systems/analysis/velocity_implicit_euler_integrator.h, line 40 at r11 (raw file):
* state x = (q,v,z) to be expressible in the following form: * q̇ = N(q) v; (1) * ẏ = fᵥ(t,q,y), (2)
None of the equations format correctly in doxygen.
There was a problem hiding this comment.
Reviewable status: 22 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform), labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri, @antequ, @edrumwri, and @note)
systems/analysis/velocity_implicit_euler_integrator.h, line 40 at r11 (raw file):
* state x = (q,v,z) to be expressible in the following form: * q̇ = N(q) v; (1) * ẏ = fᵥ(t,q,y), (2)
FYI to make Evan's indenting trick work you have to separate the indented code from the previous paragraph with a blank line (still looks nice in the header). Or, use <pre> stuff </pre>.
There was a problem hiding this comment.
Reviewable status: 23 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform), labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri, @antequ, @edrumwri, and @note)
systems/analysis/velocity_implicit_euler_integrator.h, line 681 at r11 (raw file):
// Evaluate the residual error, which is the negation of the RHS of the update // equation: // (I - h Jₗ) Δy = yⁿ - yₖ + h l(yₖ), Δy = yₖ₊₁ - yₖ
minor: sign error before "h" here?
There was a problem hiding this comment.
Reviewable status: 23 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform), labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri, @antequ, @edrumwri, and @note)
a discussion (no related file):
Previously, sherm1 (Michael Sherman) wrote…
@antequ is this PR ready for review?
Yes; it's been ready. I'll address the current comments and push in about two hours. I'm investigating Alejandro's suggestion on the side, but that's slightly more involved
antequ
force-pushed
the
dev_vie_fixed_step_pr2
branch
from
ca5149d
to
760ae1c
Compare
There was a problem hiding this comment.
Reviewable status: 21 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform), labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri, @antequ, @edrumwri, @note, and @sherm1)
systems/analysis/velocity_implicit_euler_integrator.h, line 40 at r11 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
FYI to make Evan's indenting trick work you have to separate the indented code from the previous paragraph with a blank line (still looks nice in the header). Or, use
<pre> stuff </pre>.
trying it now. Will try running Doxygen to see how it looks
systems/analysis/velocity_implicit_euler_integrator.h, line 357 at r11 (raw file):
using std::max; // TODO(antequ): Refactor this to use drake::math::ComputeNumericalGradient()
FYI @amcastro-tri , I added this TODO to investigate the refactorization once ComputeNumericalGradient supports more scalar types.
systems/analysis/velocity_implicit_euler_integrator.h, line 681 at r11 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: sign error before "h" here?
Fixed. The comment is confusing; it's saying to negate the RHS. Since the documentation at the top already defines R, I just duplicated the definition here instead.
There was a problem hiding this comment.
Reviewable status: 20 unresolved discussions, LGTM missing from assignees amcastro-tri,edrumwri, needs platform reviewer assigned, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri, @antequ, @edrumwri, @note, and @sherm1)
systems/analysis/velocity_implicit_euler_integrator.h, line 40 at r11 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
None of the equations format correctly in doxygen.
Fixed formatting and checked doxygen.
There was a problem hiding this comment.
Reviewed 1 of 2 files at r12.
Reviewable status: 20 unresolved discussions, LGTM missing from assignees amcastro-tri,edrumwri, needs platform reviewer assigned, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri, @antequ, @edrumwri, @note, and @sherm1)
There was a problem hiding this comment.
Reviewed 2 of 3 files at r17, 1 of 1 files at r18.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee edrumwri, needs platform reviewer assigned, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @antequ)
a discussion (no related file):
This is great. Thank you so much for the patience @antequ. I like that now I can form a direct mental map between the math (the meat of this PR) to the code, with minor conceptual overhead caused by our system APIs. Thanks!
systems/analysis/velocity_implicit_euler_integrator.h, line 444 at r18 (raw file):
// will change it back. The user should assume all caches are dirty after it // finishes. Context<T>* context = this->get_mutable_context();
I don't think this is worth it. It adds some conceptual overhead that is not necessary. There might be specific reasons in ImplicitIntegrator? But if all of our cache entries got dirty by this call I don't see a reason to set this back to any particular state. Again, I'd document that after this call context_ is "dirty" and the calling code is responsible to set it back to whatever is needed locally.
In terms of LOCs, we'd have 10 less lines of code :-). Less code, shorter reviews, less unit testing, lower chances of introducing a bug. It's a win-win.
systems/analysis/velocity_implicit_euler_integrator.h, line 524 at r18 (raw file):
// operation, re-constructing and factoring the iteration matrix. this->increment_num_iter_factorizations(); compute_and_factor_iteration_matrix(*Jy, h, iteration_matrix);
nit, Whenever I go back to this logic I find myself trying to remember again why each of these cases are important.
For instance, with this "case 2" I cannot remember when/why/how we could get to a state in which we do have a Jacobian but somehow no-one factored the iteration matrix yet. I believe it has to do with the integrator failing and then a attempting a smaller step size?
My point is, could we add something like three/four lines to each case in this logic walking a developer through a specific case when this is important?
There was a problem hiding this comment.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee edrumwri, needs platform reviewer assigned, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri and @antequ)
systems/analysis/velocity_implicit_euler_integrator.h, line 444 at r18 (raw file):
Previously, amcastro-tri (Alejandro Castro) wrote…
I don't think this is worth it. It adds some conceptual overhead that is not necessary. There might be specific reasons in
ImplicitIntegrator? But if all of our cache entries got dirty by this call I don't see a reason to set this back to any particular state. Again, I'd document that after this callcontext_is "dirty" and the calling code is responsible to set it back to whatever is needed locally.In terms of LOCs, we'd have 10 less lines of code :-). Less code, shorter reviews, less unit testing, lower chances of introducing a bug. It's a win-win.
FYI getting a mutable context doesn't invalidate any cache entries. That waits until mutable access to specific state variables occurs so that cache invalidation can be more surgical.
There was a problem hiding this comment.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee edrumwri, needs platform reviewer assigned, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @antequ)
systems/analysis/velocity_implicit_euler_integrator.h, line 444 at r18 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
FYI getting a mutable context doesn't invalidate any cache entries. That waits until mutable access to specific state variables occurs so that cache invalidation can be more surgical.
Sorry for not making this comment more local. Mutable state access happens at the very end of this method with:
context->SetTimeAndContinuousState(t_current, x_current);A common theme we worked on with @antequ during this review, was about making these context changes as local as possible right before they were needed. Otherwise, as in this case, it is difficult to track why they happened and who's responsibility actually is on making it happen. In my opiniont, as I told @antequ , this mutable context_ that the integrator stores, is only there as a temp variable inevitably needed to pass around in system API calls. Therefore we found it easier conceptually and in the implementation to be explicit when passing arguments around (say qk, y, q0 in this PR) and make context_ where right needed and precisely by who needed it.
There might be some effective use of cache exceptions to this @sherm1 ? But careful inspection in this PR did no reveal that yet. Maybe when @antequ adds error control we'll have to revisit this pattern. What do you think?
There was a problem hiding this comment.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee edrumwri, needs platform reviewer assigned, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @antequ)
systems/analysis/velocity_implicit_euler_integrator.h, line 444 at r18 (raw file):
Previously, amcastro-tri (Alejandro Castro) wrote…
Sorry for not making this comment more local. Mutable state access happens at the very end of this method with:
context->SetTimeAndContinuousState(t_current, x_current);A common theme we worked on with @antequ during this review, was about making these context changes as local as possible right before they were needed. Otherwise, as in this case, it is difficult to track why they happened and who's responsibility actually is on making it happen. In my opiniont, as I told @antequ , this mutable
context_that the integrator stores, is only there as a temp variable inevitably needed to pass around in system API calls. Therefore we found it easier conceptually and in the implementation to be explicit when passing arguments around (say qk, y, q0 in this PR) and makecontext_where right needed and precisely by who needed it.
There might be some effective use of cache exceptions to this @sherm1 ? But careful inspection in this PR did no reveal that yet. Maybe when @antequ adds error control we'll have to revisit this pattern. What do you think?
Oh, I see you were talking about whether it is worthwhile to save and restore the current Context state variables. I agree that's not worth the trouble if no one is depending on it later. However right now the documentation for this method promises to put the Context back, so that would need to change if the method's contract changes.
There was a problem hiding this comment.
Reviewable status: 3 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri, @antequ, and @drake-jenkins-bot)
a discussion (no related file):
Previously, amcastro-tri (Alejandro Castro) wrote…
This is great. Thank you so much for the patience @antequ. I like that now I can form a direct mental map between the math (the meat of this PR) to the code, with minor conceptual overhead caused by our system APIs. Thanks!
:lgtm_strong:
Addressed most recent review comments. @drake-jenkins-bot linux-bionic-clang-bazel-experimental-everything-valgrind-memcheck please @drake-jenkins-bot linux-bionic-clang-bazel-experimental-valgrind-memcheck please @drake-jenkins-bot linux-bionic-gcc-bazel-experimental-everything-valgrind-memcheck please @drake-jenkins-bot linux-bionic-gcc-bazel-experimental-valgrind-memcheck please
Adding +@sherm1 for platform review. In the meantime I'll squash and rebase.
systems/analysis/velocity_implicit_euler_integrator.h, line 444 at r18 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
Oh, I see you were talking about whether it is worthwhile to save and restore the current Context state variables. I agree that's not worth the trouble if no one is depending on it later. However right now the documentation for this method promises to put the Context back, so that would need to change if the method's contract changes.
Removed this here (and updated relevant comments). In a future PR I'll go through the ImplicitIntegrator parent class and remove this from CalcJacobian.
systems/analysis/velocity_implicit_euler_integrator.h, line 524 at r18 (raw file):
Previously, amcastro-tri (Alejandro Castro) wrote…
nit, Whenever I go back to this logic I find myself trying to remember again why each of these cases are important.
For instance, with this "case 2" I cannot remember when/why/how we could get to a state in which we do have a Jacobian but somehow no-one factored the iteration matrix yet. I believe it has to do with the integrator failing and then a attempting a smaller step size?My point is, could we add something like three/four lines to each case in this logic walking a developer through a specific case when this is important?
Expanded each comment to explain what failed at each stage, and why we are attempting the next thing.
antequ
force-pushed
the
dev_vie_fixed_step_pr2
branch
from
ade360b
to
540eb6e
Compare
|
a discussion (no related file): Previously, antequ (Ante Qu) wrote…
Squashed and rebased! @drake-jenkins-bot linux-bionic-clang-bazel-experimental-everything-valgrind-memcheck please @drake-jenkins-bot linux-bionic-clang-bazel-experimental-valgrind-memcheck please @drake-jenkins-bot linux-bionic-gcc-bazel-experimental-everything-valgrind-memcheck please @drake-jenkins-bot linux-bionic-gcc-bazel-experimental-valgrind-memcheck please |
There was a problem hiding this comment.
Reviewed 1 of 1 files at r19.
Reviewable status: 2 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform) (waiting on @amcastro-tri and @drake-jenkins-bot)
There was a problem hiding this comment.
Note: 12657 proposes to refactor systems/analysis so that most of the code in the .h files are moved to the .cc file. This PR puts all the new code in the .h file for the new integrator. Based on f2f with Sherm, our plan is to leave this PR as-is, leave 12657 as-is, and create a third PR after both are checked in just for moving the code. This is because both PRs have been feature-reviewed and it would create a lot more work to feature-review them again.
Reviewable status: 2 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform) (waiting on @amcastro-tri and @drake-jenkins-bot)
There was a problem hiding this comment.
#12657 (I should've added a # sign)
Reviewable status: 2 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform) (waiting on @amcastro-tri and @drake-jenkins-bot)
There was a problem hiding this comment.
Reviewed 1 of 1 files at r19.
Reviewable status: LGTM missing from assignees edrumwri,sherm1(platform)
There was a problem hiding this comment.
Checkpoint -- comments on the class documentation.
Reviewed 1 of 2 files at r15, 1 of 5 files at r16, 1 of 1 files at r18.
Reviewable status: 4 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform) (waiting on @antequ)
systems/analysis/BUILD.bazel, line 227 at r19 (raw file):
deps = [ ":implicit_integrator", ":runge_kutta2_integrator",
I'm guessing rk2 is not used by this fixed-step version of the integrator. If that's right, please remove.
systems/analysis/implicit_integrator.h, line 399 at r19 (raw file):
// Methods for derived classes to increment the factorization and Jacobian // evaluation counts. inline void increment_num_iter_factorizations() {
minor: don't add inline for methods implemented within the class declaration - they are automatically inline. (And in general inline is ignored by the compiler unless you are using it to get around the "one definition rule" (ODR).)
BTW most compilers support some kind of force_inline mechanism that implements the old-school intent of the original inline keyword. That should not be used lightly, though!
systems/analysis/velocity_implicit_euler_integrator.h, line 57 at r19 (raw file):
* To solve the nonlinear system, the velocity-implicit Euler integrator * iteratively solves for `(qⁿ⁺¹,yⁿ⁺¹)` with a Newton's method: At iteration * `k`, it finds `(qₖ₊₁,yₖ₊₁)` that satisfies
This seems somewhat misleading. Since (5) and (6) are nonlinear, but each iteration solves only a linear system, we aren't finding qₖ₊₁,yₖ₊₁ that satisfy the equations, but rather something fuzzier like improves the solution over qₖ,yₖ. If you actually satisfied that equation you would already have qⁿ⁺¹. I'm not sure how best to word that.
I got stuck here for a while trying to figure out why you had nonlinear equations in a linear step.
systems/analysis/velocity_implicit_euler_integrator.h, line 71 at r19 (raw file):
* state x. * * To solve (5-6), first define
Again "solve" seems too strong. We're linearizing and then solving the linear system, not the nonlinear system (5-6). The equations we're actually solving are (3-4).
Maybe you can say that we relax nonlinear system (3-4) into (5-6), then linearize to obtain the equations used for one Newton step, then solve that linear system and take the step. ?
There was a problem hiding this comment.
Checkpoint.
Reviewable status: 35 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform) (waiting on @antequ)
systems/analysis/velocity_implicit_euler_integrator.h, line 20 at r19 (raw file):
namespace internal { #ifndef DRAKE_DOXYGEN_CXX __attribute__((noreturn)) inline void EmitNoErrorEstimatorStatAndMessage() {
BTW this is a correct use of inline to avoid ODR violations.
systems/analysis/velocity_implicit_euler_integrator.h, line 90 at r19 (raw file):
* - [Hairer, 1996] E. Hairer and G. Wanner. Solving Ordinary Differential * Equations II (Stiff and Differential-Algebraic Problems). * Springer, 1996.
minor: if you can provide a page range that would be helpful in this reference since it is a fat book.
systems/analysis/velocity_implicit_euler_integrator.h, line 92 at r19 (raw file):
* Springer, 1996. * - [Lambert, 1991] J. D. Lambert. Numerical Methods for Ordinary Differential * Equations. John Wiley & Sons, 1991.
nit: is [Lambert, 1991] mentioned somewhere?
systems/analysis/velocity_implicit_euler_integrator.h, line 123 at r19 (raw file):
int64_t do_get_num_newton_raphson_iterations() const final { return num_nr_iterations_; }
BTW are these overrides necessary for stats? In this PR you added some protected methods for concrete integrators to use to update stats maintained in the base class. That seems much nicer than a bunch of one-line overrides. Could that method replace all of these? (In a different PR!)
systems/analysis/velocity_implicit_euler_integrator.h, line 148 at r19 (raw file):
void DoResetImplicitIntegratorStatistics() final; static void ComputeAndFactorImplicitEulerIterationMatrix(
nit insert blank line between methods
systems/analysis/velocity_implicit_euler_integrator.h, line 170 at r19 (raw file):
// Newton-Raphson fails to converge on the second try. // @param trial the attempt for this approach (1-4). Similar to other implicit // integrators, StepImplicitEuler() uses increasingly computationally
minor: Similar to other implicit integrators (doesn't seem relevant and would be out of date if things elsewhere changed)
systems/analysis/velocity_implicit_euler_integrator.h, line 175 at r19 (raw file):
// @note The time and continuous state in the context are indeterminate upon // exit. bool StepImplicitEuler(
minor: this should be StepVelocityImplicitEuler() (at first I thought you were providing a regular implicit Euler step here for comparison) The name also appears in the comment above.
systems/analysis/velocity_implicit_euler_integrator.h, line 215 at r19 (raw file):
// standard Cartesian basis vector. // In the code we hereby refer to y as "the baseline" and y' as "prime". // @param t refers to tⁿ⁺¹, the time used in the definition of l(y).
nit: elsewhere you called this tf (see e.g. previous method). Maybe just use t everywhere?
systems/analysis/velocity_implicit_euler_integrator.h, line 272 at r19 (raw file):
// on return. // @param [out] Jy is the updated and factored Jacobian matrix Jₗ(y) on // return.
minor: Are iteration_matrix and Jy always updated by this method? If not, are they really in/out parameters?
systems/analysis/velocity_implicit_euler_integrator.h, line 278 at r19 (raw file):
// @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.
minor: sounds like the caller can't know whether the context is changed on return. In that case it would be better just to say that t and state are indeterminate on return. (Same comment for next method)
systems/analysis/velocity_implicit_euler_integrator.h, line 303 at r19 (raw file):
// @param[out] iteration_matrix the updated and factored iteration matrix on // return. // @param[out] Jy the updated Jacobian matrix Jₗ(y).
minor: are iteration_matrix and Jy actully in/out? (If just out I should be able always to pass them in as garbage.)
systems/analysis/velocity_implicit_euler_integrator.h, line 323 at r19 (raw file):
// with tⁿ⁺¹, y, qₖ, qⁿ, yⁿ, and h passed in. // @param t refers to tⁿ⁺¹, the time at which to compute the residual R(y). // @param y is the generalized velocity and miscellaneous states around which
nit consider adding y (=v,z) as a reminder so that the v below won't be a surprise.
systems/analysis/velocity_implicit_euler_integrator.h, line 333 at r19 (raw file):
// @param [out] result is set to R(y). // @post The context is set to (tⁿ⁺¹, qⁿ + h N(qₖ) v, y). VectorX<T> ComputeResidualR(const T& t, const VectorX<T> y,
minor: missing & before y (doing an unnecessary copy)
(If you actually need a copy do it internally)
systems/analysis/velocity_implicit_euler_integrator.h, line 349 at r19 (raw file):
// @param [out] result is set to l(y). // @post The context is set to (tⁿ⁺¹, qⁿ + h N(qₖ) v, y). VectorX<T> ComputeLOfY(const T& t, const VectorX<T> y, const VectorX<T>& qk,
minor: missing & before y (doing an unnecessary copy)
systems/analysis/velocity_implicit_euler_integrator.h, line 353 at r19 (raw file):
// The last computed iteration matrix and factorization; the _ie_ is for // the large step and the _hie_ is for the small step
nit: this comment doesn't apply for the fixed step version
systems/analysis/velocity_implicit_euler_integrator.h, line 365 at r19 (raw file):
int64_t num_nr_iterations_{0}; // The last computed velocity+misc Jacobian matrices.
nit matrices -> matrix
systems/analysis/velocity_implicit_euler_integrator.h, line 375 at r19 (raw file):
template <class T> void VelocityImplicitEulerIntegrator<T>::DoInitialize() {
minor: all the big methods here should be implemented in the .cc file. Please add a TODO to that effect here (too painful to move them during review).
systems/analysis/velocity_implicit_euler_integrator.h, line 383 at r19 (raw file):
// Verify that the maximum step size has been set. if (isnan(this->get_maximum_step_size())) throw std::logic_error("Maximum step size has not been set!");
minor: I think other fixed-step integrators accept either max step or initial step as long as one is set? If so, this probably should also.
systems/analysis/velocity_implicit_euler_integrator.h, line 392 at r19 (raw file):
// Set an initial working accuracy so that the integrator doesn't take too // long. this->set_accuracy_in_use(1e-6);
minor: does this silently override the accuracy setting if a user sets it? That seems dangerous. How about using user accuracy if set, with this as a default if not set?
systems/analysis/velocity_implicit_euler_integrator.h, line 405 at r19 (raw file):
// subtraction as would be the case with: // MatrixX<T>::Identity(n, n) - J * h. iteration_matrix->SetAndFactorIterationMatrix(J * -h +
nit: conventional to put the scalar first in scalar multiply of a matrix (I-hJ or -hJ+I).
systems/analysis/velocity_implicit_euler_integrator.h, line 435 at r19 (raw file):
// so that the context needs only one modification. Investigate how to // refactor this logic to achieve this performance benefit while maintaining // code readibility.
nit readability
systems/analysis/velocity_implicit_euler_integrator.h, line 453 at r19 (raw file):
//// Get the system. // const System<T>& system = this->get_system();
nit: remove commented-out code
systems/analysis/velocity_implicit_euler_integrator.h, line 457 at r19 (raw file):
switch (this->get_jacobian_computation_scheme()) { case VelocityImplicitEulerIntegrator< T>::JacobianComputationScheme::kForwardDifference:
minor:
- this is defined in the base class so should be ImplicitIntegrator::etc.
- (BTW) weird to have an enum constant depend on "T". That enum class should be moved outside of a scalar-typed class (probably not in this PR though)
systems/analysis/velocity_implicit_euler_integrator.h, line 475 at r19 (raw file):
// evaluations used in computing Jacobians. this->increment_jacobian_computation_derivative_evaluations( this->get_num_derivative_evaluations() - current_ODE_evals);
nit: "current" looks odd here -- it isn't current. Better to name this something like initial_ODE_evals so it will make sense here.
systems/analysis/velocity_implicit_euler_integrator.h, line 494 at r19 (raw file):
CalcVelocityJacobian(t, h, y, qk, qn, Jy); this->increment_num_iter_factorizations(); compute_and_factor_iteration_matrix(*Jy, h, iteration_matrix);
BTW would be slightly better (and slightly more readable) to increment the count after the call to do the work. (If the method were to throw an exception, the count wouldn't include that failed attempt.) (Several instances below)
systems/analysis/velocity_implicit_euler_integrator.h, line 519 at r19 (raw file):
// the iteration matrix, using the last computed Jacobian. this->increment_num_iter_factorizations(); compute_and_factor_iteration_matrix(*Jy, h, iteration_matrix);
minor: I think this only makes sense if the step size has changed, right? If so the comment above should explain how we know that it has.
systems/analysis/velocity_implicit_euler_integrator.h, line 567 at r19 (raw file):
CalcVelocityJacobian(t, h, y, qk, qn, Jy); this->increment_num_iter_factorizations(); compute_and_factor_iteration_matrix(*Jy, h, iteration_matrix);
minor: these three lines appear repeatedly in the code (at least 3 times). Consider making a method CalcVelocityJacobianAndIterationMatrix() ?
systems/analysis/velocity_implicit_euler_integrator.h, line 578 at r19 (raw file):
// Evaluate R(y). return (y - yn - h * l_of_y).eval();
BTW I'm surprised you need .eval() here -- won't that happen automatically?
systems/analysis/velocity_implicit_euler_integrator.h, line 587 at r19 (raw file):
const T& h) { Context<T>* context = this->get_mutable_context(); const systems::ContinuousState<T>& cstate = context->get_continuous_state();
minor: please add a comment noting that the value underlying this const reference will change below. I was confused about that for a while trying to figure out what's happening here.
systems/analysis/velocity_implicit_euler_integrator.h, line 592 at r19 (raw file):
// Set the context to (t, qₖ, y) VectorX<T> x(nq+ny);
minor: unnecessary heap allocation
systems/analysis/velocity_implicit_euler_integrator.h, line 595 at r19 (raw file):
x.head(nq) = qk; x.tail(ny) = y; context->SetTimeAndContinuousState(t, x);
Please look at SemiExplicitEulerIntegrator<T>::DoStep to see how to avoid unneeded copies and heap allocations and how to document the state of the context and cache.
systems/analysis/velocity_implicit_euler_integrator.h, line 598 at r19 (raw file):
// Compute q = qⁿ + h N(qₖ) v. BasicVector<T> qdot(nq);
minor: unnecessary heap allocation
systems/analysis/velocity_implicit_euler_integrator.h, line 601 at r19 (raw file):
this->get_system().MapVelocityToQDot( *context, cstate.get_generalized_velocity(), &qdot); const VectorX<T> q = qn + h * qdot.get_value();
minor: unnecessary heap allocation
systems/analysis/velocity_implicit_euler_integrator.h, line 606 at r19 (raw file):
context->get_mutable_continuous_state() .get_mutable_generalized_position() .SetFromVector(q);
minor: should only set the q's.
Q: is this likely to be q(k+1) ? I'm wondering if the initial setting of qk in the context is always necessary -- next iteration it might already be set? Let's discuss.
It is always bad to have heap allocation going on at every integration step if you can avoid it. Should at least have TODOs wherever there is something that could (and probably should) be improved.
systems/analysis/velocity_implicit_euler_integrator.h, line 608 at r19 (raw file):
.SetFromVector(q); const ContinuousState<T>& xc_deriv = this->EvalTimeDerivatives(*context); return xc_deriv.CopyToVector().tail(ny);
minor: unnecessary heap allocation
There was a problem hiding this comment.
Reviewed 1 of 9 files at r10.
Reviewable status: 35 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform) (waiting on @antequ)
There was a problem hiding this comment.
Reviewable status: 35 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform) (waiting on @antequ)
systems/analysis/velocity_implicit_euler_integrator.h, line 20 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
BTW this is a correct use of
inlineto avoid ODR violations.
you'll have to explain this one to me. How is it useful the inline here? that is, without the inline, when would I run the risk of violating ODR (an example would be great).
There was a problem hiding this comment.
Reviewable status: 35 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform) (waiting on @antequ and @sherm1)
systems/analysis/velocity_implicit_euler_integrator.h, line 375 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: all the big methods here should be implemented in the
.ccfile. Please add a TODO to that effect here (too painful to move them during review).
an alternative would be to leave this as a blocking comment and do this move at the end of the review when we both LGTMed. One less PR.
There was a problem hiding this comment.
Addressed some comments
Reviewable status: 35 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform) (waiting on @amcastro-tri, @antequ, and @sherm1)
systems/analysis/BUILD.bazel, line 227 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
I'm guessing rk2 is not used by this fixed-step version of the integrator. If that's right, please remove.
Fixed!
systems/analysis/implicit_integrator.h, line 399 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: don't add
inlinefor methods implemented within the class declaration - they are automatically inline. (And in generalinlineis ignored by the compiler unless you are using it to get around the "one definition rule" (ODR).)BTW most compilers support some kind of
force_inlinemechanism that implements the old-school intent of the originalinlinekeyword. That should not be used lightly, though!
Fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 57 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
This seems somewhat misleading. Since (5) and (6) are nonlinear, but each iteration solves only a linear system, we aren't finding qₖ₊₁,yₖ₊₁ that satisfy the equations, but rather something fuzzier like improves the solution over qₖ,yₖ. If you actually satisfied that equation you would already have qⁿ⁺¹. I'm not sure how best to word that.
I got stuck here for a while trying to figure out why you had nonlinear equations in a linear step.
Changed the wording to indicate that it's solving equations 3 and 4 for q^n+1, y^n+1, and that it just iterates with a modified Newotn's method. I also made the wording weaker so that it finds a qk+1, yk+1 that attempts to satisfy
systems/analysis/velocity_implicit_euler_integrator.h, line 71 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
Again "solve" seems too strong. We're linearizing and then solving the linear system, not the nonlinear system (5-6). The equations we're actually solving are (3-4).
Maybe you can say that we relax nonlinear system (3-4) into (5-6), then linearize to obtain the equations used for one Newton step, then solve that linear system and take the step. ?
I've rephrased it to say "approximately satisfy". Also added the phrase "we linearize the system (5-6) to compute a Newton step"
systems/analysis/velocity_implicit_euler_integrator.h, line 90 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: if you can provide a page range that would be helpful in this reference since it is a fat book.
Added
systems/analysis/velocity_implicit_euler_integrator.h, line 92 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
nit: is [Lambert, 1991] mentioned somewhere?
Fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 123 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
BTW are these overrides necessary for stats? In this PR you added some protected methods for concrete integrators to use to update stats maintained in the base class. That seems much nicer than a bunch of one-line overrides. Could that method replace all of these? (In a different PR!)
Yes. It doesn't compile otherwise (whether we like them or not, these methods were originally designed with the assumption that all child integrators will be error-controlled); this is similar to how Evan checked in the first Radau fixed integrator. Alejandro and I also discussed this; I felt that the best solution is to not change them in this pass, especially since the next PR will implement these overrides anyways.
systems/analysis/velocity_implicit_euler_integrator.h, line 148 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
nit insert blank line between methods
Fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 170 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor:
Similar to other implicit integrators(doesn't seem relevant and would be out of date if things elsewhere changed)
Fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 175 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: this should be
StepVelocityImplicitEuler()(at first I thought you were providing a regular implicit Euler step here for comparison) The name also appears in the comment above.
Fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 215 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
nit: elsewhere you called this
tf(see e.g. previous method). Maybe just useteverywhere?
Fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 272 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: Are iteration_matrix and Jy always updated by this method? If not, are they really in/out parameters?
This is tricky. The only decisions made based on them is if the Jacobian has a size of 0, it forces it to recalculate a new Jacobian, and if the iteration matrix is not factored, it'll factorize them. I think that is enough to make them in/out.
systems/analysis/velocity_implicit_euler_integrator.h, line 278 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: sounds like the caller can't know whether the context is changed on return. In that case it would be better just to say that t and state are indeterminate on return. (Same comment for next method)
This comment does give information, in that if the context is at time t before calling this, then the caller at least knows that the time is t after calling. I changed it slightly to say the internal context may be altered on return.
systems/analysis/velocity_implicit_euler_integrator.h, line 303 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: are iteration_matrix and Jy actully in/out? (If just
outI should be able always to pass them in as garbage.)
In this implementation, passing in any garbage is fine. They are only updated if the full newton flag is turned on, and they are not inspected; no decision is made based on them.
systems/analysis/velocity_implicit_euler_integrator.h, line 323 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
nit consider adding
y (=v,z)as a reminder so that thevbelow won't be a surprise.
Fixed by clarifying the y two lines above as = (v,z)
systems/analysis/velocity_implicit_euler_integrator.h, line 333 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: missing
&before y (doing an unnecessary copy)
(If you actually need a copy do it internally)
Fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 349 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: missing
&before y (doing an unnecessary copy)
Fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 353 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
nit: this comment doesn't apply for the fixed step version
Fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 365 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
nit matrices -> matrix
Fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 375 at r19 (raw file):
Previously, amcastro-tri (Alejandro Castro) wrote…
an alternative would be to leave this as a blocking comment and do this move at the end of the review when we both LGTMed. One less PR.
Fixed with a TODO
systems/analysis/velocity_implicit_euler_integrator.h, line 392 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: does this silently override the accuracy setting if a user sets it? That seems dangerous. How about using user accuracy if set, with this as a default if not set?
The user is not allowed to set the accuracy since it's a fixed integrator, which is why I do it here. Added the isnan() check anyways.
There was a problem hiding this comment.
Reviewable status: 35 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri, @antequ, and @sherm1)
systems/analysis/velocity_implicit_euler_integrator.h, line 383 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: I think other fixed-step integrators accept either max step or initial step as long as one is set? If so, this probably should also.
Initial step target is only for error-controlled integrators. This fixed-step integrator will just use the maximum step size. This is consistent with the Fixed Step Radau PR (#11288).
Most other fixed step integrators take a step size as an argument in the constructor. I could set it up that way, too, but that would require more function signature changes to add error control, and the Radau PR doesn't do this.
systems/analysis/velocity_implicit_euler_integrator.h, line 405 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
nit: conventional to put the scalar first in scalar multiply of a matrix (I-hJ or -hJ+I).
Fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 435 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
nit readability
Fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 453 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
nit: remove commented-out code
Fixed.
systems/analysis/velocity_implicit_euler_integrator.h, line 457 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor:
- this is defined in the base class so should be ImplicitIntegrator::etc.
- (BTW) weird to have an enum constant depend on "T". That enum class should be moved outside of a scalar-typed class (probably not in this PR though)
ImplicitIntegrator:: works. Fixed.
systems/analysis/velocity_implicit_euler_integrator.h, line 475 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
nit: "current" looks odd here -- it isn't current. Better to name this something like
initial_ODE_evalsso it will make sense here.
Fixed. Changed it to existing_
systems/analysis/velocity_implicit_euler_integrator.h, line 494 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
BTW would be slightly better (and slightly more readable) to increment the count after the call to do the work. (If the method were to throw an exception, the count wouldn't include that failed attempt.) (Several instances below)
This depends on whether the count is for the number of factorizations attempted or completed. This also applies for the number of Newton-Raphson iterations, and we use that count to indicate the number of iterations attempted rather than succeeded.
I'm keeping it this way to be consistent with ImplicitIntegrator::MaybeFreshenMatrices().
systems/analysis/velocity_implicit_euler_integrator.h, line 519 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: I think this only makes sense if the step size has changed, right? If so the comment above should explain how we know that it has.
This isn't necessarily true. This could be because the context has advanced a few time-steps. I've clarified the comments.
systems/analysis/velocity_implicit_euler_integrator.h, line 567 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: these three lines appear repeatedly in the code (at least 3 times). Consider making a method CalcVelocityJacobianAndIterationMatrix() ?
This seems simple enough that I am not sure if it is helpful. The other option is to change CalcVelocityJacobian into that, but that would mean I have to add a few more function arguments, include a compute_and_factor_iteration_matrix function input.
systems/analysis/velocity_implicit_euler_integrator.h, line 578 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
BTW I'm surprised you need
.eval()here -- won't that happen automatically?
Good catch; it's no longer needed. It was needed in previous iterations because of strange behavior with the BlockRef data structures.
systems/analysis/velocity_implicit_euler_integrator.h, line 587 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: please add a comment noting that the value underlying this const reference will change below. I was confused about that for a while trying to figure out what's happening here.
I've removed this declaration and just moved context->get_continuous_state() inline, since it's only used in one location.
systems/analysis/velocity_implicit_euler_integrator.h, line 592 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: unnecessary heap allocation
Added a TODO comment
systems/analysis/velocity_implicit_euler_integrator.h, line 595 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
Please look at
SemiExplicitEulerIntegrator<T>::DoStepto see how to avoid unneeded copies and heap allocations and how to document the state of the context and cache.
Added a TODO comment above VectorX x(nq+ny);
systems/analysis/velocity_implicit_euler_integrator.h, line 598 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: unnecessary heap allocation
Removed by making the caller pass this in
systems/analysis/velocity_implicit_euler_integrator.h, line 601 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: unnecessary heap allocation
Mentioned in comments
systems/analysis/velocity_implicit_euler_integrator.h, line 606 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: should only set the q's.
Q: is this likely to be q(k+1) ? I'm wondering if the initial setting of qk in the context is always necessary -- next iteration it might already be set? Let's discuss.It is always bad to have heap allocation going on at every integration step if you can avoid it. Should at least have TODOs wherever there is something that could (and probably should) be improved.
I previously thought this way of setting q actually only invalidates the q caches, based on the last time I asked -- that get_mutable_continuous_state doesn't invalidate caches, and get_mutable_generalized_position invalidates the caches.
This is not qk+1 because the v is from yk, while qk+1 is defined with vk+1.
The comments already specifically say that the position ends up being
qⁿ + h N(qₖ) v.
If we reused some of these caches, we could easily make it so the q is iterated twice every iteration, but that would be extremely hard to document. (I've experimented with that in an earlier iteration of this integrator.)
I've added a TODO to rework this once we have more specialized Context modification functions.
systems/analysis/velocity_implicit_euler_integrator.h, line 608 at r19 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: unnecessary heap allocation
Mentioned in comments
There was a problem hiding this comment.
Reviewed 4 of 4 files at r20.
Reviewable status: 35 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri and @sherm1)
There was a problem hiding this comment.
Platform review pass complete! Just a few more things, ptal.
Reviewed 1 of 5 files at r16, 1 of 3 files at r17, 4 of 4 files at r20.
Reviewable status: 7 unresolved discussions, LGTM missing from assignees edrumwri,sherm1(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @antequ)
systems/analysis/velocity_implicit_euler_integrator.h, line 20 at r19 (raw file):
Previously, amcastro-tri (Alejandro Castro) wrote…
you'll have to explain this one to me. How is it useful the
inlinehere? that is, without theinline, when would I run the risk of violating ODR (an example would be great).
Happy to explain f2f (chromebox).
systems/analysis/velocity_implicit_euler_integrator.h, line 361 at r20 (raw file):
BasicVector<T>* qdot); // The last computed iteration matrix and factorization
nit: missing period
systems/analysis/velocity_implicit_euler_integrator.h, line 414 at r20 (raw file):
// 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.
nit: h * J in the comment to better match the (now modified) code.
systems/analysis/velocity_implicit_euler_integrator.h, line 698 at r20 (raw file):
// Update the number of Newton-Raphson iterations. num_nr_iterations_++;
nit: prefer pre-increment when possible
systems/analysis/velocity_implicit_euler_integrator.h, line 717 at r20 (raw file):
// 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
minor: "may invalidate" -> "invalidates"
systems/analysis/velocity_implicit_euler_integrator.h, line 754 at r20 (raw file):
} DRAKE_LOGGER_DEBUG("Velocity-Implicit Euler integrator convergence failed");
minor: please include time and step size being attempted in this log message
systems/analysis/velocity_implicit_euler_integrator.h, line 785 at r20 (raw file):
DRAKE_LOGGER_DEBUG( "-- requested step too small, taking explicit " "step instead");
nit: would be good to include in this log entry (1) time t0, (2) the requested step size h, and (3) the min step size that it tripped over.
systems/analysis/velocity_implicit_euler_integrator.h, line 794 at r20 (raw file):
// [Hairer 1996] validates this choice (p. 120). const VectorX<T>& xtplus_guess = xn_; bool success = StepVelocityImplicitEuler(t0, h, xn_, xtplus_guess,
btw can be const
systems/analysis/velocity_implicit_euler_integrator.h, line 801 at r20 (raw file):
DRAKE_LOGGER_DEBUG( "Velocity-Implicit Euler approach did not converge for " "step size {}",
nit: please include time t0 in this message also
systems/analysis/velocity_implicit_euler_integrator.h, line 809 at r20 (raw file):
// Set the state to the computed state. context->SetTimeAndContinuousState(t0 + h, xtplus_ie_);
BTW is this potentially unnecessary? If the last thing done in StepVelocityImplicitEuler() was this time and state then we are invalidating the cache unnecessarily here. Perhaps a TODO here to investigate?
There was a problem hiding this comment.
Reviewable status: 1 unresolved discussion, LGTM missing from assignees edrumwri,sherm1(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @sherm1)
systems/analysis/velocity_implicit_euler_integrator.h, line 361 at r20 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
nit: missing period
Fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 414 at r20 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
nit:
h * Jin the comment to better match the (now modified) code.
Fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 698 at r20 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
nit: prefer pre-increment when possible
Fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 717 at r20 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: "may invalidate" -> "invalidates"
Fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 754 at r20 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: please include time and step size being attempted in this log message
Done
systems/analysis/velocity_implicit_euler_integrator.h, line 785 at r20 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
nit: would be good to include in this log entry (1) time t0, (2) the requested step size h, and (3) the min step size that it tripped over.
Fixed
systems/analysis/velocity_implicit_euler_integrator.h, line 794 at r20 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
btw can be
const
Done
systems/analysis/velocity_implicit_euler_integrator.h, line 801 at r20 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
nit: please include time t0 in this message also
I think this will be a duplication of the above message in all cases (not entirely certain), but I guess it never hurts to have too many copies of this message.
systems/analysis/velocity_implicit_euler_integrator.h, line 809 at r20 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
BTW is this potentially unnecessary? If the last thing done in StepVelocityImplicitEuler() was this time and state then we are invalidating the cache unnecessarily here. Perhaps a TODO here to investigate?
Yes, it's necessary. I changed the logic in StepVelocityImplicitEuler so that it never sets the context to the correct computed state. This reduces the number of unnecessary context state changes.
There was a problem hiding this comment.
Addressed review comments. Going to rebase and squash now
Reviewable status: 1 unresolved discussion, LGTM missing from assignees edrumwri,sherm1(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @antequ and @sherm1)
…ptions. This PR adds a fixed-step implementation of the new first-order Velocity-Implicit Euler Integrator (Issue RobotLocomotion#12528). This is the second PR out of four. The documentation is based on the equations listed here: https://docs.google.com/document/d/118J6rkR9ghh2_3WY0O5EDXhfkgMOXXqTQ2usrhICv30/edit
antequ
force-pushed
the
dev_vie_fixed_step_pr2
branch
from
8e608cb
to
0a2ee86
Compare
There was a problem hiding this comment.
Reviewed 1 of 1 files at r21.
Reviewable status: 1 unresolved discussion, LGTM missing from assignees edrumwri,sherm1(platform) (waiting on @sherm1)
There was a problem hiding this comment.
Platform 
Ready to merge when CI is green.
Reviewed 1 of 1 files at r21.
Reviewable status: LGTM missing from assignee edrumwri
There was a problem hiding this comment.
-@edrumwri since he abandoned us :( ❤️
Reviewable status:
complete! all discussions resolved, LGTM from assignees amcastro-tri,sherm1(platform)
This PR adds a fixed-step implementation of the new first-order Velocity-Implicit Euler Integrator (Issue #12528). This is the second PR out of four.
The documentation is based on the equations listed here: https://docs.google.com/document/d/118J6rkR9ghh2_3WY0O5EDXhfkgMOXXqTQ2usrhICv30/edit
The current state of this PR also contains all the changes of #12532 together with it; this PR is a branch on top of the other PR. Do not review the files and code from that PR; I've marked them for your convenience. (The simplest way to filter is just look at the changes from the latest commit, 8ef182c. ) After that PR is merged, this PR should contain less than 1000 lines of changes. For the purposes of reviewing, the contents of this PR do not depend on understanding the other PR.
+@edrumwri +@sherm1 +@amcastro-tri
I've marked this "Do not merge", but feel free to start reviewing!
This change is