解决 Atlas IK无约束失败？

问题描述

我正在尝试解决 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 问题是微不足道的

解决方法

Atlas 机器人有一个浮动底座骨盆，其方向由四元数表示。四元数有一个单位长度约束 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]
q0[pelvis.floating_positions_start(): pelvis.floating_positions_start() + 4] = np.array([1,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())，它将打印单位长度约束。