From 90369ef35a4dbfb5119c178c914105614c0235ad Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Tue, 14 Jan 2025 14:24:00 -0800 Subject: [PATCH] [py framework] Setting context values now clones instead of owns --- bindings/pydrake/systems/BUILD.bazel | 1 + .../pydrake/systems/framework_py_semantics.cc | 134 ++++++++++++------ bindings/pydrake/systems/test/custom_test.py | 25 ++++ bindings/pydrake/systems/test/general_test.py | 17 ++- 4 files changed, 135 insertions(+), 42 deletions(-) diff --git a/bindings/pydrake/systems/BUILD.bazel b/bindings/pydrake/systems/BUILD.bazel index 395c0a164671..6ef9d8540037 100644 --- a/bindings/pydrake/systems/BUILD.bazel +++ b/bindings/pydrake/systems/BUILD.bazel @@ -356,6 +356,7 @@ drake_py_unittest( ":analysis_py", ":framework_py", ":primitives_py", + ":test_util_py", "//bindings/pydrake/common/test_utilities", "//bindings/pydrake/examples", ], diff --git a/bindings/pydrake/systems/framework_py_semantics.cc b/bindings/pydrake/systems/framework_py_semantics.cc index f61f38d50b97..e371ac16a662 100644 --- a/bindings/pydrake/systems/framework_py_semantics.cc +++ b/bindings/pydrake/systems/framework_py_semantics.cc @@ -17,20 +17,29 @@ #include "drake/systems/framework/system_output.h" using std::string; -using std::unique_ptr; -using std::vector; namespace drake { namespace pydrake { namespace { -using AbstractValuePtrList = vector>; - // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. using namespace drake::systems; constexpr auto& doc = pydrake_doc.drake.systems; +// Given a vector of (possibly null) pointers, returns a vector formed by +// calling Clone() elementwise. +template +std::vector> CloneVectorOfPointers( + const std::vector& input) { + std::vector> result; + result.reserve(input.size()); + for (const T* item : input) { + result.push_back(item != nullptr ? item->Clone() : nullptr); + } + return result; +} + // Given an InputPort or OutputPort as self, return self.Eval(context). In // python, always returns either a numpy.ndarray (when vector-valued) or the // unwrapped T in a Value (when abstract-valued). @@ -104,7 +113,10 @@ void DoScalarIndependentDefinitions(py::module m) { DefClone(&abstract_values); abstract_values // BR .def(py::init<>(), doc.AbstractValues.ctor.doc_0args) - .def(py::init(), doc.AbstractValues.ctor.doc_1args) + .def(py::init([](const std::vector& data) { + return std::make_unique(CloneVectorOfPointers(data)); + }), + py::arg("data"), doc.AbstractValues.ctor.doc_1args) .def("size", &AbstractValues::size, doc.AbstractValues.size.doc) .def("get_value", &AbstractValues::get_value, py::arg("index"), py_rvp::reference_internal, doc.AbstractValues.get_value.doc) @@ -939,24 +951,31 @@ void DefineParameters(py::module m) { auto parameters = DefineTemplateClassWithDefault>( m, "Parameters", GetPyParam(), doc.Parameters.doc); DefClone(¶meters); - using BasicVectorPtrList = vector>>; - parameters + parameters // BR .def(py::init<>(), doc.Parameters.ctor.doc_0args) - // TODO(eric.cousineau): Ensure that we can respect keep alive behavior - // with lists of pointers. - .def(py::init(), + .def(py::init([](const std::vector*>& numeric, + const std::vector& abstract) { + return std::make_unique>( + CloneVectorOfPointers(numeric), CloneVectorOfPointers(abstract)); + }), py::arg("numeric"), py::arg("abstract"), doc.Parameters.ctor.doc_2args_numeric_abstract) - .def(py::init(), py::arg("numeric"), - doc.Parameters.ctor.doc_1args_numeric) - .def(py::init(), py::arg("abstract"), - doc.Parameters.ctor.doc_1args_abstract) - .def(py::init>>(), py::arg("vec"), - // Keep alive, ownership: `vec` keeps `self` alive. - py::keep_alive<2, 1>(), doc.Parameters.ctor.doc_1args_vec) - .def(py::init>(), py::arg("value"), - // Keep alive, ownership: `value` keeps `self` alive. - py::keep_alive<2, 1>(), doc.Parameters.ctor.doc_1args_value) + .def(py::init([](const std::vector*>& numeric) { + return std::make_unique>(CloneVectorOfPointers(numeric)); + }), + py::arg("numeric"), doc.Parameters.ctor.doc_1args_numeric) + .def(py::init([](const std::vector& abstract) { + return std::make_unique>(CloneVectorOfPointers(abstract)); + }), + py::arg("abstract"), doc.Parameters.ctor.doc_1args_abstract) + .def(py::init([](const BasicVector& vec) { + return std::make_unique>(vec.Clone()); + }), + py::arg("vec"), doc.Parameters.ctor.doc_1args_vec) + .def(py::init([](const AbstractValue& value) { + return std::make_unique>(value.Clone()); + }), + py::arg("value"), doc.Parameters.ctor.doc_1args_value) .def("num_numeric_parameter_groups", &Parameters::num_numeric_parameter_groups, doc.Parameters.num_numeric_parameter_groups.doc) @@ -971,14 +990,17 @@ void DefineParameters(py::module m) { doc.Parameters.get_mutable_numeric_parameter.doc) .def("get_numeric_parameters", &Parameters::get_numeric_parameters, py_rvp::reference_internal, doc.Parameters.get_numeric_parameters.doc) - // TODO(eric.cousineau): Should this C++ code constrain the number of - // parameters??? - .def("set_numeric_parameters", &Parameters::set_numeric_parameters, - // WARNING: This will DELETE the existing parameters. See C++ - // `AddValueInstantiation` for more information. - // Keep alive, ownership: `value` keeps `self` alive. - py::keep_alive<2, 1>(), py::arg("numeric_params"), - doc.Parameters.set_numeric_parameters.doc) + .def( + "set_numeric_parameters", + [](Parameters& self, const DiscreteValues& numeric_params) { + // TODO(eric.cousineau): Should this C++ code constrain the number + // of parameters??? + // + // WARNING: This will DELETE the existing parameters. See C++ + // `AddValueInstantiation` for more information. + self.set_numeric_parameters(numeric_params.Clone()); + }, + py::arg("numeric_params"), doc.Parameters.set_numeric_parameters.doc) .def( "get_abstract_parameter", [](const Parameters* self, int index) -> auto& { @@ -996,11 +1018,14 @@ void DefineParameters(py::module m) { .def("get_abstract_parameters", &Parameters::get_abstract_parameters, py_rvp::reference_internal, doc.Parameters.get_abstract_parameters.doc) - .def("set_abstract_parameters", &Parameters::set_abstract_parameters, - // WARNING: This will DELETE the existing parameters. See C++ - // `AddValueInstantiation` for more information. - // Keep alive, ownership: `value` keeps `self` alive. - py::keep_alive<2, 1>(), py::arg("abstract_params"), + .def( + "set_abstract_parameters", + [](Parameters& self, const AbstractValues& abstract_params) { + // WARNING: This will DELETE the existing parameters. See C++ + // `AddValueInstantiation` for more information. + self.set_abstract_parameters(abstract_params.Clone()); + }, + py::arg("abstract_params"), doc.Parameters.set_abstract_parameters.doc) .def( "SetFrom", @@ -1065,14 +1090,37 @@ void DefineContinuousState(py::module m) { auto continuous_state = DefineTemplateClassWithDefault>( m, "ContinuousState", GetPyParam(), doc.ContinuousState.doc); DefClone(&continuous_state); - continuous_state - .def(py::init>>(), py::arg("state"), - doc.ContinuousState.ctor.doc_1args_state) - .def(py::init>, int, int, int>(), + continuous_state // BR + .def(py::init<>(), doc.ContinuousState.ctor.doc_0args) + // In the next pair of overloads, we'll try matching on BasicVector in + // order to preserve its subtype across cloning. In the subsequent pair + // of overloads, we'll also allow VectorBase. + .def(py::init([](const BasicVector& state) { + return std::make_unique>(state.Clone()); + }), + py::arg("state"), doc.ContinuousState.ctor.doc_1args_state) + .def(py::init([](const BasicVector& state, int num_q, int num_v, + int num_z) { + return std::make_unique>( + state.Clone(), num_q, num_v, num_z); + }), + py::arg("state"), py::arg("num_q"), py::arg("num_v"), + py::arg("num_z"), + doc.ContinuousState.ctor.doc_4args_state_num_q_num_v_num_z) + .def(py::init([](const VectorBase& state) { + return std::make_unique>( + std::make_unique>(state.CopyToVector())); + }), + py::arg("state"), doc.ContinuousState.ctor.doc_1args_state) + .def(py::init( + [](const VectorBase& state, int num_q, int num_v, int num_z) { + return std::make_unique>( + std::make_unique>(state.CopyToVector()), + num_q, num_v, num_z); + }), py::arg("state"), py::arg("num_q"), py::arg("num_v"), py::arg("num_z"), doc.ContinuousState.ctor.doc_4args_state_num_q_num_v_num_z) - .def(py::init<>(), doc.ContinuousState.ctor.doc_0args) .def("size", &ContinuousState::size, doc.ContinuousState.size.doc) .def("num_q", &ContinuousState::num_q, doc.ContinuousState.num_q.doc) .def("num_v", &ContinuousState::num_v, doc.ContinuousState.num_v.doc) @@ -1134,9 +1182,13 @@ void DefineDiscreteValues(py::module m) { m, "DiscreteValues", GetPyParam(), doc.DiscreteValues.doc); DefClone(&discrete_values); discrete_values - .def(py::init>>(), py::arg("datum"), - doc.DiscreteValues.ctor.doc_1args_datum) - .def(py::init>>&&>(), + .def(py::init([](const BasicVector& datum) { + return std::make_unique>(datum.Clone()); + }), + py::arg("datum"), doc.DiscreteValues.ctor.doc_1args_datum) + .def(py::init([](const std::vector*>& data) { + return std::make_unique>(CloneVectorOfPointers(data)); + }), py::arg("data"), doc.DiscreteValues.ctor.doc_1args_data) .def(py::init<>(), doc.DiscreteValues.ctor.doc_0args) .def("num_groups", &DiscreteValues::num_groups, diff --git a/bindings/pydrake/systems/test/custom_test.py b/bindings/pydrake/systems/test/custom_test.py index cf86f5da4f07..51572a459466 100644 --- a/bindings/pydrake/systems/test/custom_test.py +++ b/bindings/pydrake/systems/test/custom_test.py @@ -23,10 +23,12 @@ CacheEntryValue, CacheIndex, Context, + ContinuousState_, ContinuousStateIndex, DependencyTicket, Diagram, DiagramBuilder, + DiagramBuilder_, DiscreteStateIndex, DiscreteValues, EventStatus, @@ -909,6 +911,29 @@ def DoCalcTimeDerivatives(self, context, derivatives): self.assertEqual( system.EvalTimeDerivatives(context=context).size(), 6) + # The constructors for ContinuousState(state: VectorBase, ...) + # used when diagrams are in play receives special treatment in + # the bindings for ContinuousState. We'll exercise it here. + builder = DiagramBuilder_[T]() + n = 2 + for _ in range(n): + builder.AddSystem(TrivialSystem(index)) + diagram = builder.Build() + diagram_context = diagram.CreateDefaultContext() + diagram_state_copy = ContinuousState_[T]( + state=diagram_context.get_continuous_state().get_vector()) + self.assertEqual(diagram_state_copy.size(), 6*n) + diagram_state_copy = ContinuousState_[T]( + state=diagram_context.get_continuous_state().get_vector(), + num_q=2*n, + num_v=1*n, + num_z=3*n, + ) + self.assertEqual(diagram_state_copy.num_q(), 2*n) + self.assertEqual(diagram_state_copy.num_v(), 1*n) + self.assertEqual(diagram_state_copy.num_z(), 3*n) + self.assertEqual(diagram_state_copy.size(), 6*n) + def test_discrete_state_api(self): # N.B. Since this has trivial operations, we can test all scalar types. for T in [float, AutoDiffXd, Expression]: diff --git a/bindings/pydrake/systems/test/general_test.py b/bindings/pydrake/systems/test/general_test.py index 0c2d13c89ecc..7878f64c59bc 100644 --- a/bindings/pydrake/systems/test/general_test.py +++ b/bindings/pydrake/systems/test/general_test.py @@ -65,6 +65,7 @@ PassThrough, PassThrough_, ZeroOrderHold, ) +from pydrake.systems.test.test_util import MyVector2 # TODO(eric.cousineau): The scope of this test file and `custom_test.py` # is poor. Move these tests into `framework_test` and `analysis_test`, and @@ -318,7 +319,21 @@ def system_callback(system, context, event): pass def test_continuous_state_api(self): self.assertEqual(ContinuousState().size(), 0) - self.assertEqual(ContinuousState(state=BasicVector(2)).size(), 2) + custom_vector = MyVector2(np.ones(2)) + state = ContinuousState(state=custom_vector) + self.assertIsInstance(state.get_vector(), MyVector2) + self.assertEqual(state.size(), 2) + self.assertEqual(state.num_q(), 0) + self.assertEqual(state.num_v(), 0) + self.assertEqual(state.num_z(), 2) + state = ContinuousState(state=custom_vector, num_q=1, num_v=1, num_z=0) + self.assertIsInstance(state.get_vector(), MyVector2) + self.assertEqual(state.size(), 2) + self.assertEqual(state.num_q(), 1) + self.assertEqual(state.num_v(), 1) + self.assertEqual(state.num_z(), 0) + state = ContinuousState(state=BasicVector(2)) + self.assertEqual(state.size(), 2) state = ContinuousState(state=BasicVector(np.arange(6)), num_q=3, num_v=2, num_z=1) state_clone = state.Clone()