逆运动学求解器参数

Question

我一直在使用 Drake InverseKinematics 解算器。有时，即使存在解决方案，它也无法解决所需的姿势。 （我知道解决方案存在，因为如果我手动找到达到所需姿势所需的关节位置。）我想知道是否可以调整 IK 解算器内的任何参数，即步长、迭代次数，重置次数（即，如果它正在执行梯度下降之类的操作，则允许它在其他地方随机重新启动）。

此外，我相信 drake InverseKinematics 解算器不允许自碰撞。有什么办法可以关掉这个功能吗？我正在其他地方自己进行碰撞检查。即使由于自碰撞而无法实现我想要的姿势，我仍然想知道到达该位置的关节角度是多少。这将有助于调试。

这是我的参考代码：

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()
        prog.AddQuadraticErrorCost(np.identity(len(q)), q0, q)
        prog.SetInitialGuess(q, q0)

        result = Solve(self.ik.prog())

        if result is None:
            return None

        q_sol = result.get_x_val()
        if result:
            times = [0, 2]  # 2 seconds trajectory
            next_sol = q_sol.copy()
            next_sol[0] += 1.0
            key_frames: list[np.ndarray] = [q0, q_sol]

            self.plant.SetPositions(self.plant_context, q_sol)
            current_link_pose = self.plant.CalcRelativeTransform(
                self.plant_context,
                self.plant.world_frame(),
                self.plant.GetFrameByName(ik_frame),
            )

            # Find collisions in the current_link_pose

            # Check if the arrays are close enough
            are_close = np.allclose(
                current_link_pose.translation(),
                goal_pose.translation(),
                atol=0.005,
                rtol=1e-4,
            )

            are_close_rot = np.allclose(
                current_link_pose.rotation().ToQuaternion().wxyz(),
                goal_pose.rotation().ToQuaternion().wxyz(),
                atol=1e-2,
                rtol=1e-2,
            )

            if not are_close:
                logger.debug("Solver failed to reach goal pose")
                logger.debug(f"Solver translation: {current_link_pose.translation()}")
                logger.debug(f"Goal translation: {goal_pose.translation()}")
                logger.debug(f"Solver q_sol: {q_sol}")
                return None

            if not are_close_rot:
                logger.debug("Solver failed to reach goal rotation")
                logger.debug(f"Solver rotation: {rotation_to_string(current_link_pose.rotation())}")
                logger.debug(f"Goal rotation: {rotation_to_string(goal_pose.rotation())}")
                return None

            if len(key_frames) != len(times):
                msg = "Mismatch in solution size and times"
                raise ValueError(msg)

            return key_frames[1]

        return None

Answer 1

我想知道是否可以调整 IK 解算器内的任何参数，即步长、迭代次数、重置次数（即，如果它正在执行梯度下降之类的操作，则允许它在其他地方随机重新启动).

当您调用

Solve(ik.prog())

时，您可以将初始猜测和求解器选项传递为

solver_options = solvers.SolverOptions()
solver_options.SetOption(solvers.SnoptSolver.id(), OPTION_NAME, OPTION_VALUE)
Solve(ik.prog(), q_initial, solver_options)

根据您使用的求解器（SNOPT/IPOPT），您可以设置选项。有关求解器选项的完整列表，请参阅其手册 https://coin-or.github.io/Ipopt/OPTIONS.html https://web.stanford.edu/group/SOL/guides/ sndoc7.pdf。我们也有这方面的教程，请查看 drake.mit.edu -> 资源 -> 教程 -> “设置求解器参数”。