From e99ea341fa82782d10f9d0337854284efa255fc5 Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Sat, 11 Apr 2020 15:08:15 -0400 Subject: [PATCH] analysis: Add simulator_flags helper (#13013) This extends pydrake.systems.Simulator.reset_integrator's deprecation window because up to this point there was no viable replacement. --- bindings/pydrake/systems/analysis_py.cc | 56 ++++-- bindings/pydrake/systems/test/general_test.py | 13 ++ systems/analysis/BUILD.bazel | 43 +++-- systems/analysis/integrator_base.h | 3 + systems/analysis/radau_integrator.h | 14 ++ systems/analysis/simulator.h | 2 +- systems/analysis/simulator_flags.cc | 164 ++++++++++++++++++ systems/analysis/simulator_flags.h | 46 +++++ systems/analysis/simulator_gflags.cc | 78 +++------ systems/analysis/test/simulator_flags_test.cc | 39 +++++ 10 files changed, 374 insertions(+), 84 deletions(-) create mode 100644 systems/analysis/simulator_flags.cc create mode 100644 systems/analysis/simulator_flags.h create mode 100644 systems/analysis/test/simulator_flags_test.cc diff --git a/bindings/pydrake/systems/analysis_py.cc b/bindings/pydrake/systems/analysis_py.cc index 161ed4935f7b..3bb180c37126 100644 --- a/bindings/pydrake/systems/analysis_py.cc +++ b/bindings/pydrake/systems/analysis_py.cc @@ -12,6 +12,7 @@ #include "drake/systems/analysis/runge_kutta2_integrator.h" #include "drake/systems/analysis/runge_kutta3_integrator.h" #include "drake/systems/analysis/simulator.h" +#include "drake/systems/analysis/simulator_flags.h" using std::unique_ptr; @@ -113,24 +114,9 @@ PYBIND11_MODULE(analysis, m) { doc.Simulator.has_context.doc) .def("reset_context", &Simulator::reset_context, py::arg("context"), // Keep alive, ownership: `context` keeps `self` alive. - py::keep_alive<2, 1>(), doc.Simulator.reset_context.doc); - // TODO(eric.cousineau): Bind `release_context` once some form of the - // PR RobotLocomotion/pybind11#33 lands. Presently, it fails. -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - cls // BR - .def("reset_integrator", - WrapDeprecated(doc.Simulator.reset_integrator.doc_deprecated_1args, - [](Simulator* self, - std::unique_ptr> integrator) { - return self->reset_integrator(std::move(integrator)); - }), - py::arg("integrator"), - // Keep alive, ownership: `integrator` keeps `self` alive. - py::keep_alive<2, 1>(), - doc.Simulator.reset_integrator.doc_deprecated_1args); -#pragma GCC diagnostic pop - cls // BR + py::keep_alive<2, 1>(), doc.Simulator.reset_context.doc) + // TODO(eric.cousineau): Bind `release_context` once some form of the + // PR RobotLocomotion/pybind11#33 lands. Presently, it fails. .def("set_publish_every_time_step", &Simulator::set_publish_every_time_step, doc.Simulator.set_publish_every_time_step.doc) @@ -143,9 +129,43 @@ PYBIND11_MODULE(analysis, m) { .def("get_actual_realtime_rate", &Simulator::get_actual_realtime_rate, doc.Simulator.get_actual_realtime_rate.doc); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + const char* const reset_integrator_doc = + "(Deprecated.) pydrake.systems.Simulator.reset_integrator is " + "deprecated and will be removed from Drake on or after 2020-08-01. " + "Use pydrake.systems.ResetIntegratorFromFlags instead."; + cls // BR + .def("reset_integrator", + WrapDeprecated(reset_integrator_doc, + [](Simulator* self, + std::unique_ptr> integrator) { + return self->reset_integrator(std::move(integrator)); + }), + py::arg("integrator"), + // Keep alive, ownership: `integrator` keeps `self` alive. + py::keep_alive<2, 1>(), reset_integrator_doc); +#pragma GCC diagnostic pop }; type_visit(bind_nonsymbolic_scalar_types, NonSymbolicScalarPack{}); + // Simulator Flags + m // BR + .def("ResetIntegratorFromFlags", + [](Simulator* simulator, const std::string& scheme, + const double& max_step_size) { + IntegratorBase& result = + ResetIntegratorFromFlags(simulator, scheme, max_step_size); + return &result; + }, + py::arg("simulator"), py::arg("scheme"), py::arg("max_step_size"), + py_reference, + // Keep alive, reference: `return` keeps `simulator` alive. + py::keep_alive<0, 1>(), + pydrake_doc.drake.systems.ResetIntegratorFromFlags.doc) + .def("GetIntegrationSchemes", &GetIntegrationSchemes, + pydrake_doc.drake.systems.GetIntegrationSchemes.doc); + // Monte Carlo Testing { // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. diff --git a/bindings/pydrake/systems/test/general_test.py b/bindings/pydrake/systems/test/general_test.py index 6c1b638b03a5..67b4a7b717f0 100644 --- a/bindings/pydrake/systems/test/general_test.py +++ b/bindings/pydrake/systems/test/general_test.py @@ -15,7 +15,9 @@ from pydrake.examples.rimless_wheel import RimlessWheel from pydrake.symbolic import Expression from pydrake.systems.analysis import ( + GetIntegrationSchemes, IntegratorBase, IntegratorBase_, + ResetIntegratorFromFlags, RungeKutta2Integrator, RungeKutta3Integrator, SimulatorStatus, Simulator, Simulator_, ) @@ -526,6 +528,17 @@ def test_simulator_integrator_manipulation(self): # TODO(12873) We need an API for this that isn't deprecated. simulator.reset_integrator(rk3) + def test_simulator_flags(self): + system = ConstantVectorSource([1]) + simulator = Simulator(system) + + ResetIntegratorFromFlags(simulator, "runge_kutta2", 0.00123) + integrator = simulator.get_integrator() + self.assertEqual(type(integrator), RungeKutta2Integrator) + self.assertEqual(integrator.get_maximum_step_size(), 0.00123) + + self.assertGreater(len(GetIntegrationSchemes()), 5) + def test_abstract_output_port_eval(self): model_value = AbstractValue.Make("Hello World") source = ConstantValueSource(copy.copy(model_value)) diff --git a/systems/analysis/BUILD.bazel b/systems/analysis/BUILD.bazel index 16cef98be3ca..e66587e40034 100644 --- a/systems/analysis/BUILD.bazel +++ b/systems/analysis/BUILD.bazel @@ -34,6 +34,7 @@ drake_cc_package_library( ":scalar_view_dense_output", ":semi_explicit_euler_integrator", ":simulator", + ":simulator_flags", ":simulator_print_stats", ":simulator_status", ":stepwise_dense_output", @@ -41,6 +42,26 @@ drake_cc_package_library( ], ) +drake_cc_library( + name = "simulator_flags", + srcs = ["simulator_flags.cc"], + hdrs = ["simulator_flags.h"], + deps = [ + ":bogacki_shampine3_integrator", + ":explicit_euler_integrator", + ":implicit_euler_integrator", + ":radau_integrator", + ":runge_kutta2_integrator", + ":runge_kutta3_integrator", + ":runge_kutta5_integrator", + ":semi_explicit_euler_integrator", + ":simulator", + ":velocity_implicit_euler_integrator", + "//common:essential", + "//common:nice_type_name", + ], +) + drake_cc_library( name = "simulator_gflags", srcs = ["simulator_gflags.cc"], @@ -55,17 +76,7 @@ drake_cc_library( ], visibility = ["//:__subpackages__"], deps = [ - ":bogacki_shampine3_integrator", - ":explicit_euler_integrator", - ":implicit_euler_integrator", - ":radau_integrator", - ":runge_kutta2_integrator", - ":runge_kutta3_integrator", - ":runge_kutta5_integrator", - ":semi_explicit_euler_integrator", - ":simulator", - ":velocity_implicit_euler_integrator", - "//common:essential", + ":simulator_flags", "@gflags", ], ) @@ -382,6 +393,16 @@ drake_cc_library( # === test/ === +drake_cc_googletest( + name = "simulator_flags_test", + deps = [ + ":simulator", + ":simulator_flags", + "//common:nice_type_name", + "//systems/primitives:constant_vector_source", + ], +) + drake_cc_googletest( name = "simulator_status_test", deps = [ diff --git a/systems/analysis/integrator_base.h b/systems/analysis/integrator_base.h index 63d2f37b84fe..a7420ddf5780 100644 --- a/systems/analysis/integrator_base.h +++ b/systems/analysis/integrator_base.h @@ -114,6 +114,9 @@ namespace systems { (t₀, x₀). Thus, integrators advance the continuous state of a dynamical system forward in time. + Drake's subclasses of IntegratorBase should follow the naming pattern + `FooIntegrator` by convention. + @tparam_default_scalar @ingroup integrators */ diff --git a/systems/analysis/radau_integrator.h b/systems/analysis/radau_integrator.h index 0a6c0c0efa2b..e6c8243f1cf7 100644 --- a/systems/analysis/radau_integrator.h +++ b/systems/analysis/radau_integrator.h @@ -53,6 +53,8 @@ namespace systems { * * @see ImplicitIntegrator class documentation for information about implicit * integration methods in general. + * @see Radau3Integrator and Radau1Integrator alises for third- and first-order + * templates with num_stages already specified. * @note This integrator uses the integrator accuracy setting, even when run * in fixed-step mode, to limit the error in the underlying Newton-Raphson * process. See IntegratorBase::set_target_accuracy() for more info. @@ -216,6 +218,18 @@ class RadauIntegrator final : public ImplicitIntegrator { int64_t num_err_est_nr_iterations_{0}; }; +/** A third-order fully implicit integrator with error estimation. +See RadauIntegrator with `num_stages == 2` for details. +@tparam_nonsymbolic_scalar */ +template +using Radau3Integrator = RadauIntegrator; + +/** A first-order fully implicit integrator with error estimation. +See RadauIntegrator with `num_stages == 1` for details. +@tparam_nonsymbolic_scalar */ +template +using Radau1Integrator = RadauIntegrator; + } // namespace systems } // namespace drake diff --git a/systems/analysis/simulator.h b/systems/analysis/simulator.h index a0696cadc83e..d6c630051bf3 100644 --- a/systems/analysis/simulator.h +++ b/systems/analysis/simulator.h @@ -586,7 +586,7 @@ class Simulator { template DRAKE_DEPRECATED( - "2020-05-01", + "2020-08-01", "Use void or max-step-size version of reset_integrator() instead.") U* reset_integrator(std::unique_ptr integrator) { if (!integrator) diff --git a/systems/analysis/simulator_flags.cc b/systems/analysis/simulator_flags.cc new file mode 100644 index 000000000000..b09627e76999 --- /dev/null +++ b/systems/analysis/simulator_flags.cc @@ -0,0 +1,164 @@ +#include "drake/systems/analysis/simulator_flags.h" + +#include +#include +#include +#include + +#include "drake/common/drake_throw.h" +#include "drake/common/never_destroyed.h" +#include "drake/common/nice_type_name.h" +#include "drake/common/unused.h" +#include "drake/systems/analysis/bogacki_shampine3_integrator.h" +#include "drake/systems/analysis/explicit_euler_integrator.h" +#include "drake/systems/analysis/implicit_euler_integrator.h" +#include "drake/systems/analysis/radau_integrator.h" +#include "drake/systems/analysis/runge_kutta2_integrator.h" +#include "drake/systems/analysis/runge_kutta3_integrator.h" +#include "drake/systems/analysis/runge_kutta5_integrator.h" +#include "drake/systems/analysis/semi_explicit_euler_integrator.h" +#include "drake/systems/analysis/velocity_implicit_euler_integrator.h" + +namespace drake { +namespace systems { +namespace { + +using std::function; +using std::pair; +using std::string; +using std::vector; +using symbolic::Expression; + +// A functor that implements ResetIntegrator. +template +using ResetIntegratorFunc = + function*(Simulator*, const T& /* max_step_size */)>; + +// Returns (scheme, functor) pair that implements ResetIntegrator. +template +using NamedResetIntegratorFunc = + pair>; + +// Converts the class name of the `Integrator` template argument into a string +// name for the scheme, e.g., FooBarIntegrator becomes "foo_bar". +template