RuntimeError: You should not call `__bool__` / `__nonzero__` on `Formula`,
RuntimeError: You should not call `__bool__` / `__nonzero__` on `Formula`,
我正在尝试使用 drake 工具箱编写时间最少的控制代码。但是在中间，我无法理解错误信息：（请忽略这个括号中发生的事情，我只是不知道提交post需要多少细节，上帝！）
'''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?
state_next1不是符号表达式，它是符号表达式的numpy数组，所以需要做state_next1[0]。同样，您需要将 u_over_time[i] <= 1 更改为 u_over_time[i, 0] <= 1。
另一种解决问题的方法是使用 u_overt_time[i, 0] 而不是 u_over_time[i] 来计算 state_next1。修改后，你代码中的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。
行中抛出的错误
if x[0]*x[0]+x[1]*x[1]<1e-7: # x.dot(x)
return 0.
是因为你用 AddQuadraticCost 跟注，但你的成本不是二次方的。德雷克试图将符号表达式解析为二次表达式，但失败了。特别是当您检查表达式 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]
然后你就会有平滑的成本和约束。
这是一段不会抛出语法错误的代码
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 的总和，您可以将其表示为混合整数线性约束。
我用二分法写了一个答案，也是class上tedrake推荐的。但我不喜欢这种方法。迭代次数过多。我就放在这里，等我有混合整数码的时候，我会回来的。
天哪，我就是过不了代码检查...我真的很讨厌Whosebug的代码检查机制...
'''
来自 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)
return0。
别的：
return 1.
'''
#mp = 数学程序()
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"])
'''
我正在尝试使用 drake 工具箱编写时间最少的控制代码。但是在中间，我无法理解错误信息：（请忽略这个括号中发生的事情，我只是不知道提交post需要多少细节，上帝！）
'''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?
state_next1不是符号表达式，它是符号表达式的numpy数组，所以需要做state_next1[0]。同样，您需要将 u_over_time[i] <= 1 更改为 u_over_time[i, 0] <= 1。
另一种解决问题的方法是使用 u_overt_time[i, 0] 而不是 u_over_time[i] 来计算 state_next1。修改后，你代码中的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。
行中抛出的错误
if x[0]*x[0]+x[1]*x[1]<1e-7: # x.dot(x)
return 0.
是因为你用 AddQuadraticCost 跟注，但你的成本不是二次方的。德雷克试图将符号表达式解析为二次表达式，但失败了。特别是当您检查表达式 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]
然后你就会有平滑的成本和约束。
这是一段不会抛出语法错误的代码
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 的总和，您可以将其表示为混合整数线性约束。
我用二分法写了一个答案，也是class上tedrake推荐的。但我不喜欢这种方法。迭代次数过多。我就放在这里，等我有混合整数码的时候，我会回来的。
天哪，我就是过不了代码检查...我真的很讨厌Whosebug的代码检查机制...
''' 来自 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) return0。 别的： return 1. ''' #mp = 数学程序()
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"])
'''