如何用Expression类型点调用QueryObject.ComputeSignedDistanceToPoint实现避障约束？

你的报错核心是：QueryObject.ComputeSignedDistanceToPoint() 只支持数值型数组输入，无法处理Drake的符号表达式（Expression）——这个方法是用来做实时数值距离查询的，不是为符号优化设计的。

下面提供两种可行的解决思路：

思路1：手动构造符号距离表达式（无需MultibodyPlant） #

如果障碍物是简单几何（球、盒子、圆柱等），可以直接写出点到该几何的符号距离公式，再作为约束加入MathematicalProgram。

示例：球型障碍物 #

import numpy as np
from pydrake.solvers import MathematicalProgram

prog = MathematicalProgram()
# 定义待优化的点变量
x = prog.NewContinuousVariables(3, "x")

# 障碍物参数：球心(0,0,0)，半径1.0
cx, cy, cz, obstacle_radius = 0.0, 0.0, 0.0, 1.0
desired_distance = 0.5

# 计算点到球的符号距离表达式
distance_expr = np.sqrt((x[0]-cx)**2 + (x[1]-cy)**2 + (x[2]-cz)**2) - obstacle_radius
# 添加避障约束
prog.AddConstraint(distance_expr >= desired_distance)

复杂几何：用Drake的SignedDistanceFunction生成表达式 #

如果障碍物是复杂几何，可借助Drake的SignedDistanceFunction自动生成符号距离表达式：

from pydrake.geometry import GeometryInstance, Box, SignedDistanceFunction
from pydrake.math import RigidTransform
from pydrake.symbolic import Variable
from pydrake.solvers import MathematicalProgram

prog = MathematicalProgram()
# 定义待优化的点变量
p_WQ = np.array([Variable("x"), Variable("y"), Variable("z")])

# 创建障碍物几何（以盒子为例）
obstacle_geo = GeometryInstance(
    RigidTransform.Identity(),  # 障碍物在世界坐标系的位姿
    Box(1.0, 1.0, 1.0),
    "obstacle"
)

# 生成符号距离函数
sdf = SignedDistanceFunction(
    geometry=obstacle_geo,
    X_WG=RigidTransform.Identity(),
    p_WQ=p_WQ
)
distance_expr = sdf.evaluator()

# 添加避障约束
desired_distance = 0.5
prog.AddConstraint(distance_expr >= desired_distance)

思路2：改用MultibodyPlant集成场景（推荐复杂场景） #

如果你的场景涉及多个运动物体，或者需要自动处理大量几何的距离计算，推荐按照你之前了解的方式，将所有元素集成到MultibodyPlant中，利用Drake提供的DistanceConstraint直接添加避障约束。

示例代码 #

from pydrake.multibody.plant import MultibodyPlant
from pydrake.geometry import SceneGraph, GeometryInstance, Box
from pydrake.math import RigidTransform
from pydrake.solvers import MathematicalProgram
from pydrake.multibody.inverse_kinematics import DistanceConstraint

# 初始化MultibodyPlant和SceneGraph
plant = MultibodyPlant(time_step=0.0)
scene_graph = SceneGraph()
plant.RegisterAsSourceForSceneGraph(scene_graph)

# 添加锚定障碍物到SceneGraph
source_id = scene_graph.RegisterSource("obstacle_source")
obstacle_geo = GeometryInstance(
    RigidTransform([1.0, 0.0, 0.0]),  # 障碍物在世界坐标系的位置
    Box(0.5, 0.5, 0.5),
    "obstacle"
)
obstacle_id = scene_graph.RegisterAnchoredGeometry(source_id, obstacle_geo)

# 添加一个虚拟刚体，用来承载待优化的点
point_body = plant.AddRigidBody("point_body", mass=0.0)
point_frame = point_body.AddFrame("point_frame")
plant.Finalize()

# 构建优化问题
prog = MathematicalProgram()
context = plant.CreateDefaultContext()
# 定义刚体位姿为优化变量
q = prog.NewContinuousVariables(plant.num_positions(), "q")
prog.SetDecisionVariableValueInContext(q, context, plant.GetPositions(context))

# 添加避障约束：点到障碍物的距离不小于desired_distance
desired_distance = 0.5
constraint = DistanceConstraint(
    plant=plant,
    frameB=point_frame,
    geometry_id=obstacle_id,
    minimum_distance=desired_distance,
    scene_graph=scene_graph,
    context=context
)
prog.AddConstraint(constraint, q)

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