py systems: Add sugar for scalar-convertible Python systems

[EricCousineau-TRI]

Moved from #10745:

---

@RussTedrake wrote:

To be concrete, I was hoping to write this:

from pydrake.all import LinearQuadraticRegulator, VectorSystem

class Quadrotor2D(VectorSystem):
    def __init__(self):
        # two inputs (thrust), six outputs (full state)
        VectorSystem.__init__(self, 2, 6)
        # three positions, three velocities
        self._DeclareContinuousState(3, 3, 0)

        self.L = 0.25;    # length of rotor arm
        self.m = 0.486;   # mass of quadrotor
        self.I = 0.00383; # moment of inertia
        self.g = 9.81;    # gravity

    def _DoCalcVectorOutput(self, context, u, x, y):
        y[:] = x

    def _DoCalcVectorTimeDerivatives(self, context, u, x, xdot):
        xdot[0:3] = x[3:6]
        xdot[4] = -np.sin(x[2])/self.m*(u[0] + u[1])
        xdot[5] = -self.g + np.cos(x[2])/self.m*(u[0] + u[1])
        xdot[6] = self.L/self.I*(-u[0] + u[1])

plant = Quadrotor2D()

context = plant.CreateDefaultContext()
context.SetContinuousState(np.zeros([6, 1]))
context.FixInputPort(0, plant.m*plant.g/2.*np.array([1, 1]))

Q = np.diag([10, 10, 10, 1, 1, (plant.L/2./np.pi)])  
R = np.array([[0.1, 0.05], [0.05, 0.1]])  

pi = LinearQuadraticRegulator(plant, context, Q, R)

but had to (i think) instead write this

from pydrake.all import BasicVector_, LeafSystem_, LinearQuadraticRegulator, TemplateSystem 

@TemplateSystem.define("Quadrotor2D_")
def Quadrotor2D_(T):

    class Impl(LeafSystem_[T]):
        def _construct(self, converter=None):
            LeafSystem_[T].__init__(self, converter)
            # two inputs (thrust)
            self._DeclareVectorInputPort("u", BasicVector_[T](2))
            # six outputs (full state)
            self._DeclareVectorOutputPort("x", BasicVector_[T](6), self._CopyStateOut)
            # three positions, three velocities
            self._DeclareContinuousState(3, 3, 0)

            self.L = 0.25;    # length of rotor arm
            self.m = 0.486;   # mass of quadrotor
            self.I = 0.00383; # moment of inertia
            self.g = 9.81;    # gravity

        def _construct_copy(self, other, converter=None):
            Impl._construct(self, converter=converter)

        def _CopyStateOut(self, context, output):
            x = context.get_continuous_state_vector().CopyToVector()
            y = output.SetFromVector(x)

        def _DoCalcTimeDerivatives(self, context, derivatives):
            x = context.get_continuous_state_vector().CopyToVector()
            u = self.EvalVectorInput(context, 0).CopyToVector()
            q = x[:3]
            qdot = x[3:]
            qddot = np.array([-np.sin(q[2])/self.m*(u[0] + u[1]), 
                              -self.g + np.cos(x[2])/self.m*(u[0] + u[1]),
                              self.L/self.I*(-u[0] + u[1])])
            derivatives.get_mutable_vector().SetFromVector(np.concatenate((qdot, qddot)))

    return Impl

Quadrotor2D = Quadrotor2D_[None]  # Default instantiation            

plant = Quadrotor2D()

context = plant.CreateDefaultContext()
context.SetContinuousState(np.zeros([6, 1]))
context.FixInputPort(0, plant.m*plant.g/2.*np.array([1, 1]))

Q = np.diag([10, 10, 10, 1, 1, (plant.L/2./np.pi)])  
R = np.array([[0.1, 0.05], [0.05, 0.1]])  

pi = LinearQuadraticRegulator(plant, context, Q, R)

BTW -- love that the second one worked!

---

I would still like to maintain the design philosophy that the Python bindings shouldn't diverge too far from the C++ bits, unless there is meaningful sugar (in which case, there should be minimal interface coupling via forwarding).

To that end, possible solution routes:

• Write a Python wrapper class that will own the user's implementation details. Ensure that the class can access the original system (declare ports, etc. - perhaps with sugar).
  • This doesn't have to start out in Drake. We can prototype stuff for use in Anzu / Underactuated and then upstream it once we're comfortable with it.
• Use a metaclass that can inherit from "templates" to shield the user from this.
  • This may be a bit finicky, as using metaclasses can introduces a whole new host of ~features~ caveats.

[jwnimmer-tri]

I think it would be fine to invent a totally new Python API for "I want to write a simple, pure-python System with as little boilerplate as possible" that diverges from the C++ VectorSystem API. (Keeping around the APIs that still make sense is fine, to avoid user-code churn.)

For one, because that there is some debate that C++ VectorSystem should live on in the first place (#10348).

But really, the intent of VectorSystem is to make C++ sugar for common C++ use cases. There will be tradeoffs there that don't make sense for Python. I'd rather than Python get the best possible sugar / deboilerplating, than that we wed it to C++ VectorSystem.

[EricCousineau-TRI]

~I don't really think there's a solution to this, pending a novel API rewrite.~ I believe the sugar Jeremy alluded to could be prototyped using the current bindings, but I'm not sure if I'm the right person as I prefer the explicit C++ API parity, because that makes transcribing that much clearer.

Reassigning to you, Jeremy. Feel free to reassign if you feel there's someone better suited.

[EricCousineau-TRI]

Given that I did some stuff on function_system.py a ways back, will re-own this: https://github.com/EricCousineau-TRI/repro/tree/54494a5c5154f19e693e4862fbaa79cddcd78d6f/drake_stuff/drake_py_meta

Not planning on tackling this any time soon, tho.