Drake逆运动学设置Snopt max_iter报错及迭代次数调整问题

问题详情 #

使用Drake逆运动学求解器时，尝试设置max_iter参数触发以下错误：

RuntimeError: Error setting Snopt integer parameter max_iter

报错发生在代码的Solve(prog)调用处：

File "/home/jasmine/pivot/inverse_kinematics.py", line 230, in solve_with_initial_guess
    result = Solve(prog)

尝试过两种设置方式，均触发相同错误：

• 直接通过Prog设置：

  prog.SetSolverOption(SnoptSolver().solver_id(), "max_iter", 100)
  

• 通过SolverOptions设置：

  solver_options = SolverOptions()
  solver_options.SetOption(SnoptSolver().solver_id(), "max_iter", 100)
  

已通过result.get_solver_id()确认当前使用的是SnoptSolver。后续尝试使用"Major Iterations Limit"和"Minor Iterations Limit"参数，但设置后求解结果无变化，甚至输入无效参数也未触发报错，怀疑参数被静默忽略：

solver_options.SetOption(result.get_solver_id(), "Major Iterations Limit", 1e20)
solver_options.SetOption(result.get_solver_id(), "Minor Iterations Limit", 1e20)
solver_options.SetOption(result.get_solver_id(), "this is not a valid solver option", 1e20)

当前核心需求是调整迭代次数：求解结果已非常接近目标姿态，但未达到设定容差，result.is_success()返回false。相关日志如下：

DEBUG:__main__:Solver failed to find a solution
DEBUG:__main__:Solver failed to reach goal rotation
DEBUG:__main__:Solver rotation: RotationMatrix([
  [0.8792933661331211, 0.44733902919402646, -0.1634960832375823],
  [0.44716027729885643, -0.8935592665926959, -0.039994043210623165],
  [-0.16398433669193657, -0.037942457037851686, -0.9857329797027369],
])
DEBUG:__main__:Goal rotation: RotationMatrix([
  [0.8734997743742637, 0.4560948494534895, -0.17022230309248323],
  [0.4573132513200487, -0.8886430852879852, -0.03432283695932748],
  [-0.16692134176071197, -0.04786392453455308, -0.984807753012208],
])
DEBUG:__main__:Diff: [[ 0.00579359 -0.00875582  0.00672622]
 [-0.01015297 -0.00491618 -0.00567121]
 [ 0.00293701  0.00992147 -0.00092523]]

完整代码上下文：

self.ik = PydrakeInverseKinematics(self.plant, self.plant_context)
self.ee_frame = self.plant.GetFrameByName(ik_frame)

self.p_WG_lower = goal_pose.translation() - np.repeat(self.position_tolerance, 3)
self.p_WG_higher = goal_pose.translation() + np.repeat(self.position_tolerance, 3)

q0_node = initial_guess
q0 = np.array(q0_node)

self.ik.AddPositionConstraint(
    self.ee_frame,
    np.zeros(3),
    self.plant.world_frame(),
    self.p_WG_lower,
    self.p_WG_higher,
)

self.ik.AddOrientationConstraint(
    self.ee_frame,
    RotationMatrix(),
    self.plant.world_frame(),
    goal_pose.rotation(),
    0.0001,
)

self.ik.AddMinimumDistanceLowerBoundConstraint(0.001, 0.01)

prog = self.ik.get_mutable_prog()
q = self.ik.q()
prog.AddQuadraticErrorCost(np.identity(len(q)), q0, q)
prog.SetInitialGuess(q, q0)

result = Solve(prog)

prog.SetSolverOption(result.get_solver_id(), "max_iter", 100)
result = Solve(prog)

解决方案 #

1. 修正Snopt参数名称 #

Snopt的迭代次数参数并非max_iter，正确的参数名区分空格和大小写：

• Major iterations limit：控制主迭代次数上限
• Minor iterations limit：控制次迭代次数上限

之前的参数名拼写错误（采用驼峰/下划线格式），导致设置被静默忽略。正确设置方式如下：

方式一：通过Prog直接设置

# 调整主迭代次数为1000，次迭代次数为10000，可按需修改
prog.SetSolverOption(SnoptSolver().solver_id(), "Major iterations limit", 1000)
prog.SetSolverOption(SnoptSolver().solver_id(), "Minor iterations limit", 10000)

方式二：通过SolverOptions传递

solver_options = SolverOptions()
solver_options.SetOption(SnoptSolver().solver_id(), "Major iterations limit", 1000)
solver_options.SetOption(SnoptSolver().solver_id(), "Minor iterations limit", 10000)
# 求解时传入选项
result = Solve(prog, solver_options=solver_options)

2. 调整代码执行顺序 #

你的代码中先调用了Solve(prog)，之后才设置求解器选项，导致第一次求解使用的是默认参数。建议将选项设置放在第一次求解调用之前：

# 先设置求解器选项
prog.SetSolverOption(SnoptSolver().solver_id(), "Major iterations limit", 1000)
# 再执行求解
result = Solve(prog)

3. 辅助优化建议 #

若结果接近目标但未达标，除增加迭代次数外，还可尝试：

• 放宽姿态约束容差：当前设置的0.0001弧度过于严格，可尝试调整为0.001弧度
• 调整成本权重：二次误差成本使用单位矩阵，若希望优先满足约束，可减小成本权重；若偏向接近初始猜测，可增大权重
• 检查碰撞约束：AddMinimumDistanceLowerBoundConstraint设置的0.001距离阈值可能过严，适当放宽可降低求解难度

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