解算Atlas IK (无约束)失败？

Question

我正在尝试为阿特拉斯解决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

阿特拉斯机器人有一个浮动的基础骨盆，其方向由一个四元数表示。四元数具有单位长度约束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。

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

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

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