[multibody] Add IcfPartition, toward implementing constraint islands

[rpoyner-tri]

Toward #23755.

---

This change is 

[rpoyner-tri]

+(release notes: none)
+a:@joemasterjohn for feature review, please.
FYI @vincekurtz

@rpoyner-tri made 3 comments.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee joemasterjohn, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on joemasterjohn and rpoyner-tri).

---

-- commits line 2 at r1:
working: need coauthor credits

---

multibody/contact_solvers/icf/icf_partition.cc line 54 at r1 (raw file):

  // Compress the path so future queries are O(1).
  while (parent_[x] != root) {
    const int next = parent_[x];

minor: This loop body is not reached by unit tests. Should I worry?

[rpoyner-tri]

@rpoyner-tri resolved 1 discussion.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee joemasterjohn, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on joemasterjohn).

[rpoyner-tri]

@rpoyner-tri made 1 comment and resolved 1 discussion.
Reviewable status: LGTM missing from assignee joemasterjohn, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on joemasterjohn).

---

multibody/contact_solvers/icf/icf_partition.cc line 54 at r1 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

minor: This loop body is not reached by unit tests. Should I worry?

See added ForcePathCompression test.

[rpoyner-tri]

@rpoyner-tri made 1 comment.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee joemasterjohn, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on joemasterjohn).

---

multibody/contact_solvers/icf/icf_partition.cc line 21 at r3 (raw file):

  // island i, then we turn it into a prefix sum below. Items with a negative
  // island id are skipped.
  if (std::ssize(offsets_) < num_islands + 1) offsets_.resize(num_islands + 1);

nit This implementation pattern (checked resize) is kinda batty. Instead, we should just call resize() and trust the std::vector capactiy logic.

Here and throughout.

[rpoyner-tri]

@rpoyner-tri resolved 1 discussion.
Reviewable status: LGTM missing from assignee joemasterjohn, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on joemasterjohn).

[rpoyner-tri]

@joemasterjohn should I find another reviewer?

@rpoyner-tri made 1 comment.
Reviewable status: LGTM missing from assignee joemasterjohn, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on joemasterjohn).

[joemasterjohn]

Got caught up last week, but will take a look today.

@joemasterjohn made 1 comment.
Reviewable status: LGTM missing from assignee joemasterjohn, needs platform reviewer assigned, needs at least two assigned reviewers.

[joemasterjohn]

Feature review complete. PTAL.

@joemasterjohn reviewed 5 files and all commit messages, and made 17 comments.
Reviewable status: 15 unresolved discussions, LGTM missing from assignee joemasterjohn, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on rpoyner-tri).

---

multibody/contact_solvers/icf/icf_partition.h line 24 at r4 (raw file):

only allocate when the number of items or islands exceeds any previous
high-water mark. This matches ICF's "allocate once, reallocate minimally"
philosophy and keeps per-step recomputation allocation-free in steady state. */

BTW idk how to articulate why, but comments like these scream out to me as vibe-coded / LLM artifacts. Not necessarily wrong, and but not very technical, and a non-neutral tone. Like the mention of steady state is out of place here. IDK if I have a practical suggestion here, but maybe we can rewrite these in a more neutral tone with the level of technicality Drake comments tend to have?

Code quote:

only allocate when the number of items or islands exceeds any previous
high-water mark. This matches ICF's "allocate once, reallocate minimally"
philosophy and keeps per-step recomputation allocation-free in steady state. */

---

multibody/contact_solvers/icf/icf_partition.h line 55 at r4 (raw file):

  std::vector<int> offsets_;  // size = num_islands_ + 1; prefix sums.
  std::vector<int> items_;    // size = num_items_; item indices by island.
  std::vector<int> cursor_;   // size = num_islands_; scratch write cursors.

nit member comments are a not very self-explanatory.

Code quote:

  std::vector<int> offsets_;  // size = num_islands_ + 1; prefix sums.
  std::vector<int> items_;    // size = num_items_; item indices by island.
  std::vector<int> cursor_;   // size = num_islands_; scratch write cursors.

