[SAP] ContactProblemGraph provides participating cliques (#16694)

* Allows to update partial permutations after creation.
* ContactProblemGraph provides the participating cliques.

multibody/contact_solvers/sap/BUILD.bazel

@@ -12,6 +12,17 @@ package(
     default_visibility = ["//visibility:public"],
 )
 
+drake_cc_library(
+    name = "contact_problem_graph",
+    srcs = ["contact_problem_graph.cc"],
+    hdrs = ["contact_problem_graph.h"],
+    deps = [
+        ":partial_permutation",
+        "//common:essential",
+        "//common:sorted_pair",
+    ],
+)
+
 drake_cc_library(
     name = "partial_permutation",
     srcs = ["partial_permutation.cc"],
@@ -43,16 +54,6 @@ drake_cc_library(
     ],
 )
 
-drake_cc_library(
-    name = "contact_problem_graph",
-    srcs = ["contact_problem_graph.cc"],
-    hdrs = ["contact_problem_graph.h"],
-    deps = [
-        "//common:essential",
-        "//common:sorted_pair",
-    ],
-)
-
 drake_cc_googletest(
     name = "partial_permutation_test",
     deps = [

multibody/contact_solvers/sap/contact_problem_graph.cc

@@ -24,12 +24,14 @@ ContactProblemGraph::ConstraintCluster::AddConstraint(
 }
 
 ContactProblemGraph::ContactProblemGraph(int num_cliques)
-    : num_cliques_(num_cliques) {
+    : num_cliques_(num_cliques), participating_cliques_(num_cliques) {
   DRAKE_THROW_UNLESS(num_cliques >= 0);
 }
 
 int ContactProblemGraph::AddConstraint(SortedPair<int> cliques,
                                        int num_constraint_equations) {
+  participating_cliques_.push(cliques.first());
+  participating_cliques_.push(cliques.second());
   auto [iterator, new_cluster] = pair_to_cluster_index_.insert(
       std::make_pair(cliques, clusters_.size()));
   if (new_cluster) clusters_.emplace_back(std::move(cliques));

multibody/contact_solvers/sap/contact_problem_graph.h

@@ -5,6 +5,7 @@
 
 #include "drake/common/drake_copyable.h"
 #include "drake/common/sorted_pair.h"
+#include "drake/multibody/contact_solvers/sap/partial_permutation.h"
 
 namespace drake {
 namespace multibody {
@@ -134,6 +135,16 @@ class ContactProblemGraph {
     return clusters_[cluster_index];
   }
 
+  /* Returns the permutation of participating cliques.
+   We say a clique is "participating" when referenced by a cluster (an edge) in
+   the graph. Since not all cliques might be participating, their "participating
+   clique index" can differ from their original clique (node) index.
+   Participating cliques are assigned an index in the order they get referenced
+   by AddConstraint(). */
+  const PartialPermutation& participating_cliques() const {
+    return participating_cliques_;
+  }
+
  private:
   /* Helper to add a constraint between a pair of cliques. */
   int AddConstraint(SortedPair<int> cliques, int num_constrained_dofs);
@@ -151,6 +162,7 @@ class ContactProblemGraph {
   std::vector<ConstraintCluster> clusters_;
   // Map cliques pair to cluster index.
   std::unordered_map<SortedPair<int>, int> pair_to_cluster_index_;
+  PartialPermutation participating_cliques_;
 };
 
 }  // namespace internal

multibody/contact_solvers/sap/partial_permutation.cc

@@ -58,6 +58,19 @@ PartialPermutation::PartialPermutation(std::vector<int> permutation)
   }
 }
 
+PartialPermutation::PartialPermutation(int domain_size) {
+  permutation_.resize(domain_size, -1);
+}
+
+int PartialPermutation::push(int i) {
+  DRAKE_THROW_UNLESS(0 <= i && i < domain_size());
+  if (!participates(i)) {
+    permutation_[i] = permuted_domain_size();
+    inverse_permutation_.push_back(i);
+  }
+  return permuted_index(i);
+}
+
 int PartialPermutation::permuted_index(int i) const {
   DRAKE_THROW_UNLESS(0 <= i && i < domain_size());
   if (permutation_[i] < 0) {

multibody/contact_solvers/sap/partial_permutation.h

@@ -56,6 +56,20 @@ class PartialPermutation {
   // i.e. there are holes in the permuted domain.
   explicit PartialPermutation(std::vector<int> permutation);
 
+  // Constructs a partial permutation of `domain_size` and
+  // permuted_domain_size() equal to zero. In other words, participates(i) =
+  // false for in [0, domain_size). The permutation can be updated with further
+  // calls to push().
+  // @throws exception if domain_size is negative.
+  explicit PartialPermutation(int domain_size);
+
+  // If participates(i) = false, defines P(i) = permuted_domain_size() and
+  // further increases the permuted domain size. If participates(i) = true, the
+  // permutation does not change and this method simply returns P(i).
+  // @throws exception if i is not in [0, domain_size()).
+  // @returns P(i), i.e. permuted_index(i).
+  int push(int i);
+
   // The size n of the domain.
   int domain_size() const { return permutation_.size(); }
 

multibody/contact_solvers/sap/test/contact_problem_graph_test.cc

@@ -90,6 +90,14 @@ GTEST_TEST(ContactGraph, Construction) {
   for (int i = 0; i < graph.num_clusters(); ++i) {
     compare_clusters(graph.clusters()[i], graph.get_cluster(i));
   }
+
+  // Verify participating cliques.
+  const PartialPermutation& p = graph.participating_cliques();
+  EXPECT_EQ(p.domain_size(), graph.num_cliques());
+  EXPECT_EQ(p.permuted_domain_size(), 3);
+  // Participating cliques are indexed in the order added to the problem.
+  std::vector<int> expected_p = {1, 2, -1, 0};
+  EXPECT_EQ(p.permutation(), expected_p);
 }
 
 }  // namespace

multibody/contact_solvers/sap/test/partial_permutation_test.cc

@@ -19,6 +19,31 @@ GTEST_TEST(PartialPermutation, EmptyPermutation) {
   EXPECT_EQ(p.permuted_domain_size(), 0);
 }
 
+// Creates a permutation of size 6 where none of the indexes participates and
+// adds permuted indexes one at a time with push().
+GTEST_TEST(PartialPermutation, PushElements) {
+  PartialPermutation p(6);
+  EXPECT_EQ(p.domain_size(), 6);
+  EXPECT_EQ(p.permuted_domain_size(), 0);
+  for (int i = 0; i < 6; ++i) {
+    EXPECT_FALSE(p.participates(i));
+  }
+
+  EXPECT_EQ(p.push(1), 0);
+  EXPECT_EQ(p.permuted_domain_size(), 1);  // permuted domain increases.
+  EXPECT_EQ(p.push(3), 1);
+  EXPECT_EQ(p.permuted_domain_size(), 2);  // permuted domain increases.
+  EXPECT_EQ(p.push(2), 2);
+  EXPECT_EQ(p.permuted_domain_size(), 3);  // permuted domain increases.
+  EXPECT_EQ(p.push(3), 1);                 // already added.
+  EXPECT_EQ(p.permuted_domain_size(), 3);  // No change.
+  EXPECT_EQ(p.push(5), 3);
+  EXPECT_EQ(p.permuted_domain_size(), 4);  // permuted domain increases.
+
+  const std::vector<int> expected_permutation = {-1, 0, 2, 1, -1, 3};
+  EXPECT_EQ(p.permutation(), expected_permutation);
+}
+
 GTEST_TEST(PartialPermutation, Construction) {
   const std::vector<int> permutation = {-1, 0, 2, 1, -1, 3};
   PartialPermutation p(permutation);