python-3.x - 解决 Atlas IK(无约束)失败？

Question

我正在尝试解决 Atlas 的 IK 问题，以实现类似 https://www.youtube.com/watch?v=LLGuGAs0cto 的目标。在使用 AddPositionConstraint 进行几次失败尝试后，我意识到即使没有任何约束，IK 问题也会失败(Strandbeest 也会失败)。

from pydrake.all import MultibodyPlant, FindResourceOrThrow, Parser, InverseKinematics, Solve
import numpy as np

plant = MultibodyPlant(0.001)
# filename = FindResourceOrThrow("drake/examples/multibody/strandbeest/Strandbeest.urdf")
filename = FindResourceOrThrow("drake/examples/atlas/urdf/atlas_minimal_contact.urdf")
Parser(plant).AddModelFromFile(filename)
plant.Finalize()
ik = InverseKinematics(plant=plant, with_joint_limits=False)
# epsilon = 1e-2
# r_foot_position = np.array([0.0, -0.08, 0.1])
# ik.AddPositionConstraint(
        # frameB=plant.GetFrameByName("r_foot"),
        # p_BQ=np.zeros(3),
        # frameA=plant.world_frame(),
        # p_AQ_upper=r_foot_position+epsilon,
        # p_AQ_lower=r_foot_position-epsilon)
result = Solve(ik.prog())
print(f"Success? {result.is_success()}")

我有什么遗漏的吗？我认为无约束 IK 问题是微不足道的

Answer 1

Atlas 机器人有一个 float 底座骨盆，其方向由四元数表示。四元数具有单位长度约束 zᵀz = 1。另一方面，Drake 中的优化问题默认使用零值向量作为初始猜测。对于单位长度约束，这个零向量是一个不好的初始猜测，因为 zᵀz 的梯度在 z=0 时为零，因此基于梯度的非线性求解器不知道如何用零梯度移动初始猜测。

一个快速的解决方案是使用更好的初始猜测

pelvis = plant.GetBodyByName("pelvis")
q0 = np.zeros((plant.num_positions(),))
# Set the initial guess of the pelvis quaternion as [1, 0, 0, 0]
q0[pelvis.floating_positions_start(): pelvis.floating_positions_start() + 4] = np.array([1, 0, 0, 0])
# Use q0 as the initial guess
result = Solve(ik.prog(), q0)

获得合理初始猜测的更好方法是通过上下文。

context = plant.CreateDefaultContext()
q0 = plant.GetPositions(context)
result = Solve(ik.prog(), q0)

CreateDefaultContext() 将初始化四元数，使其具有单位长度。

我们有关于 debugging MathematicalProgram 的教程，当 MathematicalProgram 没有给出您想要的结果时，您可能会发现它很有用。在这里，如果您调用 result.GetInfeasibleConstraintNames(ik.prog()) ，它将打印单位长度约束。