From ec458cf83e93e3a89ada24eba125cf2253d8d152 Mon Sep 17 00:00:00 2001 From: Eric Cousineau Date: Tue, 12 Dec 2017 20:19:31 -0500 Subject: [PATCH] Add preliminary binding example for systems framework. --- WORKSPACE | 7 +- bindings/pydrake/systems/BUILD.bazel | 115 +++++++++++++ bindings/pydrake/systems/__init__.py | 23 +++ bindings/pydrake/systems/analysis_py.cc | 25 +++ bindings/pydrake/systems/drawing.py | 33 ++++ bindings/pydrake/systems/framework_py.cc | 151 ++++++++++++++++++ bindings/pydrake/systems/primitives_py.cc | 31 ++++ .../systems/test/system_general_test.py | 114 +++++++++++++ .../systems/test/system_graphviz_example.py | 24 +++ 9 files changed, 520 insertions(+), 3 deletions(-) create mode 100644 bindings/pydrake/systems/BUILD.bazel create mode 100644 bindings/pydrake/systems/__init__.py create mode 100644 bindings/pydrake/systems/analysis_py.cc create mode 100644 bindings/pydrake/systems/drawing.py create mode 100644 bindings/pydrake/systems/framework_py.cc create mode 100644 bindings/pydrake/systems/primitives_py.cc create mode 100644 bindings/pydrake/systems/test/system_general_test.py create mode 100644 bindings/pydrake/systems/test/system_graphviz_example.py diff --git a/WORKSPACE b/WORKSPACE index b4064e5edeaf..ed44c1a517c1 100644 --- a/WORKSPACE +++ b/WORKSPACE @@ -237,9 +237,10 @@ github_archive( github_archive( name = "pybind11", - repository = "RobotLocomotion/pybind11", - commit = "ffcf754ae9e766632610975d22372a86a7b63014", - sha256 = "7cd6f4efb02bf9ae17eeb2afba68023af913e61ae76e8b4254203d0eec019525", # noqa + # TODO(eric.cousineau): Merge this with RobotLocomotion. + repository = "EricCousineau-TRI/pybind11", + commit = "b7e92676552dc73613c8e18624091fa9ffd4fa54", + sha256 = "464d35d81c680ea251f3c09d6f1f2fef34e9696c49865a6cea193d6a0de9caf9", # noqa build_file = "tools/workspace/pybind11/pybind11.BUILD.bazel", ) diff --git a/bindings/pydrake/systems/BUILD.bazel b/bindings/pydrake/systems/BUILD.bazel new file mode 100644 index 000000000000..c79fabca0c39 --- /dev/null +++ b/bindings/pydrake/systems/BUILD.bazel @@ -0,0 +1,115 @@ +# -*- python -*- + +load("@drake//tools/install:install.bzl", "install") +load("//tools/lint:lint.bzl", "add_lint_tests") +load( + "//tools/skylark:pybind.bzl", + "drake_pybind_library", + "get_drake_pybind_installs", + "get_pybind_library_dest", +) +load( + "//tools/skylark:drake_py.bzl", + "drake_py_binary", + "drake_py_library", + "drake_py_test", +) +load("//tools/skylark:6996.bzl", "adjust_labels_for_drake_hoist") + +package(default_visibility = adjust_labels_for_drake_hoist([ + "//drake/bindings/pydrake:__subpackages__", +])) + +drake_py_library( + name = "module_py", + srcs = ["__init__.py"], + deps = [ + "//drake/bindings/pydrake:common_py", + # TODO(eric.cousineau): This does NOT include direct dependencies to + # submodule. This is to preserve C++-like namespaces. + ], +) + +drake_pybind_library( + name = "framework_py", + cc_so_name = "framework", + cc_srcs = ["framework_py.cc"], + py_deps = [ + ":module_py", + ], +) + +drake_pybind_library( + name = "primitives_py", + cc_so_name = "primitives", + cc_srcs = ["primitives_py.cc"], + py_deps = [ + ":framework_py", + ":module_py", + ], +) + +drake_pybind_library( + name = "analysis_py", + cc_so_name = "analysis", + cc_srcs = ["analysis_py.cc"], + py_deps = [ + ":framework_py", + ":module_py", + ], +) + +drake_py_library( + name = "drawing_py", + srcs = ["drawing.py"], + deps = [":module_py"], + # TODO(eric.cousineau): Expose information to allow `imports = ...` to be + # defined, rather than rely on `module_py`. +) + +PYBIND_LIBRARIES = [ + ":analysis_py", + ":framework_py", + ":primitives_py", +] + +PY_LIBRARIES = [ + ":drawing_py", + ":module_py", +] + +drake_py_library( + name = "systems", + deps = PYBIND_LIBRARIES + PY_LIBRARIES, +) + +install( + name = "install", + targets = PY_LIBRARIES, + py_dest = get_pybind_library_dest(), + deps = get_drake_pybind_installs(PYBIND_LIBRARIES), +) + +drake_py_test( + name = "system_general_test", + size = "small", + srcs = ["test/system_general_test.py"], + deps = [ + ":analysis_py", + ":framework_py", + ":primitives_py", + ], +) + +drake_py_binary( + name = "system_graphviz_example", + srcs = ["test/system_graphviz_example.py"], + deps = [ + ":analysis_py", + ":drawing_py", + ":framework_py", + ":primitives_py", + ], +) + +add_lint_tests() diff --git a/bindings/pydrake/systems/__init__.py b/bindings/pydrake/systems/__init__.py new file mode 100644 index 000000000000..60616b735242 --- /dev/null +++ b/bindings/pydrake/systems/__init__.py @@ -0,0 +1,23 @@ +from __future__ import absolute_import, print_function + +# TODO(eric.cousineau): Should we attempt to modularize this dependency graph? +# How about separating this setup (development) from a more pure approach +# (deployment)? + +try: + from .analysis import * +except ImportError as e: + print(e) + pass + +try: + from .framework import * +except ImportError as e: + print(e) + pass + +try: + from .primitives import * +except ImportError as e: + print(e) + pass diff --git a/bindings/pydrake/systems/analysis_py.cc b/bindings/pydrake/systems/analysis_py.cc new file mode 100644 index 000000000000..9064d9606449 --- /dev/null +++ b/bindings/pydrake/systems/analysis_py.cc @@ -0,0 +1,25 @@ +#include + +#include "drake/systems/analysis/simulator.h" + +namespace py = pybind11; + +using std::unique_ptr; + +PYBIND11_MODULE(analysis, m) { + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake::systems; + + auto py_iref = py::return_value_policy::reference_internal; + + m.doc() = "Bindings for the analysis portion of the Systems framework."; + + using T = double; + + py::class_>(m, "Simulator") + .def(py::init&>()) + .def(py::init&, unique_ptr>>()) + .def("Initialize", &Simulator::Initialize) + .def("StepTo", &Simulator::StepTo) + .def("get_mutable_context", &Simulator::get_mutable_context, py_iref); +} diff --git a/bindings/pydrake/systems/drawing.py b/bindings/pydrake/systems/drawing.py new file mode 100644 index 000000000000..87423ea6fcc8 --- /dev/null +++ b/bindings/pydrake/systems/drawing.py @@ -0,0 +1,33 @@ +# @file +# Provides general visualization utilities. This is NOT related to `rendering`. +# @note This is an optional module, dependent on `pydot` and `matplotlib` being +# installed. + +from StringIO import StringIO + +import matplotlib.image as mpimg +import matplotlib.pyplot as plt +import pydot + + +# TODO(eric.cousineau): Move `plot_graphviz` to something more accessible to +# `call_python_client`. + + +def plot_graphviz(dot_text): + """Renders a DOT graph in matplotlib.""" + # @ref https://stackoverflow.com/a/18522941/7829525 + # Tried (reason ignored): pydotplus (`pydot` works), networkx + # (`read_dot` does not work robustly?), pygraphviz (coupled with + # `networkx`). + g = pydot.graph_from_dot_data(dot_text) + s = StringIO() + g.write_png(s) + s.seek(0) + plt.axis('off') + return plt.imshow(plt.imread(s), aspect="equal") + + +def plot_system_graphviz(system): + """Renders a System's Graphviz representation in `matplotlib`. """ + return plot_graphviz(system.GetGraphvizString()) diff --git a/bindings/pydrake/systems/framework_py.cc b/bindings/pydrake/systems/framework_py.cc new file mode 100644 index 000000000000..06eae693b5d7 --- /dev/null +++ b/bindings/pydrake/systems/framework_py.cc @@ -0,0 +1,151 @@ +#include +#include +#include +#include + +#include "drake/common/nice_type_name.h" +#include "drake/systems/framework/abstract_values.h" +#include "drake/systems/framework/basic_vector.h" +#include "drake/systems/framework/context.h" +#include "drake/systems/framework/diagram.h" +#include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/framework/leaf_context.h" +#include "drake/systems/framework/leaf_system.h" +#include "drake/systems/framework/output_port_value.h" +#include "drake/systems/framework/subvector.h" +#include "drake/systems/framework/supervector.h" +#include "drake/systems/framework/system.h" + +namespace py = pybind11; + +using std::make_unique; +using std::unique_ptr; +using std::vector; + +PYBIND11_MODULE(framework, m) { + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake; + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake::systems; + + // Aliases for commonly used return value policies. + // `py_ref` is used when `keep_alive` is explicitly used (e.g. for extraction + // methods, like `GetMutableSubsystemState`). + auto py_ref = py::return_value_policy::reference; + // `py_iref` is used when pointers / lvalue references are returned (no need + // for `keep_alive`, as it is implicit. + auto py_iref = py::return_value_policy::reference_internal; + + m.doc() = "Bindings for the core Systems framework."; + + // 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; + + // 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_>(m, "System") + .def("set_name", &System::set_name) + .def("get_input_port", &System::get_input_port, py_iref) + .def("get_output_port", &System::get_output_port, py_iref) + .def("CreateDefaultContext", &System::CreateDefaultContext) + .def("AllocateOutput", &System::AllocateOutput) + .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()); + }); + + py::class_, System>(m, "LeafSystem"); + + py::class_>(m, "Context") + .def("FixInputPort", + py::overload_cast>>( + &Context::FixInputPort)) + .def("get_time", &Context::get_time) + .def("Clone", &Context::Clone) + .def("__copy__", &Context::Clone) + .def("get_state", &Context::get_state, py_iref) + .def("get_mutable_state", &Context::get_mutable_state, py_iref); + + py::class_, Context>(m, "LeafContext"); + + py::class_, System>(m, "Diagram") + .def("GetMutableSubsystemState", + [](Diagram* self, const System& arg1, Context* arg2) + -> auto&& { + // @note Use `auto&&` to get perfect forwarding. + // @note Compiler does not like `py::overload_cast` with this setup? + return self->GetMutableSubsystemState(arg1, arg2); + }, py_ref, py::keep_alive<0, 3>()); + + // Glue mechanisms. + py::class_>(m, "DiagramBuilder") + .def(py::init<>()) + .def( + "AddSystem", + [](DiagramBuilder* self, unique_ptr> arg1) { + return self->AddSystem(std::move(arg1)); + }) + .def("Connect", + py::overload_cast&, const InputPortDescriptor&>( + &DiagramBuilder::Connect)) + .def("ExportInput", &DiagramBuilder::ExportInput) + .def("ExportOutput", &DiagramBuilder::ExportOutput) + .def("Build", &DiagramBuilder::Build) + .def("BuildInto", &DiagramBuilder::BuildInto); + + py::class_>(m, "OutputPort"); + py::class_>(m, "SystemOutput"); + + py::class_>(m, "InputPortDescriptor"); + + // Value types. + py::class_>(m, "VectorBase") + .def("CopyToVector", &VectorBase::CopyToVector) + .def("SetFromVector", &VectorBase::SetFromVector); + + py::class_, VectorBase>(m, "BasicVector") + .def(py::init>()) + .def("get_value", &BasicVector::get_value); + + py::class_, VectorBase>(m, "Supervector"); + + py::class_, VectorBase>(m, "Subvector"); + + // TODO(eric.cousineau): Interfacing with the C++ abstract value types may be + // a tad challenging. This should be more straightforward once + // scalar-type conversion is supported, as the template-exposure mechanisms + // should be relatively similar. + py::class_(m, "AbstractValue"); + + // Parameters. + // TODO(eric.cousineau): Fill this out. + py::class_>(m, "Parameters"); + + // State. + py::class_>(m, "State") + .def(py::init<>()) + .def("get_continuous_state", + &State::get_continuous_state, py_iref) + .def("get_mutable_continuous_state", + &State::get_mutable_continuous_state, py_iref); + + // - Constituents. + py::class_>(m, "ContinuousState") + .def(py::init<>()) + .def("get_vector", &ContinuousState::get_vector, py_iref) + .def("get_mutable_vector", + &ContinuousState::get_mutable_vector, py_iref); + + py::class_>(m, "DiscreteValues"); + + py::class_(m, "AbstractValues"); +} diff --git a/bindings/pydrake/systems/primitives_py.cc b/bindings/pydrake/systems/primitives_py.cc new file mode 100644 index 000000000000..6fc884d50009 --- /dev/null +++ b/bindings/pydrake/systems/primitives_py.cc @@ -0,0 +1,31 @@ +#include +#include + +#include "drake/systems/primitives/adder.h" +#include "drake/systems/primitives/constant_value_source.h" +#include "drake/systems/primitives/constant_vector_source.h" +#include "drake/systems/primitives/integrator.h" + +namespace py = pybind11; + +PYBIND11_MODULE(primitives, m) { + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake; + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake::systems; + + m.doc() = "Bindings for the primitives portion of the Systems framework."; + + using T = double; + + py::class_, LeafSystem>(m, "ConstantVectorSource") + .def(py::init>()); + + py::class_, LeafSystem>(m, "ConstantValueSource"); + + py::class_, LeafSystem>(m, "Adder") + .def(py::init()); + + py::class_, LeafSystem>(m, "Integrator") + .def(py::init()); +} diff --git a/bindings/pydrake/systems/test/system_general_test.py b/bindings/pydrake/systems/test/system_general_test.py new file mode 100644 index 000000000000..2709f653dd45 --- /dev/null +++ b/bindings/pydrake/systems/test/system_general_test.py @@ -0,0 +1,114 @@ +#!/usr/bin/env python +# -*- coding: utf8 -*- + +from __future__ import print_function + +import unittest +import numpy as np + +from pydrake.systems import ( + # framework + BasicVector, + Diagram, + DiagramBuilder, + # WitnessFunctionDirection + # primitives + Adder, + ConstantVectorSource, + Integrator, + # analysis + Simulator, + ) + + +class TestSystemGeneral(unittest.TestCase): + def test_simulator_ctor(self): + # Create simple system. + system = ConstantVectorSource([1]) + + # Create simulator with basic constructor. + simulator = Simulator(system) + simulator.Initialize() + simulator.StepTo(1) + + # Create simulator specifying context. + context = system.CreateDefaultContext() + simulator = Simulator(system, context) + simulator.StepTo(1) + + def test_basic_diagram(self): + # Similar to: //systems/framework:diagram_test, ExampleDiagram + size = 3 + + builder = DiagramBuilder() + adder0 = builder.AddSystem(Adder(2, size)) + adder0.set_name("adder0") + adder1 = builder.AddSystem(Adder(2, size)) + adder1.set_name("adder1") + + integrator = builder.AddSystem(Integrator(size)) + integrator.set_name("integrator") + + builder.Connect(adder0.get_output_port(0), adder1.get_input_port(0)) + builder.Connect(adder1.get_output_port(0), + integrator.get_input_port(0)) + + builder.ExportInput(adder0.get_input_port(0)) + builder.ExportInput(adder0.get_input_port(1)) + builder.ExportInput(adder1.get_input_port(1)) + builder.ExportOutput(integrator.get_output_port(0)) + + diagram = builder.Build() + # TODO(eric.cousineau): Figure out simple unicode handling, if + # necessary. (Restore the snowman.) + diagram.set_name("test_diagram") + + simulator = Simulator(diagram) + context = simulator.get_mutable_context() + + # Create and attach inputs. + # TODO(eric.cousineau): Not seeing any assertions being printed if no + # inputs are connected. Need to check this behavior. + input0 = BasicVector(np.array([0.1, 0.2, 0.3])) + context.FixInputPort(0, input0) + input1 = BasicVector([0.02, 0.03, 0.04]) + context.FixInputPort(1, input1) + input2 = BasicVector([0.003, 0.004, 0.005]) + context.FixInputPort(2, input2) + + # Initialize integrator states. + def get_mutable_continuous_state(system): + return (diagram.GetMutableSubsystemState(system, context) + .get_mutable_continuous_state()) + + integrator_xc = get_mutable_continuous_state(integrator) + integrator_xc.get_mutable_vector().SetFromVector([0, 1, 2]) + + simulator.Initialize() + + # Simulate briefly, and take full-context snapshots at intermediate + # points. + n = 6 + times = np.linspace(0, 1, n) + context_log = [] + for t in times: + simulator.StepTo(t) + # Record snapshot of *entire* context. + context_log.append(context.Clone()) + + xc_initial = np.array([0, 1, 2]) + xc_final = np.array([0.123, 1.234, 2.345]) + + for i, context_i in enumerate(context_log): + t = times[i] + self.assertEqual(context_i.get_time(), t) + xc = (context_i.get_state().get_continuous_state() + .get_vector().CopyToVector()) + xc_expected = (float(i) / (n - 1) * (xc_final - xc_initial) + + xc_initial) + print("xc[t = {}] = {}".format(t, xc)) + self.assertTrue(np.allclose(xc, xc_expected)) + + +if __name__ == '__main__': + unittest.main() diff --git a/bindings/pydrake/systems/test/system_graphviz_example.py b/bindings/pydrake/systems/test/system_graphviz_example.py new file mode 100644 index 000000000000..f0fbeac24b0b --- /dev/null +++ b/bindings/pydrake/systems/test/system_graphviz_example.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python + +import matplotlib.pyplot as plt + +from pydrake.systems import DiagramBuilder, Adder +from pydrake.systems.drawing import plot_system_graphviz + +builder = DiagramBuilder() +size = 1 +adders = [ + builder.AddSystem(Adder(1, size)), + builder.AddSystem(Adder(1, size)), +] +for i, adder in enumerate(adders): + adder.set_name("adders[{}]".format(i)) +builder.Connect(adders[0].get_output_port(0), adders[1].get_input_port(0)) +builder.ExportInput(adders[0].get_input_port(0)) +builder.ExportOutput(adders[1].get_output_port(0)) + +diagram = builder.Build() +diagram.set_name("system_graphviz_example") + +plot_system_graphviz(diagram) +plt.show()