---

multibody/contact_solvers/icf/icf_partition.h line 69 at r4 (raw file):

Islands are numbered by the smallest clique index they contain, so the
numbering is deterministic across runs (and independent of thread scheduling).

BTW To be truly deterministic, the DOF ordering within the island needs to be stable as well, correct? Should that be mentioned?

Code quote:

Islands are numbered by the smallest clique index they contain, so the
numbering is deterministic across runs (and independent of thread scheduling).

---

multibody/contact_solvers/icf/icf_partition.h line 69 at r4 (raw file):

Islands are numbered by the smallest clique index they contain, so the
numbering is deterministic across runs (and independent of thread scheduling).

Vince's prototype included thread-level parallelism, but the initial results didn't seem to support the effort to maintain island parallelism in the solver. We should decide for this first pass, and remove any reference to parallelism in the first implementation.

Code quote:

(and independent of thread scheduling).

---

multibody/contact_solvers/icf/icf_partition.h line 123 at r4 (raw file):

  int num_islands_{0};
  std::vector<int> parent_;          // union-find forest, size = num_cliques_.
  std::vector<int> root_to_island_;  // scratch root→island, size = nc.

nit These comments are not very self explanatory. I can ascertain that parent_ contains the current parent in the Union-Find data structure. But I'm not sure about root_to_island_. Also I don't believe nc was ever defined and is mixed with num_cliques_.

Code quote:

  std::vector<int> parent_;          // union-find forest, size = num_cliques_.
  std::vector<int> root_to_island_;  // scratch root→island, size = nc.

---

multibody/contact_solvers/icf/icf_partition.h line 41 at r5 (raw file):

  int num_islands() const { return num_islands_; }
  int num_items() const { return num_items_; }

nit: num_items() deserves a one-line doc; its value is subtle (the number of items placed, i.e., excluding items omitted for negative island ids — not the size of the island_of_item input). E.g. "Returns the number of items assigned to some island; omitted items are not counted."

Code quote:

  int num_items() const { return num_items_; }

---

multibody/contact_solvers/icf/icf_partition.h line 68 at r5 (raw file):

subproblems.

Islands are numbered by the smallest clique index they contain, so the

nit: "Islands are numbered by the smallest clique index they contain" could be misread as island id == smallest clique index. I'd tighten this doc up a bit.

Suggestion:

Islands are numbered in increasing order of the smallest clique index they contain,

---

