Drake逆运动学求解器参数调优与自碰撞关闭技术问询

一、关闭自带自碰撞检测 #

你的代码中通过AddMinimumDistanceLowerBoundConstraint手动添加了自碰撞检测约束，要关闭它直接删除这一行即可：

# 移除这行代码，关闭自带碰撞检测
# self.ik.AddMinimumDistanceLowerBoundConstraint(0.001, 0.01)

删除后求解器将不再考虑自碰撞，直接输出满足位姿约束的关节角，适配你的调试需求。

二、调优求解器参数解决已知解无法求解问题 #

当已知存在可行解但求解失败时，可以通过配置SolverOptions调整求解器的迭代次数、容差等参数，同时优化约束和成本函数：

1. 设置求解器参数 #

使用SolverOptions对求解器进行精细化配置，比如增加最大迭代次数、调整收敛容差。示例代码修改如下：

from pydrake.solvers import SolverOptions

# ... 原代码中约束和成本函数添加完成后 ...

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

# 配置求解器参数
options = SolverOptions()
# 设置IPOPT最大迭代次数（根据需求调整，默认可能为1000）
options.SetOption("ipopt.max_iter", 5000)
# 调整相对收敛容差
options.SetOption("ipopt.rel_tol", 1e-6)
# 若使用SNOPT求解器，可替换为对应参数：
# options.SetOption("snopt.max_iterations", 5000)

# 传入options调用Solve
result = Solve(self.ik.prog(), solver_options=options)

2. 其他优化方向 #

• 约束容差调整：检查position_tolerance和orientation_tolerance是否设置过小，导致求解器难以满足约束。适当放宽容差（比如位置容差从0.001调整到0.005）可能帮助求解器找到解。
• 成本函数权重优化：当前使用单位矩阵作为二次成本的权重，可尝试增大初始猜测附近的权重，或针对关键关节调整权重，引导求解器向已知解方向搜索：

  # 示例：放大第一个关节的权重，让求解器更贴近初始猜测
  weight_matrix = np.identity(len(q))
  weight_matrix[0] *= 10
  prog.AddQuadraticErrorCost(weight_matrix, q0, q)
  

• 初始猜测验证：确保传入的initial_guess确实是目标位姿对应的关节角，可通过self.plant.CalcRelativeTransform验证初始猜测是否满足目标位姿约束。

修改后的关键代码片段 #

def solve_with_initial_guess(
        self,
        goal_pose: RigidTransform,
        initial_guess: NodeLike,
        ik_frame: str = "gripper_frame",
    ) -> npt.NDArray[np.float64] | None:
        self.ik = PydrakeInverseKinematics(self.plant, self.plant_context)  # type: ignore  # noqa: PGH003
        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(),
            self.orientation_tolerance,
        )

        # 移除碰撞约束，关闭自带自碰撞检测
        # self.ik.AddMinimumDistanceLowerBoundConstraint(0.001, 0.01)

        prog = self.ik.get_mutable_prog()
        q = self.ik.q()
        # 可选：调整成本函数权重
        weight_matrix = np.identity(len(q))
        weight_matrix[0] *= 10
        prog.AddQuadraticErrorCost(weight_matrix, q0, q)
        prog.SetInitialGuess(q, q0)

        # 配置求解器参数
        from pydrake.solvers import SolverOptions
        options = SolverOptions()
        options.SetOption("ipopt.max_iter", 5000)
        options.SetOption("ipopt.rel_tol", 1e-6)

        result = Solve(self.ik.prog(), solver_options=options)

        # ... 后续验证代码保持不变 ...

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