diff --git a/systems/analysis/simulator_config.cc b/systems/analysis/simulator_config.cc new file mode 100644 index 000000000000..6e62f279c799 --- /dev/null +++ b/systems/analysis/simulator_config.cc @@ -0,0 +1 @@ +#include "sim/common/simulator_config.h" diff --git a/systems/analysis/simulator_config.h b/systems/analysis/simulator_config.h new file mode 100644 index 000000000000..83793cff3282 --- /dev/null +++ b/systems/analysis/simulator_config.h @@ -0,0 +1,35 @@ +#pragma once + +#include + +#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 + 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 diff --git a/systems/analysis/simulator_config_functions.cc b/systems/analysis/simulator_config_functions.cc new file mode 100644 index 000000000000..02bcde243e3f --- /dev/null +++ b/systems/analysis/simulator_config_functions.cc @@ -0,0 +1,78 @@ +#include "sim/common/simulator_config_functions.h" + +#include +#include + +#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* simulator, + const SimulatorConfig& config) { + DRAKE_THROW_UNLESS(simulator != nullptr); + IntegratorBase& 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 { + public: + DummySystem() {} +}; +std::string GetIntegrationSchemeName(const IntegratorBase& integrator) { + const std::string current_type = NiceTypeName::Get(integrator); + Simulator dummy_simulator(std::make_unique()); + 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& simulator) { + SimulatorConfig result; + const IntegratorBase& 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 diff --git a/systems/analysis/simulator_config_functions.h b/systems/analysis/simulator_config_functions.h new file mode 100644 index 000000000000..608cfd46c807 --- /dev/null +++ b/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* simulator, + const SimulatorConfig& config); + +/// Report the simulator's current config. +SimulatorConfig ExtractSimulatorConfig( + const drake::systems::Simulator& simulator); + +} // namespace sim +} // namespace anzu diff --git a/systems/analysis/test/simulator_config_functions_test.cc b/systems/analysis/test/simulator_config_functions_test.cc new file mode 100644 index 000000000000..296493f9cc2a --- /dev/null +++ b/systems/analysis/test/simulator_config_functions_test.cc @@ -0,0 +1,57 @@ +#include "sim/common/simulator_config_functions.h" + +#include + +#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 { + public: + DummySystem() {} +}; + +GTEST_TEST(SimulatorConfigFunctionsTest, ExtractDefaultsTest) { + const DummySystem dummy; + drake::systems::Simulator 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 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