[Python] Inverse Kinematics fails to solve for nearby positions (within 3cm)

I have implemented the following inverse kinematics program in Drake:

def solveIK(plant: MultibodyPlant, plant_context: Context, target_pose: RigidTransform, frame_name: str, q0=1e-10*np.ones(7)):
ik = InverseKinematics(plant, plant_context=plant_context, with_joint_limits=True)
ik.AddPositionConstraint(plant.GetFrameByName(frame_name),
np.array([0,0,0]),
plant.world_frame(),
target_pose.translation(),
target_pose.translation()
)
ik.AddOrientationConstraint(
plant.GetFrameByName(frame_name),
RotationMatrix(RollPitchYaw(0,0,0)),
plant.world_frame(),
target_pose.rotation(),
0.0
)
prog = ik.get_mutable_prog()
q = ik.q()
if np.abs(q0).sum() > 1e-4:
prog.AddQuadraticErrorCost(np.eye(7), q0, q)
prog.SetInitialGuess(q, q0)
solver = SnoptSolver()
solver_options = SolverOptions()
solver_options.SetOption(solver.id(), "Major iterations limit", 2000)
result = solver.Solve(prog, solver_options=solver_options)
if not result.is_success():
print(target_pose)
print(result.get_solver_details().info)
raise ValueError("Inverse Kinematics Failed")
q_res = result.GetSolution()
return q_res


However, I have issues with this code reliably solving for joint angles.

It solves correctly for the following end effector position and orientation: Position [ 0.478, -0.022, 0.13 ] RPY: [pi,0,pi]

I can see that this is within the robot's workspace and the robot is not near singularity or the joint limits when the end effector is in this pose.

However, when I set position to [0.5, -0.022, 0.13] (keeping the orientation the same) this code fails to solve. I have tried a few other positions and I get the same behaviour - sometimes solving, sometimes not. The SNOPT error when my code can't solve is '13 - nonlinear infeasibilities minimized'. I do not understand what this means in the context of inverse kinematics.

Could you please suggest to me why this code does not always solve the IK problem?

Continue reading...