Class implementation in Python

Question

I'm trying to create a class in Python and having some issues.

First I tried inheriting a VectorSystem to do trajectory optimization and I get the error regarding it not having 'AutoDiff'

RuntimeError: The object named [] of type drake::pydrake::(anonymous)::Impl::PyVectorSystem does not support ToAutoDiffXd

Code:

import numpy as np

from pydrake.systems.framework import VectorSystem
from pydrake.all import (DirectCollocation, PiecewisePolynomial, Solve)

# Define the system.
class ex1(VectorSystem):
    def __init__(self):
        VectorSystem.__init__(self, 
            1,                           # 1 input.
            2)                           # 2 outputs.
        self.DeclareContinuousState(2)   # 2 state variable.

    # xdot(t) = -x(t) - y(t); ydot(t) = -y(t) - x(t) + u
    def DoCalcVectorTimeDerivatives(self, context, u, x, xdot):
        xdot[:] = np.array([-x[0] - x[1],  -x[1] - x[0] + u[0]])#.reshape(3,1) #u[0]

    # y(t) = x(t)
    def DoCalcVectorOutput(self, context, u, x, y):
        y[:] = x

    def runDircol(self, x0, xf, tf0):
        N = 11
        umax = 10.

        context = self.CreateDefaultContext()
        dircol  = DirectCollocation(self, context, num_time_samples=N,
                           minimum_timestep=0.1, maximum_timestep=1.0)
        u = dircol.input()
        dircol.AddEqualTimeIntervalsConstraints()
        dircol.AddConstraintToAllKnotPoints(u[0] <=  .5*umax)
        dircol.AddConstraintToAllKnotPoints(u[0] >= -.5*umax)
        dircol.AddBoundingBoxConstraint(x0, x0, dircol.initial_state())
        dircol.AddBoundingBoxConstraint(xf, xf, dircol.final_state())

        R = 10.0  # Cost on input "effort".
        dircol.AddRunningCost(R*u[0]**2)

        # Add a final cost equal to the total duration.
        dircol.AddFinalCost(dircol.time())

        initial_x_trajectory = \
            PiecewisePolynomial.FirstOrderHold([0., tf0],     np.column_stack((x0, xf)))
        dircol.SetInitialTrajectory(PiecewisePolynomial(),     initial_x_trajectory)

        result = Solve(dircol)
        print(result.get_solver_id().name())
        print(result.get_solution_result())
        assert(result.is_success())

        #import pdb; pdb.set_trace()
        xtraj = dircol.ReconstructStateTrajectory(result)
        utraj = dircol.ReconstructInputTrajectory(result)

        return utraj,xtraj


if __name__ == "__main__":
    # Declare model
    plant = ex1()  # Default instantiation

    # Trajectory optimization
    x0 = (0.0,0.0)  #Initial state that trajectory should start from
    xf = (1.0,1.0)  #Final desired state
    tf0 = 0.5       # Guess for how long trajectory should take
    utraj, xtraj = plant.runDircol(x0, xf, tf0)

Second, I tried to inherit from the LeafSystem and had issues due to the templates. I cannot create a context by using plant.CreateDefaultContext(). I get the error:

TypeError: unbound method CreateDefaultContext() must be called with ex1_[float] instance as first argument (got nothing instead)

And if I use plant().CreateDefaultContext() I get weird errors afterwards like getting wrong context.num_output_ports() or not being able to call plant.ToSymbolic() (TypeError: unbound method ToSymbolic() must be called with ex1_[float] instance as first argument (got nothing instead)) etc ... Code:

import numpy as np
from pydrake.all import LeafSystem_
from pydrake.systems.scalar_conversion import TemplateSystem

@TemplateSystem.define("ex1_")
def ex1_(T):
    class Impl(LeafSystem_[T]):
        def _construct(self, converter=None):
            LeafSystem_[T].__init__(self, converter)
            # one inputs 
            self.DeclareVectorInputPort("u", BasicVector_[T](1))
            # two outputs (full state)
            self.DeclareVectorOutputPort("x", BasicVector_[T](2), self.CopyStateOut)
            # two positions, no velocities
            self.DeclareContinuousState(2, 0, 0)

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

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

        def DoCalcTimeDerivatives(self, context, derivatives):
            x = context.get_continuous_state_vector().CopyToVector()
            u = self.EvalVectorInput(context, 0).CopyToVector()

            xdot[:] = np.array([-x[0] - x[1],  -x[1] - x[0] + u[0]])     #.reshape(3,1) #u[0]
            derivatives.get_mutable_vector().SetFromVector(xdot)


    return Impl

if __name__ == "__main__":
    # Declare model
    plant = ex1_[None]  # Default instantiation

    #context = plant.CreateDefaultContext(DubinsPlant_[None]())
    context = plant().CreateDefaultContext()
    import pdb; pdb.set_trace()
    sym_system = plant.ToSymbolic()

Would appreciate any help on solving one of these issues.

(Running on Ubuntu 16.04)

Is "Second" a failed attempt to solve "First", or a separate question entirely? — jonrsharpe, Aug 15 '19 at 15:56
It's another approach for modeling a system. I thought VectorSystem would be easier to deal with. In the meanwhile, couple of people answered me in private that for the second approach I should write _plant = ex1_[None]()_ and everything else should be fine. It worked. — GuyS, Aug 15 '19 at 23:22

score 0 · Accepted Answer · answered Aug 15 '19 at 23:46

To answer your second question, plant is not an instantiation of ex1_[None]. So plant.ToSymbolic() will not work. A workable solution would be:

if __name__ == "__main__":
    # Declare model
    ex1 = ex1_[None]
    plant = ex1()
    context = plant.CreateDefaultContext()
    ad_system = plant.ToAutoDiffXd()
    sym_system = plant.ToSymbolic()

score 0 · Answer 2 · answered Aug 19 '19 at 16:53

To answer your first question, I've unfortunately not updated VectorSystem to support subclassed type conversion: https://github.com/RobotLocomotion/drake/issues/10745

Let me try that out in the next few minutes.

EDIT: Ah, may be more complicated. Please see update in the issue.

Class implementation in Python

2 Answers2