Integrate SimulatorConfig into existing configuration code (RobotLoco…

…motion#14135)

* Moved `simulator_flags` code to `simulator_config_functions`.

* Deprecated `simulator_flags`.

* Added additional documentation.

bindings/pydrake/systems/analysis_py.cc

@@ -11,7 +11,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"
+#include "drake/systems/analysis/simulator_config_functions.h"
 #include "drake/systems/analysis/simulator_print_stats.h"
 
 using std::unique_ptr;

systems/analysis/BUILD.bazel

@@ -50,18 +50,7 @@ drake_cc_library(
     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",
+        ":simulator_config_functions",
     ],
 )
 
@@ -79,7 +68,8 @@ drake_cc_library(
     ],
     visibility = ["//:__subpackages__"],
     deps = [
-        ":simulator_flags",
+        ":simulator_config",
+        ":simulator_config_functions",
         "@gflags",
     ],
 )
@@ -98,9 +88,19 @@ drake_cc_library(
     srcs = ["simulator_config_functions.cc"],
     hdrs = ["simulator_config_functions.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",
         ":simulator_config",
-        ":simulator_flags",
+        ":velocity_implicit_euler_integrator",
+        "//common:essential",
+        "//common:nice_type_name",
         "//systems/framework:leaf_system",
     ],
 )
@@ -417,23 +417,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_config_functions_test",
     deps = [
         ":simulator",
         ":simulator_config_functions",
+        "//common:nice_type_name",
+        "//common/test_utilities:expect_no_throw",
         "//common/yaml",
         "//systems/framework:leaf_system",
+        "//systems/primitives:constant_vector_source",
     ],
 )
 

systems/analysis/simulator_config_functions.cc

@@ -1,48 +1,89 @@
 #include "drake/systems/analysis/simulator_config_functions.h"
 
+#include <cctype>
+#include <initializer_list>
 #include <memory>
-#include <string>
+#include <stdexcept>
+#include <utility>
 
+#include "drake/common/drake_throw.h"
+#include "drake/common/never_destroyed.h"
 #include "drake/common/nice_type_name.h"
