From f075f88bc84cb614b40dce992db682e70ffe2a89 Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Sun, 11 Feb 2018 17:00:54 -0500 Subject: [PATCH] add bindings for barycentric mesh, barycentric mesh system, and value iteration --- bindings/pydrake/BUILD.bazel | 28 ++++++++++- bindings/pydrake/all.py | 1 + bindings/pydrake/examples/BUILD.bazel | 4 +- bindings/pydrake/examples/pendulum_py.cc | 2 + bindings/pydrake/math_py.cc | 38 +++++++++++++++ bindings/pydrake/systems/BUILD.bazel | 30 +++++++++++- bindings/pydrake/systems/controllers_py.cc | 28 +++++++++++ bindings/pydrake/systems/primitives_py.cc | 5 ++ .../pydrake/systems/test/controllers_test.py | 41 ++++++++++++++++ bindings/pydrake/test/math_test.py | 47 +++++++++++++++++++ math/barycentric.h | 1 + 11 files changed, 221 insertions(+), 4 deletions(-) create mode 100644 bindings/pydrake/math_py.cc create mode 100644 bindings/pydrake/systems/controllers_py.cc create mode 100644 bindings/pydrake/systems/test/controllers_test.py create mode 100644 bindings/pydrake/test/math_test.py diff --git a/bindings/pydrake/BUILD.bazel b/bindings/pydrake/BUILD.bazel index 7ef64071bd31..c1c873bde91d 100644 --- a/bindings/pydrake/BUILD.bazel +++ b/bindings/pydrake/BUILD.bazel @@ -81,6 +81,21 @@ drake_py_library( ], ) +drake_pybind_library( + name = "math_py", + cc_deps = [ + "//math:barycentric", + ], + cc_so_name = "math", + cc_srcs = ["math_py.cc"], + package_info = PACKAGE_INFO, + py_deps = [ + ":common_py", + ], + py_srcs = [ + ], +) + # TODO(eric.cousineau): Make private. drake_cc_library( name = "symbolic_types_pybind", @@ -103,6 +118,7 @@ drake_pybind_library( PY_LIBRARIES_WITH_INSTALL = [ ":autodiffutils_py", ":common_py", + ":math_py", ":symbolic_py", "//bindings/pydrake/examples", "//bindings/pydrake/multibody", @@ -132,8 +148,8 @@ drake_py_library( install( name = "install", - targets = PY_LIBRARIES + [":all_py"], py_dest = PACKAGE_INFO.py_dest, + targets = PY_LIBRARIES + [":all_py"], visibility = ["//visibility:public"], deps = get_drake_py_installs(PY_LIBRARIES_WITH_INSTALL), ) @@ -210,6 +226,16 @@ drake_py_test( deps = [":autodiffutils_py"], ) +drake_py_test( + name = "math_test", + size = "small", + srcs = ["test/math_test.py"], + main = "test/math_test.py", + deps = [ + ":math_py", + ], +) + drake_py_test( name = "symbolic_variables_test", size = "small", diff --git a/bindings/pydrake/all.py b/bindings/pydrake/all.py index 1bc4a79f8069..a5b8c3fdbe53 100644 --- a/bindings/pydrake/all.py +++ b/bindings/pydrake/all.py @@ -25,6 +25,7 @@ from .autodiffutils import * from .common import * from .forwarddiff import * +from .math import * from .symbolic import * # Submodules. diff --git a/bindings/pydrake/examples/BUILD.bazel b/bindings/pydrake/examples/BUILD.bazel index bf78e93eaf1b..3937d184e7f0 100644 --- a/bindings/pydrake/examples/BUILD.bazel +++ b/bindings/pydrake/examples/BUILD.bazel @@ -46,6 +46,7 @@ drake_pybind_library( package_info = PACKAGE_INFO, py_deps = [ ":module_py", + "//bindings/pydrake/systems", ], ) @@ -65,8 +66,8 @@ drake_py_library( install( name = "install", - targets = PY_LIBRARIES, py_dest = PACKAGE_INFO.py_dest, + targets = PY_LIBRARIES, deps = get_drake_py_installs(PY_LIBRARIES_WITH_INSTALL), ) @@ -75,7 +76,6 @@ drake_py_test( size = "small", deps = [ ":pendulum_py", - "//bindings/pydrake/systems", ], ) diff --git a/bindings/pydrake/examples/pendulum_py.cc b/bindings/pydrake/examples/pendulum_py.cc index 5909b72c71f8..61b6bc3d1995 100644 --- a/bindings/pydrake/examples/pendulum_py.cc +++ b/bindings/pydrake/examples/pendulum_py.cc @@ -19,6 +19,8 @@ PYBIND11_MODULE(pendulum, m) { m.doc() = "Bindings for the Pendulum example."; + py::module::import("pydrake.systems.framework"); + // TODO(eric.cousineau): At present, we only bind doubles. // In the future, we will bind more scalar types, and enable scalar // conversion. diff --git a/bindings/pydrake/math_py.cc b/bindings/pydrake/math_py.cc new file mode 100644 index 000000000000..8b377e92c36e --- /dev/null +++ b/bindings/pydrake/math_py.cc @@ -0,0 +1,38 @@ +#include +#include +#include +#include + +#include "drake/bindings/pydrake/pydrake_pybind.h" +#include "drake/math/barycentric.h" + +namespace drake { +namespace pydrake { + +PYBIND11_MODULE(math, m) { + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake::math; + + m.doc() = "Bindings for //math."; + + // 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; + + py::class_>(m, "BarycentricMesh") + .def(py::init::MeshGrid>()) + .def("get_input_grid", &BarycentricMesh::get_input_grid) + .def("get_input_size", &BarycentricMesh::get_input_size) + .def("get_num_mesh_points", &BarycentricMesh::get_num_mesh_points) + .def("get_num_interpolants", &BarycentricMesh::get_num_interpolants) + .def("get_mesh_point", overload_cast_explicit,int> + (&BarycentricMesh::get_mesh_point)) + .def("Eval", overload_cast_explicit,const Eigen::Ref>&, + const Eigen::Ref>&>( + &BarycentricMesh::Eval)) + .def("MeshValuesFrom", &BarycentricMesh::MeshValuesFrom); +} + +} // namespace pydrake +} // namespace drake diff --git a/bindings/pydrake/systems/BUILD.bazel b/bindings/pydrake/systems/BUILD.bazel index fda6f4cd8b96..194ff56cdfa1 100644 --- a/bindings/pydrake/systems/BUILD.bazel +++ b/bindings/pydrake/systems/BUILD.bazel @@ -66,6 +66,9 @@ drake_pybind_library( drake_pybind_library( name = "primitives_py", + cc_deps = [ + "//systems/primitives:barycentric_system", + ], cc_so_name = "primitives", cc_srcs = ["primitives_py.cc"], package_info = PACKAGE_INFO, @@ -86,6 +89,22 @@ drake_pybind_library( ], ) +drake_pybind_library( + name = "controllers_py", + cc_deps = [ + "//systems/controllers:dynamic_programming", + ], + cc_so_name = "controllers", + cc_srcs = ["controllers_py.cc"], + package_info = PACKAGE_INFO, + py_deps = [ + ":framework_py", + ":module_py", + ":primitives_py", + "//bindings/pydrake:math_py", + ], +) + drake_pybind_library( name = "sensors_py", cc_deps = [ @@ -151,8 +170,8 @@ drake_py_library( install( name = "install", - targets = PY_LIBRARIES + [":all_py"], py_dest = PACKAGE_INFO.py_dest, + targets = PY_LIBRARIES + [":all_py"], deps = get_drake_py_installs(PY_LIBRARIES_WITH_INSTALL), ) @@ -225,6 +244,15 @@ drake_py_test( ], ) +drake_py_test( + name = "controllers_test", + size = "small", + deps = [ + ":controllers_py", + "//bindings/pydrake/examples:pendulum_py", + ], +) + drake_py_test( name = "sensors_test", size = "small", diff --git a/bindings/pydrake/systems/controllers_py.cc b/bindings/pydrake/systems/controllers_py.cc new file mode 100644 index 000000000000..d5c48e2bcca9 --- /dev/null +++ b/bindings/pydrake/systems/controllers_py.cc @@ -0,0 +1,28 @@ +#include +#include +#include +#include + +#include "drake/bindings/pydrake/pydrake_pybind.h" +#include "drake/systems/controllers/dynamic_programming.h" + +namespace drake { +namespace pydrake { + +PYBIND11_MODULE(controllers, m) { + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake::systems::controllers; + + py::module::import("pydrake.math"); + py::module::import("pydrake.systems.primitives"); + + py::class_(m, "DynamicProgrammingOptions") + .def(py::init<>()) + .def_readwrite("discount_factor", + &DynamicProgrammingOptions::discount_factor); + + m.def("FittedValueIteration", &FittedValueIteration); +} + +} // namespace pydrake +} // namespace drake \ No newline at end of file diff --git a/bindings/pydrake/systems/primitives_py.cc b/bindings/pydrake/systems/primitives_py.cc index 634b6c602465..39056d22081f 100644 --- a/bindings/pydrake/systems/primitives_py.cc +++ b/bindings/pydrake/systems/primitives_py.cc @@ -3,6 +3,7 @@ #include "drake/bindings/pydrake/pydrake_pybind.h" #include "drake/systems/primitives/adder.h" +#include "drake/systems/primitives/barycentric_system.h" #include "drake/systems/primitives/constant_value_source.h" #include "drake/systems/primitives/constant_vector_source.h" #include "drake/systems/primitives/integrator.h" @@ -40,6 +41,10 @@ PYBIND11_MODULE(primitives, m) { .def("sample_times", &SignalLogger::sample_times) .def("data", &SignalLogger::data); + py::class_, LeafSystem>(m, "BarycentricMeshSystem") + .def(py::init,const Eigen::Ref>&>()); + // TODO(eric.cousineau): Add more systems as needed. } diff --git a/bindings/pydrake/systems/test/controllers_test.py b/bindings/pydrake/systems/test/controllers_test.py new file mode 100644 index 000000000000..a10e560e4a8f --- /dev/null +++ b/bindings/pydrake/systems/test/controllers_test.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python + +from __future__ import print_function + +import unittest +import math +import numpy as np + +from pydrake.examples.pendulum import PendulumPlant +from pydrake.systems.analysis import Simulator +from pydrake.math import BarycentricMesh +from pydrake.systems.controllers import ( + DynamicProgrammingOptions, FittedValueIteration) + + +class TestControllers(unittest.TestCase): + def test_fitted_value_iteration_pendulum(self): + plant = PendulumPlant() + simulator = Simulator(plant) + + def quadratic_regulator_cost(context): + print("got here") +# x = context.get_continuous_state_vector().CopyToVector() +# u = plant.EvalVectorInput(context,0) +# print(type(x.dot(x) + u.dot(u))) + return 0 + + state_mesh = [set(np.linspace(0,2*math.pi,51)), + set(np.linspace(-10,10,51))] + input_limit = 2 + input_mesh = [set(np.linspace(-input_limit, input_limit, 9))] + timestep = 0.01 + + options = DynamicProgrammingOptions() + + policy, value_function = FittedValueIteration(simulator, + quadratic_regulator_cost, + state_mesh, input_mesh, timestep, options) + +if __name__ == '__main__': + unittest.main() diff --git a/bindings/pydrake/test/math_test.py b/bindings/pydrake/test/math_test.py new file mode 100644 index 000000000000..17a3f16269f4 --- /dev/null +++ b/bindings/pydrake/test/math_test.py @@ -0,0 +1,47 @@ +from __future__ import absolute_import, division, print_function + +import unittest +from pydrake.math import (BarycentricMesh) +import numpy as np + +class TestBarycentricMesh(unittest.TestCase): + def testSpelling(self): + mesh = BarycentricMesh([{0, 1}, {0, 1}]) + values = np.array([[0, 1, 2, 3]]) + + mesh.get_input_grid() + self.assertTrue(mesh.get_input_size() == 2) + self.assertTrue(mesh.get_num_mesh_points() == 4) + self.assertTrue(mesh.get_num_interpolants() == 3) + self.assertTrue((mesh.get_mesh_point(0) == [0., 0.]).all()) + self.assertTrue(mesh.Eval(values, (0, 1))[0] == 2) + + def testMeshValuesFrom(self): + mesh = BarycentricMesh([{0, 1}, {0, 1}]) + + def mynorm(x): + return [x.dot(x)] + + values = mesh.MeshValuesFrom(mynorm) + self.assertTrue(values.size == 4) + + def testSurf(self): + mesh = BarycentricMesh([{0, 1}, {0, 1}]) + values = np.array([[0, 1, 2, 3]]) + + import matplotlib.pyplot as plt + from mpl_toolkits.mplot3d import Axes3D + fig = plt.figure() + ax = fig.add_subplot(111, projection='3d') + + X, Y = np.meshgrid(list(mesh.get_input_grid()[0]), list(mesh.get_input_grid()[1])) + Z = X + for i in range(0, X.size): + Z.itemset(i, mesh.Eval(values,(X.item(i),Y.item(i)))[0]) + + ax.plot_surface(X,Y,Z) +# plt.show() + + +if __name__ == '__main__': + unittest.main() diff --git a/math/barycentric.h b/math/barycentric.h index 96e6891508cb..1dfcc230422b 100644 --- a/math/barycentric.h +++ b/math/barycentric.h @@ -59,6 +59,7 @@ class BarycentricMesh { explicit BarycentricMesh(MeshGrid input_grid); // Accessor methods. + const MeshGrid& get_input_grid() const { return input_grid_; } int get_input_size() const { return input_grid_.size(); } int get_num_mesh_points() const { int num_mesh_points = 1;