Issues with comparing magnitude symbolic expressions in Pydrake

Question

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

Answer 1

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)