-#include "drake/systems/analysis/integrator_base.h"
-#include "drake/systems/analysis/simulator_flags.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"
 #include "drake/systems/framework/leaf_system.h"
 
 namespace drake {
 namespace systems {
+namespace {
 
-using drake::NiceTypeName;
-using drake::systems::IntegratorBase;
-using drake::systems::Simulator;
+using std::function;
+using std::pair;
+using std::string;
+using std::vector;
 
-void ApplySimulatorConfig(
-    Simulator<double>* simulator,
-    const SimulatorConfig& config) {
-  DRAKE_THROW_UNLESS(simulator != nullptr);
-  IntegratorBase<double>& integrator =
-      drake::systems::ResetIntegratorFromFlags(
-          simulator, config.integration_scheme, config.max_step_size);
-  if (integrator.supports_error_estimation()) {
-    integrator.set_fixed_step_mode(!config.use_error_control);
+// A functor that implements ResetIntegrator.
+template <typename T>
+using ResetIntegratorFunc =
+    function<IntegratorBase<T>*(Simulator<T>*, const T& /* max_step_size */)>;
+
+// Returns (scheme, functor) pair that implements ResetIntegrator.
+template <typename T>
+using NamedResetIntegratorFunc =
+    pair<string, ResetIntegratorFunc<T>>;
+
+// Converts the class name of the `Integrator` template argument into a string
+// name for the scheme, e.g., FooBarIntegrator<double> becomes "foo_bar".
+template <template <typename> class Integrator>
+string GetIntegratorName() {
+  // Get the class name, e.g., FooBarIntegrator<double>.
+  string full_name = NiceTypeName::Get<Integrator<double>>();
+  string class_name = NiceTypeName::RemoveNamespaces(full_name);
+  if (class_name == "RadauIntegrator<double,1>") {
+    class_name = "Radau1Integrator<double>";
+  } else if (class_name == "RadauIntegrator<double,2>") {
+    class_name = "Radau3Integrator<double>";
   }
-  if (!integrator.get_fixed_step_mode()) {
-    integrator.set_target_accuracy(config.accuracy);
+
+  // Strip off "Integrator<double>" suffix to leave just "FooBar".
+  const string suffix = "Integrator<double>";
+  DRAKE_DEMAND(class_name.size() > suffix.size());
+  const size_t suffix_begin = class_name.size() - suffix.size();
+  DRAKE_DEMAND(class_name.substr(suffix_begin) == suffix);
+  const string camel_name = class_name.substr(0, suffix_begin);
+
+  // Convert "FooBar to "foo_bar".
+  string result;
+  for (char ch : camel_name) {
+    if (std::isupper(ch)) {
+      if (!result.empty()) { result.push_back('_'); }
+      result.push_back(std::tolower(ch));
+    } else {
+      result.push_back(ch);
+    }
   }
-  simulator->set_target_realtime_rate(config.target_realtime_rate);
-  simulator->set_publish_at_initialization(config.publish_every_time_step);
-  simulator->set_publish_every_time_step(config.publish_every_time_step);
+  return result;
 }
 
-namespace {
 // A hollow shell of a System.  TODO(jeremy.nimmer) Move into drake primitives.
-class DummySystem final : public drake::systems::LeafSystem<double> {
+class DummySystem final : public LeafSystem<double> {
  public:
   DummySystem() {}
 };
-std::string GetIntegrationSchemeName(const IntegratorBase<double>& integrator) {
-  const std::string current_type = NiceTypeName::Get(integrator);
+// N.B In a roundabout way the string returned here is generated by
+// GetIntegratorName().
+string GetIntegrationSchemeName(const IntegratorBase<double>& integrator) {
+  const string current_type = NiceTypeName::Get(integrator);
   Simulator<double> dummy_simulator(std::make_unique<DummySystem>());
-  for (const auto& scheme : drake::systems::GetIntegrationSchemes()) {
+  for (const auto& scheme : GetIntegrationSchemes()) {
     ResetIntegratorFromFlags(&dummy_simulator, scheme, 0.001);
     if (NiceTypeName::Get(dummy_simulator.get_integrator()) == current_type) {
       return scheme;
@@ -51,8 +92,114 @@ std::string GetIntegrationSchemeName(const IntegratorBase<double>& integrator) {
   throw std::runtime_error(
       "Unrecognized integration scheme " + current_type);
 }
+
+// Returns (scheme, functor) pair to implement reset for this `Integrator`.
+// This would be much simpler if all integrators accepted a max_step_size.
+template <typename T, template <typename> class Integrator>
+NamedResetIntegratorFunc<T> MakeResetter() {
+  constexpr bool is_fixed_step = std::is_constructible_v<
+      Integrator<T>,
+      const System<T>&, T, Context<T>*>;
+  constexpr bool is_error_controlled = std::is_constructible_v<
+      Integrator<T>,
+      const System<T>&, Context<T>*>;
+  static_assert(is_fixed_step ^ is_error_controlled);
+  return NamedResetIntegratorFunc<T>(
+      GetIntegratorName<Integrator>(),
+      [](Simulator<T>* simulator, const T& max_step_size) {
+        if constexpr (is_fixed_step) {
+          IntegratorBase<T>& result =
+              simulator->template reset_integrator<Integrator<T>>(
+                  max_step_size);
+          return &result;
+        } else {
+          IntegratorBase<T>& result =
+              simulator->template reset_integrator<Integrator<T>>();
+          result.set_maximum_step_size(max_step_size);
+          return &result;
+        }
+      });
+}
+
+// Returns the full list of supported (scheme, functor) pairs.  N.B. The list
+// here must be kept in sync with the help string in simulator_gflags.cc.
+template <typename T>
+const vector<NamedResetIntegratorFunc<T>>& GetAllNamedResetIntegratorFuncs() {
+  static const never_destroyed<vector<NamedResetIntegratorFunc<T>>> result{
+    std::initializer_list<NamedResetIntegratorFunc<T>>{
+      // Keep this list sorted alphabetically.
+      MakeResetter<T, BogackiShampine3Integrator>(),
+      MakeResetter<T, ExplicitEulerIntegrator>(),
+      MakeResetter<T, ImplicitEulerIntegrator>(),
+      MakeResetter<T, Radau1Integrator>(),
+      MakeResetter<T, Radau3Integrator>(),
+      MakeResetter<T, RungeKutta2Integrator>(),
+      MakeResetter<T, RungeKutta3Integrator>(),
+      MakeResetter<T, RungeKutta5Integrator>(),
+      MakeResetter<T, SemiExplicitEulerIntegrator>(),
+      MakeResetter<T, VelocityImplicitEulerIntegrator>(),
+  }};
+  return result.access();
+}
+
 }  // namespace
 
+template <typename T>
+IntegratorBase<T>& ResetIntegratorFromFlags(
+    Simulator<T>* simulator,
+    const string& scheme,
+    const T& max_step_size) {
+  DRAKE_THROW_UNLESS(simulator != nullptr);
+
+  const auto& name_func_pairs = GetAllNamedResetIntegratorFuncs<T>();
+  for (const auto& [one_name, one_func] : name_func_pairs) {
+    if (scheme == one_name) {
+      return *one_func(simulator, max_step_size);
+    }
+  }
+  throw std::runtime_error(fmt::format(
+      "Unknown integration scheme: {}", scheme));
+}
+
+const vector<string>& GetIntegrationSchemes() {
+  static const never_destroyed<vector<string>> result{[]() {
+    vector<string> names;
+    const auto& name_func_pairs = GetAllNamedResetIntegratorFuncs<double>();
+    for (const auto& [one_name, one_func] : name_func_pairs) {
+      names.push_back(one_name);
+      unused(one_func);
+    }
+    return names;
+  }()};
+  return result.access();
+}
+
+// Explicit instantiations.
+// We can't support T=symbolic::Expression because Simulator doesn't support it.
+template IntegratorBase<double>& ResetIntegratorFromFlags(
+    Simulator<double>*, const string&, const double&);
+template IntegratorBase<AutoDiffXd>& ResetIntegratorFromFlags(
+    Simulator<AutoDiffXd>*, const string&, const AutoDiffXd&);
+
+void ApplySimulatorConfig(
+    Simulator<double>* simulator,
+    const SimulatorConfig& config) {
+  DRAKE_THROW_UNLESS(simulator != nullptr);
+  IntegratorBase<double>& integrator = ResetIntegratorFromFlags(
+          simulator, config.integration_scheme, config.max_step_size);
+  if (integrator.supports_error_estimation()) {
+    integrator.set_fixed_step_mode(!config.use_error_control);
+  }
+  if (!integrator.get_fixed_step_mode()) {
+    integrator.set_target_accuracy(config.accuracy);
+  }
+  simulator->set_target_realtime_rate(config.target_realtime_rate);
+  // It is almost always the case we want these two next flags to be either both
+  // true or both false. Otherwise we could miss the first publish at t = 0.
+  simulator->set_publish_at_initialization(config.publish_every_time_step);
+  simulator->set_publish_every_time_step(config.publish_every_time_step);
+}
+
 SimulatorConfig ExtractSimulatorConfig(
     const Simulator<double>& simulator) {
   SimulatorConfig result;

systems/analysis/simulator_config_functions.h

@@ -1,18 +1,67 @@
 #pragma once
 
+#include <string>
+#include <vector>
+
+#include "drake/common/default_scalars.h"
+#include "drake/systems/analysis/integrator_base.h"
 #include "drake/systems/analysis/simulator.h"
 #include "drake/systems/analysis/simulator_config.h"
 
 namespace drake {
 namespace systems {
 
-/// Modifies the `simulator` to use the given config.  (Always replaces the
-/// Integrator with a new one; be careful not to keep old references around.)
+/** @addtogroup simulation
+ @{
+ @defgroup simulator_configuration Simulator configuration
+
+ Configuration helpers to control Simulator and IntegratorBase settings.
+ @}
+ */
+
+/** Resets the integrator used to advanced the continuous time dynamics of the
+system associated with `simulator` according to the given arguments.
+
+@param[in,out] simulator On input, a valid pointer to a Simulator. On output
+  the integrator for `simulator` is reset according to the given arguments.
+@param[in] scheme Integration scheme to be used, e.g., "runge_kutta2".  See
+  GetIntegrationSchemes() for a the list of valid options.
+@param[in] max_step_size The IntegratorBase::set_maximum_step_size() value.
+@returns A reference to the newly created integrator owned by `simulator`.
+
+@tparam_default_nonsymbolic_scalar
+@ingroup simulator_configuration */
+template <typename T>
+IntegratorBase<T>& ResetIntegratorFromFlags(
+    Simulator<T>* simulator,
+    const std::string& scheme,
+    const T& max_step_size);
+
+/** Returns the allowed string values for the `scheme` parameter in
+ResetIntegratorFromFlags() and SimulatorConfig::integration_scheme.
+
+@ingroup simulator_configuration */
+const std::vector<std::string>& GetIntegrationSchemes();
+
+/** Modifies the `simulator` to use the given config.  (Always replaces the
+Integrator with a new one; be careful not to keep old references around.)
+
+@param[in,out] simulator On input, a valid pointer to a Simulator. On output
+  the integretor for `simulator` is reset according to the given `config`.
+@param[in] config Configuration to be used. Contains values for both the
+  integrator and the simulator.
+
+@ingroup simulator_configuration */
 void ApplySimulatorConfig(
     drake::systems::Simulator<double>* simulator,
     const SimulatorConfig& config);
 
-/// Reports the simulator's current configuration.
+/** Reports the simulator's current configuration, including the configuration
+of the integrator.
+
+@param[in] simulator The Simulator to extract the configuration from.
+
+@ingroup simulator_configuration */
 SimulatorConfig ExtractSimulatorConfig(
     const drake::systems::Simulator<double>& simulator);