multibody/contact_solvers/icf/icf_partition.cc line 15 at r5 (raw file):

                          std::span<const int> island_of_item) {
  DRAKE_DEMAND(num_islands >= 0);
  const int num_input = static_cast<int>(island_of_item.size());

nit: use ssize() instead.

Suggestion:

  const int num_input = std::ssize(island_of_item);

---

multibody/contact_solvers/icf/icf_partition.cc line 22 at r5 (raw file):

  // island id are skipped.
  offsets_.resize(num_islands + 1);
  std::fill(offsets_.begin(), offsets_.begin() + num_islands + 1, 0);

nit: resize + fill can be accomplished with one call to assign

Suggestion:

  offsets_.assign(num_islands + 1, 0);

---

multibody/contact_solvers/icf/icf_partition.cc line 65 at r5 (raw file):

  const std::vector<std::vector<int>>& neighbors = pattern.neighbors();
  const int nc = std::ssize(neighbors);
  num_cliques_ = nc;

nit: using nc everywhere as a shorthand alias for num_cliques_ seems a bit hacky. Maybe just use num_cliques_ everywhere.

Code quote:

  const int nc = std::ssize(neighbors);
  num_cliques_ = nc;

---

multibody/contact_solvers/icf/icf_partition.cc line 79 at r5 (raw file):

        const int ri = Find(i);
        const int rj = Find(j);
        if (ri != rj) parent_[ri] = rj;

nit: Might be worth it to mention that we are not doing union-by-rank here deliberately, only path compression later. From what I remember TA-ing an algorithms course way back when is that if you omit one or the other you get worst case O(log(n)), and you need both for O(α(n)).

Code quote:

        if (ri != rj) parent_[ri] = rj;

---

multibody/contact_solvers/icf/icf_partition.cc line 88 at r5 (raw file):

  // giving a deterministic, scheduling-independent labeling.
  root_to_island_.resize(nc);
  std::fill(root_to_island_.begin(), root_to_island_.begin() + nc, -1);

nit: resize + fill can be accomplished with one call to assign

Suggestion:

  root_to_island_.assign(nc, -1);

---

multibody/contact_solvers/icf/icf_partition.cc line 98 at r5 (raw file):

  island_cliques_.Build(num_islands_,
                        std::span<const int>(clique_to_island_.data(), nc));

nit: It's safe to call clique_to_island() here.

Suggestion:

  island_cliques_.Build(num_islands_, clique_to_island());

---

multibody/contact_solvers/icf/test/icf_partition_test.cc line 125 at r5 (raw file):

  const auto local = p.clique_local_index();
  EXPECT_EQ(std::vector<int>(local.begin(), local.end()),
            (std::vector<int>{0, 0, 0, 1, 1, 2}));

BTW We're missing coverage for the span overload of clique_to_island(). You have coverage here for the span and indexed overload of clique_local_index(). We should likewise have it for clique_to_island().

Code quote:

  const auto local = p.clique_local_index();
  EXPECT_EQ(std::vector<int>(local.begin(), local.end()),
            (std::vector<int>{0, 0, 0, 1, 1, 2}));

---

multibody/contact_solvers/icf/test/icf_partition_test.cc line 149 at r5 (raw file):

  // even though the island structure is completely different. Build the pattern
  // outside the guard so we measure only Compute().
  const BlockSparsityPattern pattern = MakePattern(6, {{0, 1}, {1, 2}});

nit: Consider testing a case where the number of cliques shrinks as well as this case where it stays the same.

Code quote:

  // A subsequent Compute() on a problem within those bounds must not allocate,
  // even though the island structure is completely different. Build the pattern
  // outside the guard so we measure only Compute().
  const BlockSparsityPattern pattern = MakePattern(6, {{0, 1}, {1, 2}});

---

multibody/contact_solvers/icf/test/icf_partition_test.cc line 199 at r5 (raw file):

  EXPECT_EQ(map.items(2).size(), 1);
}

BTW Consider adding coverage for the 0-clique case (e.g. Build(0, {})) and the all negative case (e.g. Build(n, {-1, -1, -1, ...})).

[rpoyner-tri]

@rpoyner-tri made 1 comment.
Reviewable status: 16 unresolved discussions, LGTM missing from assignee joemasterjohn, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on rpoyner-tri).

---

multibody/contact_solvers/icf/icf_partition.h line 24 at r4 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

BTW idk how to articulate why, but comments like these scream out to me as vibe-coded / LLM artifacts. Not necessarily wrong, and but not very technical, and a non-neutral tone. Like the mention of steady state is out of place here. IDK if I have a practical suggestion here, but maybe we can rewrite these in a more neutral tone with the level of technicality Drake comments tend to have?

Agreed. All of this "grow only propaganda sounds like copy-paste from the prompt itself.

[rpoyner-tri]

@rpoyner-tri made 1 comment and resolved 16 discussions.
Reviewable status: LGTM missing from assignee joemasterjohn, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on joemasterjohn).

---

multibody/contact_solvers/icf/icf_partition.h line 69 at r4 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

BTW To be truly deterministic, the DOF ordering within the island needs to be stable as well, correct? Should that be mentioned?

It's mentioned at island_cliques(), so maybe good enough?

[rpoyner-tri]

Cleanups done. Ready to stamp?

