diff --git a/bindings/pydrake/BUILD.bazel b/bindings/pydrake/BUILD.bazel index eed3acceaa60..9ab1b9aeab24 100644 --- a/bindings/pydrake/BUILD.bazel +++ b/bindings/pydrake/BUILD.bazel @@ -207,11 +207,25 @@ drake_py_library( srcs = ["test/algebra_test_util.py"], ) +# Test ODR (One Definition Rule). +drake_pybind_library( + name = "autodiffutils_test_util_py", + testonly = 1, + add_install = False, + cc_deps = [ + ":autodiff_types_pybind", + ], + cc_so_name = "test/autodiffutils_test_util", + cc_srcs = ["test/autodiffutils_test_util_py.cc"], + package_info = PACKAGE_INFO, +) + drake_py_unittest( name = "autodiffutils_test", deps = [ ":algebra_test_util_py", ":autodiffutils_py", + ":autodiffutils_test_util_py", ], ) diff --git a/bindings/pydrake/autodiff_types_pybind.h b/bindings/pydrake/autodiff_types_pybind.h index b2c1e6119326..9fc53ac6710a 100644 --- a/bindings/pydrake/autodiff_types_pybind.h +++ b/bindings/pydrake/autodiff_types_pybind.h @@ -1,6 +1,7 @@ #pragma once #include +#include "pybind11/eigen.h" #include "pybind11/pybind11.h" #include "drake/bindings/pydrake/pydrake_pybind.h" diff --git a/bindings/pydrake/autodiffutils_py.cc b/bindings/pydrake/autodiffutils_py.cc index b8c7b46f0010..e45a9bab2b56 100644 --- a/bindings/pydrake/autodiffutils_py.cc +++ b/bindings/pydrake/autodiffutils_py.cc @@ -25,6 +25,7 @@ PYBIND11_MODULE(_autodiffutils_py, m) { py::class_ autodiff(m, "AutoDiffXd"); autodiff + .def(py::init()) .def(py::init()) .def("value", [](const AutoDiffXd& self) { return self.value(); @@ -74,6 +75,9 @@ PYBIND11_MODULE(_autodiffutils_py, m) { }, py::is_operator()) .def("__abs__", [](const AutoDiffXd& x) { return abs(x); }); + py::implicitly_convertible(); + py::implicitly_convertible(); + // Add overloads for `math` functions. auto math = py::module::import("pydrake.math"); MirrorDef(&math, &autodiff) diff --git a/bindings/pydrake/pydrake_pybind.h b/bindings/pydrake/pydrake_pybind.h index 1e1860624443..5bf9d920b9c9 100644 --- a/bindings/pydrake/pydrake_pybind.h +++ b/bindings/pydrake/pydrake_pybind.h @@ -12,6 +12,8 @@ namespace pydrake { /** @page python_bindings Python Bindings +# Overview + Drake uses [pybind11](http://pybind11.readthedocs.io/en/stable/) for binding its C++ API to Python. @@ -19,6 +21,33 @@ At present, a fork of `pybind11` is used which permits bindings matrices with `dtype=object`, passing `unique_ptr` objects, and prevents aliasing for Python classes derived from `pybind11` classes. +## `pybind11` Tips + +### Python Types + +Throughout the Drake code, Python types provided by `pybind11` are used, such +as `py::handle`, `py::object`, `py::module`, `py::str`, `py::list`, etc. +For an overview, see the +[pybind11 reference](http://pybind11.readthedocs.io/en/stable/reference.html). + +All of these are effectively thin wrappers around `PyObject*`, and thus can be +cheaply copied. + +Mutating the referred-to object also does not require passing by reference, so +you can always pass the object by value for functions, but you should document +your method if it mutates the object in a non-obvious fashion. + +### Python Type Conversions + +You can implicit convert between `py::object` and its derived classes (such +as `py::list`, `py::class_`, etc.), assuming the actual Python types agree. +You may also implicitly convert from `py::object` (and its derived classes) to +`py::handle`. + +If you wish to convert a `py::handle` (or `PyObject*`) to `py::object` or a +derived class, you should use +[`py::reinterpret_borrow<>`](http://pybind11.readthedocs.io/en/stable/reference.html#_CPPv218reinterpret_borrow6handle). + # Conventions ## Target Conventions diff --git a/bindings/pydrake/solvers/mathematicalprogram_py.cc b/bindings/pydrake/solvers/mathematicalprogram_py.cc index e232765eb477..be6d4eaa64f8 100644 --- a/bindings/pydrake/solvers/mathematicalprogram_py.cc +++ b/bindings/pydrake/solvers/mathematicalprogram_py.cc @@ -258,7 +258,7 @@ PYBIND11_MODULE(_mathematicalprogram_py, m) { &MathematicalProgram::AddBoundingBoxConstraint)) .def("AddBoundingBoxConstraint", [](MathematicalProgram* self, double lb, double ub, - const Eigen::Ref>& vars) { + const Eigen::Ref>& vars) { return self->AddBoundingBoxConstraint(lb, ub, vars); }) .def("AddConstraint", diff --git a/bindings/pydrake/systems/BUILD.bazel b/bindings/pydrake/systems/BUILD.bazel index 2b8909af8d24..572515f438d5 100644 --- a/bindings/pydrake/systems/BUILD.bazel +++ b/bindings/pydrake/systems/BUILD.bazel @@ -44,7 +44,11 @@ drake_py_library( drake_cc_library( name = "systems_pybind", hdrs = ["systems_pybind.h"], - deps = ["//bindings/pydrake/util:cpp_template_pybind"], + deps = [ + "//bindings/pydrake:autodiff_types_pybind", + "//bindings/pydrake:symbolic_types_pybind", + "//bindings/pydrake/util:cpp_template_pybind", + ], ) drake_pybind_library( @@ -68,6 +72,8 @@ drake_pybind_library( package_info = PACKAGE_INFO, py_deps = [ ":module_py", + "//bindings/pydrake:autodiffutils_py", + "//bindings/pydrake:symbolic_py", "//bindings/pydrake/util:cpp_template_py", ], ) @@ -75,6 +81,7 @@ drake_pybind_library( drake_pybind_library( name = "primitives_py", cc_deps = [ + ":systems_pybind", "//bindings/pydrake/util:drake_optional_pybind", ], cc_so_name = "primitives", @@ -88,6 +95,9 @@ drake_pybind_library( drake_pybind_library( name = "analysis_py", + cc_deps = [ + ":systems_pybind", + ], cc_so_name = "analysis", cc_srcs = ["analysis_py.cc"], package_info = PACKAGE_INFO, diff --git a/bindings/pydrake/systems/analysis_py.cc b/bindings/pydrake/systems/analysis_py.cc index 5392a9130f87..5e0e441c97c1 100644 --- a/bindings/pydrake/systems/analysis_py.cc +++ b/bindings/pydrake/systems/analysis_py.cc @@ -1,6 +1,7 @@ #include "pybind11/pybind11.h" #include "drake/bindings/pydrake/pydrake_pybind.h" +#include "drake/bindings/pydrake/systems/systems_pybind.h" #include "drake/systems/analysis/integrator_base.h" #include "drake/systems/analysis/simulator.h" @@ -15,44 +16,51 @@ PYBIND11_MODULE(analysis, m) { m.doc() = "Bindings for the analysis portion of the Systems framework."; - using T = double; - - py::class_>(m, "IntegratorBase") - .def("set_fixed_step_mode", &IntegratorBase::set_fixed_step_mode) - .def("get_fixed_step_mode", &IntegratorBase::get_fixed_step_mode) - .def("set_target_accuracy", &IntegratorBase::set_target_accuracy) - .def("get_target_accuracy", &IntegratorBase::get_target_accuracy) - .def("set_maximum_step_size", &IntegratorBase::set_maximum_step_size) - .def("get_maximum_step_size", &IntegratorBase::get_maximum_step_size) - .def("set_requested_minimum_step_size", - &IntegratorBase::set_requested_minimum_step_size) - .def("get_requested_minimum_step_size", - &IntegratorBase::get_requested_minimum_step_size) - .def("set_throw_on_minimum_step_size_violation", - &IntegratorBase::set_throw_on_minimum_step_size_violation) - .def("get_throw_on_minimum_step_size_violation", - &IntegratorBase::get_throw_on_minimum_step_size_violation); - - py::class_>(m, "Simulator") - .def(py::init&>(), - // Keep alive, reference: `self` keeps `System` alive. - py::keep_alive<1, 2>()) - .def(py::init&, unique_ptr>>(), - // Keep alive, reference: `self` keeps `System` alive. - py::keep_alive<1, 2>(), - // Keep alive, ownership: `Context` keeps `self` alive. - py::keep_alive<3, 1>()) - .def("Initialize", &Simulator::Initialize) - .def("StepTo", &Simulator::StepTo) - .def("get_context", &Simulator::get_context, py_reference_internal) - .def("get_integrator", &Simulator::get_integrator, py_reference_internal) - .def("get_mutable_integrator", &Simulator::get_mutable_integrator, - py_reference_internal) - .def("get_mutable_context", &Simulator::get_mutable_context, - py_reference_internal) - .def("set_publish_every_time_step", - &Simulator::set_publish_every_time_step) - .def("set_target_realtime_rate", &Simulator::set_target_realtime_rate); + py::module::import("pydrake.systems.framework"); + + auto bind_scalar_types = [m](auto dummy) { + using T = decltype(dummy); + DefineTemplateClassWithDefault>( + m, "IntegratorBase", GetPyParam()) + .def("set_fixed_step_mode", &IntegratorBase::set_fixed_step_mode) + .def("get_fixed_step_mode", &IntegratorBase::get_fixed_step_mode) + .def("set_target_accuracy", &IntegratorBase::set_target_accuracy) + .def("get_target_accuracy", &IntegratorBase::get_target_accuracy) + .def("set_maximum_step_size", &IntegratorBase::set_maximum_step_size) + .def("get_maximum_step_size", &IntegratorBase::get_maximum_step_size) + .def("set_requested_minimum_step_size", + &IntegratorBase::set_requested_minimum_step_size) + .def("get_requested_minimum_step_size", + &IntegratorBase::get_requested_minimum_step_size) + .def("set_throw_on_minimum_step_size_violation", + &IntegratorBase::set_throw_on_minimum_step_size_violation) + .def("get_throw_on_minimum_step_size_violation", + &IntegratorBase::get_throw_on_minimum_step_size_violation); + + DefineTemplateClassWithDefault>( + m, "Simulator", GetPyParam()) + .def(py::init&>(), + // Keep alive, reference: `self` keeps `System` alive. + py::keep_alive<1, 2>()) + .def(py::init&, unique_ptr>>(), + // Keep alive, reference: `self` keeps `System` alive. + py::keep_alive<1, 2>(), + // Keep alive, ownership: `Context` keeps `self` alive. + py::keep_alive<3, 1>()) + .def("Initialize", &Simulator::Initialize) + .def("StepTo", &Simulator::StepTo) + .def("get_context", &Simulator::get_context, py_reference_internal) + .def("get_integrator", &Simulator::get_integrator, + py_reference_internal) + .def("get_mutable_integrator", &Simulator::get_mutable_integrator, + py_reference_internal) + .def("get_mutable_context", &Simulator::get_mutable_context, + py_reference_internal) + .def("set_publish_every_time_step", + &Simulator::set_publish_every_time_step) + .def("set_target_realtime_rate", &Simulator::set_target_realtime_rate); + }; + type_visit(bind_scalar_types, pysystems::NonSymbolicScalarPack{}); } } // namespace pydrake diff --git a/bindings/pydrake/systems/framework_py.cc b/bindings/pydrake/systems/framework_py.cc index af36d1c2ad62..9f624f51bf8e 100644 --- a/bindings/pydrake/systems/framework_py.cc +++ b/bindings/pydrake/systems/framework_py.cc @@ -9,6 +9,11 @@ namespace pydrake { PYBIND11_MODULE(framework, m) { m.doc() = "Bindings for the core Systems framework."; + // Import autodiff and symbolic modules so that their types are declared for + // use as template parameters. + py::module::import("pydrake.autodiffutils"); + py::module::import("pydrake.symbolic"); + // Incorporate definitions as pieces (to speed up compilation). DefineFrameworkPySystems(m); DefineFrameworkPySemantics(m); diff --git a/bindings/pydrake/systems/framework_py_semantics.cc b/bindings/pydrake/systems/framework_py_semantics.cc index 52179bd20869..56de64396d08 100644 --- a/bindings/pydrake/systems/framework_py_semantics.cc +++ b/bindings/pydrake/systems/framework_py_semantics.cc @@ -22,11 +22,6 @@ using std::vector; namespace drake { namespace pydrake { -// TODO(eric.cousineau): At present, we only bind doubles. -// In the future, we will bind more scalar types, and enable scalar -// conversion. -using T = double; - using pysystems::DefClone; void DefineFrameworkPySemantics(py::module m) { @@ -49,215 +44,9 @@ void DefineFrameworkPySemantics(py::module m) { BindTypeSafeIndex(m, "NumericParameterIndex"); BindTypeSafeIndex(m, "AbstractParameterIndex"); - py::class_>(m, "Context") - .def("get_num_input_ports", &Context::get_num_input_ports) - .def("FixInputPort", - py::overload_cast>>( - &Context::FixInputPort), - py_reference_internal, - // Keep alive, ownership: `BasicVector` keeps `self` alive. - py::keep_alive<3, 1>()) - .def("FixInputPort", - py::overload_cast>( - &Context::FixInputPort), - py_reference_internal, - // Keep alive, ownership: `AbstractValue` keeps `self` alive. - py::keep_alive<3, 1>()) - .def("get_time", &Context::get_time) - .def("set_time", &Context::set_time) - .def("set_accuracy", &Context::set_accuracy) - .def("get_accuracy", &Context::get_accuracy) - .def("Clone", &Context::Clone) - .def("__copy__", &Context::Clone) - .def("__deepcopy__", [](const Context* self, py::dict /* memo */) { - return self->Clone(); - }) - .def("get_state", &Context::get_state, py_reference_internal) - .def("get_mutable_state", &Context::get_mutable_state, - py_reference_internal) - // Sugar methods - // - Continuous. - .def("get_continuous_state_vector", - &Context::get_continuous_state_vector, - py_reference_internal) - .def("get_mutable_continuous_state_vector", - &Context::get_mutable_continuous_state_vector, - py_reference_internal) - // - Discrete. - .def("get_discrete_state_vector", - &Context::get_discrete_state_vector, - py_reference_internal) - .def("get_mutable_discrete_state_vector", - [](Context* self) -> auto& { - return self->get_mutable_discrete_state().get_mutable_vector(); - }, - py_reference_internal) - // - Abstract. - .def("get_num_abstract_states", &Context::get_num_abstract_states) - .def("get_abstract_state", - [](const Context* self) -> auto& { - return self->get_abstract_state(); - }, - py_reference_internal) - .def("get_abstract_state", - [](const Context* self, int index) -> auto& { - return self->get_abstract_state().get_value(index); - }, - py_reference_internal) - .def("get_mutable_abstract_state", - [](Context* self) -> auto& { - return self->get_mutable_abstract_state(); - }, - py_reference_internal) - .def("get_mutable_abstract_state", - [](Context* self, int index) -> auto& { - return self->get_mutable_abstract_state().get_mutable_value(index); - }, - py_reference_internal); - - py::class_, Context>(m, "LeafContext"); - - // Event mechanisms. - py::class_>(m, "Event"); - py::class_, Event>(m, "PublishEvent"); - py::class_, Event>(m, "DiscreteUpdateEvent"); - - // Glue mechanisms. - py::class_>(m, "DiagramBuilder") - .def(py::init<>()) - .def( - "AddSystem", - [](DiagramBuilder* self, unique_ptr> arg1) { - return self->AddSystem(std::move(arg1)); - }, - // Keep alive, ownership: `System` keeps `self` alive. - py::keep_alive<2, 1>()) - .def("Connect", - py::overload_cast&, const InputPortDescriptor&>( - &DiagramBuilder::Connect)) - .def("ExportInput", &DiagramBuilder::ExportInput, py_reference_internal) - .def("ExportOutput", &DiagramBuilder::ExportOutput, - py_reference_internal) - .def("Build", &DiagramBuilder::Build, - // Keep alive, transitive: `return` keeps `self` alive. - py::keep_alive<1, 0>()) - .def("BuildInto", &DiagramBuilder::BuildInto, - // Keep alive, transitive: `Diagram` keeps `self` alive. - py::keep_alive<2, 1>()); - - // TODO(eric.cousineau): Figure out how to handle template-specialized method - // signatures(e.g. GetValue()). py::class_(m, "FreestandingInputPortValue"); - py::class_>(m, "OutputPort") - .def("size", &OutputPort::size) - .def("get_index", &OutputPort::get_index); - - py::class_> system_output(m, "SystemOutput"); - DefClone(&system_output); - system_output - .def("get_num_ports", &SystemOutput::get_num_ports) - .def("get_data", &SystemOutput::get_data, - py_reference_internal) - .def("get_vector_data", &SystemOutput::get_vector_data, - py_reference_internal); - - py::class_>(m, "InputPortDescriptor") - .def("size", &InputPortDescriptor::size) - .def("get_data_type", &InputPortDescriptor::get_data_type) - .def("get_index", &InputPortDescriptor::get_index); - - // Parameters. - py::class_> parameters(m, "Parameters"); - DefClone(¶meters); - using BasicVectorPtrList = vector>>; using AbstractValuePtrList = vector>; - parameters - .def(py::init<>()) - // TODO(eric.cousineau): Ensure that we can respect keep alive behavior - // with lists of pointers. - .def(py::init(), - py::arg("numeric"), py::arg("abstract")) - .def(py::init(), py::arg("numeric")) - .def(py::init(), py::arg("abstract")) - .def(py::init>>(), py::arg("vec"), - // Keep alive, ownership: `vec` keeps `self` alive. - py::keep_alive<2, 1>()) - .def(py::init>(), py::arg("value"), - // Keep alive, ownership: `value` keeps `self` alive. - py::keep_alive<2, 1>()) - .def("num_numeric_parameters", &Parameters::num_numeric_parameters) - .def("num_abstract_parameters", &Parameters::num_abstract_parameters) - .def("get_numeric_parameter", &Parameters::get_numeric_parameter, - py_reference_internal, py::arg("index")) - .def("get_mutable_numeric_parameter", - &Parameters::get_mutable_numeric_parameter, - py_reference_internal, py::arg("index")) - .def("get_numeric_parameters", &Parameters::get_numeric_parameters, - py_reference_internal) - // 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")) - .def("get_abstract_parameter", - [](const Parameters* self, int index) -> auto& { - return self->get_abstract_parameter(index); - }, - py_reference_internal, py::arg("index")) - .def("get_mutable_abstract_parameter", - [](Parameters* self, int index) -> auto& { - return self->get_mutable_abstract_parameter(index); - }, - py_reference_internal, py::arg("index")) - .def("get_abstract_parameters", &Parameters::get_abstract_parameters, - py_reference_internal) - .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("SetFrom", &Parameters::SetFrom); - - // State. - py::class_>(m, "State") - .def(py::init<>()) - .def("get_continuous_state", &State::get_continuous_state, - py_reference_internal) - .def("get_mutable_continuous_state", - &State::get_mutable_continuous_state, py_reference_internal) - .def("get_discrete_state", - overload_cast_explicit&>( - &State::get_discrete_state), - py_reference_internal) - .def("get_mutable_discrete_state", - overload_cast_explicit&>( - &State::get_mutable_discrete_state), - py_reference_internal); - - // - Constituents. - py::class_>(m, "ContinuousState") - .def(py::init<>()) - .def("get_vector", &ContinuousState::get_vector, py_reference_internal) - .def("get_mutable_vector", - &ContinuousState::get_mutable_vector, py_reference_internal); - - py::class_> discrete_values(m, "DiscreteValues"); - DefClone(&discrete_values); - discrete_values - .def("num_groups", &DiscreteValues::num_groups) - .def("get_data", &DiscreteValues::get_data, py_reference_internal) - .def("get_vector", - overload_cast_explicit&, int>( - &DiscreteValues::get_vector), - py_reference_internal, py::arg("index") = 0) - .def("get_mutable_vector", - overload_cast_explicit&, int>( - &DiscreteValues::get_mutable_vector), - py_reference_internal, py::arg("index") = 0); - // N.B. `AbstractValues` provides the ability to reference non-owned values, // without copying them. For consistency with other model-value Python // bindings, only the ownership variant is exposed. @@ -271,6 +60,227 @@ void DefineFrameworkPySemantics(py::module m) { .def("get_mutable_value", &AbstractValues::get_mutable_value, py_reference_internal) .def("CopyFrom", &AbstractValues::CopyFrom); + + auto bind_common_scalar_types = [m](auto dummy) { + using T = decltype(dummy); + DefineTemplateClassWithDefault>( + m, "Context", GetPyParam()) + .def("get_num_input_ports", &Context::get_num_input_ports) + .def("FixInputPort", + py::overload_cast>>( + &Context::FixInputPort), + py_reference_internal, + // Keep alive, ownership: `BasicVector` keeps `self` alive. + py::keep_alive<3, 1>()) + .def("FixInputPort", + py::overload_cast>( + &Context::FixInputPort), + py_reference_internal, + // Keep alive, ownership: `AbstractValue` keeps `self` alive. + py::keep_alive<3, 1>()) + .def("get_time", &Context::get_time) + .def("set_time", &Context::set_time) + .def("set_accuracy", &Context::set_accuracy) + .def("get_accuracy", &Context::get_accuracy) + .def("Clone", &Context::Clone) + .def("__copy__", &Context::Clone) + .def("__deepcopy__", [](const Context* self, py::dict /* memo */) { + return self->Clone(); + }) + .def("get_state", &Context::get_state, py_reference_internal) + .def("get_mutable_state", &Context::get_mutable_state, + py_reference_internal) + // Sugar methods + // - Continuous. + .def("get_continuous_state_vector", + &Context::get_continuous_state_vector, + py_reference_internal) + .def("get_mutable_continuous_state_vector", + &Context::get_mutable_continuous_state_vector, + py_reference_internal) + // - Discrete. + .def("get_discrete_state_vector", + &Context::get_discrete_state_vector, + py_reference_internal) + .def("get_mutable_discrete_state_vector", + [](Context* self) -> auto& { + return self->get_mutable_discrete_state().get_mutable_vector(); + }, + py_reference_internal) + // - Abstract. + .def("get_num_abstract_states", &Context::get_num_abstract_states) + .def("get_abstract_state", + [](const Context* self) -> auto& { + return self->get_abstract_state(); + }, + py_reference_internal) + .def("get_abstract_state", + [](const Context* self, int index) -> auto& { + return self->get_abstract_state().get_value(index); + }, + py_reference_internal) + .def("get_mutable_abstract_state", + [](Context* self) -> auto& { + return self->get_mutable_abstract_state(); + }, + py_reference_internal) + .def("get_mutable_abstract_state", + [](Context* self, int index) -> auto& { + return self->get_mutable_abstract_state().get_mutable_value(index); + }, + py_reference_internal); + + DefineTemplateClassWithDefault, Context>( + m, "LeafContext", GetPyParam()); + + // Event mechanisms. + DefineTemplateClassWithDefault>(m, "Event", GetPyParam()); + DefineTemplateClassWithDefault, Event>( + m, "PublishEvent", GetPyParam()); + DefineTemplateClassWithDefault, Event>( + m, "DiscreteUpdateEvent", GetPyParam()); + + // Glue mechanisms. + DefineTemplateClassWithDefault>( + m, "DiagramBuilder", GetPyParam()) + .def(py::init<>()) + .def( + "AddSystem", + [](DiagramBuilder* self, unique_ptr> arg1) { + return self->AddSystem(std::move(arg1)); + }, + // Keep alive, ownership: `System` keeps `self` alive. + py::keep_alive<2, 1>()) + .def("Connect", + py::overload_cast< + const OutputPort&, const InputPortDescriptor&>( + &DiagramBuilder::Connect)) + .def("ExportInput", &DiagramBuilder::ExportInput, + py_reference_internal) + .def("ExportOutput", &DiagramBuilder::ExportOutput, + py_reference_internal) + .def("Build", &DiagramBuilder::Build, + // Keep alive, transitive: `return` keeps `self` alive. + py::keep_alive<1, 0>()) + .def("BuildInto", &DiagramBuilder::BuildInto, + // Keep alive, transitive: `Diagram` keeps `self` alive. + py::keep_alive<2, 1>()); + + DefineTemplateClassWithDefault>( + m, "OutputPort", GetPyParam()) + .def("size", &OutputPort::size) + .def("get_index", &OutputPort::get_index); + + auto system_output = DefineTemplateClassWithDefault>( + m, "SystemOutput", GetPyParam()); + DefClone(&system_output); + system_output + .def("get_num_ports", &SystemOutput::get_num_ports) + .def("get_data", &SystemOutput::get_data, + py_reference_internal) + .def("get_vector_data", &SystemOutput::get_vector_data, + py_reference_internal); + + DefineTemplateClassWithDefault>( + m, "InputPortDescriptor", GetPyParam()) + .def("size", &InputPortDescriptor::size) + .def("get_data_type", &InputPortDescriptor::get_data_type) + .def("get_index", &InputPortDescriptor::get_index); + + // Parameters. + auto parameters = DefineTemplateClassWithDefault>( + m, "Parameters", GetPyParam()); + DefClone(¶meters); + using BasicVectorPtrList = vector>>; + parameters + .def(py::init<>()) + // TODO(eric.cousineau): Ensure that we can respect keep alive behavior + // with lists of pointers. + .def(py::init(), + py::arg("numeric"), py::arg("abstract")) + .def(py::init(), py::arg("numeric")) + .def(py::init(), py::arg("abstract")) + .def(py::init>>(), py::arg("vec"), + // Keep alive, ownership: `vec` keeps `self` alive. + py::keep_alive<2, 1>()) + .def(py::init>(), py::arg("value"), + // Keep alive, ownership: `value` keeps `self` alive. + py::keep_alive<2, 1>()) + .def("num_numeric_parameters", &Parameters::num_numeric_parameters) + .def("num_abstract_parameters", &Parameters::num_abstract_parameters) + .def("get_numeric_parameter", &Parameters::get_numeric_parameter, + py_reference_internal, py::arg("index")) + .def("get_mutable_numeric_parameter", + &Parameters::get_mutable_numeric_parameter, + py_reference_internal, py::arg("index")) + .def("get_numeric_parameters", &Parameters::get_numeric_parameters, + py_reference_internal) + // 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")) + .def("get_abstract_parameter", + [](const Parameters* self, int index) -> auto& { + return self->get_abstract_parameter(index); + }, + py_reference_internal, py::arg("index")) + .def("get_mutable_abstract_parameter", + [](Parameters* self, int index) -> auto& { + return self->get_mutable_abstract_parameter(index); + }, + py_reference_internal, py::arg("index")) + .def("get_abstract_parameters", &Parameters::get_abstract_parameters, + py_reference_internal) + .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("SetFrom", &Parameters::SetFrom); + + // State. + DefineTemplateClassWithDefault>(m, "State", GetPyParam()) + .def(py::init<>()) + .def("get_continuous_state", &State::get_continuous_state, + py_reference_internal) + .def("get_mutable_continuous_state", + &State::get_mutable_continuous_state, py_reference_internal) + .def("get_discrete_state", + overload_cast_explicit&>( + &State::get_discrete_state), + py_reference_internal) + .def("get_mutable_discrete_state", + overload_cast_explicit&>( + &State::get_mutable_discrete_state), + py_reference_internal); + + // - Constituents. + DefineTemplateClassWithDefault>( + m, "ContinuousState", GetPyParam()) + .def(py::init<>()) + .def("get_vector", &ContinuousState::get_vector, py_reference_internal) + .def("get_mutable_vector", + &ContinuousState::get_mutable_vector, py_reference_internal); + + auto discrete_values = DefineTemplateClassWithDefault>( + m, "DiscreteValues", GetPyParam()); + DefClone(&discrete_values); + discrete_values + .def("num_groups", &DiscreteValues::num_groups) + .def("get_data", &DiscreteValues::get_data, py_reference_internal) + .def("get_vector", + overload_cast_explicit&, int>( + &DiscreteValues::get_vector), + py_reference_internal, py::arg("index") = 0) + .def("get_mutable_vector", + overload_cast_explicit&, int>( + &DiscreteValues::get_mutable_vector), + py_reference_internal, py::arg("index") = 0); + }; + type_visit(bind_common_scalar_types, pysystems::CommonScalarPack{}); } } // namespace pydrake diff --git a/bindings/pydrake/systems/framework_py_systems.cc b/bindings/pydrake/systems/framework_py_systems.cc index 9e038f3ed176..c20cb18816aa 100644 --- a/bindings/pydrake/systems/framework_py_systems.cc +++ b/bindings/pydrake/systems/framework_py_systems.cc @@ -33,337 +33,365 @@ using systems::PublishEvent; using systems::DiscreteUpdateEvent; using systems::DiscreteValues; -// TODO(eric.cousineau): At present, we only bind doubles. -// In the future, we will bind more scalar types, and enable scalar -// conversion. -using T = double; - -class PySystem : public py::wrapper> { - public: - using Base = py::wrapper>; - using Base::Base; - // Expose protected methods for binding. - using Base::DeclareInputPort; -}; - -class LeafSystemPublic : public LeafSystem { - public: - using Base = LeafSystem; - using Base::Base; - - // N.B. These function methods are still typed as (LeafSystem::*)(...), - // since they are more or less visibility imports. - // Defining methods here won't work, as it will become - // (LeafSystemPublic::*)(...), since this typeid is not exposed in pybind. - // If needed, solution is to expose it as an intermediate type if needed. - - // Expose protected methods for binding, no need for virtual overrides - // (ordered by how they are bound). - using Base::DeclareVectorOutputPort; - using Base::DeclarePeriodicPublish; - using Base::DeclareContinuousState; - using Base::DeclareDiscreteState; - using Base::DeclarePeriodicDiscreteUpdate; - using Base::DeclareAbstractState; - - // Because `LeafSystem::DoPublish` is protected, and we had to override - // this method in `PyLeafSystem`, expose the method here for direct(-ish) - // access. - // (Otherwise, we get an error about inaccessible downcasting when trying to - // bind `PyLeafSystem::DoPublish` to `py::class_, ...>`. - using Base::DoPublish; - using Base::DoHasDirectFeedthrough; - using Base::DoCalcDiscreteVariableUpdates; -}; - -// Provide flexible inheritance to leverage prior binding information, per -// documentation: -// http://pybind11.readthedocs.io/en/stable/advanced/classes.html#combining-virtual-functions-and-inheritance // NOLINT -template -class PyLeafSystemBase : public py::wrapper { - public: - using Base = py::wrapper; - using Base::Base; - - // Trampoline virtual methods. - void DoPublish( - const Context& context, - const vector*>& events) const override { - // Yuck! We have to dig in and use internals :( - // We must ensure that pybind only sees pointers, since this method may - // be called from C++, and pybind will not have seen these objects yet. - // @see https://github.com/pybind/pybind11/issues/1241 - // TODO(eric.cousineau): Figure out how to supply different behavior, - // possibly using function wrapping. - PYBIND11_OVERLOAD_INT( - void, LeafSystem, "_DoPublish", &context, events); - // If the macro did not return, use default functionality. - Base::DoPublish(context, events); - } - - optional DoHasDirectFeedthrough( - int input_port, int output_port) const override { - PYBIND11_OVERLOAD_INT( - optional, LeafSystem, "_DoHasDirectFeedthrough", - input_port, output_port); - // If the macro did not return, use default functionality. - return Base::DoHasDirectFeedthrough(input_port, output_port); - } - - void DoCalcDiscreteVariableUpdates( - const Context& context, - const std::vector*>& events, - DiscreteValues* discrete_state) const override { - // See `DoPublish` for explanation. - PYBIND11_OVERLOAD_INT( - void, LeafSystem, "_DoCalcDiscreteVariableUpdates", - &context, events, discrete_state); - // If the macro did not return, use default functionality. - Base::DoCalcDiscreteVariableUpdates(context, events, discrete_state); - } -}; - -using PyLeafSystem = PyLeafSystemBase<>; - -class VectorSystemPublic : public VectorSystem { - public: - using Base = VectorSystem; - - VectorSystemPublic(int inputs, int outputs) - : Base(inputs, outputs) {} - - using Base::EvalVectorInput; - using Base::GetVectorState; - - // Virtual methods, for explicit bindings. - using Base::DoCalcVectorOutput; - using Base::DoCalcVectorTimeDerivatives; - using Base::DoCalcVectorDiscreteVariableUpdates; -}; - -class PyVectorSystem : public py::wrapper { - public: - using Base = py::wrapper; - using Base::Base; - - // Trampoline virtual methods. - void DoPublish( - const Context& context, - const vector*>& events) const override { - // Copied from above, since we cannot use `PyLeafSystemBase` due to final - // overrides of some methods. - // TODO(eric.cousineau): Make this more granular? - PYBIND11_OVERLOAD_INT( - void, VectorSystem, "_DoPublish", &context, events); - // If the macro did not return, use default functionality. - Base::DoPublish(context, events); - } - - optional DoHasDirectFeedthrough( - int input_port, int output_port) const override { - PYBIND11_OVERLOAD_INT( - optional, VectorSystem, "_DoHasDirectFeedthrough", - input_port, output_port); - // If the macro did not return, use default functionality. - return Base::DoHasDirectFeedthrough(input_port, output_port); - } - - void DoCalcVectorOutput( - const Context& context, - const Eigen::VectorBlock>& input, - const Eigen::VectorBlock>& state, - Eigen::VectorBlock>* output) const override { - // WARNING: Mutating `output` will not work when T is AutoDiffXd, - // Expression, etc. - // @see https://github.com/pybind/pybind11/pull/1152#issuecomment-340091423 - // TODO(eric.cousineau): Either wrap to have this return a value (blech), - // or solve the upstream issue. - PYBIND11_OVERLOAD_INT( - void, VectorSystem, "_DoCalcVectorOutput", - // N.B. Passing `Eigen::Map<>` derived classes by reference rather than - // pointer to ensure conceptual clarity. pybind11 `type_caster` - // struggles with types of `Map*`, but not `Map&`. - &context, input, state, ToEigenRef(output)); - // If the macro did not return, use default functionality. - Base::DoCalcVectorOutput(context, input, state, output); - } - - void DoCalcVectorTimeDerivatives( - const Context& context, - const Eigen::VectorBlock>& input, - const Eigen::VectorBlock>& state, - Eigen::VectorBlock>* derivatives) const override { - // WARNING: Mutating `derivatives` will not work when T is AutoDiffXd, - // Expression, etc. See above. - PYBIND11_OVERLOAD_INT( - void, VectorSystem, "_DoCalcVectorTimeDerivatives", - &context, input, state, ToEigenRef(derivatives)); - // If the macro did not return, use default functionality. - Base::DoCalcVectorOutput(context, input, state, derivatives); - } - - void DoCalcVectorDiscreteVariableUpdates( - const Context& context, - const Eigen::VectorBlock>& input, - const Eigen::VectorBlock>& state, - Eigen::VectorBlock>* next_state) const override { - // WARNING: Mutating `next_state` will not work when T is AutoDiffXd, - // Expression, etc. See above. - PYBIND11_OVERLOAD_INT( - void, VectorSystem, "_DoCalcVectorDiscreteVariableUpdates", - &context, input, state, ToEigenRef(next_state)); - // If the macro did not return, use default functionality. - Base::DoCalcVectorDiscreteVariableUpdates( - context, input, state, next_state); +// Provides a templated 'namespace'. +template +struct Impl { + class PySystem : public py::wrapper> { + public: + using Base = py::wrapper>; + using Base::Base; + // Expose protected methods for binding. + using Base::DeclareInputPort; + }; + + class LeafSystemPublic : public LeafSystem { + public: + using Base = LeafSystem; + using Base::Base; + + // N.B. These function methods are still typed as (LeafSystem::*)(...), + // since they are more or less visibility imports. + // Defining methods here won't work, as it will become + // (LeafSystemPublic::*)(...), since this typeid is not exposed in pybind. + // If needed, solution is to expose it as an intermediate type if needed. + + // Expose protected methods for binding, no need for virtual overrides + // (ordered by how they are bound). + using Base::DeclareVectorOutputPort; + using Base::DeclarePeriodicPublish; + using Base::DeclareContinuousState; + using Base::DeclareDiscreteState; + using Base::DeclarePeriodicDiscreteUpdate; + using Base::DeclareAbstractState; + + // Because `LeafSystem::DoPublish` is protected, and we had to override + // this method in `PyLeafSystem`, expose the method here for direct(-ish) + // access. + // (Otherwise, we get an error about inaccessible downcasting when trying to + // bind `PyLeafSystem::DoPublish` to `py::class_, ...>`. + using Base::DoPublish; + using Base::DoHasDirectFeedthrough; + using Base::DoCalcDiscreteVariableUpdates; + }; + + // Provide flexible inheritance to leverage prior binding information, per + // documentation: + // http://pybind11.readthedocs.io/en/stable/advanced/classes.html#combining-virtual-functions-and-inheritance // NOLINT + template + class PyLeafSystemBase : public py::wrapper { + public: + using Base = py::wrapper; + using Base::Base; + + // Trampoline virtual methods. + void DoPublish( + const Context& context, + const vector*>& events) const override { + // Yuck! We have to dig in and use internals :( + // We must ensure that pybind only sees pointers, since this method may + // be called from C++, and pybind will not have seen these objects yet. + // @see https://github.com/pybind/pybind11/issues/1241 + // TODO(eric.cousineau): Figure out how to supply different behavior, + // possibly using function wrapping. + PYBIND11_OVERLOAD_INT( + void, LeafSystem, "_DoPublish", &context, events); + // If the macro did not return, use default functionality. + Base::DoPublish(context, events); + } + + optional DoHasDirectFeedthrough( + int input_port, int output_port) const override { + PYBIND11_OVERLOAD_INT( + optional, LeafSystem, "_DoHasDirectFeedthrough", + input_port, output_port); + // If the macro did not return, use default functionality. + return Base::DoHasDirectFeedthrough(input_port, output_port); + } + + void DoCalcDiscreteVariableUpdates( + const Context& context, + const std::vector*>& events, + DiscreteValues* discrete_state) const override { + // See `DoPublish` for explanation. + PYBIND11_OVERLOAD_INT( + void, LeafSystem, "_DoCalcDiscreteVariableUpdates", + &context, events, discrete_state); + // If the macro did not return, use default functionality. + Base::DoCalcDiscreteVariableUpdates(context, events, discrete_state); + } + }; + + using PyLeafSystem = PyLeafSystemBase<>; + + class VectorSystemPublic : public VectorSystem { + public: + using Base = VectorSystem; + + VectorSystemPublic(int inputs, int outputs) + : Base(inputs, outputs) {} + + using Base::EvalVectorInput; + using Base::GetVectorState; + + // Virtual methods, for explicit bindings. + using Base::DoCalcVectorOutput; + using Base::DoCalcVectorTimeDerivatives; + using Base::DoCalcVectorDiscreteVariableUpdates; + }; + + class PyVectorSystem : public py::wrapper { + public: + using Base = py::wrapper; + using Base::Base; + + // Trampoline virtual methods. + void DoPublish( + const Context& context, + const vector*>& events) const override { + // Copied from above, since we cannot use `PyLeafSystemBase` due to final + // overrides of some methods. + // TODO(eric.cousineau): Make this more granular? + PYBIND11_OVERLOAD_INT( + void, VectorSystem, "_DoPublish", &context, events); + // If the macro did not return, use default functionality. + Base::DoPublish(context, events); + } + + optional DoHasDirectFeedthrough( + int input_port, int output_port) const override { + PYBIND11_OVERLOAD_INT( + optional, VectorSystem, "_DoHasDirectFeedthrough", + input_port, output_port); + // If the macro did not return, use default functionality. + return Base::DoHasDirectFeedthrough(input_port, output_port); + } + + void DoCalcVectorOutput( + const Context& context, + const Eigen::VectorBlock>& input, + const Eigen::VectorBlock>& state, + Eigen::VectorBlock>* output) const override { + // WARNING: Mutating `output` will not work when T is AutoDiffXd, + // Expression, etc. See + // https://github.com/pybind/pybind11/pull/1152#issuecomment-340091423 + // TODO(eric.cousineau): This will be resolved once dtype=custom is + // resolved. + PYBIND11_OVERLOAD_INT( + void, VectorSystem, "_DoCalcVectorOutput", + // N.B. Passing `Eigen::Map<>` derived classes by reference rather + // than pointer to ensure conceptual clarity. pybind11 `type_caster` + // struggles with types of `Map*`, but not `Map&`. + &context, input, state, ToEigenRef(output)); + // If the macro did not return, use default functionality. + Base::DoCalcVectorOutput(context, input, state, output); + } + + void DoCalcVectorTimeDerivatives( + const Context& context, + const Eigen::VectorBlock>& input, + const Eigen::VectorBlock>& state, + Eigen::VectorBlock>* derivatives) const override { + // WARNING: Mutating `derivatives` will not work when T is AutoDiffXd, + // Expression, etc. See above. + PYBIND11_OVERLOAD_INT( + void, VectorSystem, "_DoCalcVectorTimeDerivatives", + &context, input, state, ToEigenRef(derivatives)); + // If the macro did not return, use default functionality. + Base::DoCalcVectorOutput(context, input, state, derivatives); + } + + void DoCalcVectorDiscreteVariableUpdates( + const Context& context, + const Eigen::VectorBlock>& input, + const Eigen::VectorBlock>& state, + Eigen::VectorBlock>* next_state) const override { + // WARNING: Mutating `next_state` will not work when T is AutoDiffXd, + // Expression, etc. See above. + PYBIND11_OVERLOAD_INT( + void, VectorSystem, "_DoCalcVectorDiscreteVariableUpdates", + &context, input, state, ToEigenRef(next_state)); + // If the macro did not return, use default functionality. + Base::DoCalcVectorDiscreteVariableUpdates( + context, input, state, next_state); + } + }; + + static void DoDefinitions(py::module m) { + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake::systems; + + // TODO(eric.cousineau): Resolve `str_py` workaround. + auto str_py = py::eval("str"); + + // TODO(eric.cousineau): Show constructor, but somehow make sure `pybind11` + // knows this is abstract? + DefineTemplateClassWithDefault, PySystem>( + m, "System", GetPyParam()) + .def("set_name", &System::set_name) + // Topology. + .def("get_num_input_ports", &System::get_num_input_ports) + .def("get_input_port", + &System::get_input_port, py_reference_internal) + .def("get_num_output_ports", &System::get_num_input_ports) + .def("get_output_port", + &System::get_output_port, py_reference_internal) + .def( + "_DeclareInputPort", &PySystem::DeclareInputPort, + py_reference_internal, + py::arg("type"), py::arg("size"), py::arg("random_type") = nullopt) + // - Feedthrough. + .def("HasAnyDirectFeedthrough", &System::HasAnyDirectFeedthrough) + .def("HasDirectFeedthrough", + overload_cast_explicit( + &System::HasDirectFeedthrough), + py::arg("output_port")) + .def("HasDirectFeedthrough", + overload_cast_explicit( + &System::HasDirectFeedthrough), + py::arg("input_port"), py::arg("output_port")) + // Context. + .def("CreateDefaultContext", &System::CreateDefaultContext) + .def("AllocateOutput", &System::AllocateOutput) + .def( + "EvalVectorInput", + [](const System* self, const Context& arg1, int arg2) { + return self->EvalVectorInput(arg1, arg2); + }, py_reference, + // Keep alive, ownership: `return` keeps `Context` alive. + py::keep_alive<0, 2>()) + .def( + "EvalAbstractInput", + [](const System* self, const Context& arg1, int arg2) { + return self->EvalAbstractInput(arg1, arg2); + }, py_reference, + // Keep alive, ownership: `return` keeps `Context` alive. + py::keep_alive<0, 2>()) + .def("CalcOutput", &System::CalcOutput) + // Sugar. + .def( + "GetGraphvizString", + [str_py](const System* self) { + // @note This is a workaround; for some reason, + // casting this using `py::str` does not work, but directly + // calling the Python function (`str_py`) does. + return str_py(self->GetGraphvizString()); + }) + // Events. + .def("Publish", + overload_cast_explicit&>( + &System::Publish)) + // Scalar types. + .def("ToAutoDiffXd", [](const System& self) { + return self.ToAutoDiffXd(); + }) + .def("ToAutoDiffXdMaybe", &System::ToAutoDiffXdMaybe) + .def("ToSymbolic", [](const System& self) { + return self.ToSymbolic(); + }) + .def("ToSymbolicMaybe", &System::ToSymbolicMaybe); + + // Don't use a const-rvalue as a function handle parameter, as pybind11 + // wants to copy it? + // TODO(eric.cousineau): Make a helper wrapper for this; file a bug in + // pybind11 (since these are arguments). + using CalcVectorPtrCallback = + std::function*, BasicVector*)>; + + DefineTemplateClassWithDefault, PyLeafSystem, System>( + m, "LeafSystem", GetPyParam()) + .def(py::init<>()) + .def( + "_DeclareVectorOutputPort", + [](PyLeafSystem* self, const BasicVector& arg1, + CalcVectorPtrCallback arg2) -> auto&& { + typename LeafOutputPort::CalcVectorCallback wrapped = + [arg2](const Context& nest_arg1, BasicVector* nest_arg2) { + return arg2(&nest_arg1, nest_arg2); + }; + return self->DeclareVectorOutputPort(arg1, wrapped); + }, py_reference_internal) + .def("_DeclarePeriodicPublish", &PyLeafSystem::DeclarePeriodicPublish, + py::arg("period_sec"), py::arg("offset_sec") = 0.) + .def("_DoPublish", &LeafSystemPublic::DoPublish) + // System attributes. + .def("_DoHasDirectFeedthrough", + &LeafSystemPublic::DoHasDirectFeedthrough) + // Continuous state. + .def("_DeclareContinuousState", + py::overload_cast(&LeafSystemPublic::DeclareContinuousState), + py::arg("num_state_variables")) + .def("_DeclareContinuousState", + py::overload_cast( + &LeafSystemPublic::DeclareContinuousState), + py::arg("num_q"), py::arg("num_v"), py::arg("num_z")) + .def("_DeclareContinuousState", + py::overload_cast&>( + &LeafSystemPublic::DeclareContinuousState), + py::arg("model_vector")) + // TODO(eric.cousineau): Ideally the downstream class of `BasicVector` + // should expose `num_q`, `num_v`, and `num_z`? + .def("_DeclareContinuousState", + py::overload_cast&, int, int, int>( + &LeafSystemPublic::DeclareContinuousState), + py::arg("model_vector"), + py::arg("num_q"), py::arg("num_v"), py::arg("num_z")) + // Discrete state. + // TODO(eric.cousineau): Should there be a `BasicVector<>` overload? + .def("_DeclareDiscreteState", &LeafSystemPublic::DeclareDiscreteState) + .def("_DeclarePeriodicDiscreteUpdate", + &LeafSystemPublic::DeclarePeriodicDiscreteUpdate, + py::arg("period_sec"), py::arg("offset_sec") = 0.) + .def("_DoCalcDiscreteVariableUpdates", + &LeafSystemPublic::DoCalcDiscreteVariableUpdates) + // Abstract state. + .def("_DeclareAbstractState", + &LeafSystemPublic::DeclareAbstractState, + // Keep alive, ownership: `AbstractValue` keeps `self` alive. + py::keep_alive<2, 1>()); + + DefineTemplateClassWithDefault, System>( + m, "Diagram", GetPyParam()) + .def("GetMutableSubsystemState", + [](Diagram* self, const System& arg1, Context* arg2) + -> auto& { + // @note Compiler does not like `py::overload_cast` with this setup? + return self->GetMutableSubsystemState(arg1, arg2); + }, py_reference, + // Keep alive, ownership: `return` keeps `Context` alive. + py::keep_alive<0, 3>()) + .def("GetMutableSubsystemContext", + [](Diagram* self, const System& arg1, Context* arg2) + -> auto&& { + return self->GetMutableSubsystemContext(arg1, arg2); + }, py_reference, + // Keep alive, ownership: `return` keeps `Context` alive. + py::keep_alive<0, 3>()); + + // N.B. This will effectively allow derived classes of `VectorSystem` to + // override `LeafSystem` methods, disrespecting `final`-ity. + // This could be changed (see https://stackoverflow.com/a/2425785), but meh, + // we're already abusing Python and C++ enough. + DefineTemplateClassWithDefault< + VectorSystem, PyVectorSystem, LeafSystem>( + m, "VectorSystem", GetPyParam()) + .def(py::init([](int inputs, int outputs) { + return new PyVectorSystem(inputs, outputs); + })); + // TODO(eric.cousineau): Bind virtual methods once we provide a function + // wrapper to convert `Map*` arguments. + // N.B. This could be mitigated by using `EigenPtr` in public interfaces in + // upstream code. } }; } // namespace void DefineFrameworkPySystems(py::module m) { - // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. - using namespace drake::systems; - - // TODO(eric.cousineau): Resolve `str_py` workaround. - auto str_py = py::eval("str"); - - // TODO(eric.cousineau): Show constructor, but somehow make sure `pybind11` - // knows this is abstract? - py::class_, PySystem>(m, "System") - .def("set_name", &System::set_name) - // Topology. - .def("get_input_port", &System::get_input_port, py_reference_internal) - .def("get_output_port", &System::get_output_port, py_reference_internal) - .def( - "_DeclareInputPort", &PySystem::DeclareInputPort, py_reference_internal, - py::arg("type"), py::arg("size"), py::arg("random_type") = nullopt) - // - Feedthrough. - .def("HasAnyDirectFeedthrough", &System::HasAnyDirectFeedthrough) - .def("HasDirectFeedthrough", - overload_cast_explicit(&System::HasDirectFeedthrough), - py::arg("output_port")) - .def("HasDirectFeedthrough", - overload_cast_explicit( - &System::HasDirectFeedthrough), - py::arg("input_port"), py::arg("output_port")) - // Context. - .def("CreateDefaultContext", &System::CreateDefaultContext) - .def("AllocateOutput", &System::AllocateOutput) - .def( - "EvalVectorInput", - [](const System* self, const Context& arg1, int arg2) { - return self->EvalVectorInput(arg1, arg2); - }, py_reference, - // Keep alive, ownership: `return` keeps `Context` alive. - py::keep_alive<0, 2>()) - .def( - "EvalAbstractInput", - [](const System* self, const Context& arg1, int arg2) { - return self->EvalAbstractInput(arg1, arg2); - }, py_reference, - // Keep alive, ownership: `return` keeps `Context` alive. - py::keep_alive<0, 2>()) - .def("CalcOutput", &System::CalcOutput) - // Sugar. - .def( - "GetGraphvizString", - [str_py](const System* self) { - // @note This is a workaround; for some reason, - // casting this using `py::str` does not work, but directly - // calling the Python function (`str_py`) does. - return str_py(self->GetGraphvizString()); - }) - // Events. - .def("Publish", - overload_cast_explicit&>(&System::Publish)); - - // Don't use a const-rvalue as a function handle parameter, as pybind11 wants - // to copy it? - // TODO(eric.cousineau): Make a helper wrapper for this; file a bug in - // pybind11 (since these are arguments). - using CalcVectorPtrCallback = - std::function*, BasicVector*)>; - - py::class_, PyLeafSystem, System>(m, "LeafSystem") - .def(py::init<>()) - .def( - "_DeclareVectorOutputPort", - [](PyLeafSystem* self, const BasicVector& arg1, - CalcVectorPtrCallback arg2) -> auto&& { - typename LeafOutputPort::CalcVectorCallback wrapped = - [arg2](const Context& nest_arg1, BasicVector* nest_arg2) { - return arg2(&nest_arg1, nest_arg2); - }; - return self->DeclareVectorOutputPort(arg1, wrapped); - }, py_reference_internal) - .def("_DeclarePeriodicPublish", &PyLeafSystem::DeclarePeriodicPublish, - py::arg("period_sec"), py::arg("offset_sec") = 0.) - .def("_DoPublish", &LeafSystemPublic::DoPublish) - // System attributes. - .def("_DoHasDirectFeedthrough", - &LeafSystemPublic::DoHasDirectFeedthrough) - // Continuous state. - .def("_DeclareContinuousState", - py::overload_cast(&LeafSystemPublic::DeclareContinuousState), - py::arg("num_state_variables")) - .def("_DeclareContinuousState", - py::overload_cast( - &LeafSystemPublic::DeclareContinuousState), - py::arg("num_q"), py::arg("num_v"), py::arg("num_z")) - .def("_DeclareContinuousState", - py::overload_cast&>( - &LeafSystemPublic::DeclareContinuousState), - py::arg("model_vector")) - // TODO(eric.cousineau): Ideally the downstream class of `BasicVector` - // should expose `num_q`, `num_v`, and `num_z`? - .def("_DeclareContinuousState", - py::overload_cast&, int, int, int>( - &LeafSystemPublic::DeclareContinuousState), - py::arg("model_vector"), - py::arg("num_q"), py::arg("num_v"), py::arg("num_z")) - // Discrete state. - // TODO(eric.cousineau): Should there be a `BasicVector<>` overload? - .def("_DeclareDiscreteState", &LeafSystemPublic::DeclareDiscreteState) - .def("_DeclarePeriodicDiscreteUpdate", - &LeafSystemPublic::DeclarePeriodicDiscreteUpdate, - py::arg("period_sec"), py::arg("offset_sec") = 0.) - .def("_DoCalcDiscreteVariableUpdates", - &LeafSystemPublic::DoCalcDiscreteVariableUpdates) - // Abstract state. - .def("_DeclareAbstractState", - &LeafSystemPublic::DeclareAbstractState, - // Keep alive, ownership: `AbstractValue` keeps `self` alive. - py::keep_alive<2, 1>()); - - py::class_, System>(m, "Diagram") - .def("GetMutableSubsystemState", - [](Diagram* self, const System& arg1, Context* arg2) - -> auto& { - // @note Compiler does not like `py::overload_cast` with this setup? - return self->GetMutableSubsystemState(arg1, arg2); - }, py_reference, - // Keep alive, ownership: `return` keeps `Context` alive. - py::keep_alive<0, 3>()) - .def("GetMutableSubsystemContext", - [](Diagram* self, const System& arg1, Context* arg2) - -> auto&& { - return self->GetMutableSubsystemContext(arg1, arg2); - }, py_reference, - // Keep alive, ownership: `return` keeps `Context` alive. - py::keep_alive<0, 3>()); - - // N.B. This will effectively allow derived classes of `VectorSystem` to - // override `LeafSystem` methods, disrespecting `final`-ity. - // This could be changed (see https://stackoverflow.com/a/2425785), but meh, - // we're already abusing Python and C++ enough. - py::class_, PyVectorSystem, LeafSystem>(m, "VectorSystem") - .def(py::init([](int inputs, int outputs) { - return new PyVectorSystem(inputs, outputs); - })); - // TODO(eric.cousineau): Bind virtual methods once we provide a function - // wrapper to convert `Map*` arguments. - // N.B. This could be mitigated by using `EigenPtr` in public interfaces in - // upstream code. + auto bind_common_scalar_types = [m](auto dummy) { + using T = decltype(dummy); + Impl::DoDefinitions(m); + }; + type_visit(bind_common_scalar_types, pysystems::CommonScalarPack{}); } } // namespace pydrake diff --git a/bindings/pydrake/systems/framework_py_values.cc b/bindings/pydrake/systems/framework_py_values.cc index abbdf1c4fca1..28ef07b8f4aa 100644 --- a/bindings/pydrake/systems/framework_py_values.cc +++ b/bindings/pydrake/systems/framework_py_values.cc @@ -17,11 +17,6 @@ using std::string; namespace drake { namespace pydrake { -// TODO(eric.cousineau): At present, we only bind doubles. -// In the future, we will bind more scalar types, and enable scalar -// conversion. -using T = double; - using pysystems::AddValueInstantiation; using pysystems::DefClone; @@ -29,33 +24,49 @@ void DefineFrameworkPyValues(py::module m) { // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. using namespace drake::systems; - // Value types. - py::class_>(m, "VectorBase") - .def("CopyToVector", &VectorBase::CopyToVector) - .def("SetFromVector", &VectorBase::SetFromVector) - .def("size", &VectorBase::size); - - // TODO(eric.cousineau): Make a helper function for the Eigen::Ref<> patterns. - py::class_, VectorBase> basic_vector(m, "BasicVector"); - DefClone(&basic_vector); - basic_vector - // N.B. Place `init>` `init` so that we do not implicitly - // convert scalar-size `np.array` objects to `int` (since this is normally - // permitted). - .def(py::init>()) - .def(py::init()) - .def("get_value", - [](const BasicVector* self) -> Eigen::Ref> { - return self->get_value(); - }, py_reference_internal) - .def("get_mutable_value", - [](BasicVector* self) -> Eigen::Ref> { - return self->get_mutable_value(); - }, py_reference_internal); - - py::class_, VectorBase>(m, "Supervector"); - - py::class_, VectorBase>(m, "Subvector"); + auto bind_common_scalar_types = [m](auto dummy) { + using T = decltype(dummy); + // Value types. + DefineTemplateClassWithDefault>( + m, "VectorBase", GetPyParam()) + .def("CopyToVector", &VectorBase::CopyToVector) + .def("SetFromVector", &VectorBase::SetFromVector) + .def("size", &VectorBase::size); + + // TODO(eric.cousineau): Make a helper function for the Eigen::Ref<> + // patterns. + auto basic_vector = + DefineTemplateClassWithDefault, VectorBase>( + m, "BasicVector", GetPyParam()); + DefClone(&basic_vector); + basic_vector + // N.B. Place `init>` `init` so that we do not implicitly + // convert scalar-size `np.array` objects to `int` (since this is normally + // permitted). + .def(py::init>()) + .def(py::init()) + .def("get_value", + [](const BasicVector* self) -> Eigen::Ref> { + return self->get_value(); + }, py_reference_internal) + // TODO(eric.cousineau): Remove this once `get_value` is changed, or + // reference semantics are changed for custom dtypes. + .def("_get_value_copy", + [](const BasicVector* self) -> VectorX { + return self->get_value(); + }) + .def("get_mutable_value", + [](BasicVector* self) -> Eigen::Ref> { + return self->get_mutable_value(); + }, py_reference_internal); + + DefineTemplateClassWithDefault, VectorBase>( + m, "Supervector", GetPyParam()); + + DefineTemplateClassWithDefault, VectorBase>( + m, "Subvector", GetPyParam()); + }; + type_visit(bind_common_scalar_types, pysystems::CommonScalarPack{}); // `AddValueInstantiation` will define methods specific to `T` for // `Value`. Since Python is nominally dynamic, these methods are diff --git a/bindings/pydrake/systems/primitives_py.cc b/bindings/pydrake/systems/primitives_py.cc index 10bc627ace5f..90fbcde16829 100644 --- a/bindings/pydrake/systems/primitives_py.cc +++ b/bindings/pydrake/systems/primitives_py.cc @@ -2,6 +2,7 @@ #include "pybind11/pybind11.h" #include "drake/bindings/pydrake/pydrake_pybind.h" +#include "drake/bindings/pydrake/systems/systems_pybind.h" #include "drake/bindings/pydrake/util/drake_optional_pybind.h" #include "drake/systems/primitives/adder.h" #include "drake/systems/primitives/affine_system.h" @@ -28,56 +29,101 @@ PYBIND11_MODULE(primitives, m) { py::module::import("pydrake.systems.framework"); - using T = double; - - py::class_, LeafSystem>(m, "Adder").def(py::init()); - - py::class_, LeafSystem>(m, "AffineSystem") - .def(py::init&, - const Eigen::Ref&, - const Eigen::Ref&, - const Eigen::Ref&, - const Eigen::Ref&, - const Eigen::Ref&, double>(), - py::arg("A"), py::arg("B"), py::arg("f0"), py::arg("C"), - py::arg("D"), py::arg("y0"), py::arg("time_period") = 0.0) - // TODO(eric.cousineau): Fix these to return references instead of copies. - .def("A", - overload_cast_explicit(&AffineSystem::A)) - .def("B", - overload_cast_explicit(&AffineSystem::B)) - .def("f0", - overload_cast_explicit(&AffineSystem::f0)) - .def("C", - overload_cast_explicit(&AffineSystem::C)) - .def("D", - overload_cast_explicit(&AffineSystem::D)) - .def("y0", - overload_cast_explicit(&AffineSystem::y0)) - .def("time_period", &AffineSystem::time_period); - - py::class_, LeafSystem>(m, - "BarycentricMeshSystem") - .def(py::init, - const Eigen::Ref>&>()) - .def("get_mesh", &BarycentricMeshSystem::get_mesh) - .def("get_output_values", &BarycentricMeshSystem::get_output_values); - - py::class_, LeafSystem>(m, "ConstantValueSource"); - - py::class_, LeafSystem>(m, "ConstantVectorSource") - .def(py::init>()); - - py::class_, LeafSystem>(m, "Integrator") - .def(py::init()); - - py::class_, AffineSystem>(m, "LinearSystem") - .def(py::init&, - const Eigen::Ref&, - const Eigen::Ref&, - const Eigen::Ref&, double>(), - py::arg("A"), py::arg("B"), py::arg("C"), py::arg("D"), - py::arg("time_period") = 0.0); + auto bind_common_scalar_types = [m](auto dummy) { + using T = decltype(dummy); + + DefineTemplateClassWithDefault, LeafSystem>( + m, "Adder", GetPyParam()) + .def(py::init()); + + DefineTemplateClassWithDefault, LeafSystem>( + m, "AffineSystem", GetPyParam()) + .def(py::init&, + const Eigen::Ref&, + const Eigen::Ref&, + const Eigen::Ref&, + const Eigen::Ref&, + const Eigen::Ref&, double>(), + py::arg("A"), py::arg("B"), py::arg("f0"), py::arg("C"), + py::arg("D"), py::arg("y0"), py::arg("time_period") = 0.0) + // TODO(eric.cousineau): Fix these to return references instead of + // copies. + .def("A", overload_cast_explicit( + &AffineSystem::A)) + .def("B", overload_cast_explicit( + &AffineSystem::B)) + .def("f0", overload_cast_explicit( + &AffineSystem::f0)) + .def("C", overload_cast_explicit( + &AffineSystem::C)) + .def("D", overload_cast_explicit( + &AffineSystem::D)) + .def("y0", overload_cast_explicit( + &AffineSystem::y0)) + .def("time_period", &AffineSystem::time_period); + + DefineTemplateClassWithDefault, LeafSystem>( + m, "ConstantValueSource", GetPyParam()); + + DefineTemplateClassWithDefault, LeafSystem>( + m, "ConstantVectorSource", GetPyParam()) + .def(py::init>()); + + DefineTemplateClassWithDefault, LeafSystem>( + m, "Integrator", GetPyParam()) + .def(py::init()); + + DefineTemplateClassWithDefault, AffineSystem>( + m, "LinearSystem", GetPyParam()) + .def(py::init&, + const Eigen::Ref&, + const Eigen::Ref&, + const Eigen::Ref&, double>(), + py::arg("A"), py::arg("B"), py::arg("C"), py::arg("D"), + py::arg("time_period") = 0.0); + + DefineTemplateClassWithDefault, LeafSystem>( + m, "Multiplexer", GetPyParam()) + .def(py::init(), py::arg("num_scalar_inputs")) + .def(py::init>(), py::arg("input_sizes")) + .def(py::init&>(), py::arg("model_vector")); + + DefineTemplateClassWithDefault, LeafSystem>( + m, "PassThrough", GetPyParam()) + .def(py::init()) + .def(py::init()); + + DefineTemplateClassWithDefault, LeafSystem>( + m, "Saturation", GetPyParam()) + .def(py::init&, const VectorX&>(), py::arg + ("min_value"), py::arg("max_value")); + + DefineTemplateClassWithDefault, LeafSystem>( + m, "SignalLogger", GetPyParam()) + .def(py::init()) + .def(py::init()) + .def("sample_times", &SignalLogger::sample_times) + .def("data", &SignalLogger::data) + .def("reset", &SignalLogger::reset); + + DefineTemplateClassWithDefault, LeafSystem>( + m, "WrapToSystem", GetPyParam()) + .def(py::init()) + .def("set_interval", &WrapToSystem::set_interval); + + DefineTemplateClassWithDefault, LeafSystem>( + m, "ZeroOrderHold", GetPyParam()) + .def(py::init()); + }; + type_visit(bind_common_scalar_types, pysystems::CommonScalarPack{}); + + py::class_, LeafSystem>( + m, "BarycentricMeshSystem") + .def(py::init, + const Eigen::Ref>&>()) + .def("get_mesh", &BarycentricMeshSystem::get_mesh) + .def("get_output_values", + &BarycentricMeshSystem::get_output_values); m.def("Linearize", &Linearize, py::arg("system"), py::arg("context"), py::arg("input_port_index") = systems::kUseFirstInputIfItExists, @@ -99,33 +145,6 @@ PYBIND11_MODULE(primitives, m) { m.def("IsObservable", &IsObservable, py::arg("sys"), py::arg("threshold") = nullopt); - py::class_, LeafSystem>(m, "PassThrough") - .def(py::init()) - .def(py::init()); - - py::class_, LeafSystem>(m, "Saturation") - .def(py::init&, const VectorX&>(), py::arg - ("min_value"), py::arg("max_value")); - - py::class_, LeafSystem>(m, "SignalLogger") - .def(py::init()) - .def(py::init()) - .def("sample_times", &SignalLogger::sample_times) - .def("data", &SignalLogger::data) - .def("reset", &SignalLogger::reset); - - py::class_, LeafSystem>(m, "WrapToSystem") - .def(py::init()) - .def("set_interval", &WrapToSystem::set_interval); - - py::class_, LeafSystem>(m, "ZeroOrderHold") - .def(py::init()); - - py::class_, LeafSystem>(m, "Multiplexer") - .def(py::init(), py::arg("num_scalar_inputs")) - .def(py::init>(), py::arg("input_sizes")) - .def(py::init&>(), py::arg("model_vector")); - // TODO(eric.cousineau): Add more systems as needed. } diff --git a/bindings/pydrake/systems/systems_pybind.h b/bindings/pydrake/systems/systems_pybind.h index 115c7c830181..9025b7d8522c 100644 --- a/bindings/pydrake/systems/systems_pybind.h +++ b/bindings/pydrake/systems/systems_pybind.h @@ -5,7 +5,9 @@ #include +#include "drake/bindings/pydrake/autodiff_types_pybind.h" #include "drake/bindings/pydrake/pydrake_pybind.h" +#include "drake/bindings/pydrake/symbolic_types_pybind.h" #include "drake/bindings/pydrake/util/cpp_param_pybind.h" #include "drake/bindings/pydrake/util/cpp_template_pybind.h" #include "drake/common/drake_throw.h" @@ -86,6 +88,23 @@ be destroyed when it is replaced, since it is stored using `unique_ptr<>`. return py_class; } +/// Type pack defining common scalar types. +// N.B. This should be kept in sync with the `*_DEFAULT_SCALARS` macro in +// `default_scalars.h`. +using CommonScalarPack = type_pack< + double, + AutoDiffXd, + symbolic::Expression + >; + +/// Type pack for non-symbolic common scalar types. +// N.B. This should be kept in sync with the `*_DEFAULT_NONSYMBOLIC_SCALARS` +// macro in `default_scalars.h`. +using NonSymbolicScalarPack = type_pack< + double, + AutoDiffXd + >; + } // namespace pysystems } // namespace pydrake } // namespace drake diff --git a/bindings/pydrake/systems/test/custom_test.py b/bindings/pydrake/systems/test/custom_test.py index b27d9dca83be..d7fbe9b4a360 100644 --- a/bindings/pydrake/systems/test/custom_test.py +++ b/bindings/pydrake/systems/test/custom_test.py @@ -6,14 +6,16 @@ import unittest import numpy as np +from pydrake.autodiffutils import AutoDiffXd +from pydrake.symbolic import Expression from pydrake.systems.analysis import ( Simulator, ) from pydrake.systems.framework import ( AbstractValue, - BasicVector, + BasicVector, BasicVector_, DiagramBuilder, - LeafSystem, + LeafSystem, LeafSystem_, PortDataType, VectorSystem, ) @@ -50,6 +52,12 @@ def _calc_sum(self, context, sum_data): sum += input_vector.get_value() +# TODO(eric.cousineau): Make this class work with custom scalar types once +# referencing with custom dtypes lands. +# WARNING: At present, dtype=object matrices are NOT well supported, and may +# produce unexecpted results (e.g. references not actually being respected). + + class CustomVectorSystem(VectorSystem): def __init__(self, is_discrete): # VectorSystem only supports pure Continuous or pure Discrete. @@ -272,27 +280,32 @@ def __init__(self): diagram.GetMutableSubsystemContext(system, context) is not None) def test_continuous_state_api(self): - - class TrivialSystem(LeafSystem): - def __init__(self, index): - LeafSystem.__init__(self) - num_q = 2 - num_v = 1 - num_z = 3 - num_state = num_q + num_v + num_z - if index == 0: - self._DeclareContinuousState(num_state_variables=num_state) - elif index == 1: - self._DeclareContinuousState( - num_q=num_q, num_v=num_v, num_z=num_z) - elif index == 2: - self._DeclareContinuousState(BasicVector(num_state)) - elif index == 3: - self._DeclareContinuousState( - BasicVector(num_state), - num_q=num_q, num_v=num_v, num_z=num_z) - - for index in range(4): - system = TrivialSystem(index) - context = system.CreateDefaultContext() - self.assertEquals(context.get_continuous_state_vector().size(), 6) + # N.B. Since this has trivial operations, we can test all scalar types. + for T in [float, AutoDiffXd, Expression]: + + class TrivialSystem(LeafSystem_[T]): + def __init__(self, index): + LeafSystem_[T].__init__(self) + num_q = 2 + num_v = 1 + num_z = 3 + num_state = num_q + num_v + num_z + if index == 0: + self._DeclareContinuousState( + num_state_variables=num_state) + elif index == 1: + self._DeclareContinuousState( + num_q=num_q, num_v=num_v, num_z=num_z) + elif index == 2: + self._DeclareContinuousState( + BasicVector_[T](num_state)) + elif index == 3: + self._DeclareContinuousState( + BasicVector_[T](num_state), + num_q=num_q, num_v=num_v, num_z=num_z) + + for index in range(4): + system = TrivialSystem(index) + context = system.CreateDefaultContext() + self.assertEquals( + context.get_continuous_state_vector().size(), 6) diff --git a/bindings/pydrake/systems/test/general_test.py b/bindings/pydrake/systems/test/general_test.py index 94043bb921e2..e86c2a1db608 100644 --- a/bindings/pydrake/systems/test/general_test.py +++ b/bindings/pydrake/systems/test/general_test.py @@ -7,59 +7,172 @@ import unittest import numpy as np +from pydrake.autodiffutils import ( + AutoDiffXd, + ) +from pydrake.symbolic import ( + Expression, + ) from pydrake.systems.analysis import ( - Simulator, + IntegratorBase_, + Simulator, Simulator_, ) from pydrake.systems.framework import ( - BasicVector, - Diagram, - DiagramBuilder, + BasicVector, BasicVector_, + Context_, + ContinuousState_, + Diagram, Diagram_, + DiagramBuilder, DiagramBuilder_, + DiscreteUpdateEvent_, + DiscreteValues_, + Event_, + InputPortDescriptor_, + LeafContext_, + LeafSystem_, + OutputPort_, + Parameters_, + PublishEvent_, + State_, + Subvector_, + Supervector_, + System_, + SystemOutput_, + VectorBase_, + VectorSystem_, ) +from pydrake.systems import primitives from pydrake.systems.primitives import ( - Adder, + Adder, Adder_, AffineSystem, - ConstantVectorSource, + ConstantVectorSource, ConstantVectorSource_, Integrator, LinearSystem, SignalLogger, ) +# TODO(eric.cousineau): The scope of this test file and and `custom_test.py` +# is poor. Move these tests into `framework_test` and `analysis_test`, and +# ensure that the tests reflect this, even if there is some coupling. -class TestGeneral(unittest.TestCase): - def test_simulator_ctor(self): - # Create simple system. - system = ConstantVectorSource([1]) - - def check_output(context): - # Check number of output ports and value for a given context. - output = system.AllocateOutput(context) - self.assertEquals(output.get_num_ports(), 1) - system.CalcOutput(context, output) - value = output.get_vector_data(0).get_value() - self.assertTrue(np.allclose([1], value)) - # Create simulator with basic constructor. - simulator = Simulator(system) - simulator.Initialize() - simulator.set_target_realtime_rate(0) - simulator.set_publish_every_time_step(True) - self.assertTrue(simulator.get_context() is - simulator.get_mutable_context()) - check_output(simulator.get_context()) - simulator.StepTo(1) - - # Create simulator specifying context. - context = system.CreateDefaultContext() - context.set_time(0.) - - context.set_accuracy(1e-4) - self.assertEquals(context.get_accuracy(), 1e-4) +class TestGeneral(unittest.TestCase): + def _check_instantiations(self, template, supports_symbolic=True): + default_cls = template[None] + self.assertTrue(template[float] is default_cls) + self.assertTrue(template[AutoDiffXd] is not default_cls) + if supports_symbolic: + self.assertTrue(template[Expression] is not default_cls) + + def _compare_system_instances(self, lhs, rhs): + # Compares two different scalar type instantiation instances of a + # system. + self.assertEqual(lhs.get_num_input_ports(), rhs.get_num_input_ports()) + self.assertEqual( + lhs.get_num_output_ports(), rhs.get_num_output_ports()) + for i in range(lhs.get_num_input_ports()): + lhs_port = lhs.get_input_port(i) + rhs_port = rhs.get_input_port(i) + self.assertEqual(lhs_port.size(), rhs_port.size()) + for i in range(lhs.get_num_output_ports()): + lhs_port = lhs.get_output_port(i) + rhs_port = rhs.get_output_port(i) + self.assertEqual(lhs_port.size(), rhs_port.size()) + + def test_instantiations(self): + # Quick check of instantions for given types. + # N.B. These checks are ordered according to their binding definitions + # in the corresponding source file. + # `analysis_py.cc` + self._check_instantiations(IntegratorBase_, False) + self._check_instantiations(Simulator_, False) + # `analysis_py_semantics.cc` + self._check_instantiations(Context_) + self._check_instantiations(LeafContext_) + self._check_instantiations(Event_) + self._check_instantiations(PublishEvent_) + self._check_instantiations(DiscreteUpdateEvent_) + self._check_instantiations(DiagramBuilder_) + self._check_instantiations(OutputPort_) + self._check_instantiations(SystemOutput_) + self._check_instantiations(InputPortDescriptor_) + self._check_instantiations(Parameters_) + self._check_instantiations(State_) + self._check_instantiations(ContinuousState_) + self._check_instantiations(DiscreteValues_) + # `analysis_py_systems.cc` + self._check_instantiations(System_) + self._check_instantiations(LeafSystem_) + self._check_instantiations(Diagram_) + self._check_instantiations(VectorSystem_) + # `analysis_py_values.cc` + self._check_instantiations(VectorBase_) + self._check_instantiations(BasicVector_) + self._check_instantiations(Supervector_) + self._check_instantiations(Subvector_) + + def test_scalar_type_conversion(self): + for T in [float, AutoDiffXd, Expression]: + system = Adder_[T](1, 1) + # N.B. Current scalar conversion does not permit conversion to and + # from the same type. + if T != AutoDiffXd: + methods = [Adder_[T].ToAutoDiffXd, Adder_[T].ToAutoDiffXdMaybe] + for method in methods: + system_ad = method(system) + self.assertIsInstance(system_ad, System_[AutoDiffXd]) + self._compare_system_instances(system, system_ad) + if T != Expression: + methods = [Adder_[T].ToSymbolic, Adder_[T].ToSymbolicMaybe] + for method in methods: + system_sym = method(system) + self.assertIsInstance(system_sym, System_[Expression]) + self._compare_system_instances(system, system_sym) - # @note `simulator` now owns `context`. - simulator = Simulator(system, context) - self.assertTrue(simulator.get_context() is context) - check_output(context) - simulator.StepTo(1) + def test_simulator_ctor(self): + # Tests a simple simulation for supported scalar types. + for T in [float, AutoDiffXd]: + # Create simple system. + system = ConstantVectorSource_[T]([1.]) + + def check_output(context): + # Check number of output ports and value for a given context. + output = system.AllocateOutput(context) + self.assertEquals(output.get_num_ports(), 1) + system.CalcOutput(context, output) + if T == float: + value = output.get_vector_data(0).get_value() + self.assertTrue(np.allclose([1], value)) + elif T == AutoDiffXd: + value = output.get_vector_data(0)._get_value_copy() + # TODO(eric.cousineau): Define `isfinite` ufunc, if + # possible, to use for `np.allclose`. + self.assertEqual(value.shape, (1,)) + self.assertEqual(value[0], AutoDiffXd(1.)) + else: + raise RuntimeError("Bad T: {}".format(T)) + + # Create simulator with basic constructor. + simulator = Simulator_[T](system) + simulator.Initialize() + simulator.set_target_realtime_rate(0) + simulator.set_publish_every_time_step(True) + self.assertTrue(simulator.get_context() is + simulator.get_mutable_context()) + check_output(simulator.get_context()) + simulator.StepTo(1) + + # Create simulator specifying context. + context = system.CreateDefaultContext() + context.set_time(0.) + + context.set_accuracy(1e-4) + self.assertEquals(context.get_accuracy(), 1e-4) + + # @note `simulator` now owns `context`. + simulator = Simulator_[T](system, context) + self.assertTrue(simulator.get_context() is context) + check_output(context) + simulator.StepTo(1) def test_copy(self): # Copy a context using `deepcopy` or `clone`. diff --git a/bindings/pydrake/systems/test/primitives_test.py b/bindings/pydrake/systems/test/primitives_test.py index 217706874ed3..77151d76df14 100644 --- a/bindings/pydrake/systems/test/primitives_test.py +++ b/bindings/pydrake/systems/test/primitives_test.py @@ -1,6 +1,8 @@ import unittest import numpy as np +from pydrake.autodiffutils import AutoDiffXd +from pydrake.symbolic import Expression from pydrake.systems.analysis import Simulator from pydrake.systems.framework import ( AbstractValue, @@ -12,22 +14,24 @@ MyVector2, ) from pydrake.systems.primitives import ( - Adder, - AffineSystem, - ConstantVectorSource, + Adder, Adder_, + AffineSystem, AffineSystem_, + ConstantValueSource_, + ConstantVectorSource, ConstantVectorSource_, ControllabilityMatrix, FirstOrderTaylorApproximation, - Integrator, + Integrator, Integrator_, IsControllable, IsObservable, Linearize, - LinearSystem, - Multiplexer, + LinearSystem, LinearSystem_, + Multiplexer, Multiplexer_, ObservabilityMatrix, - PassThrough, - Saturation, - SignalLogger, - WrapToSystem, + PassThrough, PassThrough_, + Saturation, Saturation_, + SignalLogger, SignalLogger_, + WrapToSystem, WrapToSystem_, + ZeroOrderHold_, ) @@ -41,6 +45,29 @@ def compare_value(test, a, b): class TestGeneral(unittest.TestCase): + def _check_instantiations(self, template, supports_symbolic=True): + default_cls = template[None] + self.assertTrue(template[float] is default_cls) + self.assertTrue(template[AutoDiffXd] is not default_cls) + if supports_symbolic: + self.assertTrue(template[Expression] is not default_cls) + + def test_instantiations(self): + # TODO(eric.cousineau): Refine tests once NumPy functionality is + # resolved for dtype=object, or dtype=custom is used. + self._check_instantiations(Adder_) + self._check_instantiations(AffineSystem_) + self._check_instantiations(ConstantValueSource_) + self._check_instantiations(ConstantVectorSource_) + self._check_instantiations(Integrator_) + self._check_instantiations(LinearSystem_) + self._check_instantiations(Multiplexer_) + self._check_instantiations(PassThrough_) + self._check_instantiations(Saturation_) + self._check_instantiations(SignalLogger_) + self._check_instantiations(WrapToSystem_) + self._check_instantiations(ZeroOrderHold_) + def test_signal_logger(self): # Log the output of a simple diagram containing a constant # source and an integrator. diff --git a/bindings/pydrake/systems/trajectory_optimization_py.cc b/bindings/pydrake/systems/trajectory_optimization_py.cc index b067429ee392..cd617159381d 100644 --- a/bindings/pydrake/systems/trajectory_optimization_py.cc +++ b/bindings/pydrake/systems/trajectory_optimization_py.cc @@ -15,6 +15,8 @@ PYBIND11_MODULE(trajectory_optimization, m) { // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. using namespace drake::systems::trajectory_optimization; + using solvers::VectorXDecisionVariable; + py::module::import("pydrake.symbolic"); py::module::import("pydrake.systems.framework"); py::module::import("pydrake.systems.primitives"); @@ -25,22 +27,35 @@ PYBIND11_MODULE(trajectory_optimization, m) { .def("time", &MultipleShooting::time) .def("timestep", &MultipleShooting::timestep) .def("fixed_timestep", &MultipleShooting::fixed_timestep) + // TODO(eric.cousineau): The original bindings returned references + // instead of copies using VectorXBlock. Restore this once dtype=custom + // is resolved. .def("state", - overload_cast_explicit( - &MultipleShooting::state)) + [](const MultipleShooting& self) -> VectorXDecisionVariable { + return self.state(); + }) .def("state", - overload_cast_explicit< - Eigen::VectorBlock, int>( - &MultipleShooting::state)) - .def("initial_state", &MultipleShooting::initial_state) - .def("final_state", &MultipleShooting::final_state) + [](const MultipleShooting& self, int index) -> + VectorXDecisionVariable { + return self.state(index); + }) + .def("initial_state", + [](const MultipleShooting& self) -> VectorXDecisionVariable { + return self.initial_state(); + }) + .def("final_state", + [](const MultipleShooting& self) -> VectorXDecisionVariable { + return self.final_state(); + }) .def("input", - overload_cast_explicit( - &MultipleShooting::input)) + [](const MultipleShooting& self) -> VectorXDecisionVariable { + return self.input(); + }) .def("input", - overload_cast_explicit< - Eigen::VectorBlock, int>( - &MultipleShooting::input)) + [](const MultipleShooting& self, int index) -> + VectorXDecisionVariable { + return self.input(index); + }) .def("AddRunningCost", [](MultipleShooting& prog, const symbolic::Expression& g) { prog.AddRunningCost(g); diff --git a/bindings/pydrake/test/autodiffutils_test.py b/bindings/pydrake/test/autodiffutils_test.py index 4004a5f2af0a..82ab55fc8a1a 100644 --- a/bindings/pydrake/test/autodiffutils_test.py +++ b/bindings/pydrake/test/autodiffutils_test.py @@ -10,6 +10,10 @@ import pydrake.math as drake_math from pydrake.test.algebra_test_util import ScalarAlgebra, VectorizedAlgebra +from pydrake.test.autodiffutils_test_util import ( + autodiff_scalar_pass_through, + autodiff_vector_pass_through, +) # Use convenience abbreviation. AD = AutoDiffXd @@ -46,6 +50,16 @@ def test_scalar_api(self): self.assertEquals(str(a), "AD{1.0, nderiv=2}") self.assertEquals(repr(a), "") self._check_scalar(a, a) + # Test construction from `float` and `int`. + self._check_scalar(AD(1), AD(1., [])) + self._check_scalar(AD(1.), AD(1., [])) + # Test implicit conversion. + self._check_scalar( + autodiff_scalar_pass_through(1), # int + AD(1., [])) + self._check_scalar( + autodiff_scalar_pass_through(1.), # float + AD(1., [])) def test_array_api(self): a = AD(1, [1., 0]) @@ -68,6 +82,13 @@ def test_array_api(self): self.assertFalse(isinstance(x[0, 0], AD)) x = np.eye(3).astype(AD) self.assertFalse(isinstance(x[0, 0], AD)) + # Test implicit conversion. + self._check_array( + autodiff_vector_pass_through([1, 2]), # int + [AD(1., []), AD(2., [])]) + self._check_array( + autodiff_vector_pass_through([1., 2.]), # float + [AD(1., []), AD(2., [])]) def _check_algebra(self, algebra): a_scalar = AD(1, [1., 0]) diff --git a/bindings/pydrake/test/autodiffutils_test_util_py.cc b/bindings/pydrake/test/autodiffutils_test_util_py.cc new file mode 100644 index 000000000000..6a86d16588e3 --- /dev/null +++ b/bindings/pydrake/test/autodiffutils_test_util_py.cc @@ -0,0 +1,25 @@ +#include "pybind11/eigen.h" +#include "pybind11/pybind11.h" + +#include "drake/bindings/pydrake/autodiff_types_pybind.h" +#include "drake/bindings/pydrake/pydrake_pybind.h" + +namespace drake { +namespace pydrake { + +PYBIND11_MODULE(autodiffutils_test_util, m) { + m.doc() = "Utilities for testing Eigen AutoDiff Scalars"; + + py::module::import("pydrake.autodiffutils"); + + // For testing implicit argument conversion. + m.def("autodiff_scalar_pass_through", [](const AutoDiffXd& value) { + return value; + }); + m.def("autodiff_vector_pass_through", [](const VectorX& value) { + return value; + }); +} + +} // namespace pydrake +} // namespace drake diff --git a/bindings/pydrake/util/BUILD.bazel b/bindings/pydrake/util/BUILD.bazel index 4873fc97c38d..5b911851c18c 100644 --- a/bindings/pydrake/util/BUILD.bazel +++ b/bindings/pydrake/util/BUILD.bazel @@ -261,6 +261,7 @@ drake_pybind_cc_googletest( cc_deps = [ ":cpp_template_pybind", "//common:nice_type_name", + "//common/test_utilities:expect_throws_message", ], py_deps = [":cpp_template_py"], ) diff --git a/bindings/pydrake/util/cpp_template.py b/bindings/pydrake/util/cpp_template.py index b5448ae00e03..0ce1ebc8d757 100644 --- a/bindings/pydrake/util/cpp_template.py +++ b/bindings/pydrake/util/cpp_template.py @@ -164,6 +164,12 @@ def _on_add(self, param, cls): # Update class name for easier debugging. if self._override_meta: cls.__name__ = self._instantiation_name(param) + # Define `__qualname__` in Python2 because that's what `pybind11` + # uses when showing function signatures when an overload cannot be + # found. + # TODO(eric.cousineau): When porting to Python3 / six, try to + # ensure this handles nesting. + cls.__qualname__ = cls.__name__ cls.__module__ = self._module_name diff --git a/bindings/pydrake/util/cpp_template_pybind.h b/bindings/pydrake/util/cpp_template_pybind.h index 38134d474b2f..df1a7d4ec38b 100644 --- a/bindings/pydrake/util/cpp_template_pybind.h +++ b/bindings/pydrake/util/cpp_template_pybind.h @@ -71,6 +71,28 @@ inline py::object AddTemplateClass( return py_template; } +/// Provides a convenience wrapper for defining a template class instantiation +/// and a default instantiation (if not already defined). +/// The default instantiation is named `default_name`, while the template is +/// named `default_name + template_suffix`. +/// @return pybind11 class +template +py::class_ DefineTemplateClassWithDefault( + py::handle scope, const std::string& default_name, py::tuple param, + const std::string& template_suffix = "_") { + const std::string template_name = default_name + template_suffix; + // Define class with temporary name. + py::class_ py_class( + scope, TemporaryClassName().c_str()); + // Register instantiation. + AddTemplateClass(scope, template_name, py_class, param); + // Declare default instantiation if it does not already exist. + if (!py::hasattr(scope, default_name.c_str())) { + scope.attr(default_name.c_str()) = py_class; + } + return py_class; +} + /// Declares a template function. /// @param scope Parent scope of the template. /// @param name Name of the template. diff --git a/bindings/pydrake/util/test/cpp_template_pybind_test.cc b/bindings/pydrake/util/test/cpp_template_pybind_test.cc index f9c8e24bc881..077ac8b3a28f 100644 --- a/bindings/pydrake/util/test/cpp_template_pybind_test.cc +++ b/bindings/pydrake/util/test/cpp_template_pybind_test.cc @@ -13,6 +13,7 @@ #include "pybind11/pybind11.h" #include "drake/common/nice_type_name.h" +#include "drake/common/test_utilities/expect_throws_message.h" using std::string; using std::vector; @@ -56,6 +57,15 @@ GTEST_TEST(CppTemplateTest, TemplateClass) { CheckValue("DefaultInst().GetNames()", expected_1); CheckValue("SimpleTemplate[int]().GetNames()", expected_1); CheckValue("SimpleTemplate[int, float]().GetNames()", expected_2); + + m.def("simple_func", [](const SimpleTemplate&) {}); + + // Check error message if a function is called with the incorrect arguments. + // N.B. We use `[\s\S]` because C++ regex does not have an equivalent of + // Python re's DOTALL flag. + DRAKE_EXPECT_THROWS_MESSAGE( + py::eval("simple_func('incorrect value')"), std::runtime_error, + R"([\s\S]*incompatible function arguments[\s\S]*\(arg0: __main__\.SimpleTemplate\[int\]\)[\s\S]*)"); // NOLINT } template diff --git a/common/default_scalars.h b/common/default_scalars.h index c00b2149793d..59b308a3244d 100644 --- a/common/default_scalars.h +++ b/common/default_scalars.h @@ -34,6 +34,8 @@ /// @endcode /// /// See also @ref system_scalar_conversion. +// N.B. `CommonScalarPack` in `systems_pybind.h` should be kept in sync +// with this. #define DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( \ SomeType) \ DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( \ @@ -45,6 +47,8 @@ template SomeType<::drake::symbolic::Expression>; /// macro should only be used in .cc files, never in .h files. This is /// identical to DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS /// except that it does not define support for any drake::symbolic types. +// N.B. `NonSymbolicScalarPack` in `systems_pybind.h` should be kept in sync +// with this. #define \ DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( \ SomeType) \ diff --git a/doc/python_bindings.rst b/doc/python_bindings.rst index 1a5c709db571..38ee4579d22c 100644 --- a/doc/python_bindings.rst +++ b/doc/python_bindings.rst @@ -42,7 +42,7 @@ MOSEK, without building tests: cmake -DWITH_GUROBI=ON -DWITH_MOSEK=ON ../drake Using the Python Bindings -========================= +------------------------- To use the Drake Python bindings, follow the build steps above or ensure that you have installed Drake appropriately. You will also need to have your @@ -82,11 +82,11 @@ To check this: ``${GUROBI_INCLUDE_DIR}`` via CMake. What's Available from Python -============================ +---------------------------- The most up-to-date demonstrations of what can be done using ``pydrake`` are the ``pydrake`` unit tests themselves. You can see all of them inside the -``drake/bindings/python/pydrake/test`` folder in the Drake source code. +``drake/bindings/python/pydrake/**/test`` folders in the Drake source code. Here's an example snippet of code from ``pydrake``: @@ -138,8 +138,84 @@ explicity refer to each symbol: simulator = pydrake.systems.analysis.Simulator( pydrake.multibody.rigid_body_plant.RigidBodyPlant(tree)) +Documentation +============= + +There is not yet a comprehensive API documentation for the Python bindings +(tracked by `#7914 `_). + +In general, the Python API should be close to the +`C++ API `_. There are some exceptions: + +C++ Template Instantiations in Python +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +When you define a general class template, e.g. +``template class Value``, something like ``Value`` is +called the instantiation. + +For certain C++ templated types, they are exposed in Pythons also as templates; +the parameter types (in this case, ``T``) are the Python-equivalent types to the +C++ type. Some examples: + ++---------------------------------+--------------------------------------+ +| C++ | Python | ++=================================+======================================+ +| ``std::string`` | ``str`` | ++---------------------------------+--------------------------------------+ +| ``double`` | ``float``, ``np.double``, | +| | ``np.float64``, ``ctypes.c_double`` | ++---------------------------------+--------------------------------------+ +| ``drake::AutoDiffXd`` | ``pydrake.autodiffutils.AutoDiffXd`` | ++---------------------------------+--------------------------------------+ +| ``drake::symbolic::Expression`` | ``pydrake.symbolic.Expression`` | ++---------------------------------+--------------------------------------+ + +Thus, the instantiation ``Value`` will be bound in Python as +``Value[str]``. + +Scalar Types +^^^^^^^^^^^^ + +Most classes in the Systems framework and in the multibody dynamics +computational framework are templated on a scalar type, ``T``. +For convenience (and backwards compatibility) in Python, a slightly different +binding convention is used. + +For example, ``Adder`` is a Systems primitive which has a user-defined +number of inputs and outputs a single port which is the sum of all of the +inputs. + +In C++, you would access the instantiations using ``Adder``, +``Adder``, and ``Adder`` for common scalar types. + +In Python, ``Adder`` actually refers to the "default" instantiation, the +``Adder`` C++ class. To access other instantiations, you should add an +``_`` to the end of the C++ class name to get the Python template and then +provide the parameters in square braces, ``[...]``. In this example, you should +use ``Adder_[T]``. + +To illustrate, you can print out the string representations of ``Adder``, +``Adder_``, and some of its instantiations in Python: + +.. code-block:: pycon + + >>> from pydrake.systems.primitives import Adder, Adder_ + >>> print(Adder) + + >>> print(Adder_) + + >>> from pydrake.autodiffutils import AutoDiffXd + >>> from pydrake.symbolic import Expression + >>> print(Adder_[float]) + + >>> print(Adder_[AutoDiffXd]) + + >>> print(Adder_[Expression]) + + For Developers -============== +-------------- If you are developing Python bindings, please see the Doxygen page for `Python Bindings `_. diff --git a/tools/workspace/pybind11/repository.bzl b/tools/workspace/pybind11/repository.bzl index b86c29317bd5..4aba3fad43f4 100644 --- a/tools/workspace/pybind11/repository.bzl +++ b/tools/workspace/pybind11/repository.bzl @@ -5,9 +5,9 @@ load("@drake//tools/workspace:github.bzl", "github_archive") _REPOSITORY = "RobotLocomotion/pybind11" -_COMMIT = "77211dfa6933858bdad00a77867ee4ca4b80cfd0" +_COMMIT = "14c31ad56c48c5afa420602632967c4cfc6563e2" -_SHA256 = "bbc405fbe4225f8a6e86bc6a21372957f11db2953b601d98eb7ef12c9f6473b6" +_SHA256 = "b2baf39084a89c0e21b5573e6e4799fb8f01fe2c5b1d77f0835c005a8ea5eaac" def pybind11_repository( name,