我试图使用Drake工具箱编写最少时间的控制代码。但是在中间，我无法理解错误信息：（请忽略此括号中发生的事情，我只是不知道提交该帖子需要多少细节，上帝！）
'''python
从pydrake.all导入MathematicalProgram，求解
将numpy导入为np
def g(x):
if abs(x)<1e-7:
return 0.
else:
return 1.
mp = MathematicalProgram()
state_initial = np.asarray([1., 0])
position_goal = np.asarray([0, 0])
N=100
dt=0.01
u_over_time=mp.NewContinuousVariables(1,"u_0")
states_over_time = np.asarray([state_initial])
for k in range(1,N):
u = mp.NewContinuousVariables(1, "u_%d" % k)
state =mp.NewContinuousVariables(2,"state_%d" % k)
u_over_time = np.vstack((u_over_time, u))
states_over_time = np.vstack((states_over_time,state))
print "Number of decision vars", mp.num_vars()
for i in range(N-1):
state_next0 = states_over_time[i,0]+ dt*states_over_time[i,1]
state_next1 = states_over_time[i,1]+ dt*u_over_time[i]
mp.AddLinearConstraint(states_over_time[i+1,0]>=state_next0)
mp.AddLinearConstraint(states_over_time[i+1,1]>=state_next1)
mp.AddLinearConstraint(states_over_time[i+1,0]<=state_next0)
mp.AddLinearConstraint(states_over_time[i+1,1]<=state_next1)
mp.AddLinearConstraint(u_over_time[i]<=1.)
mp.AddLinearConstraint(u_over_time[i]>=-1.)
And the error info is :
---------------------------------------------------------------------------
RuntimeError Traceback (most recent call last)
<ipython-input-2-be1aa565be42> in <module>()
29 state_next1 = states_over_time[i,1]+ dt*u_over_time[i]
30 mp.AddLinearConstraint(states_over_time[i+1,0]>=state_next0)
---> 31 mp.AddLinearConstraint(states_over_time[i+1,1]>=state_next1)
32 mp.AddLinearConstraint(states_over_time[i+1,0]<=state_next0)
33 mp.AddLinearConstraint(states_over_time[i+1,1]<=state_next1)
RuntimeError: You should not call `__bool__` / `__nonzero__` on `Formula`. If you are trying to make a map with `Variable`, `Expression`, or `Polynomial` as keys (and then access the map in Python), please use pydrake.common.containers.EqualToDict`.
May I know what's happening here? Thanks
----------------update line-----------------
I modified the code as you told me. Now the code now becomes:
'''python
from pydrake.all import MathematicalProgram, Solve
import numpy as np
def g(x):
if abs(x)<1e-7:
return 0.
else:
return 1.
mp = MathematicalProgram()
state_initial = np.asarray([1., 0])
position_goal = np.asarray([0, 0])
N=100
dt=0.01
u_over_time=mp.NewContinuousVariables(1,"u_0")
states_over_time = np.asarray([state_initial])
for k in range(1,N):
u = mp.NewContinuousVariables(1, "u_%d" % k)
state =mp.NewContinuousVariables(2,"state_%d" % k)
u_over_time = np.vstack((u_over_time, u))
states_over_time = np.vstack((states_over_time,state))
print "Number of decision vars", mp.num_vars()
for i in range(N-1):
state_next0 = states_over_time[i,0]+ dt*states_over_time[i,1]
state_next1 = states_over_time[i,1]+ dt*u_over_time[i,0]
mp.AddLinearConstraint(states_over_time[i+1,0]>=state_next0[0])
mp.AddLinearConstraint(states_over_time[i+1,1]>=state_next1[0])
mp.AddLinearConstraint(states_over_time[i+1,0]<=state_next0[0])
mp.AddLinearConstraint(states_over_time[i+1,1]<=state_next1[0])
mp.AddLinearConstraint(u_over_time[i,0]<=1.)
mp.AddLinearConstraint(u_over_time[i,0]>=-1.)
'''
And the error info is:
TypeError Traceback (most recent call last)
<ipython-input-7-82e68c2ebfaa> in <module>()
27 state_next0 = states_over_time[i,0]+ dt*states_over_time[i,1]
28 state_next1 = states_over_time[i,1]+ dt*u_over_time[i,0]
---> 29 mp.AddLinearConstraint(states_over_time[i+1,0]>=state_next0[0])
30 mp.AddLinearConstraint(states_over_time[i+1,1]>=state_next1[0])
31 mp.AddLinearConstraint(states_over_time[i+1,0]<=state_next0[0])
TypeError: 'float' object has no attribute '__getitem__'
What's the problem this time? Thanks.
(Btw, one of my complain is that, the error info always not that effective to give the hint of where the problem is...)
-----------------update 2nd time line--------------------
Now a similar problem happened to the g(x), the code:
'''
from pydrake.all import MathematicalProgram, Solve
import numpy as np
def g(x):
print 'x=',x
print 'x[0]=',x[0]
if x[0]*x[0]+x[1]*x[1]<1e-7: # x.dot(x)
return 0.
else:
return 1.
mp = MathematicalProgram()
state_initial = np.asarray([1., 0])
#position_goal = np.asarray([0, 0]) # already in g(x)
N=100
dt=0.01
u_over_time=mp.NewContinuousVariables(1,"u_0")
states_over_time = np.asarray([state_initial])
for k in range(1,N):
u = mp.NewContinuousVariables(1, "u_%d" % k)
state =mp.NewContinuousVariables(2,"state_%d" % k)
u_over_time = np.vstack((u_over_time, u))
states_over_time = np.vstack((states_over_time,state))
print "Number of decision vars", mp.num_vars()
for i in range(N-1):
state_next0 = states_over_time[i,0]+ dt*states_over_time[i,1]
state_next1 = states_over_time[i,1]+ dt*u_over_time[i,0]
mp.AddLinearConstraint(states_over_time[i+1,0]>=state_next0)
mp.AddLinearConstraint(states_over_time[i+1,1]>=state_next1)
mp.AddLinearConstraint(states_over_time[i+1,0]<=state_next0)
mp.AddLinearConstraint(states_over_time[i+1,1]<=state_next1)
mp.AddLinearConstraint(u_over_time[i,0]<=1.)
mp.AddLinearConstraint(u_over_time[i,0]>=-1.)
reward=np.zeros((N,1))
for i in range(N):
reward[i]=g(states_over_time[i,:])
mp.AddQuadraticCost(reward.dot(reward))
result=Solve(mp)
'''
This time neither x or x[0] could solve the problem. the output info is :
Number of decision vars 298
x= [1.0 0.0]
x[0]= 1.0
x= [Variable('state_1(0)', Continuous) Variable('state_1(1)', Continuous)]
x[0]= state_1(0)
---------------------------------------------------------------------------
RuntimeError Traceback (most recent call last)
<ipython-input-8-08d1cd75397e> in <module>()
37 reward=np.zeros((N,1))
38 for i in range(N):
---> 39 reward[i]=g(states_over_time[i,:])
40
41 mp.AddQuadraticCost(reward.dot(reward))
<ipython-input-8-08d1cd75397e> in g(x)
5 print 'x=',x
6 print 'x[0]=',x[0]
----> 7 if x[0]*x[0]+x[1]*x[1]<1e-7: # x.dot(x)
8 return 0.
9 else:
RuntimeError: You should not call `__bool__` / `__nonzero__` on `Formula`. If you are trying to make a map with `Variable`, `Expression`, or `Polynomial` as keys (and then access the map in Python), please use pydrake.common.containers.EqualToDict`.
What can I do this time? Thanks
Btw, you see in the code i print x or x[0] only once, but i got two different answer? funny, isn't it? why is this?
答案 0 :(得分：1)
state_next1不是符号表达式，它是符号表达式的numpy数组，因此您需要执行state_next1[0]。同样，您需要将u_over_time[i] <= 1更改为u_over_time[i, 0] <= 1。
解决问题的另一种方法是使用state_next1而不是u_overt_time[i, 0]计算u_over_time[i]。修改后，代码中的for循环应为
for i in range(N-1):
state_next0 = states_over_time[i,0]+ dt*states_over_time[i,1]
state_next1 = states_over_time[i,1]+ dt*u_over_time[i, 0]
mp.AddLinearConstraint(states_over_time[i+1,0]>=state_next0)
mp.AddLinearConstraint(states_over_time[i+1,1]>=state_next1)
mp.AddLinearConstraint(states_over_time[i+1,0]<=state_next0)
mp.AddLinearConstraint(states_over_time[i+1,1]<=state_next1)
mp.AddLinearConstraint(u_over_time[i, 0]<=1.)
mp.AddLinearConstraint(u_over_time[i, 0]>=-1.)
我将u_over_time[i]更改为u_over_time[i, 0]，您在其中定义了state_next1。
答案 1 :(得分：0)
该行中引发的错误
if x[0]*x[0]+x[1]*x[1]<1e-7: # x.dot(x)
return 0.
是因为您使用AddQuadraticCost进行了呼叫，但是费用却不是二次方。 Drake尝试将符号表达式解析为二次表达式，但失败了。具体来说，当您检查表达式x [0] * x [0] + x [1] * x [1] <1e-7时，Drake失败。此类“ if”语句不能具有二次方费用。
您的费用的数学公式是什么？您是否真的要施加g(x)函数中定义的成本，即如果x'* x <1e-7，则g（x）= 0，否则g（x）= 1？这是一个非常糟糕的代价（在所有地方几乎都是恒定的，但是在原点附近有从1到0的离散跳跃）。
由于您想解决最短时间的最优控制问题，因此建议您更改公式，并使dt成为问题中的决策变量。即您将具有动态约束
x[n+1] = x[n] + f(x[n], u[n]) * dt[n]
最终状态约束
x[N] = x_desired
初始状态约束
x[0] = x_initial
您的费用功能是最大限度地减少时间
min sum_i dt[i]
那么您将获得平稳的成本和约束。
答案 2 :(得分：0)
这是一段不会引发语法错误的代码
from pydrake.all import MathematicalProgram, Solve
import numpy as np
def g(x):
x_squared_norm = np.power(x.reshape((2, -1)), 2)
return np.sum(x_squared_norm > 1e-7)
mp = MathematicalProgram()
state_initial = np.asarray([1., 0])
#position_goal = np.asarray([0, 0]) # already in g(x)
N=100
dt=0.01
u_over_time=mp.NewContinuousVariables(1,"u_0")
states_over_time = np.asarray([state_initial])
for k in range(1,N):
u = mp.NewContinuousVariables(1, "u_%d" % k)
state =mp.NewContinuousVariables(2,"state_%d" % k)
u_over_time = np.vstack((u_over_time, u))
states_over_time = np.vstack((states_over_time,state))
print "Number of decision vars", mp.num_vars()
for i in range(N-1):
state_next0 = states_over_time[i,0]+ dt*states_over_time[i,1]
state_next1 = states_over_time[i,1]+ dt*u_over_time[i,0]
mp.AddLinearConstraint(states_over_time[i+1,0]>=state_next0)
mp.AddLinearConstraint(states_over_time[i+1,1]>=state_next1)
mp.AddLinearConstraint(states_over_time[i+1,0]<=state_next0)
mp.AddLinearConstraint(states_over_time[i+1,1]<=state_next1)
mp.AddLinearConstraint(u_over_time[i,0]<=1.)
mp.AddLinearConstraint(u_over_time[i,0]>=-1.)
mp.AddCost(g, vars=states_over_time[1:,:].reshape((1, -1)).squeeze())
result=Solve(mp)
请注意，我更改了g的定义，并命名为mp.AddCost而不是mp.AddQuadraticCost。 mp.AddQuadraticCost期望一个二次符号表达式。您的代码中的表达式不是二次表达式（代价中包含if语句，而二次费用中不允许包含if语句。）
这段代码应该可以正常运行，但是我不知道它是否可以找到解决方案。同样，这种成本是不可区分的，因此任何基于梯度的非线性求解器都会遇到麻烦。
如果您真的不想将问题解决为非线性优化问题，则可以考虑将问题重新公式化为混合整数程序。也就是说，您的成本是一堆二进制变量b[i]（即b[i] = 1 if |x[i, 0]| > epsilon or |x[i, 1]| > epsilon; otherwise b[i] = 0）的总和，您可以将其公式化为混合整数线性约束。
答案 3 :(得分：0)
我用二等分法写了一个答案，这也是泰德雷克在课堂上推荐的。但我不喜欢这种方法。迭代次数过多。我只是把它放在这里，当我有一个混合的整数代码时，我会回来的。
上帝，我只是无法通过代码检查...我真的很讨厌stackoverflow的代码检查机制...
''' 从pydrake.all导入MathematicalProgram，求解 将numpy导入为np 导入matplotlib.pyplot作为plt ''' 定义g（x）： 打印'x ='，x 打印'x [0] ='，x [0] 如果x [0] * x [0] + x [1] * x [1] <1e-7：＃x.dot（x） 返回0。 其他： 返回1。 ''' #mp = MathematicalProgram（）
state_initial = np.asarray([1., 0])
#position_goal = np.asarray([0, 0]) # already in g(x)
#N=201
dt=0.01
upper=1000; lower=1;
N=upper
while upper-lower>1:
print '---------------------'
print 'N=',N
mp = MathematicalProgram()
u_over_time=mp.NewContinuousVariables(1,"u_0")
states_over_time = mp.NewContinuousVariables(2,"state intial")
mp.AddLinearConstraint(states_over_time[0]==np.asarray([state_initial[0]]))
mp.AddLinearConstraint(states_over_time[1]==np.asarray([state_initial[1]]))
#states_over_time = np.asarray([state_initial])
for k in range(1,N):
u = mp.NewContinuousVariables(1, "u_%d" % k)
state =mp.NewContinuousVariables(2,"state_%d" % k)
u_over_time = np.vstack((u_over_time, u))
states_over_time = np.vstack((states_over_time,state))
print "Number of decision vars", mp.num_vars()
for i in range(N-1):
state_next0 = states_over_time[i,0]+ dt*states_over_time[i,1]
state_next1 = states_over_time[i,1]+ dt*u_over_time[i,0]
mp.AddLinearConstraint(states_over_time[i+1,0]>=state_next0)
mp.AddLinearConstraint(states_over_time[i+1,1]>=state_next1)
mp.AddLinearConstraint(states_over_time[i+1,0]<=state_next0)
mp.AddLinearConstraint(states_over_time[i+1,1]<=state_next1)
mp.AddLinearConstraint(u_over_time[i,0]<=1.)
mp.AddLinearConstraint(u_over_time[i,0]>=-1.)
'''
reward=np.zeros((N,1))
for i in range(N):
reward[i]=g(states_over_time[i,:])
'''
mp.AddLinearConstraint(states_over_time[-1,0]<=1e-7)
mp.AddLinearConstraint(states_over_time[-1,1]<=1e-7)
mp.AddLinearConstraint(states_over_time[-1,0]>=1e-7)
mp.AddLinearConstraint(states_over_time[-1,1]>=1e-7)
#mp.AddQuadraticCost(reward.dot(reward))
result=Solve(mp)
print result.is_success()
if result.is_success():
upper=N
else:
lower=N
N=lower+int((upper-lower)/2.0)
N=upper
#print result.is_success()
print 'least time=',dt*N
u_over_time=result.GetSolution(u_over_time)
states_over_time=result.GetSolution(states_over_time)
#print 'u=',u_over_time
#print 'last state=',states_over_time[-1,:]
fig, ax = plt.subplots(2, 1)
plt.subplot(2, 1, 1);plt.plot(np.arange(dt, dt*N, dt),u_over_time);
plt.legend(["u against t"])
plt.subplot(2, 1, 2);plt.plot(states_over_time[:,0],states_over_time[:,1]);
plt.legend(["phase portrait"])
'''