From 3c5fdb677f75cfe6c304a27eb17727eddb444314 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Sat, 21 Feb 2026 09:35:26 -0500 Subject: [PATCH 1/2] feat: resolve contacts by finding closest relevant geometries --- MODULE.bazel | 2 +- .../sampling_based_c3_controller.cc | 31 +++++-------------- 2 files changed, 8 insertions(+), 25 deletions(-) diff --git a/MODULE.bazel b/MODULE.bazel index b2d801bd9..66eefe4d3 100644 --- a/MODULE.bazel +++ b/MODULE.bazel @@ -93,7 +93,7 @@ bazel_dep(name = "c3") git_override( module_name = "c3", remote = "https://github.com/DAIRLab/c3.git", - commit = "7e99d566b57deff9cfc02b016ae06930c6634da5" + commit = "d75914212557f615d794e49a7cc30e099f01a6d0" ) diff --git a/systems/controllers/sampling_based_c3_controller.cc b/systems/controllers/sampling_based_c3_controller.cc index 10f64d069..5e094907f 100644 --- a/systems/controllers/sampling_based_c3_controller.cc +++ b/systems/controllers/sampling_based_c3_controller.cc @@ -1824,7 +1824,7 @@ vector> SamplingC3Controller::GetResolvedContactPairs( int n_contacts = std::accumulate(resolve_contacts_to_list.begin(), resolve_contacts_to_list.end(), 0); std::vector> resolved_contacts; - resolved_contacts.reserve(n_contacts); + resolved_contacts.clear(); for (int i = 0; i < contact_geoms.size(); i++) { DRAKE_DEMAND(contact_geoms[i].size() >= resolve_contacts_to_list[i]); @@ -1832,29 +1832,12 @@ vector> SamplingC3Controller::GetResolvedContactPairs( const auto& candidates = contact_geoms[i]; const int num_to_select = resolve_contacts_to_list[i]; - if (verbose && candidates.size() > 1) { - std::cout << "Contact pair " << i << " : choosing between:" << std::endl; - } - - std::vector distances; - distances.reserve(candidates.size()); - - for (const auto& pair : candidates) { - multibody::GeomGeomCollider collider(plant, pair); - auto [phi_i, J_i] = - collider.EvalPolytope(context, num_friction_directions[i]); - distances.push_back(phi_i); - } - - for (int j = 0; j < num_to_select; ++j) { - auto min_it = std::min_element(distances.begin(), distances.end()); - int min_index = std::distance(distances.begin(), min_it); - resolved_contacts.push_back(candidates[min_index]); - distances[min_index] = std::numeric_limits::infinity(); - - if (verbose && candidates.size() > 1) { - std::cout << " --> Chose option " << min_index << std::endl; - } + auto active_contacts = LCSFactory::GetNClosestContactPairs( + plant, context, contact_geoms[i], num_to_select); + if (!active_contacts.empty()) { + resolved_contacts.insert(resolved_contacts.end(), + active_contacts.begin(), + active_contacts.end()); } } DRAKE_DEMAND(resolved_contacts.size() == n_contacts); From dfb6714f6b88a80f5e1dff9fb64c3e6a34873d67 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Mon, 23 Feb 2026 13:21:17 -0500 Subject: [PATCH 2/2] build: update c3 commit and add comments --- MODULE.bazel | 2 +- systems/controllers/sampling_based_c3_controller.cc | 11 +++++++++-- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/MODULE.bazel b/MODULE.bazel index 66eefe4d3..b28b9b71e 100644 --- a/MODULE.bazel +++ b/MODULE.bazel @@ -93,7 +93,7 @@ bazel_dep(name = "c3") git_override( module_name = "c3", remote = "https://github.com/DAIRLab/c3.git", - commit = "d75914212557f615d794e49a7cc30e099f01a6d0" + commit = "ee740029598ea0f990cae2beca709d1eeed651f2" ) diff --git a/systems/controllers/sampling_based_c3_controller.cc b/systems/controllers/sampling_based_c3_controller.cc index 5e094907f..02f660658 100644 --- a/systems/controllers/sampling_based_c3_controller.cc +++ b/systems/controllers/sampling_based_c3_controller.cc @@ -1826,6 +1826,14 @@ vector> SamplingC3Controller::GetResolvedContactPairs( std::vector> resolved_contacts; resolved_contacts.clear(); + // contact_geoms represents contact pair groups, where each group may contain + // contact pairs between two objects, or between one object and multiple + // objects eg. the end-effector and multiple objects in the environment. + // resolve_contacts_to_list represents how many contact pairs to resolve to + // for each contact pair group. For each contact pair group, select the + // closest contact pairs up to the number specified by + // resolve_contacts_to_list for that group, and add those to the + // resolved_contacts list. for (int i = 0; i < contact_geoms.size(); i++) { DRAKE_DEMAND(contact_geoms[i].size() >= resolve_contacts_to_list[i]); @@ -1835,8 +1843,7 @@ vector> SamplingC3Controller::GetResolvedContactPairs( auto active_contacts = LCSFactory::GetNClosestContactPairs( plant, context, contact_geoms[i], num_to_select); if (!active_contacts.empty()) { - resolved_contacts.insert(resolved_contacts.end(), - active_contacts.begin(), + resolved_contacts.insert(resolved_contacts.end(), active_contacts.begin(), active_contacts.end()); } }