Python中的类实现

Question

我试图用Python创建一个类，但遇到了一些问题。你知道吗

首先我尝试继承一个向量系统来进行轨迹优化，但我得到的错误是没有“AutoDiff”

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

代码：

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)

其次，我尝试从LeafSystem继承，但由于模板问题。我无法使用创建上下文plant.CreateDefaultContext(). 我得到一个错误：

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

如果我使用plant（）.CreateDefaultContext（），我会在之后出现奇怪的错误，比如出错context.num输出端口（）或无法呼叫plant.to符号() （TypeError:必须使用ex1[float]实例作为第一个参数调用unbound method ToSymbolic（）等。。。 代码：

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

如果您能帮我解决其中一个问题，我将不胜感激。你知道吗

（在Ubuntu 16.04上运行）

Answer 1

为了回答您的第一个问题，很遗憾我没有更新VectorSystem以支持子类类型转换： https://github.com/RobotLocomotion/drake/issues/10745

我过几分钟再试试。你知道吗

编辑：啊，可能更复杂。请参阅本期更新。你知道吗

Answer 2

回答第二个问题，plant不是ex1_[None]的实例。所以plant.ToSymbolic()将不起作用。可行的解决方案是：

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