simulator: copy configuration helpers from Anzu

 - Co-authored-by: Jeremy Nimmer <[email protected]>

systems/analysis/simulator_config.cc

@@ -0,0 +1 @@
+#include "sim/common/simulator_config.h"

systems/analysis/simulator_config.h

@@ -0,0 +1,35 @@
+#pragma once
+
+#include <string>
+
+#include "drake/common/name_value.h"
+
+// TODO(jeremy.nimmer) Move this file into Drake once we like how it works.
+// See https://github.com/RobotLocomotion/drake/issues/12903.
+
+namespace anzu {
+namespace sim {
+
+// N.B. The names and defaults here match drake/systems/analysis exactly.
+/// The set of configurable properties on a simulator.
+struct SimulatorConfig {
+  template <typename Archive>
+  void Serialize(Archive* a) {
+    a->Visit(DRAKE_NVP(integration_scheme));
+    a->Visit(DRAKE_NVP(max_step_size));
+    a->Visit(DRAKE_NVP(accuracy));
+    a->Visit(DRAKE_NVP(use_error_control));
+    a->Visit(DRAKE_NVP(target_realtime_rate));
+    a->Visit(DRAKE_NVP(publish_every_time_step));
+  }
+
+  std::string integration_scheme{"runge_kutta3"};
+  double max_step_size{1.0e-3};
+  double accuracy{1.0e-2};
+  bool use_error_control{true};
+  double target_realtime_rate{0.0};
+  double publish_every_time_step{false};
+};
+
+}  // namespace sim
+}  // namespace anzu

systems/analysis/simulator_config_functions.cc

@@ -0,0 +1,78 @@
+#include "sim/common/simulator_config_functions.h"
+
+#include <memory>
+#include <string>
+
+#include "drake/common/nice_type_name.h"
+#include "drake/systems/analysis/integrator_base.h"
+#include "drake/systems/analysis/simulator_flags.h"
+#include "drake/systems/primitives/symbolic_vector_system.h"
+
+namespace anzu {
+namespace sim {
+
+using drake::NiceTypeName;
+using drake::systems::IntegratorBase;
+using drake::systems::Simulator;
+using drake::systems::SymbolicVectorSystemBuilder;
+
+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);
+  }
+  if (!integrator.get_fixed_step_mode()) {
+    integrator.set_target_accuracy(config.accuracy);
+  }
+  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);
+}
+
+namespace {
+// A hollow shell of a System.  TODO(jeremy.nimmer) Move into drake primitives.
+class DummySystem final : public drake::systems::LeafSystem<double> {
+ public:
+  DummySystem() {}
+};
+std::string GetIntegrationSchemeName(const IntegratorBase<double>& integrator) {
+  const std::string current_type = NiceTypeName::Get(integrator);
+  Simulator<double> dummy_simulator(std::make_unique<DummySystem>());
+  for (const auto& scheme : drake::systems::GetIntegrationSchemes()) {
+    ResetIntegratorFromFlags(&dummy_simulator, scheme, 0.001);
+    if (NiceTypeName::Get(dummy_simulator.get_integrator()) == current_type) {
+      return scheme;
+    }
+  }
+  throw std::runtime_error(
+      "Unrecognized integration scheme " + current_type);
+}
+}  // namespace
+
+SimulatorConfig ExtractSimulatorConfig(
+    const Simulator<double>& simulator) {
+  SimulatorConfig result;
+  const IntegratorBase<double>& integrator = simulator.get_integrator();
+  result.integration_scheme = GetIntegrationSchemeName(integrator);
+  result.max_step_size = integrator.get_maximum_step_size();
+  if (integrator.supports_error_estimation()) {
+    result.use_error_control = !integrator.get_fixed_step_mode();
+    const double accuracy_in_use = integrator.get_accuracy_in_use();
+    DRAKE_DEMAND(!std::isnan(accuracy_in_use));
+    result.accuracy = accuracy_in_use;
+  } else {
+    result.use_error_control = false;
+    result.accuracy = 0.0;
+  }
+  result.target_realtime_rate = simulator.get_target_realtime_rate();
+  result.publish_every_time_step = simulator.get_publish_every_time_step();
+  return result;
+}
+
+}  // namespace sim
+}  // namespace anzu

