CODEWITHSUNDEEP
python-3.xdrake
Rufus
Rufus

Reputation: 5566

Solving Atlas IK (no constraints) fails?

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

Upvotes: 2

Views: 103

Answers (1)

Hongkai Dai
Hongkai Dai

Reputation: 2766

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.

Upvotes: 2

Related Questions