I'm writing some code based on the potential iLQR template from the underactuated robotics course (google colab), and for my trajectory optimization problem in particular, I added collision avoidance costs as a part of the trajectory cost (because I have a three-agent system). The code is as follows:
eps = 1e-6
def cost_avoidance(x): # `hard coded` for 3 agents! so x must have shape (12,) and u has shape (6,)
m = sym if x.dtype == object else np
threshold = 0.5 #meters -> this threshold is perhaps a realistic distance
#at which aerodynamic interference between quadcopters start to affect each other
pos_x1_coor = [x[0], x[1]]
pos_x2_coor = [x[4], x[5]]
pos_x3_coor = [x[8] , x[9]]
#Euclidean distance between agent i and j
d_12 = m.sqrt((pos_x1_coor[0]-pos_x2_coor[0]+eps)**2+(pos_x1_coor[1]-pos_x2_coor[1]+eps)**2)
d_13 = m.sqrt((pos_x1_coor[0]-pos_x3_coor[0]+eps)**2+(pos_x3_coor[1]-pos_x3_coor[1]+eps)**2)
d_23 = m.sqrt((pos_x2_coor[0]-pos_x3_coor[0]+eps)**2+(pos_x2_coor[1]-pos_x3_coor[1]+eps)**2)
d_12_cost = 0
d_13_cost = 0
d_23_cost = 0
if d_12 < threshold:
d_12_cost = 10*d_12
if d_23 < threshold:
d_23_cost = 10*d_23
if d_13 < threshold:
d_13_cost = 10*d_13
return -(d_12_cost+d_13_cost+d_23_cost)
def cost_stage(x, u):
c_avoid = cost_avoidance(x)
c_trj = cost_trj(x,u)
return c_avoid + c_trj
x_sym = np.array([sym.Variable("x_{}".format(i)) for i in range(3*n_x)])
u_sym = np.array([sym.Variable("u_{}".format(i)) for i in range(3*n_u)])
x = x_sym
u = u_sym
l = cost_stage(x,u)
The issue arises at the bottom line when i tried to compute the stage cost symbolically. The error message says
RuntimeError Traceback (most recent call last)
<ipython-input-163-d3a4ccc6cac8> in <module>()
4 u = u_sym
5
----> 6 l = cost_stage(x,u)
7 #l_x = sym.Jacobian([l], x).ravel()
8 #l_x
1 frames
<ipython-input-159-10c338807d59> in cost_stage(x, u)
1 def cost_stage(x, u):
2
----> 3 c_avoid = cost_avoidance(x)
4 c_trj = cost_trj(x,u)
5
<ipython-input-157-32474b2ff1aa> in cost_avoidance(x)
19 d_23_cost = 0
20
---> 21 if d_12 < threshold:
22 d_12_cost = 10*d_12
23
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`.
Intuitively I know that this is due to how Pydrake handles symbolic comparison: in my avoidance cost function, I only impose a penalty when the distance between agents are below a certain threshold. I wonder how I should fix this issue?
Edit:
According to the error message, I should use pydrake.common.containers.EqualToDict , it is not working:
if pydrake.common.containers.EqualToDict(d_12) < threshold:
d_12_cost = 10*d_12
I haven't looked carefully, but would if_then_else expression resolve your problem?
d_12_cost = if_then_else(d_12 < threshold, 10*d_12, 0)