pydrake systems: Update general and custom system uses for custom dtypes

bindings/pydrake/systems/BUILD.bazel

@@ -282,6 +282,7 @@ drake_py_unittest(
         ":analysis_py",
         ":framework_py",
         ":primitives_py",
+        ":scalar_conversion_py",
         ":test_util_py",
     ],
 )

bindings/pydrake/systems/framework_py_systems.cc

@@ -139,6 +139,9 @@ struct Impl {
     VectorSystemPublic(int inputs, int outputs)
         : Base(inputs, outputs) {}
 
+    VectorSystemPublic(SystemScalarConverter converter, int inputs, int outputs)
+        : Base(std::move(converter), inputs, outputs) {}
+
     using Base::EvalVectorInput;
     using Base::GetVectorState;
 
@@ -385,7 +388,11 @@ struct Impl {
         m, "VectorSystem", GetPyParam<T>())
         .def(py::init([](int inputs, int outputs) {
           return new PyVectorSystem(inputs, outputs);
-        }));
+        }), py::arg("inputs"), py::arg("outputs"))
+        .def(py::init(
+            [](int inputs, int outputs, SystemScalarConverter converter) {
+              return new PyVectorSystem(std::move(converter), inputs, outputs);
+            }), py::arg("inputs"), py::arg("outputs"), py::arg("converter"));
     // TODO(eric.cousineau): Bind virtual methods once we provide a function
     // wrapper to convert `Map<Derived>*` arguments.
     // N.B. This could be mitigated by using `EigenPtr` in public interfaces in

bindings/pydrake/systems/test/custom_test.py

@@ -9,18 +9,19 @@
 from pydrake.autodiffutils import AutoDiffXd
 from pydrake.symbolic import Expression
 from pydrake.systems.analysis import (
-    Simulator,
+    Simulator, Simulator_,
     )
 from pydrake.systems.framework import (
     AbstractValue,
     BasicVector, BasicVector_,
-    DiagramBuilder,
+    DiagramBuilder, DiagramBuilder_,
     LeafSystem, LeafSystem_,
     PortDataType,
-    VectorSystem,
+    VectorSystem, VectorSystem_,
     )
