我正在尝试在 Python 中创建一个类并遇到一些问题。
首先，我尝试继承 VectorSystem 来进行轨迹优化，但我得到了关于它没有“AutoDiff”的错误
RuntimeError：名为 [] 的类型为 drake::pydrake::(anonymous)::Impl::PyVectorSystem 的对象不支持 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：必须使用 ex1_[float] 实例作为第一个参数调用未绑定的方法 CreateDefaultContext()（什么都没有）
如果我使用 plant().CreateDefaultContext() 之后会出现奇怪的错误，例如错误的 context.num_output_ports() 或无法调用 plant.ToSymbolic() （TypeError: unbound method 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 上运行）