Pydrake中是否存在符号向量2-范数函数？符号形式避障成本函数报错咨询

You're right—Pydrake's symbolic module doesn't have a linalg submodule, which is why your code throws that AttributeError. But don't worry, we can manually implement the 2-norm for symbolic vectors, and adjust your cost function to work with symbolic expressions properly.

Step 1: Fix the Pairwise Distance Calculation #

Instead of relying on m.linalg.norm, we'll handle numpy and symbolic cases separately. For symbolic vectors, the 2-norm is just the square root of the sum of squared elements—this matches exactly what np.linalg.norm does for axis=0.

Here's the updated compute_pairwise_distance function:

import itertools
import numpy as np
from pydrake import symbolic as sym

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]]
    
    # Split norm calculation for numpy vs symbolic
    if m is np:
        return np.linalg.norm(dX, axis=0)
    else:
        # Manual 2-norm for symbolic vectors: sqrt(sum of squares)
        return sym.sqrt(sym.sum(sym.square(dX), axis=0))

Step 2: Adjust the Cost Function for Symbolic Expressions #

Your original cost calculation uses boolean indexing (distances[distances<threshold]), which doesn't work for symbolic expressions—symbolic variables can't be used as indices. Instead, we'll use sym.where to create a piecewise function that applies the cost only when the distance is below the threshold.

Updated cost_avoidance function:

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)
    
    if m is np:
        cost_avoid = np.sum((distances[distances<threshold]-threshold)**2)*1000
    else:
        # Use symbolic where to create piecewise cost terms
        cost_terms = [sym.where(d < threshold, (d - threshold)**2, 0) for d in distances]
        cost_avoid = sym.sum(cost_terms) * 1000
    return cost_avoid

Why This Works #

• For numpy arrays, we keep using np.linalg.norm as before—no breaking changes for numerical cases.
• For symbolic vectors, constructing the norm manually ensures we get the exact 2-norm behavior, and sym.where lets us replicate the threshold logic in a way that's compatible with symbolic expressions.

Pydrake's symbolic module doesn't include a pre-built norm function, so this manual approach is the standard way to handle this scenario.

内容的提问来源于stack exchange，提问作者Randy Chen