Add DiscreteTimeApproximation for general continuous-time systems #22652
Conversation
There was a problem hiding this comment.
Reviewed 8 of 8 files at r1, all commit messages.
Reviewable status: 9 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @wei-chen-li)
a discussion (no related file):
+@RussTedrake for feature review.
This is great! Another one that's been on my list for a long time.
systems/primitives/discrete_time_approximation.h line 15 at r1 (raw file):
/// discrete-time dynamics is given by @f$ x[n+1] = f_d(n,x[n],u[n]) = x[n] + /// \int_{t[n]}^{t[n+1]} f(t,x(t),u[n]) \, dt @f$, where the integration is /// performed using numerical Integrators.
nit: integrators (lower case) or "using a numerical Integrator" singular.. so that doxygen makes a link to the Integrator class.
systems/primitives/discrete_time_approximation.cc line 150 at r1 (raw file):
const std::unique_ptr<const System<T>> continuous_system_; const std::unique_ptr<Context<T>> continuous_context_;
i'm afraid you need to register this as state or as a cache variable... you can't keep state like this in a mutable parameter. amongst other things, it would not be thread safe.
See the LuenbergerObserver implementation for one relevant example.
systems/primitives/BUILD.bazel line 131 at r1 (raw file):
deps = [ "//systems/analysis:simulator", "//systems/analysis:simulator_config_functions",
@jwnimmer-tri can weigh in here, but I think we don't want this dependency cycle (the folder /systems/analysis already depends on /systems/framework, we don't want to introduce the cyclic dependency).
Recommendation: let's move this method to /systems/analysis.
bindings/pydrake/systems/test/primitives_test.py line 965 at r1 (raw file):
assert_array_close(discrete_system.y0(), y0d) def test_discrete_time_approximation_system(self):
use the @numpy_compare.check_all_types (for nonsymbolic) here?
bindings/pydrake/systems/test/primitives_test.py line 969 at r1 (raw file):
system=Integrator_[float](1), time_period=0.01, time_offset=0.0) sys.ToScalarType[AutoDiffXd]()
why this? can we check some invariant of the system instead... e.g. that it has the right number of discrete states?
systems/primitives/test/discrete_time_approximation_test.cc line 32 at r1 (raw file):
const DiscreteValues<double>& state = sys->EvalUniquePeriodicDiscreteUpdate(*context); context->SetDiscreteState(state);
it would be slightly better to use AdvanceTo here; it puts more of the context under test. is there a reason you did not? (if yes, can you document it here?)
systems/primitives/test/discrete_time_approximation_test.cc line 173 at r1 (raw file):
</transmission> </robot> )";
btw: why not just use the cartpole urdf that's already in the file tree? presumably it's because you want to lock in the specific parameters? I guess that's ok.
systems/primitives/test/discrete_time_approximation_test.cc line 246 at r1 (raw file):
std::shared_ptr controller = controllers::LinearQuadraticRegulator( *discrete_sys_, *discrete_context_, Q, R);
add a check here that the returned system is also a DifferenceEquationSystem?
There was a problem hiding this comment.
Reviewable status: 10 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @wei-chen-li)
systems/primitives/discrete_time_approximation.cc line 155 at r1 (raw file):
const SimulatorConfig integrator_config_; Simulator<T> simulator__; IntegratorBase<T>* integrator_;
are these also holding internal state? (I fear that they are, otherwise they would be const?)
There was a problem hiding this comment.
Reviewable status: 10 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @wei-chen-li)
systems/primitives/discrete_time_approximation.cc line 155 at r1 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
are these also holding internal state? (I fear that they are, otherwise they would be const?)
to clarify my thoughts: the simulator is ok, but the integrator is holding state.
There was a problem hiding this comment.
Reviewable status: 8 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, missing label for release notes
systems/primitives/BUILD.bazel line 131 at r1 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
@jwnimmer-tri can weigh in here, but I think we don't want this dependency cycle (the folder /systems/analysis already depends on /systems/framework, we don't want to introduce the cyclic dependency).
Recommendation: let's move this method to /systems/analysis.
Done.
systems/primitives/discrete_time_approximation.cc line 150 at r1 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
i'm afraid you need to register this as state or as a cache variable... you can't keep state like this in a mutable parameter. amongst other things, it would not be thread safe.
See the LuenbergerObserver implementation for one relevant example.
The internal maintained continuous system context is now a cache variable.
systems/primitives/discrete_time_approximation.cc line 155 at r1 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
to clarify my thoughts: the simulator is ok, but the integrator is holding state.
The integrator is now initialized in every time it needs to be used.
systems/primitives/test/discrete_time_approximation_test.cc line 32 at r1 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
it would be slightly better to use AdvanceTo here; it puts more of the context under test. is there a reason you did not? (if yes, can you document it here?)
Done.
systems/primitives/test/discrete_time_approximation_test.cc line 246 at r1 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
add a check here that the returned system is also a DifferenceEquationSystem?
The controller is stateless.
bindings/pydrake/systems/test/primitives_test.py line 965 at r1 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
use the @numpy_compare.check_all_types (for nonsymbolic) here?
Done.
bindings/pydrake/systems/test/primitives_test.py line 969 at r1 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
why this? can we check some invariant of the system instead... e.g. that it has the right number of discrete states?
Done.
There was a problem hiding this comment.
Reviewable status: 8 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, missing label for release notes
systems/primitives/BUILD.bazel line 131 at r1 (raw file):
Previously, wei-chen-li (Wei-Chen Li) wrote…
Done.
I have also moved the LinearSystem and AffineSystem version to /system/analysis. Otherwise, from pydrake.all import DiscreteTimeApproximation will only import a subset of the overloads.
There was a problem hiding this comment.
Reviewed 13 of 16 files at r2, 3 of 3 files at r3, all commit messages.
Reviewable status: 5 unresolved discussions, LGTM missing from assignees RussTedrake(platform),sherm1(platform), missing label for release notes (waiting on @wei-chen-li)
bindings/pydrake/systems/test/primitives_test.py line 965 at r1 (raw file):
Previously, wei-chen-li (Wei-Chen Li) wrote…
Done.
not done?
systems/primitives/BUILD.bazel line 131 at r1 (raw file):
Previously, wei-chen-li (Wei-Chen Li) wrote…
I have also moved the
LinearSystemandAffineSystemversion to /system/analysis. Otherwise,from pydrake.all import DiscreteTimeApproximationwill only import a subset of the overloads.
Yes. This is a bit unfortunate. Even though these methods are brand new, it happened that we just had a monthly release the other day. So they were published in their original location; that might mean we need a deprecation shim. @jwnimmer-tri -- wdyt?
systems/analysis/discrete_time_approximation.cc line 168 at r3 (raw file):
DiscreteValues<T>* out) const { Simulator<T> simulator(*continuous_system_); ApplySimulatorConfig(integrator_config_, &simulator);
This is safe (good), but probably quite inefficient. Let's just add a TODO in here to move the simulator / integrator into the context?
+@sherm1 in case he has any sage advice on this. (the issue we were trying to avoid is having a mutable integrator as a mutable member variable / aka undeclared state. (I wish there was a variant of IntegrateWithMultipleStepsToTime that was marked const, and didn't do any book-keeping... that would be more clean, I think?) . Is calling reset_context and then Initialize sufficient to make the integrator effectively stateless?
But, to be clear, I think landing this as-is with a TODO is fine.
systems/analysis/test/discrete_time_approximation_test.cc line 259 at r3 (raw file):
auto z_inv = builder.AddSystem<DiscreteTimeDelay>(time_period_, 0, num_states); // Add a discrete-time delay to avoid algebraic loop.
whoops. no. this is a red flag. if the original system wasn't direct feedthrough, then the discrete approximation should not be, either. we need to copy over the dependencies tickets from the original output ports, etc, into the newly declared output ports.
There was a problem hiding this comment.
Reviewable status: 6 unresolved discussions, LGTM missing from assignees sherm1(platform),RussTedrake(platform), missing label for release notes
bindings/pydrake/systems/test/primitives_test.py line 965 at r1 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
not done?
Done.
systems/analysis/discrete_time_approximation.cc line 174 at r3 (raw file):
continuous_context_cache_entry_ ->template Eval<Context<T>>(discrete_context) .Clone();
The .Clone() here is probably allocates heap and is quite expensive.
|
-- https://drake.mit.edu/stable.html#exemptions Given that the change for the user is just fixing an |
There was a problem hiding this comment.
Reviewable status: 7 unresolved discussions, LGTM missing from assignees sherm1(platform),RussTedrake(platform), missing label for release notes (waiting on @wei-chen-li)
systems/analysis/discrete_time_approximation.cc line 168 at r3 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
This is safe (good), but probably quite inefficient. Let's just add a TODO in here to move the simulator / integrator into the context?
+@sherm1 in case he has any sage advice on this. (the issue we were trying to avoid is having a mutable integrator as a mutable member variable / aka undeclared state. (I wish there was a variant of IntegrateWithMultipleStepsToTime that was marked const, and didn't do any book-keeping... that would be more clean, I think?) . Is calling
reset_contextand thenInitializesufficient to make the integrator effectively stateless?But, to be clear, I think landing this as-is with a TODO is fine.
Most of the integrators' internal state is harmless statistics (number of steps taken, etc.) but variable-step and implicit integrators have significant internal state (e.g. sizes of previous steps, Jacobians that are re-used as long as possible). None of that is stored in the Context so I don't think reset_context() helps. A call to Initialize() would nuke any internal state though. There is also a Reset() function that wipes out the configuration also.
systems/analysis/discrete_time_approximation.h line 53 at r4 (raw file):
/// @param system The continuous-time System. /// @param time_period The discrete time period. /// @param time_period The discrete time offset.
typo: time_period -> time_offset
There was a problem hiding this comment.
Reviewed 1 of 2 files at r4.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes
systems/analysis/test/discrete_time_approximation_test.cc line 259 at r3 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
whoops. no. this is a red flag. if the original system wasn't direct feedthrough, then the discrete approximation should not be, either. we need to copy over the dependencies tickets from the original output ports, etc, into the newly declared output ports.
The code required to get the dependencies tickets from the original output ports is quite nasty. I requires dynamic_casting the output port.
There was a problem hiding this comment.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes
systems/analysis/test/discrete_time_approximation_test.cc line 259 at r3 (raw file):
Previously, wei-chen-li (Wei-Chen Li) wrote…
The code required to get the dependencies tickets from the original output ports is quite nasty. I requires dynamic_casting the output port.
Ignore the above comment. I have fixed it in the newest revision.
There was a problem hiding this comment.
Reviewed 1 of 3 files at r5, all commit messages.
Reviewable status: 6 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @wei-chen-li)
a discussion (no related file):
Previously, RussTedrake (Russ Tedrake) wrote…
+@RussTedrake for feature review.
This is great! Another one that's been on my list for a long time.
the interactions between the simulator and the cache have gotten very complicated. I took your branch and started playing with what it would look like if we could put the Simulator itself in the cache, instead. Everything would get much cleaner, and more obviously correct/future-proof, imo. But, I don't think we can put a Simulator as a cache entry just yet because Simulator is not clonable.
My recommendation is to take one of two paths:
- we could land this now, but don't use a simulator / integrator at all... just always do euler integration. that's what a number of the trajectory optimization codes do because we didn't grapple with these issues yet.
- step back and implement Clone for Simulator. I/we could help with that. but it would mean putting this PR on hold while we do it.
We could do both -- land (1) now and then follow-up with the more general form after doing (2).
My quick experiment is here: db776b6
WDYT?
systems/analysis/discrete_time_approximation.cc line 108 at r6 (raw file):
continuous_context_cache_entry_ = &this->DeclareCacheEntry( "continuous system context", *continuous_context_model_value_, &DiscreteTimeSystem<T>::CopyAllSources,
nit: the name "Sources" confused me here... it's not the sources ticket, etc.
recommentation: "CopyAllSources"=>"CopyAllContext" (same for the ExceptInput variety)
systems/analysis/discrete_time_approximation.cc line 112 at r6 (raw file):
// Another cache entry for the continuous system context, but this one does // not copy the input port values. continuous_context2_cache_entry_ = &this->DeclareCacheEntry(
This seems a strange way to accomplish the goal of making sure that your output port really has no direct feedthrough... but I would go sofar as to say you don't need it. If a continuous-time system has poorly marked the direct feedthrough property, then that's an upstream problem... and your check is not actually avoiding that problem (refusing to update the input port values is not equivalent to not accessing the input port values from the output port).
Recommendation: just keep the one continuous context, and go ahead and Fix the input port values in the context in the output methods, too. continuous systems which are not direct feedthrough will not access those values.
Or am I missing something?
There was a problem hiding this comment.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes
systems/analysis/discrete_time_approximation.cc line 112 at r6 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
This seems a strange way to accomplish the goal of making sure that your output port really has no direct feedthrough... but I would go sofar as to say you don't need it. If a continuous-time system has poorly marked the direct feedthrough property, then that's an upstream problem... and your check is not actually avoiding that problem (refusing to update the input port values is not equivalent to not accessing the input port values from the output port).
Recommendation: just keep the one continuous context, and go ahead and Fix the input port values in the context in the output methods, too. continuous systems which are not direct feedthrough will not access those values.
Or am I missing something?
Assume our discrete-time plant is in a direct feedback loop, as in the last unit test case. And we propose to use only one continuous context cache entry. This is what happens on the stack trace:
1. output_port.Eval()
2. continuous_context_cache_entry_->Eval()
3. CopyAllContext()
4. this->EvalAbstractInput()
5. output_port.Eval()
4 calls 5 because of the feedback. And oops, we have an infinite recursion.
There was a problem hiding this comment.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes
a discussion (no related file):
Previously, RussTedrake (Russ Tedrake) wrote…
the interactions between the simulator and the cache have gotten very complicated. I took your branch and started playing with what it would look like if we could put the Simulator itself in the cache, instead. Everything would get much cleaner, and more obviously correct/future-proof, imo. But, I don't think we can put a Simulator as a cache entry just yet because Simulator is not clonable.
My recommendation is to take one of two paths:
- we could land this now, but don't use a simulator / integrator at all... just always do euler integration. that's what a number of the trajectory optimization codes do because we didn't grapple with these issues yet.
- step back and implement Clone for Simulator. I/we could help with that. but it would mean putting this PR on hold while we do it.
We could do both -- land (1) now and then follow-up with the more general form after doing (2).
My quick experiment is here: db776b6
WDYT?
For path 1, I would be uncomfortable using euler integration, as the calculated Jacobians (via autodiff) would be way off. If we must hardcode an integration scheme here, I would prefer at least a third-order method.
Alternatively, we can modify from
Simulator<T> simulator(*continuous_system_);
ApplySimulatorConfig(integrator_config_, &simulator);
auto& integrator = simulator.get_mutable_integrator();
to
RungeKutta3Integrator<T> integrator(*continuous_system_);
Initializing an integrator should be much cheaper than initializing a simulator?
For path 2, if implementing Clone for Simulator is too hard or unresonable, it is sufficient for our use case to implement Clone for IntegratorBase.
I would prefer to simply write RungeKutta3Integrator<T> integrator(*continuous_system_); here and merge this PR before the next release date. And revisit this after Clone is implemented.
There was a problem hiding this comment.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes
a discussion (no related file):
Previously, wei-chen-li (Wei-Chen Li) wrote…
For path 1, I would be uncomfortable using euler integration, as the calculated Jacobians (via autodiff) would be way off. If we must hardcode an integration scheme here, I would prefer at least a third-order method.
Alternatively, we can modify fromSimulator<T> simulator(*continuous_system_); ApplySimulatorConfig(integrator_config_, &simulator); auto& integrator = simulator.get_mutable_integrator();to
RungeKutta3Integrator<T> integrator(*continuous_system_);Initializing an integrator should be much cheaper than initializing a simulator?
For path 2, if implementing Clone for Simulator is too hard or unresonable, it is sufficient for our use case to implement Clone for IntegratorBase.
I would prefer to simply write
RungeKutta3Integrator<T> integrator(*continuous_system_);here and merge this PR before the next release date. And revisit this after Clone is implemented.
I found a little problem in your quick experiment. .AdvanceTo() can't be called on const Simulator<T>&.
There was a problem hiding this comment.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes
a discussion (no related file):
I don't think we can put the simulator in a std::shared_ptr and cache it, can we?
Value< T > Class Template Reference
T Must be copy-constructible or cloneable. Must not be a pointer, array, nor have const, volatile, or reference qualifiers.
There was a problem hiding this comment.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes
systems/analysis/discrete_time_approximation.cc line 112 at r6 (raw file):
Previously, wei-chen-li (Wei-Chen Li) wrote…
Assume our discrete-time plant is in a direct feedback loop, as in the last unit test case. And we propose to use only one continuous context cache entry. This is what happens on the stack trace:
1. output_port.Eval() 2. continuous_context_cache_entry_->Eval() 3. CopyAllContext() 4. this->EvalAbstractInput() 5. output_port.Eval()4 calls 5 because of the feedback. And oops, we have an infinite recursion.
If an output port is declared with all_sources_except_input_ports_ticket(), it must not request a cache entry declared with all_sources_ticket().
There was a problem hiding this comment.
Reviewed all commit messages.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @wei-chen-li)
a discussion (no related file):
Previously, wei-chen-li (Wei-Chen Li) wrote…
I don't think we can put the simulator in a std::shared_ptr and cache it, can we?
Value< T > Class Template Reference
T Must be copy-constructible or cloneable. Must not be a pointer, array, nor have const, volatile, or reference qualifiers.
Sorry if i wasn't clear: my simulator-as-cache experiment doesn't compile yet (because simulator is not clonable, etc). I was just fleshing out for myself what the implications for the code would be if it was clonable.
Making the integrator clonable is absolutely a first required step to making the simulator clonable. If that's all that's required, then great!
Let me take a little time to see how difficult it the clonable integrators would be.
There was a problem hiding this comment.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes
a discussion (no related file):
Previously, RussTedrake (Russ Tedrake) wrote…
Sorry if i wasn't clear: my simulator-as-cache experiment doesn't compile yet (because simulator is not clonable, etc). I was just fleshing out for myself what the implications for the code would be if it was clonable.
Making the integrator clonable is absolutely a first required step to making the simulator clonable. If that's all that's required, then great!
Let me take a little time to see how difficult it the clonable integrators would be.
It's me who did not make myself clear. I was trying to point out that we cannot simply do
const Simulator<T>& simulator = simulator_cache_entry_->Eval<Simulator<T>>(discrete_context);
simulator.AdvanceTo(discrete_context.get_time() + time_period_);
because advancing the simulator modifies its internal continuous context, and the next time we try to retrieve it (for example when calculating the output port value), the context will be out of date. This is why simulator_cache_entry_->Eval() returns a const reference, which safeguards us from calling AdvanceTo().
What we need to do, at least, is
simulator_cache_entry_->Eval<Simulator<T>>(discrete_context);
CacheEntryValue& cache_entry_value = simulator_cache_entry_->get_mutable_cache_entry_value(discrete_context);
cache_entry_value.mark_out_of_date();
Simulator<T>& simulator = cache_entry_value.GetMutableValueOrThrow<Simulator<T>>();
simulator.AdvanceTo(discrete_context.get_time() + time_period_);
This is why we need such complex interaction will the cache.
This is an extension of #22601.
This change is