Unexpected behavior in SNOPT feasibility check

[Joaoloula]

I've encountered a situation where 1) calling SNOPT on a problem that is feasible by construction yields kInfeasibleConstraints, 2) the problem goes away if a random initialization is used. It's an inverse kinematics problem, and the code is here:

from pydrake.all import (
    MathematicalProgram,
    SnoptSolver,
    MultibodyPlant,
    RigidTransform,
    Parser,
)
import numpy as np

arm_urdf_path = "arm.urdf"
random_initialization = True

# cached (q, x) ground-truth pair
q = np.array([1.34093164, 1.51048232, -1.43221495])
x = np.array([0.26965300914356294, 1.2598206870374395, 0])

# create plant and convert it to autodiff
plant = MultibodyPlant(0)
parser = Parser(plant=plant)
parser.AddModelFromFile(file_name=arm_urdf_path, model_name="arm")
plant.WeldFrames(
    plant.world_frame(), plant.GetFrameByName("link0"), RigidTransform(),
)
plant.Finalize()
autodiffplant = plant.ToAutoDiffXd()
context = autodiffplant.CreateDefaultContext()

# forward kinematics
def forward_kinematics(arm_joints):
    autodiffplant.SetPositions(
        context, autodiffplant.GetModelInstanceByName("arm"), arm_joints
    )

    X_WA = autodiffplant.EvalBodyPoseInWorld(
        context, autodiffplant.GetBodyByName("link3")
    )
    return X_WA.translation()

# make sure FK(q) == x
assert all(forward_kinematics(q) == x)

# optimization
prog = MathematicalProgram()
q_var = prog.NewContinuousVariables(rows=plant.num_positions(), name="arm_q")
prog.AddConstraint(
    forward_kinematics, lb=x, ub=x, vars=q_var,
)

if random_initialization:
    prog.SetInitialGuess(q_var, np.random.uniform(size=plant.num_positions()))

solver = SnoptSolver()
result = solver.Solve(prog)
print(result.get_solution_result())

and here's the simple 3-dof model for reproduction:

<?xml version="1.0"?>
<robot name="arm">
    <link name="link0"/>
    <link name="link1"/>
    <link name="link2"/>
    <link name="link3"/>
        <joint name="q1" type="revolute">
        <origin xyz="1 0 0" rpy="0 0 0"/>
        <parent link="link0"/>
        <child link="link1"/>
        <axis xyz="0 0 1"/>
    </joint>
        <joint name="q2" type="revolute">
        <origin xyz="1 0 0" rpy="0 0 0"/>
        <parent link="link1"/>
        <child link="link2"/>
        <axis xyz="0 0 1"/>
    </joint>
        <joint name="q3" type="revolute">
        <origin xyz="1 0 0" rpy="0 0 0"/>
        <parent link="link2"/>
        <child link="link3"/>
        <axis xyz="0 0 1"/>
    </joint>
        <transmission name="q1" type="SimpleTransmission">
            <actuator name="q1_actuator"/>
            <joint name="q1"/>
        </transmission>
        <transmission name="q2" type="SimpleTransmission">
            <actuator name="q2_actuator"/>
            <joint name="q2"/>
        </transmission>
        <transmission name="q3" type="SimpleTransmission">
            <actuator name="q3_actuator"/>
            <joint name="q3"/>
        </transmission>
</robot>

Is this behavior correct? I assume this has something to do with the problem not being well-posed, but my understanding is that the behavior is still strange---I'm happy for the solver to fail here, but the infeasible constraints flag seems misleading. I've tried adding a tolerance, both in the constraints and through SolverOptions, and the output only changes around values of ~1. Let me know if there are clarifications or variations I could run that would be helpful.

[jwnimmer-tri]

Good morning, @Joaoloula. For questions like this, we'd ask for you please to post the question to StackOverflow, instead of the issue database. (Use the #drake tag to get our attention there.)

---

I'll also add that from a very quick skim of the example above, it looks like the problematic situation has all revolute joints initialized to a zero initial guess prior to solving. That particular kind of configuration has historically been very difficult for SNOPT to solve. I consider it a best practice to set revolute joints initial guesses to non-zero -- even a fairly small initial angle seems to be enough to unstick SNOPT. I do not know whether kInfeasibleConstraints is to be expected in this case.