From 326b1696ba6550927de87457d140dbbf90fdd631 Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Thu, 4 Apr 2024 11:27:47 -0700 Subject: [PATCH] [planning] Match RobotDiagram default time step to MbP's default This fixes some crashes when using models that need discrete-time-only modeling features, such as implicit PD control. --- bindings/pydrake/planning/planning_py_robot_diagram.cc | 3 ++- bindings/pydrake/planning/test/robot_diagram_test.py | 9 ++++++++- planning/robot_diagram_builder.h | 9 +++++++-- planning/test/robot_diagram_test.cc | 9 ++++++++- 4 files changed, 25 insertions(+), 5 deletions(-) diff --git a/bindings/pydrake/planning/planning_py_robot_diagram.cc b/bindings/pydrake/planning/planning_py_robot_diagram.cc index dcd9dd58e158..97fe087005af 100644 --- a/bindings/pydrake/planning/planning_py_robot_diagram.cc +++ b/bindings/pydrake/planning/planning_py_robot_diagram.cc @@ -55,7 +55,8 @@ void DefinePlanningRobotDiagram(py::module m) { auto cls = DefineTemplateClassWithDefault( m, "RobotDiagramBuilder", GetPyParam(), cls_doc.doc); cls // BR - .def(py::init(), py::arg("time_step") = 0.0, cls_doc.ctor.doc) + .def(py::init(), py::arg("time_step") = 0.001, + cls_doc.ctor.doc) .def("builder", overload_cast_explicit&>( &Class::builder), diff --git a/bindings/pydrake/planning/test/robot_diagram_test.py b/bindings/pydrake/planning/test/robot_diagram_test.py index 1016a607e1b3..8249a7d6a306 100644 --- a/bindings/pydrake/planning/test/robot_diagram_test.py +++ b/bindings/pydrake/planning/test/robot_diagram_test.py @@ -6,11 +6,18 @@ from pydrake.common.test_utilities import numpy_compare from pydrake.geometry import SceneGraph_ from pydrake.multibody.parsing import Parser -from pydrake.multibody.plant import MultibodyPlant_ +from pydrake.multibody.plant import MultibodyPlant_, MultibodyPlantConfig from pydrake.systems.framework import Context_, DiagramBuilder_ class TestRobotDiagram(unittest.TestCase): + @numpy_compare.check_all_types + def test_robot_diagram_builder_default_time_step(self, T): + Class = mut.RobotDiagramBuilder_[T] + dut = Class() + self.assertEqual(dut.plant().time_step(), + MultibodyPlantConfig().time_step) + @numpy_compare.check_all_types def test_robot_diagram_builder(self, T): """Tests the full RobotDiagramBuilder API. diff --git a/planning/robot_diagram_builder.h b/planning/robot_diagram_builder.h index 18bcd6f0268c..5c042292fb7d 100644 --- a/planning/robot_diagram_builder.h +++ b/planning/robot_diagram_builder.h @@ -29,8 +29,13 @@ class RobotDiagramBuilder { public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(RobotDiagramBuilder) - /** Constructs with the specified time step for the contained plant. */ - explicit RobotDiagramBuilder(double time_step = 0.0); + /** Constructs with the specified time step for the contained plant. + @param time_step Governs whether the MultibodyPlant is modeled as a discrete + system (`time_step > 0`) or as a continuous system (`time_step = 0`). See + @ref time_advancement_strategy "Choice of Time Advancement Strategy" for + further details. The default here matches the default value from + multibody::MultibodyPlantConfig. */ + explicit RobotDiagramBuilder(double time_step = 0.001); ~RobotDiagramBuilder(); diff --git a/planning/test/robot_diagram_test.cc b/planning/test/robot_diagram_test.cc index 0940af072f70..22fdd1288578 100644 --- a/planning/test/robot_diagram_test.cc +++ b/planning/test/robot_diagram_test.cc @@ -17,6 +17,7 @@ namespace { using geometry::SceneGraph; using multibody::MultibodyPlant; +using multibody::MultibodyPlantConfig; using multibody::Parser; using symbolic::Expression; using systems::Context; @@ -32,7 +33,13 @@ std::unique_ptr> MakeSampleDut() { return builder; } -GTEST_TEST(RobotDiagramBuilderTest, TimeStep) { +GTEST_TEST(RobotDiagramBuilderTest, TimeStepDefault) { + const MultibodyPlantConfig default_plant_config; + auto builder = std::make_unique>(); + EXPECT_EQ(builder->plant().time_step(), default_plant_config.time_step); +} + +GTEST_TEST(RobotDiagramBuilderTest, TimeStepExplicit) { const double time_step = 0.01; auto builder = std::make_unique>(time_step); EXPECT_EQ(builder->plant().time_step(), time_step);