Is there a vector 2-norm function in Pydrake?

Question

I have defined the following function to compute the pairwise distances between positions of different agents:

def compute_pairwise_distance(X, x_dims):
    """Compute the distance between each pair of agents"""
    assert len(set(x_dims)) == 1
    m = sym if X.dtype == object else np

    n_agents = len(x_dims)
    n_states = x_dims[0]
    
    pair_inds = np.array(list(itertools.combinations(range(n_agents), 2)))
    X_agent = X.reshape(-1, n_agents, n_states).swapaxes(0, 2)
    dX = X_agent[:2, pair_inds[:, 0]] - X_agent[:2, pair_inds[:, 1]]


    return m.linalg.norm(dX, axis=0)

where X is a 2-dimensional array, and x_dim refers to the dimension of the state vector of each agent as a list, such as [4,4,4], which means there are 3 agents each with 4 state vectors. However, since I'd like to include this pairwise distance metric into a collision-avoidance cost function in symbolic form, the following error occurred:

def cost_avoidance(x,x_dim):
    #`x` here is a 1-dimensional vector 
    m = sym if x.dtype == object else np
    if len(x_dim) == 1:
        
        return 0
    
    threshold = 0.5 #threshold distance below which cost avoidance is activated

    distances = compute_pairwise_distance(x,x_dim)

    cost_avoid = np.sum((distances[distances<threshold]-threshold)**2)*1000
 
    return cost_avoid

<ipython-input-46-eeef96aeac91> in cost_avoidance(x, x_dim)
      7     threshold = 0.5 #threshold distance below which cost avoidance is activated
      8 
----> 9     distances = compute_pairwise_distance(x,x_dim)
     10 
     11     cost_avoid = np.sum((distances[distances<threshold]-threshold)**2)*1000

<ipython-input-45-8b13423fbdb8> in compute_pairwise_distance(X, x_dims)
     14     #     return torch.linalg.norm(dX, dim=0)
     15 
---> 16     return m.linalg.norm(dX, axis=0)

AttributeError: module 'pydrake.symbolic' has no attribute 'linalg'

It seems like I must use a symbolic version of vector 2-norm. However, I can't find one according to the documentation. Is there a symbolic vector 2-norm function in Pydrake at all?

https://drake.mit.edu/pydrake/index.html

Answer 1

If you do want the 2-norm in symbolic form, then np.sqrt(x.dot(x)) will do the trick:

import numpy as np
from pydrake.all import MakeVectorVariable

x = MakeVectorVariable(2, 'x')
print(np.sqrt(x.dot(x)))

But if your goal is collision avoidance, you might want to take a look at Drake's implementation of MinimumDistanceConstraint. There are a lot of details in there that make things work well -- like smoothing potential discontinuities at zero distance and/or when the closest body changes to a different collision pair.