[multibody] Add IcfPartition, toward implementing constraint islands

bindings/generated_docstrings/multibody_contact_solvers_icf.h

@@ -23,6 +23,7 @@
 // #include "drake/multibody/contact_solvers/icf/icf_external_systems_linearizer.h"
 // #include "drake/multibody/contact_solvers/icf/icf_linear_feedback_gains.h"
 // #include "drake/multibody/contact_solvers/icf/icf_model.h"
+// #include "drake/multibody/contact_solvers/icf/icf_partition.h"
 // #include "drake/multibody/contact_solvers/icf/icf_search_direction_data.h"
 // #include "drake/multibody/contact_solvers/icf/icf_solver.h"
 // #include "drake/multibody/contact_solvers/icf/icf_solver_parameters.h"

multibody/contact_solvers/icf/BUILD.bazel

@@ -21,6 +21,7 @@ drake_cc_package_library(
         ":icf_external_systems_linearizer",
         ":icf_linear_feedback_gains",
         ":icf_model",
+        ":icf_partition",
         ":icf_search_direction_data",
         ":icf_solver",
         ":icf_solver_parameters",
@@ -94,6 +95,16 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "icf_partition",
+    srcs = ["icf_partition.cc"],
+    hdrs = ["icf_partition.h"],
+    deps = [
+        "//common:essential",
+        "//multibody/contact_solvers:block_sparse_lower_triangular_or_symmetric_matrix",  # noqa
+    ],
+)
+
 drake_cc_library(
     name = "icf_search_direction_data",
     hdrs = ["icf_search_direction_data.h"],
@@ -299,6 +310,14 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "icf_partition_test",
+    deps = [
+        ":icf_partition",
+        "//common/test_utilities:limit_malloc",
+    ],
+)
+
 drake_cc_googletest(
     name = "icf_solver_test",
     deps = [

multibody/contact_solvers/icf/icf_partition.cc

@@ -0,0 +1,116 @@
+#include "drake/multibody/contact_solvers/icf/icf_partition.h"
+
+#include <algorithm>
+#include <iterator>
+
+namespace drake {
+namespace multibody {
+namespace contact_solvers {
+namespace icf {
+namespace internal {
+
+void IslandItemMap::Build(int num_islands,
+                          std::span<const int> island_of_item) {
+  DRAKE_DEMAND(num_islands >= 0);
+  const int num_input = static_cast<int>(island_of_item.size());
+  num_islands_ = num_islands;
+
+  // Count the items per island. offsets_[i + 1] temporarily holds the count for
+  // island i, then we turn it into a prefix sum below. Items with a negative
+  // island id are skipped.
+  offsets_.resize(num_islands + 1);
+  std::fill(offsets_.begin(), offsets_.begin() + num_islands + 1, 0);
+  int num_placed = 0;
+  for (int k = 0; k < num_input; ++k) {
+    const int island = island_of_item[k];
+    if (island < 0) continue;
+    DRAKE_ASSERT(island < num_islands);
+    ++offsets_[island + 1];
+    ++num_placed;
+  }
+  num_items_ = num_placed;
+  for (int i = 0; i < num_islands; ++i) {
+    offsets_[i + 1] += offsets_[i];
+  }
+
+  // Scatter each item into its island's slot, advancing a per-island cursor.
+  // Scanning k in increasing order keeps each island's items sorted.
+  cursor_.resize(num_islands);
+  std::copy(offsets_.begin(), offsets_.begin() + num_islands, cursor_.begin());
+  items_.resize(num_placed);
+  for (int k = 0; k < num_input; ++k) {
+    const int island = island_of_item[k];
+    if (island < 0) continue;
+    items_[cursor_[island]++] = k;
+  }
+}
+
+int IcfPartition::Find(int x) {
+  // Find the root.
+  int root = x;
+  while (parent_[root] != root) root = parent_[root];
+  // Compress the path so future queries are O(1).
+  while (parent_[x] != root) {
+    const int next = parent_[x];
+    parent_[x] = root;
+    x = next;
+  }
+  return root;
+}
+
+void IcfPartition::Compute(
+    const contact_solvers::internal::BlockSparsityPattern& pattern) {
+  const std::vector<std::vector<int>>& neighbors = pattern.neighbors();
+  const int nc = std::ssize(neighbors);
+  num_cliques_ = nc;
+
+  // Initialize the union-find forest with every clique as its own root.
+  parent_.resize(nc);
+  for (int c = 0; c < nc; ++c) parent_[c] = c;
+
+  // Union cliques across every off-diagonal edge. neighbors[j] lists the rows
+  // i ≥ j of the nonzero blocks in column j (with neighbors[j][0] == j), so
+  // iterating all (j, i) with i != j visits every edge exactly once.
+  for (int j = 0; j < nc; ++j) {
+    for (int i : neighbors[j]) {
+      if (i != j) {
+        const int ri = Find(i);
+        const int rj = Find(j);
+        if (ri != rj) parent_[ri] = rj;
+      }
+    }
+  }
+
+  // Assign island ids. Numbering cliques in increasing order makes island i
+  // the component whose smallest clique index is the i-th smallest overall,
+  // giving a deterministic, scheduling-independent labeling.
+  root_to_island_.resize(nc);
+  std::fill(root_to_island_.begin(), root_to_island_.begin() + nc, -1);
+  clique_to_island_.resize(nc);
+  num_islands_ = 0;
+  for (int c = 0; c < nc; ++c) {
+    const int r = Find(c);
+    if (root_to_island_[r] < 0) root_to_island_[r] = num_islands_++;
+    clique_to_island_[c] = root_to_island_[r];
+  }
+
+  island_cliques_.Build(num_islands_,
+                        std::span<const int>(clique_to_island_.data(), nc));
+
+  // Assign each clique its local index: its position within its island's
+  // (ascending) clique list. This is the block index the clique maps to in the
+  // island's local sub-Hessian.
+  clique_local_index_.resize(nc);
+  for (int i = 0; i < num_islands_; ++i) {
+    const std::span<const int> cliques = island_cliques_.items(i);
+    for (int pos = 0; pos < std::ssize(cliques); ++pos) {
+      clique_local_index_[cliques[pos]] = pos;
+    }
+  }
+}
+
+}  // namespace internal
+}  // namespace icf
+}  // namespace contact_solvers
+}  // namespace multibody
+}  // namespace drake

multibody/contact_solvers/icf/icf_partition.h

@@ -0,0 +1,133 @@
+#pragma once
+
+#include <span>
+#include <vector>
+
+#include "drake/common/drake_assert.h"
+#include "drake/common/drake_copyable.h"
+#include "drake/multibody/contact_solvers/block_sparse_lower_triangular_or_symmetric_matrix.h"
+
+namespace drake {
+namespace multibody {
+namespace contact_solvers {
+namespace icf {
+namespace internal {
+
+/* A compressed-storage (CSR-like) map from an island index to the list of item
+indices assigned to that island. It is used to group items (cliques or
+constraints) by the island (connected component) they belong to, so that
+per-island computations can iterate only over the items they own.
+
+Storage is grow-only: repeated calls to Build() reuse existing capacity and
+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. */
+class IslandItemMap {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(IslandItemMap);
+  IslandItemMap() = default;
+
+  /* Groups items by island via a counting sort. On return, items(i) lists
+  every item index k (in increasing order) for which island_of_item[k] == i.
+  Items with a negative island id are omitted from all groups; this is used to
+  drop items that belong to no island (e.g., anchored bodies).
+  @param num_islands     The total number of islands (≥ 0).
+  @param island_of_item  island_of_item[k] is the island of item k, or a
+                         negative value to omit item k.
+  @pre Every non-negative entry of island_of_item is in [0, num_islands). */
+  void Build(int num_islands, std::span<const int> island_of_item);
+
+  int num_islands() const { return num_islands_; }
+  int num_items() const { return num_items_; }
+
+  /* Returns the items assigned to the given island, in increasing order. */
+  std::span<const int> items(int island) const {
+    DRAKE_ASSERT(0 <= island && island < num_islands_);
+    return std::span<const int>(items_.data() + offsets_[island],
+                                offsets_[island + 1] - offsets_[island]);
+  }
+
+ private:
+  int num_islands_{0};
+  int num_items_{0};
+  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.
+};
+
+/* Partitions the cliques of an ICF problem into islands: the connected
+components of the clique-connectivity graph. Two cliques are in the same island
+iff they are coupled (directly or transitively) by a constraint, i.e., iff they
+are connected in the block sparsity pattern of the Hessian.
+
+Because the Hessian is block-diagonal across islands, the cost is additive over
+islands, and the gradient is block-disjoint, each island is an independent
+convex subproblem that can be solved on its own. This class identifies those
+subproblems.
+
+Islands are numbered by the smallest clique index they contain, so the
+numbering is deterministic across runs (and independent of thread scheduling).
+All storage is grow-only across calls to Compute(). */
+class IcfPartition {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(IcfPartition);
+  IcfPartition() = default;
+
+  /* Computes the islands of the clique graph encoded by `pattern`. The diagonal
+  blocks of `pattern` are the cliques (graph nodes); an off-diagonal nonzero
+  block (i, j) is an edge between cliques i and j. */
+  void Compute(const contact_solvers::internal::BlockSparsityPattern& pattern);
+
+  int num_cliques() const { return num_cliques_; }
+  int num_islands() const { return num_islands_; }
+
+  /* Returns the island index of the given clique. */
+  int clique_to_island(int clique) const {
+    DRAKE_ASSERT(0 <= clique && clique < num_cliques_);
+    return clique_to_island_[clique];
+  }
+
+  /* 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 {
+    return std::span<const int>(clique_to_island_.data(), num_cliques_);
+  }
+
+  /* Returns the cliques belonging to the given island, in increasing order. */
+  std::span<const int> island_cliques(int island) const {
+    return island_cliques_.items(island);
+  }
+
+  /* Returns the local index of the given clique within its island, i.e., its
+  position (0-based) in island_cliques(clique_to_island(clique)). This is the
+  block index the clique maps to in the island's local sub-Hessian. Because
+  island_cliques() is sorted ascending, local indices preserve the global clique
+  ordering within an island. */
+  int clique_local_index(int clique) const {
+    DRAKE_ASSERT(0 <= clique && clique < num_cliques_);
+    return clique_local_index_[clique];
+  }
+
+  /* Returns the per-clique local-index map, indexed by clique. */
+  std::span<const int> clique_local_index() const {
+    return std::span<const int>(clique_local_index_.data(), num_cliques_);
+  }
+
+ private:
+  /* Union-find "find" with path compression, operating on parent_. */
+  int Find(int x);
+
+  int num_cliques_{0};
+  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.
+  std::vector<int> clique_to_island_;    // size = num_cliques_.
+  std::vector<int> clique_local_index_;  // size = num_cliques_.
+  IslandItemMap island_cliques_;
+};
+
+}  // namespace internal
+}  // namespace icf
+}  // namespace contact_solvers
+}  // namespace multibody
+}  // namespace drake