@rpoyner-tri made 1 comment.
Reviewable status: LGTM missing from assignee joemasterjohn, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on joemasterjohn).

[rpoyner-tri]

+a:@sherm1 for platform review, please.

@rpoyner-tri made 1 comment.
Reviewable status: LGTM missing from assignees joemasterjohn,sherm1(platform) (waiting on joemasterjohn and sherm1).

[joemasterjohn]

feature.

@joemasterjohn reviewed 5 files and all commit messages, and made 1 comment.
Reviewable status: LGTM missing from assignee sherm1(platform) (waiting on sherm1).

[sherm1]

Neat! Platform with a few nits.

@sherm1 reviewed 5 files and all commit messages, and made 6 comments.
Reviewable status: 5 unresolved discussions (waiting on rpoyner-tri).

---

multibody/contact_solvers/icf/icf_partition.h line 16 at r6 (raw file):

namespace internal {

/* A compressed-storage (CSR-like) map from an island index to the list of item

BTW I didn't know the "CSR" acronym. Consider spelling it out ("Compressed Sparse Row"?)

---

multibody/contact_solvers/icf/icf_partition.h line 40 at r6 (raw file):

  int num_islands() const { return num_islands_; }
  /* Returns the total number of items assigned to some island; omitted items

nit: missing blank line between methods

---

multibody/contact_solvers/icf/icf_partition.h line 98 at r6 (raw file):

  /* Returns the per-clique island assignment, indexed by clique. Useful for
  mapping a constraint's representative clique to its island. */
  std::span<const int> clique_to_island() const {

BTW why a span here rather than const std::vector<int>& ? In my mind span has the implication of a sub-sequence yet this is really just a data member accessor. Cost me some time trying to understand it.

---

multibody/contact_solvers/icf/icf_partition.h line 118 at r6 (raw file):

  /* Returns the per-clique local-index map, indexed by clique. */
  std::span<const int> clique_local_index() const {

BTW also just an accessor?

---

multibody/contact_solvers/icf/icf_partition.cc line 67 at r6 (raw file):

  // Initialize the union-find forest with every clique as its own root.
  parent_.resize(num_cliques_);
  for (int c = 0; c < num_cliques_; ++c) parent_[c] = c;

nit: this data member parent_ seems misnamed. It gets used for a number of things that don't seem very parent-like. root_ might be closer but maybe something noncommittal like clique_temp_? If it is used for multiple purposes shared just to save memory and cache access, you could use local aliases to give it names that would make the code more readable locally.

[rpoyner-tri]

@rpoyner-tri made 1 comment and resolved 4 discussions.
Reviewable status: 1 unresolved discussion (waiting on sherm1).

---

multibody/contact_solvers/icf/icf_partition.cc line 67 at r6 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: this data member parent_ seems misnamed. It gets used for a number of things that don't seem very parent-like. root_ might be closer but maybe something noncommittal like clique_temp_? If it is used for multiple purposes shared just to save memory and cache access, you could use local aliases to give it names that would make the code more readable locally.

I'm torn. If you go look up canonical implementations of union-find, there is pretty much always something called parent. Sometimes its a field or method on a node; in our case, "nodes" are just clique indices, so this vector parent_ is a complete lookup table for the "parent" function in our little graph.

I've added some doc to the declaration; see what you think.

[sherm1]

@sherm1 reviewed 3 files and all commit messages, made 1 comment, and resolved 1 discussion.
Reviewable status: complete! all discussions resolved, LGTM from assignees sherm1(platform),joemasterjohn (waiting on rpoyner-tri).

---

multibody/contact_solvers/icf/icf_partition.cc line 67 at r6 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

I'm torn. If you go look up canonical implementations of union-find, there is pretty much always something called parent. Sometimes its a field or method on a node; in our case, "nodes" are just clique indices, so this vector parent_ is a complete lookup table for the "parent" function in our little graph.

I've added some doc to the declaration; see what you think.

I see, it's kind of a term-of-art. The added documentation looks good, thanks.