diff --git a/MODULE.bazel b/MODULE.bazel index b2d801bd9..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 = "7e99d566b57deff9cfc02b016ae06930c6634da5" + commit = "ee740029598ea0f990cae2beca709d1eeed651f2" ) diff --git a/systems/controllers/sampling_based_c3_controller.cc b/systems/controllers/sampling_based_c3_controller.cc index 10f64d069..02f660658 100644 --- a/systems/controllers/sampling_based_c3_controller.cc +++ b/systems/controllers/sampling_based_c3_controller.cc @@ -1824,37 +1824,27 @@ 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(); + + // 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]); 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);