Use IRIS regions in nonlinear IK

[rcory]

Nonlinear IK with collision avoidance constraints can be brittle, depending on the robot+environment. A potential improvement to the MinimumDistanceConstraint IK formulation could leverage the IRIS algorithm to compute a set of collision free regions in configuration space whose union covers the configuration space of interest (in an offline procedure), and then use these resulting regions as "seeds" in a series of linearly-constrained optimization problems executed in parallel during runtime (online procedure). The hope is that using the convex IRIS containment constraints makes the optimization more robust, in addition to providing seeds (regions) that cover the space of interest.

One potential workflow could look like the following:

Offline Phase: The goal is to compute a sufficient number of IRIS regions to cover the robot configuration space of interest. This is done by manually tele-operating the robot around the scene and computing/saving IRIS regions as you go. (A slightly different version of this could also use random sampling in a more automated fashion.)

Steps in this offline phase would be:

1. Run a GUI that loads the robot+environment, allowing the user to teleop the robot to various poses that lie in the configuration space of interest. For example, one could teleop the robot using a game pad, or using pose sliders (see #19513).
2. As the user teleops the robot around, a visual indicator in the GUI tells the user whether the current robot configuration is contained in an existing IRIS region, or whether a new IRIS region needs to be computed with that configuration as the seed. Newly computed IRIS regions are added to a record and saved to disk.
3. Once the configuration space of interest is covered by IRIS regions, we stop.

Online Phase: During online operation we load the stored IRIS record and call IRIS_IK with a target end-effector pose. This will kick off a series of optimizations in parallel, where each optimization is constrained to a single IRIS region (via the convex IRIS containment constraints). If the real IK solution is contained in any of the provided IRIS regions, at least one of the optimizations should converge to that solution (since IRIS regions may overlap).

IRIS builder GUI and Gamepad teleop are tracked in #19514.

This issue will mainly track the implementation of IRIS_IK, which loads IRIS regions, sets up optimization + constraints, kicks off parallel solves.

cc @RussTedrake

[RussTedrake]

Thanks! The IRIS Builder concept is already being tracked in #19514. I don't have a preference to the gamepad teleop being tracked in that issue or this one.

The IRIS_IK is the main concept I want to track here.

[rcory]

Got it. Description has been updated.