如何将符号表达式中的符号变量替换为AutoDiffXd变量用于非凸规划？

错误原因 #

你遇到的TypeError是因为symbolic.Expression.Substitute()方法仅支持将符号变量替换为symbolic类型的表达式/变量，而优化器运行时传入的是AutoDiffXd类型变量，二者类型不匹配导致报错。Substitute的作用是生成新的符号表达式，而非计算表达式在数值/AutoDiff变量上的结果。

解决方案 #

方案一：直接添加符号表达式成本（推荐） #

Drake的MathematicalProgram支持直接将符号表达式作为成本添加，无需自定义函数。只需指定符号变量到程序中优化变量的映射，Drake会自动处理AutoDiff的计算逻辑：

from pydrake.all import (
    MathematicalProgram,
    SnoptSolver,
    Variable,
)

# 从磁盘加载的符号变量与表达式
x = Variable("x")
cost_expression = x**2

prog = MathematicalProgram()
x_vars = prog.NewContinuousVariables(1, "x")

# 直接添加符号表达式，关联符号变量与优化变量
prog.AddCost(cost_expression, {x: x_vars[0]})

solver = SnoptSolver()
result = solver.Solve(prog)

方案二：自定义成本函数时使用Evaluate方法 #

如果需要在成本函数中添加额外逻辑，可使用symbolic.Expression.Evaluate()方法替代Substitute。Evaluate支持将表达式在float、AutoDiffXd等类型的变量值上计算结果：

from pydrake.all import (
    MathematicalProgram,
    SnoptSolver,
    Variable,
)

# 从磁盘加载的符号变量与表达式
x = Variable("x")
cost_expression = x**2

prog = MathematicalProgram()
x_vars = prog.NewContinuousVariables(1, "x")

def cost_func(vars):
    var = vars[0]
    # 计算表达式在AutoDiffXd变量上的结果
    return cost_expression.Evaluate({x: var})

prog.AddCost(cost_func, vars=x_vars)

solver = SnoptSolver()
result = solver.Solve(prog)

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