From 06e1deb12539f05eff968c5bd9e11d259916f268 Mon Sep 17 00:00:00 2001 From: Alejandro Castro Date: Wed, 2 Mar 2022 17:54:03 -0500 Subject: [PATCH] [SAP] ContactProblemGraph provides participating cliques (#16694) * Allows to update partial permutations after creation. * ContactProblemGraph provides the participating cliques. --- multibody/contact_solvers/sap/BUILD.bazel | 21 ++++++++-------- .../sap/contact_problem_graph.cc | 4 ++- .../sap/contact_problem_graph.h | 12 +++++++++ .../sap/partial_permutation.cc | 13 ++++++++++ .../contact_solvers/sap/partial_permutation.h | 14 +++++++++++ .../sap/test/contact_problem_graph_test.cc | 8 ++++++ .../sap/test/partial_permutation_test.cc | 25 +++++++++++++++++++ 7 files changed, 86 insertions(+), 11 deletions(-) diff --git a/multibody/contact_solvers/sap/BUILD.bazel b/multibody/contact_solvers/sap/BUILD.bazel index 101aad6543ba..3a82625cb4e4 100644 --- a/multibody/contact_solvers/sap/BUILD.bazel +++ b/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 = [ diff --git a/multibody/contact_solvers/sap/contact_problem_graph.cc b/multibody/contact_solvers/sap/contact_problem_graph.cc index feca59be57fd..826bacd581ba 100644 --- a/multibody/contact_solvers/sap/contact_problem_graph.cc +++ b/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 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)); diff --git a/multibody/contact_solvers/sap/contact_problem_graph.h b/multibody/contact_solvers/sap/contact_problem_graph.h index 6fd74a729af3..f13fc9a86628 100644 --- a/multibody/contact_solvers/sap/contact_problem_graph.h +++ b/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 cliques, int num_constrained_dofs); @@ -151,6 +162,7 @@ class ContactProblemGraph { std::vector clusters_; // Map cliques pair to cluster index. std::unordered_map, int> pair_to_cluster_index_; + PartialPermutation participating_cliques_; }; } // namespace internal diff --git a/multibody/contact_solvers/sap/partial_permutation.cc b/multibody/contact_solvers/sap/partial_permutation.cc index a5efe788c1fd..ca7ad2afe6e4 100644 --- a/multibody/contact_solvers/sap/partial_permutation.cc +++ b/multibody/contact_solvers/sap/partial_permutation.cc @@ -58,6 +58,19 @@ PartialPermutation::PartialPermutation(std::vector 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) { diff --git a/multibody/contact_solvers/sap/partial_permutation.h b/multibody/contact_solvers/sap/partial_permutation.h index cb839eb50506..25f863a8014c 100644 --- a/multibody/contact_solvers/sap/partial_permutation.h +++ b/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 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(); } diff --git a/multibody/contact_solvers/sap/test/contact_problem_graph_test.cc b/multibody/contact_solvers/sap/test/contact_problem_graph_test.cc index a6a12813aeae..bb10e38a66c2 100644 --- a/multibody/contact_solvers/sap/test/contact_problem_graph_test.cc +++ b/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 expected_p = {1, 2, -1, 0}; + EXPECT_EQ(p.permutation(), expected_p); } } // namespace diff --git a/multibody/contact_solvers/sap/test/partial_permutation_test.cc b/multibody/contact_solvers/sap/test/partial_permutation_test.cc index a04204a4898c..7e39d996279a 100644 --- a/multibody/contact_solvers/sap/test/partial_permutation_test.cc +++ b/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 expected_permutation = {-1, 0, 2, 1, -1, 3}; + EXPECT_EQ(p.permutation(), expected_permutation); +} + GTEST_TEST(PartialPermutation, Construction) { const std::vector permutation = {-1, 0, 2, 1, -1, 3}; PartialPermutation p(permutation);