+from pydrake.systems.scalar_conversion import TemplateSystem
 from pydrake.systems.primitives import (
-    ZeroOrderHold,
+    ZeroOrderHold, ZeroOrderHold_,
     )
 
 from pydrake.systems.test.test_util import (
@@ -34,110 +35,142 @@ def noop(*args, **kwargs):
     pass
 
 
-class CustomAdder(LeafSystem):
-    # Reimplements `Adder`.
-    def __init__(self, num_inputs, size):
-        LeafSystem.__init__(self)
-        for i in xrange(num_inputs):
-            self._DeclareInputPort(PortDataType.kVectorValued, size)
-        self._DeclareVectorOutputPort(BasicVector(size), self._calc_sum)
-
-    def _calc_sum(self, context, sum_data):
-        # @note This will NOT work if the scalar type is AutoDiff or symbolic,
-        # since they are not stored densely.
-        sum = sum_data.get_mutable_value()
-        sum[:] = 0
-        for i in xrange(context.get_num_input_ports()):
-            input_vector = self.EvalVectorInput(context, i)
-            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.
-        # Dimensions:
-        #   1 Input, 2 States, 3 Outputs.
-        VectorSystem.__init__(self, 1, 3)
-        self._is_discrete = is_discrete
-        if self._is_discrete:
-            self._DeclareDiscreteState(2)
-        else:
-            self._DeclareContinuousState(2)
-        # Record calls for testing.
-        self.has_called = []
+@TemplateSystem.define("CustomAdder_")
+def CustomAdder_(T):
+
+    class Impl(LeafSystem_[T]):
+        # Reimplements `Adder`.
+        def _construct(self, num_inputs, size, converter=None):
+            LeafSystem_[T].__init__(self, converter=converter)
+            self._num_inputs = num_inputs
+            self._size = size
+            for i in xrange(num_inputs):
+                self._DeclareInputPort(PortDataType.kVectorValued, size)
+            self._DeclareVectorOutputPort(
+                BasicVector_[T](size), self._calc_sum)
+
+        def _construct_copy(self, other, converter=None):
+            Impl._construct(
+                self, other._num_inputs, other._size, converter=converter)
+
+        def _calc_sum(self, context, sum_data):
+            sum = sum_data.get_mutable_value()
+            sum[:] = 0.
+            for i in xrange(context.get_num_input_ports()):
+                input_vector = self.EvalVectorInput(context, i)
+                sum += input_vector.get_value()
+
+    return Impl
+
+
+@TemplateSystem.define("CustomVectorSystem_")
+def CustomVectorSystem_(T):
+
+    class Impl(VectorSystem_[T]):
+        def _construct(self, is_discrete, converter=None):
+            # VectorSystem only supports pure Continuous or pure Discrete.
+            # Dimensions:
+            #   1 Input, 2 States, 3 Outputs.
+            VectorSystem_[T].__init__(self, 1, 3, converter=converter)
+            self._is_discrete = is_discrete
+            if self._is_discrete:
+                self._DeclareDiscreteState(2)
+            else:
+                self._DeclareContinuousState(2)
+            # Record calls for testing.
+            self.has_called = []
+
+        def _construct_copy(self, other, converter=None):
+            Impl._construct(self, other._is_discrete, converter=converter)
+
+        def _DoCalcVectorOutput(self, context, u, x, y):
+            y[:] = np.hstack([u, x])
+            self.has_called.append("output")
+
+        def _DoCalcVectorTimeDerivatives(self, context, u, x, x_dot):
+            x_dot[:] = x + u
+            self.has_called.append("continuous")
 
-    def _DoCalcVectorOutput(self, context, u, x, y):
-        y[:] = np.hstack([u, x])
-        self.has_called.append("output")
+        def _DoCalcVectorDiscreteVariableUpdates(self, context, u, x, x_n):
+            x_n[:] = x + 2*u
+            self.has_called.append("discrete")
 
-    def _DoCalcVectorTimeDerivatives(self, context, u, x, x_dot):
-        x_dot[:] = x + u
-        self.has_called.append("continuous")
+        def _DoHasDirectFeedthrough(self, input_port, output_port):
+            self.has_called.append("feedthrough")
+            return True
 
-    def _DoCalcVectorDiscreteVariableUpdates(self, context, u, x, x_n):
-        x_n[:] = x + 2*u
-        self.has_called.append("discrete")
+    return Impl
 
-    def _DoHasDirectFeedthrough(self, input_port, output_port):
-        self.has_called.append("feedthrough")
-        return True
+
+# Default instantiations.
+CustomAdder = CustomAdder_[None]
+CustomVectorSystem = CustomVectorSystem_[None]
 
 
 class TestCustom(unittest.TestCase):
-    def _create_adder_system(self):
-        system = CustomAdder(2, 3)
+    def assertArrayEqual(self, lhs, rhs):
+        # TODO(eric.cousineau): Place in `pydrake.test.unittest_mixins`.
+        lhs, rhs = np.array(lhs), np.array(rhs)
+        if lhs.dtype == Expression or rhs.dtype == Expression:
+            lhs, rhs = lhs.astype(Expression), rhs.astype(Expression)
+            self.assertTrue(Expression.equal_to(lhs, rhs).all())
+        else:
+            self.assertTrue(np.allclose(lhs, rhs))
+
+    def _create_adder_system(self, T):
+        system = CustomAdder_[T](2, 3)
         return system
 
-    def _fix_adder_inputs(self, context):
+    def _fix_adder_inputs(self, context, T):
         self.assertEquals(context.get_num_input_ports(), 2)
-        context.FixInputPort(0, BasicVector([1, 2, 3]))
-        context.FixInputPort(1, BasicVector([4, 5, 6]))
+        context.FixInputPort(0, BasicVector_[T]([1, 2, 3]))
+        context.FixInputPort(1, BasicVector_[T]([4, 5, 6]))
 
     def test_adder_execution(self):
-        system = self._create_adder_system()
-        context = system.CreateDefaultContext()
-        self._fix_adder_inputs(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([5, 7, 9], value))
+        for T in (float, AutoDiffXd, Expression):
+            system = self._create_adder_system(T)
+            context = system.CreateDefaultContext()
+            self._fix_adder_inputs(context, T)
+            output = system.AllocateOutput(context)
+            self.assertEquals(output.get_num_ports(), 1)
+            system.CalcOutput(context, output)
+            value = output.get_vector_data(0).get_value()
+            value_expected = np.array([5, 7, 9])
+            self.assertArrayEqual(value_expected, value)
 
     def test_adder_simulation(self):
-        builder = DiagramBuilder()
-        adder = builder.AddSystem(self._create_adder_system())
-        adder.set_name("custom_adder")
-        # Add ZOH so we can easily extract state.
-        zoh = builder.AddSystem(ZeroOrderHold(0.1, 3))
-        zoh.set_name("zoh")
-
-        builder.ExportInput(adder.get_input_port(0))
-        builder.ExportInput(adder.get_input_port(1))
-        builder.Connect(adder.get_output_port(0), zoh.get_input_port(0))
-        diagram = builder.Build()
-        context = diagram.CreateDefaultContext()
-        self._fix_adder_inputs(context)
-
-        simulator = Simulator(diagram, context)
-        simulator.Initialize()
-        simulator.StepTo(1)
-        # Ensure that we have the outputs we want.
-        value = (diagram.GetMutableSubsystemContext(zoh, context)
-                 .get_discrete_state_vector().get_value())
-        self.assertTrue(np.allclose([5, 7, 9], value))
+        for T in (float, AutoDiffXd):
+            builder = DiagramBuilder_[T]()
+            adder = builder.AddSystem(self._create_adder_system(T))
+            adder.set_name("custom_adder")
+            # Add ZOH so we can easily extract state.
+            zoh = builder.AddSystem(ZeroOrderHold_[T](0.1, 3))
+            zoh.set_name("zoh")
+
+            builder.ExportInput(adder.get_input_port(0))
+            builder.ExportInput(adder.get_input_port(1))
+            builder.Connect(adder.get_output_port(0), zoh.get_input_port(0))
+            diagram = builder.Build()
+            context = diagram.CreateDefaultContext()
+            self._fix_adder_inputs(context, T)
+
+            simulator = Simulator_[T](diagram, context)
+            simulator.Initialize()
+            simulator.StepTo(1)
+            # Ensure that we have the outputs we want.
+            value = (diagram.GetMutableSubsystemContext(zoh, context)
+                     .get_discrete_state_vector().get_value())
+            self.assertTrue(np.allclose([5, 7, 9], value))
 
     def test_leaf_system_overrides(self):
+        map(self._check_leaf_system_overrides, (float, AutoDiffXd))
+
+    def _check_leaf_system_overrides(self, T):
         test = self
 
-        class TrivialSystem(LeafSystem):
+        class TrivialSystem(LeafSystem_[T]):
             def __init__(self):
-                LeafSystem.__init__(self)
+                LeafSystem_[T].__init__(self)
                 self.called_publish = False
                 self.called_feedthrough = False
                 self.called_discrete = False
@@ -151,19 +184,19 @@ def __init__(self):
                 # Ensure that we have inputs / outputs to call direct
                 # feedthrough.
                 self._DeclareInputPort(PortDataType.kVectorValued, 1)
-                self._DeclareVectorOutputPort(BasicVector(1), noop)
+                self._DeclareVectorOutputPort(BasicVector_[T](1), noop)
 
             def _DoPublish(self, context, events):
                 # Call base method to ensure we do not get recursion.
-                LeafSystem._DoPublish(self, context, events)
+                LeafSystem_[T]._DoPublish(self, context, events)
                 self.called_publish = True
 
             def _DoHasDirectFeedthrough(self, input_port, output_port):
                 # Test inputs.
                 test.assertEquals(input_port, 0)
                 test.assertEquals(output_port, 0)
                 # Call base method to ensure we do not get recursion.
-                base_return = LeafSystem._DoHasDirectFeedthrough(
+                base_return = LeafSystem_[T]._DoHasDirectFeedthrough(
                     self, input_port, output_port)
                 test.assertTrue(base_return is None)
                 # Return custom methods.
@@ -173,7 +206,7 @@ def _DoHasDirectFeedthrough(self, input_port, output_port):
             def _DoCalcDiscreteVariableUpdates(
                     self, context, events, discrete_state):
                 # Call base method to ensure we do not get recursion.
-                LeafSystem._DoCalcDiscreteVariableUpdates(
+                LeafSystem_[T]._DoCalcDiscreteVariableUpdates(
                     self, context, events, discrete_state)
                 self.called_discrete = True
 
@@ -194,13 +227,17 @@ def _DoCalcDiscreteVariableUpdates(
             system.HasDirectFeedthrough(input_port=0, output_port=0))
 
     def test_vector_system_overrides(self):
+        map(self._check_vector_system_overrides,
+            (float, AutoDiffXd, Expression))
+
+    def _check_vector_system_overrides(self, T):
         dt = 0.5
         for is_discrete in [False, True]:
-            system = CustomVectorSystem(is_discrete)
+            system = CustomVectorSystem_[T](is_discrete)
             context = system.CreateDefaultContext()
 
-            u = np.array([1.])
-            context.FixInputPort(0, BasicVector(u))
+            u = np.array([1])
+            context.FixInputPort(0, BasicVector_[T](u))
 
             # Dispatch virtual calls from C++.
             output = call_vector_system_overrides(
@@ -221,12 +258,12 @@ def test_vector_system_overrides(self):
             x0 = [0., 0.]
             c = is_discrete and 2 or 1*dt
             x_expected = x0 + c*u
-            self.assertTrue(np.allclose(x, x_expected))
+            self.assertArrayEqual(x, x_expected)
 
             # Check output.
             y_expected = np.hstack([u, x])
             y = output.get_vector_data(0).get_value()
-            self.assertTrue(np.allclose(y, y_expected))
+            self.assertArrayEqual(y, y_expected)
 
     def test_context_api(self):
         # Capture miscellaneous functions not yet tested.

bindings/pydrake/systems/test/general_test.py

@@ -139,17 +139,8 @@ def check_output(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))
+                value = output.get_vector_data(0).get_value()
+                self.assertTrue(np.allclose([1], value))
 
             # Create simulator with basic constructor.
             simulator = Simulator_[T](system)