Class 在 Python 中实施

Question

我正在尝试在 Python 中创建一个 class，但遇到了一些问题。

首先我尝试继承一个 VectorSystem 来进行轨迹优化，但我得到关于它没有 '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_output_ports() 或无法调用 plant.ToSymbolic() （TypeError：未绑定方法 ToSymbolic() 必须使用 ex1_[float] 实例作为第一个参数调用（没有得到任何东西））等... 代码：

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

回答你的第二个问题，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()

Answer 2

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

让我在接下来的几分钟内尝试一下。

编辑：啊，可能更复杂。请查看问题中的更新。

Class 在 Python 中实施

Class implementation in Python

python

drake