Solving Atlas IK (no constraints) fails?

Question

I am trying to solve the IK for Atlas to achieve something like https://www.youtube.com/watch?v=LLGuGAs0cto. After a couple of failed attempts with AddPositionConstraint, I realized that the IK problem fails even with no constraints at all (also fails with 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()}")

Is there something I'm missing? I would think a no constraint IK problem would be trivial

Hongkai Dai · Accepted Answer · 2021-03-31T16:26:28.633

Atlas robot has a floating-base pelvis, whose orientation is represented by a quaternion. The quaternion has a unit-length constraint zᵀz = 1. On the other hand, the optimization problem in Drake uses a zero-valued vector as the initial guess by default. This zero vector is a bad initial guess for the unit length constraint, as the gradient of zᵀz is zero at z=0, hence the gradient based nonlinear solver doesn't know how to move the initial guess with a zero gradient.

A quick solution is to use a better initial guess

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)

A better approach to obtain a reasonable initial guess is through context.

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

CreateDefaultContext() will initialize the quaternion such that is has a unit length.

We have a tutorial on debugging MathematicalProgram, you might find it useful when MathematicalProgram doesn't give you the desired results. Here if you call result.GetInfeasibleConstraintNames(ik.prog()) it will print the unit length constraint.

Solving Atlas IK (no constraints) fails?

1 Answers1