systems/analysis/simulator_config_functions.h

@@ -0,0 +1,23 @@
+#pragma once
+
+#include "drake/systems/analysis/simulator.h"
+#include "sim/common/simulator_config.h"
+
+// TODO(jeremy.nimmer) Move this file into Drake once we like how it works.
+// See https://github.com/RobotLocomotion/drake/issues/12903.
+
+namespace anzu {
+namespace sim {
+
+/// Modify the simulator to use the given config.  (Always replaces the
+/// Integrator with a new one; be careful not to keep old references around.)
+void ApplySimulatorConfig(
+    drake::systems::Simulator<double>* simulator,
+    const SimulatorConfig& config);
+
+/// Report the simulator's current config.
+SimulatorConfig ExtractSimulatorConfig(
+    const drake::systems::Simulator<double>& simulator);
+
+}  // namespace sim
+}  // namespace anzu

systems/analysis/test/simulator_config_functions_test.cc

@@ -0,0 +1,57 @@
+#include "sim/common/simulator_config_functions.h"
+
+#include <gtest/gtest.h>
+
+#include "drake/systems/framework/leaf_system.h"
+
+namespace anzu {
+namespace sim {
+namespace {
+
+// A hollow shell of a System.
+class DummySystem final : public drake::systems::LeafSystem<double> {
+ public:
+  DummySystem() {}
+};
+
+GTEST_TEST(SimulatorConfigFunctionsTest, ExtractDefaultsTest) {
+  const DummySystem dummy;
+  drake::systems::Simulator<double> simulator(dummy);
+
+  // We can always extract the defaults.
+  const SimulatorConfig defaults = ExtractSimulatorConfig(simulator);
+  EXPECT_EQ(defaults.integration_scheme, "runge_kutta3");
+  // TODO(jeremy.nimmer) Drake returns incorrect defaults for max_step_size and
+  // accuracy.  Once Drake is fixed, we should EXPECT_EQ instead here.
+  EXPECT_GT(defaults.max_step_size, 0);
+  EXPECT_GT(defaults.accuracy, 0);
+  EXPECT_EQ(defaults.use_error_control, true);
+  EXPECT_EQ(defaults.target_realtime_rate, 0.0);
+  EXPECT_EQ(defaults.publish_every_time_step, false);
+}
+
+GTEST_TEST(SimulatorConfigFunctionsTest, RoundTripTest) {
+  SimulatorConfig bespoke;
+  bespoke.integration_scheme = "runge_kutta5";
+  bespoke.max_step_size = 3.0e-3;
+  bespoke.accuracy = 3.0e-2;
+  bespoke.use_error_control = true;
+  bespoke.target_realtime_rate = 3.0;
+  bespoke.publish_every_time_step = true;
+
+  const DummySystem dummy;
+  drake::systems::Simulator<double> simulator(dummy);
+  ApplySimulatorConfig(&simulator, bespoke);
+
+  const SimulatorConfig readback = ExtractSimulatorConfig(simulator);
+  EXPECT_EQ(readback.integration_scheme, bespoke.integration_scheme);
+  EXPECT_EQ(readback.max_step_size, bespoke.max_step_size);
+  EXPECT_EQ(readback.accuracy, bespoke.accuracy);
+  EXPECT_EQ(readback.use_error_control, bespoke.use_error_control);
+  EXPECT_EQ(readback.target_realtime_rate, bespoke.target_realtime_rate);
+  EXPECT_EQ(readback.publish_every_time_step, bespoke.publish_every_time_step);
+}
+
+}  // namespace
+}  // namespace sim
+}  // namespace anzu