如何在Drake的MathematicalProgram中无代价求解梯度临界点？

• 整理约束系统
  你已经通过Jacobian得到了符号化梯度，直接把梯度等于零向量作为新的等式约束，和你已有的约束条件（等式/不等式）整合到一起——本质上这是在求解带约束的非线性方程组：梯度为0 + 原有约束。

• 基于Drake的MathematicalProgram搭建问题

  1. 初始化规划器：

     from pydrake.all import MathematicalProgram, SnoptSolver, Vector1d
     import numpy as np
     
     prog = MathematicalProgram()
     

  2. 添加2x1连续变量：

     x = prog.NewContinuousVariables(2, "x")
     

  3. 导入所有约束：
     • 先添加你已有的约束，比如等式约束constraint_eq(x) = 0：

       prog.AddConstraint(constraint_eq(x), Vector1d(0), Vector1d(0))
       

       不等式约束constraint_ineq(x) <= 0：

       prog.AddConstraint(constraint_ineq(x), -np.inf, Vector1d(0))
       

     • 再添加梯度为0的核心约束（假设grad(x)是你的符号化梯度表达式）：

       prog.AddConstraint(grad(x) == np.zeros(2))
       

• 配置求解器并运行

  1. 选择支持非线性约束的求解器（比如SNOPT或IPOPT），以SNOPT为例：

     solver = SnoptSolver()
     

  2. （可选但推荐）设置变量初始猜测值，非线性求解对初始点敏感：

     prog.SetInitialGuess(x, np.array([0.0, 0.0]))  # 替换为合理的初始值
     

  3. 执行求解并获取结果：

     result = solver.Solve(prog)
     if result.is_success():
         solution_x = result.GetSolution(x)
         print("找到的临界点：", solution_x)
     else:
         print("求解失败，状态信息：", result.get_solver_details())
     

• 验证结果
  将求解得到的solution_x代入梯度表达式和原约束，确认梯度数值接近零向量，且所有约束都满足（允许微小的数值误差）

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