Implement Constraint Islands in ICF/CENIC

[vincekurtz]

This is an extremely sloppy, vibe-coded implementation of constraint islands for ICF and CENIC. This is similar to the version in MuJoCo, but over our unconstrained convex formulation.

Interestingly, islanding appears to significantly improve simulation speed even with a single thread (islands are solved sequentially). The main benchmark is examples/multibody/box_stacks:box_stacks, which simulates a bunch of falling objects as shown in the image below:

Running a 2s simulation with 90 hydro-enabled boxes with fixed-step ICF and islands:
bazel run examples/multibody/box_stacks:box_stacks -- --fixed_step=1 --max_step_size=0.01 --use_islands=1
This runs in 1.56s on my laptop.

Running the same 2s simulation without islands (same as current Drake master):
bazel run examples/multibody/box_stacks:box_stacks -- --fixed_step=1 --max_step_size=0.01 --use_islands=0
This takes 10.0 seconds on my laptop.

More thorough benchmarks, 2 second simulation of falling objects, NxM indicates N stacks of M objects:

This branch also includes the dishrack "hero demo" from the CENIC paper:
bazel run examples/hero_demo:convex_integrator_playback -- --visualize=0 --num_threads=4 --use_islands=1
On my machine, I see a modest speedup (25s vs 35s) when using islands and 4 threads.

The main reason for the benefit appears to be that solving each island separately appears to be much much cheaper than solving all islands together. So much so that we get a net speedup even though the islanded version takes more iterations overall:

Toward #23755, cc @joemasterjohn @rpoyner-tri

---

This change is 

[joemasterjohn]

Thanks, Vince! cc: @sherm1 for visibility as well.

The initial results from testing the first vibe-coded version look very promising. We should definitely talk about what is needed to get this into shape. My hope would be that this feature would be on-par with, if not more performant than manual joint-locking/item-locking. Having the clique topology analysis and island segregation is already a much more elegant solution than the proximity based item-locking.

@joemasterjohn made 1 comment.
Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes.

[joemasterjohn]

Ok, after briefly reviewing the code, I think this would be best broken up into 3 PRs to stay under the line limit for PRs as well as making this more manageable for reviewers:

1. Code for the new data structure and analysis (i.e. icf_partition.h/cc and tests)
2. Island aware overloads to icf_model and constraint pools
3. Hooking up the full feature into icf_solver (i.e. all the remaining code in the first commit here after PRs 1 & 2 are merged).

We can keep this draft PR open and rebased as we peel off those 3 PRs. WDYT?

@joemasterjohn made 1 comment.
Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes.

[rpoyner-tri]

I'm getting to the end game of joint locking, so could possibly help with code shuffling here. Let me know.

@rpoyner-tri made 1 comment.
Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes.

[vincekurtz]

That 3 PR train makes a lot of sense to me! Let me know if I can do anything/provide any input/etc to help make things go smoother.

[rpoyner-tri]

If you like, I can start trying structure some PRs.

@rpoyner-tri made 1 comment.
Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes.

[vincekurtz]

I like!

[rpoyner-tri]

Are you available to steal any joint locking feature reviews from Joe :) ?

@rpoyner-tri made 1 comment.
Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes.

[joemasterjohn]

Thanks, @rpoyner-tri! If you can structure the PRs, I can serve as feature reviewer.

@joemasterjohn made 1 comment.
Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes.

[vincekurtz]

Are you available to steal any joint locking feature reviews from Joe :) ?

If you're really desperate I suppose I could, but tbh I have no idea what's going on in joint locking world at the moment

[rpoyner-tri]

No worries.

@rpoyner-tri made 1 comment.
Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes.

[rpoyner-tri]

Do you have a sense of what the co-author credits should be in the patches? Vince + some coding agent?

@rpoyner-tri made 1 comment.
Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes.

[rpoyner-tri]

cf. #24635

[rpoyner-tri]

cf. #24636

@rpoyner-tri made 1 comment.
Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes.

[vincekurtz]

Do you have a sense of what the co-author credits should be in the patches? Vince + some coding agent?

I assume something like the linux kernel's pattern would apply here. I used Claude Opus 4.8.

[rpoyner-tri]

Perfect. Thanks!

@rpoyner-tri made 1 comment.
Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes.

[rpoyner-tri]

+a:@rpoyner-tri is signed up to shepherd this code into CENIC, though the main work will take place in other PRs. See:

• #24636
• #24635

@rpoyner-tri made 1 comment.
Reviewable status: LGTM missing from assignee rpoyner-tri(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes.

[rpoyner-tri]

+(status: do not merge) +(status: do not review) Keeping this alive for the nice summary statement.

@rpoyner-tri made 1 comment.
Reviewable status: LGTM missing from assignee rpoyner-tri(platform), needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes.