RuntimeError: You should not call `__bool__` / `__nonzero__` on `Formula`, - jupyter-notebook

Question

I was trying to write a least time control code, using drake toolbox. But in the middle, I cannot understand the error info: (please ignore things happened in this parentheis, i just don't know how much detail is needed to submit the post, god!)
'''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]
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?

Answer 1

state_next1 is not a symbolic expression, it is a numpy array of symbolic expression, so you need to do state_next1[0]. Similarly you will need to change u_over_time[i] <= 1 to u_over_time[i, 0] <= 1.
The other way to solve the problem is to compute state_next1 using u_overt_time[i, 0] instead of u_over_time[i]. After modification, the for loop in your code should be
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.)
I changed u_over_time[i] to u_over_time[i, 0] where you define state_next1.

Answer 2

The error thrown in the line
if x[0]*x[0]+x[1]*x[1]<1e-7: # x.dot(x)
return 0.
is because you called with AddQuadraticCost, but your cost is not quadratic. Drake tries to parse the symbolic expression as a quadratic expression, and failed. Specifically Drake fails when you check if the expression x[0] * x[0] + x[1] * x[1] < 1e-7. No quadratic cost can have this type of "if" statement.
What is the mathematical formulation of your cost? Do you really want to impose the cost as defined in your g(x) function, that if x'*x < 1e-7, then g(x) = 0, otherwise g(x) = 1? This is a pretty bad cost (it is almost constant everywhere, but have discrete jumps from 1 to 0 near the origin).
Since you want to solve a least time optimal control problem, I would suggest to change your formulation, and make dt a decision variable in your problem. Namely you will have the dynamic constraint
x[n+1] = x[n] + f(x[n], u[n]) * dt[n]
The final state constraint
x[N] = x_desired
The initial state constraint
x[0] = x_initial
And your cost function is to minimize the time
min sum_i dt[i]
Then you will have smooth cost and constraint.

Answer 3

Here is a piece of code that doesn't throw syntax error
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)
Notice that I changed the definition of g, and called mp.AddCost instead of mp.AddQuadraticCost. mp.AddQuadraticCost expects a quadratic symbolic expression. The expression in your code is not quadratic (it has an if statement in the cost, and quadratic cost doesn't allow if statement.).
This code should run without error, but I don't know if it can find the solution. Again this cost is not differentiable, so any gradient based nonlinear solver will have trouble.
If you really don't want to solve the problem as a nonlinear optimization problem, you can consider to re-formulate the problem as a mixed-integer program. Namely your cost is the summation of a bunch of binary variables b[i], that b[i] = 1 if |x[i, 0]| > epsilon or |x[i, 1]| > epsilon; otherwise b[i] = 0, and your can formulate this as a mixed-integer linear constraints.

Answer 4

I wrote an answer by bisection method, which also recommended by tedrake on class. but I don't like this method. Too many iterations. I just put it here, when i have a mixed integer code, i will back.
god, i just cannot pass the code check...i really hate the code check machanism of stackoverflow...
'''
from pydrake.all import MathematicalProgram, Solve
import numpy as np
import matplotlib.pyplot as plt
'''
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=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"])
'''