add bindings for barycentric mesh, barycentric mesh system, and value…

… iteration

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",

bindings/pydrake/all.py

@@ -25,6 +25,7 @@
 from .autodiffutils import *
 from .common import *
 from .forwarddiff import *
+from .math import *
 from .symbolic import *
 
 # Submodules.

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",
     ],
 )
 

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.

bindings/pydrake/math_py.cc

@@ -0,0 +1,38 @@
+#include <pybind11/eigen.h>
+#include <pybind11/functional.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#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_<BarycentricMesh<T>>(m, "BarycentricMesh")
+      .def(py::init<BarycentricMesh<T>::MeshGrid>())
+      .def("get_input_grid", &BarycentricMesh<T>::get_input_grid)
+      .def("get_input_size", &BarycentricMesh<T>::get_input_size)
+      .def("get_num_mesh_points", &BarycentricMesh<T>::get_num_mesh_points)
+      .def("get_num_interpolants", &BarycentricMesh<T>::get_num_interpolants)
+      .def("get_mesh_point", overload_cast_explicit<VectorX<T>,int>
+          (&BarycentricMesh<T>::get_mesh_point))
+      .def("Eval", overload_cast_explicit<VectorX<T>,const Eigen::Ref<const MatrixX<T>>&,
+          const Eigen::Ref<const VectorX<T>>&>(
+          &BarycentricMesh<T>::Eval))
+      .def("MeshValuesFrom", &BarycentricMesh<T>::MeshValuesFrom);
+}
+
+}  // namespace pydrake
+}  // namespace drake

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",

bindings/pydrake/systems/controllers_py.cc

@@ -0,0 +1,28 @@
+#include <pybind11/eigen.h>
+#include <pybind11/functional.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#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_<DynamicProgrammingOptions>(m, "DynamicProgrammingOptions")
+      .def(py::init<>())
+      .def_readwrite("discount_factor",
+                    &DynamicProgrammingOptions::discount_factor);
+
+  m.def("FittedValueIteration", &FittedValueIteration);
+}
+
+}  // namespace pydrake
+}  // namespace drake

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<T>::sample_times)
     .def("data", &SignalLogger<T>::data);
 
+  py::class_<BarycentricMeshSystem<T>, LeafSystem<T>>(m, "BarycentricMeshSystem")
+    .def(py::init<math::BarycentricMesh<T>,const Eigen::Ref<const
+    MatrixX<T>>&>());
+
   // TODO(eric.cousineau): Add more systems as needed.
 }
 

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()

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()

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;