From d75914212557f615d794e49a7cc30e099f01a6d0 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Sat, 21 Feb 2026 00:39:03 -0500 Subject: [PATCH 1/4] feat: add preprocessing for lcs factory contact config --- multibody/geom_geom_collider.h | 26 ++++----- multibody/lcs_factory.cc | 94 +++++++++++++++++++++++++++------ multibody/lcs_factory.h | 21 ++++++++ multibody/lcs_factory_options.h | 8 +++ 4 files changed, 121 insertions(+), 28 deletions(-) diff --git a/multibody/geom_geom_collider.h b/multibody/geom_geom_collider.h index f1f45ae..ffce8c0 100644 --- a/multibody/geom_geom_collider.h +++ b/multibody/geom_geom_collider.h @@ -101,7 +101,6 @@ class GeomGeomCollider { std::pair, drake::VectorX> CalcWitnessPoints( const drake::systems::Context& context); - private: /** * @brief A struct to hold the results of a geometry query. * @@ -143,6 +142,19 @@ class GeomGeomCollider { Eigen::Vector3d p_BCb; }; + /** + * @brief Gets the geometry query result. + * + * This function queries the MultibodyPlant for the signed distance and + * closest points between the two geometries. + * + * @param context The context for the MultibodyPlant. + * @return A GeometryQueryResult struct containing the results of the query. + */ + GeometryQueryResult GetGeometryQueryResult( + const drake::systems::Context& context) const; + + private: /** * @brief Internal helper function for EvalPolytope and EvalPlanar. * @@ -206,18 +218,6 @@ class GeomGeomCollider { const Eigen::Vector3d& contact_normal, const Eigen::Vector3d& planar_normal) const; - /** - * @brief Gets the geometry query result. - * - * This function queries the MultibodyPlant for the signed distance and - * closest points between the two geometries. - * - * @param context The context for the MultibodyPlant. - * @return A GeometryQueryResult struct containing the results of the query. - */ - GeometryQueryResult GetGeometryQueryResult( - const drake::systems::Context& context) const; - /** * @brief Determines if the geometry pair consists of a sphere and a mesh. * diff --git a/multibody/lcs_factory.cc b/multibody/lcs_factory.cc index 0ae0c3d..23537d9 100644 --- a/multibody/lcs_factory.cc +++ b/multibody/lcs_factory.cc @@ -48,35 +48,65 @@ LCSFactory::LCSFactory( frictionless_(contact_model_ == ContactModel::kFrictionlessSpring), dt_(options.dt) { DRAKE_DEMAND(options.contact_pair_configs.has_value()); - n_contacts_ = options.contact_pair_configs.value().size(); + + n_contacts_ = 0; mu_.clear(); n_friction_directions_per_contact_.clear(); contact_pairs_.clear(); planar_normal_direction_per_contact_.clear(); + + // Process each contact pair configuration to populate contact pairs, friction + // coefficients, and friction directions. This allows for different friction + // properties for different pairs of bodies. for (auto& pair : options.contact_pair_configs.value()) { + std::vector> collision_geom_pairs; std::vector body_A_collision_geoms = - plant.GetCollisionGeometriesForBody(plant.GetBodyByName(pair.body_A)); + plant_.GetCollisionGeometriesForBody(plant_.GetBodyByName(pair.body_A)); std::vector body_B_collision_geoms = - plant.GetCollisionGeometriesForBody(plant.GetBodyByName(pair.body_B)); + plant_.GetCollisionGeometriesForBody(plant_.GetBodyByName(pair.body_B)); + + // If no specific indices are provided, use all collision geometries for + // each body. + if (pair.body_A_collision_geom_indices.empty()) { + pair.body_A_collision_geom_indices.resize(body_A_collision_geoms.size()); + std::iota(pair.body_A_collision_geom_indices.begin(), + pair.body_A_collision_geom_indices.end(), 0); + } + if (pair.body_B_collision_geom_indices.empty()) { + pair.body_B_collision_geom_indices.resize(body_B_collision_geoms.size()); + std::iota(pair.body_B_collision_geom_indices.begin(), + pair.body_B_collision_geom_indices.end(), 0); + } + for (int i : pair.body_A_collision_geom_indices) { for (int j : pair.body_B_collision_geom_indices) { - contact_pairs_.emplace_back(SortedPair( + collision_geom_pairs.emplace_back(SortedPair( body_A_collision_geoms[i], body_B_collision_geoms[j])); - mu_.push_back(pair.mu); - n_friction_directions_per_contact_.push_back( - pair.num_friction_directions); } } - if (pair.num_friction_directions == 1) { - DRAKE_DEMAND(pair.planar_normal_direction.has_value()); - DRAKE_DEMAND(pair.planar_normal_direction.value().size() == 3); - planar_normal_direction_per_contact_.push_back( - pair.planar_normal_direction.value()); - } else { - planar_normal_direction_per_contact_.push_back({}); - } + + // If no limit on the number of contact pairs is specified, or if the limit + // is equal to the total number of pairs, return all pairs. + if (pair.num_active_contact_pairs.has_value() && + pair.num_active_contact_pairs.value() != collision_geom_pairs.size()) + collision_geom_pairs = + GetNClosestContactPairs(plant_, context_, collision_geom_pairs, + pair.num_active_contact_pairs.value()); + + contact_pairs_.insert(contact_pairs_.end(), collision_geom_pairs.begin(), + collision_geom_pairs.end()); + mu_.insert(mu_.end(), collision_geom_pairs.size(), pair.mu); + n_friction_directions_per_contact_.insert( + n_friction_directions_per_contact_.end(), collision_geom_pairs.size(), + pair.num_friction_directions); + planar_normal_direction_per_contact_.insert( + planar_normal_direction_per_contact_.end(), collision_geom_pairs.size(), + pair.planar_normal_direction.value_or(std::array{})); + + n_contacts_ += collision_geom_pairs.size(); } + n_lambda_ = multibody::LCSFactory::GetNumContactVariables( contact_model_, n_contacts_, n_friction_directions_per_contact_); Jt_row_sizes_ = 2 * Eigen::Map( @@ -84,6 +114,40 @@ LCSFactory::LCSFactory( n_friction_directions_per_contact_.size()); } +std::vector> LCSFactory::GetNClosestContactPairs( + const MultibodyPlant& plant, const Context& context, + const std::vector>& contact_pairs, unsigned int N) { + // If the limit is zero, return an empty list. + if (N == 0) return {}; + + DRAKE_DEMAND(N <= contact_pairs.size()); + + // Compute signed distance for each contact pair and sort by distance. Then + // select the N closest pairs. + std::vector>> distance_pairs; + distance_pairs.reserve(contact_pairs.size()); + + for (const auto& geom_pair : contact_pairs) { + multibody::GeomGeomCollider collider(plant, geom_pair); + auto query_result = collider.GetGeometryQueryResult(context); + distance_pairs.emplace_back(query_result.distance, geom_pair); + } + + // Partition so that the N smallest are before distance_pairs.begin() + N + std::nth_element( + distance_pairs.begin(), distance_pairs.begin() + N, distance_pairs.end(), + [](const auto& a, const auto& b) { return a.first < b.first; }); + + // Extract first N elements (not sorted among themselves) + std::vector> active_contact_pairs; + active_contact_pairs.reserve(N); + for (int i = 0; i < N; ++i) { + active_contact_pairs.push_back(distance_pairs[i].second); + } + + return active_contact_pairs; +} + LCSFactory::LCSFactory( const MultibodyPlant& plant, Context& context, const MultibodyPlant& plant_ad, Context& context_ad, diff --git a/multibody/lcs_factory.h b/multibody/lcs_factory.h index 57274db..f8e13e0 100644 --- a/multibody/lcs_factory.h +++ b/multibody/lcs_factory.h @@ -83,6 +83,27 @@ class LCSFactory { contact_geoms, const LCSFactoryOptions& options); + /** + * @brief Retrieves the N closest contact pairs from the provided set. + * + * This method evaluates the signed distance for each contact pair and returns + * the N pairs with the smallest (most negative or closest to zero) distances. + * + * @param plant The MultibodyPlant to query for contact information. + * @param context The context containing the current state. + * @param contact_pairs Vector of all potential contact geometry pairs. + * @param N The number of closest contact pairs to return. + * @return std::vector> Vector + * of the N closest contact geometry pairs. + */ + static std::vector> + GetNClosestContactPairs( + const drake::multibody::MultibodyPlant& plant, + const drake::systems::Context& context, + const std::vector>& + contact_pairs, + unsigned int N); + /** * @brief Generates a Linear Complementarity System (LCS). * diff --git a/multibody/lcs_factory_options.h b/multibody/lcs_factory_options.h index b85597c..b234e02 100644 --- a/multibody/lcs_factory_options.h +++ b/multibody/lcs_factory_options.h @@ -16,6 +16,13 @@ struct ContactPairConfig { std::optional> planar_normal_direction; // optional normal vector for planar contact double mu; // friction coefficient + std::optional + num_active_contact_pairs; // Number of geometry-geometry contact pairs to + // consider (e.g., if body_A has 10 geoms and + // body_B has 5 geoms, there are 50 possible + // pairs. Set this to limit how many are used + // in the LCS. If unspecified, all pairs are + // considered.) template void Serialize(Archive* a) { @@ -26,6 +33,7 @@ struct ContactPairConfig { a->Visit(DRAKE_NVP(num_friction_directions)); a->Visit(DRAKE_NVP(planar_normal_direction)); a->Visit(DRAKE_NVP(mu)); + a->Visit(DRAKE_NVP(num_active_contact_pairs)); } }; From 6924818e3b9ffd7f837a8f3b229c04b37c304c10 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Sat, 21 Feb 2026 10:09:27 -0500 Subject: [PATCH 2/4] test: add unit tests --- multibody/test/multibody_test.cc | 183 +++++++++++++++++++++++++++++++ 1 file changed, 183 insertions(+) diff --git a/multibody/test/multibody_test.cc b/multibody/test/multibody_test.cc index b8ebb4e..d7fb687 100644 --- a/multibody/test/multibody_test.cc +++ b/multibody/test/multibody_test.cc @@ -426,6 +426,189 @@ GTEST_TEST(GeomGeomColliderTest, SphereMeshDistance) { EXPECT_TRUE(J_contact.allFinite()); } +// Parameterized test for GetNClosestContactPairs with different values of N +class GetNClosestContactPairsTest + : public ::testing::TestWithParam { + protected: + void SetUp() override { + // Setup plant with scene graph for collision detection + DiagramBuilder builder; + std::tie(plant_ptr, scene_graph) = + AddMultibodyPlantSceneGraph(&builder, 0.0); + + // Use fixed seed for reproducibility + std::mt19937 rng(42); + std::uniform_real_distribution height_dist(0.05, 0.5); + + // Add 5 cubes at random heights + for (int i = 0; i < 5; ++i) { + std::string cube_name = "cube_" + std::to_string(i); + object_names.push_back(cube_name); + + double height = height_dist(rng); + + // Create cube shape (0.1m x 0.1m x 0.1m) + auto cube_shape = drake::geometry::Box(0.1, 0.1, 0.1); + const auto& cube_body = plant_ptr->AddRigidBody( + cube_name, drake::multibody::SpatialInertia::MakeUnitary()); + + // Position cube at height above ground + drake::math::RigidTransformd cube_pose( + Eigen::Quaterniond::Identity(), + Eigen::Vector3d(i * 0.2, 0.0, height)); + + plant_ptr->RegisterCollisionGeometry( + cube_body, drake::math::RigidTransformd(), cube_shape, + cube_name + "_collision", + drake::multibody::CoulombFriction(0.5, 0.5)); + + plant_ptr->SetDefaultFreeBodyPose(cube_body, cube_pose); + + // Store height for this object + object_heights[cube_name] = height; + } + + // Add 5 spheres at random heights + for (int i = 0; i < 5; ++i) { + std::string sphere_name = "sphere_" + std::to_string(i); + object_names.push_back(sphere_name); + + double height = height_dist(rng); + + // Create sphere shape (radius = 0.05m) + auto sphere_shape = drake::geometry::Sphere(0.05); + const auto& sphere_body = plant_ptr->AddRigidBody( + sphere_name, drake::multibody::SpatialInertia::MakeUnitary()); + + // Position sphere at height above ground (offset in y direction) + drake::math::RigidTransformd sphere_pose( + Eigen::Quaterniond::Identity(), + Eigen::Vector3d(i * 0.2, 0.3, height)); + + plant_ptr->RegisterCollisionGeometry( + sphere_body, drake::math::RigidTransformd(), sphere_shape, + sphere_name + "_collision", + drake::multibody::CoulombFriction(0.5, 0.5)); + + plant_ptr->SetDefaultFreeBodyPose(sphere_body, sphere_pose); + + // Store height for this object + object_heights[sphere_name] = height; + } + + // Add ground plane (table) + double table_thickness = 0.05; + double table_width = 2.0; + auto table_shape = + drake::geometry::Box(table_width, table_width, table_thickness); + const auto& table_body = plant_ptr->AddRigidBody( + "table", drake::multibody::SpatialInertia::MakeUnitary()); + + drake::math::RigidTransformd table_pose( + Eigen::Quaterniond::Identity(), + Eigen::Vector3d(0.0, 0.0, -table_thickness / 2.0)); + + plant_ptr->RegisterCollisionGeometry( + table_body, drake::math::RigidTransformd(), table_shape, + "table_collision", drake::multibody::CoulombFriction(1.0, 1.0)); + + plant_ptr->WeldFrames(plant_ptr->world_frame(), table_body.body_frame(), + table_pose); + + plant_ptr->Finalize(); + + // Build diagram and get context + diagram = builder.Build(); + diagram_context = diagram->CreateDefaultContext(); + context = + &diagram->GetMutableSubsystemContext(*plant_ptr, diagram_context.get()); + + // Create contact pairs between all objects and table + auto table_geoms = plant_ptr->GetCollisionGeometriesForBody( + plant_ptr->GetBodyByName("table")); + + for (const auto& object_name : object_names) { + auto object_geoms = plant_ptr->GetCollisionGeometriesForBody( + plant_ptr->GetBodyByName(object_name)); + + if (!object_geoms.empty() && !table_geoms.empty()) { + auto pair = SortedPair(object_geoms[0], table_geoms[0]); + all_pairs.push_back(pair); + + // Map contact pair to object height (proxy for distance) + pair_to_distance[pair] = object_heights[object_name]; + } + } + + ASSERT_EQ(all_pairs.size(), 10); // 5 cubes + 5 spheres + } + + MultibodyPlant* plant_ptr; + SceneGraph* scene_graph; + std::unique_ptr> diagram; + std::unique_ptr> diagram_context; + Context* context; + + std::vector object_names; + std::map object_heights; + std::vector> all_pairs; + std::map, double> pair_to_distance; +}; + +TEST_P(GetNClosestContactPairsTest, SelectsClosestN) { + unsigned int N = GetParam(); + + // Handle case where N exceeds available pairs + if (N > all_pairs.size()) { + // DRAKE_DEMAND aborts the process, not throws an exception + EXPECT_DEATH( + LCSFactory::GetNClosestContactPairs(*plant_ptr, *context, all_pairs, N), + "condition 'N <= contact_pairs.size\\(\\)' failed"); + return; + } + + // Get N closest contact pairs + auto selected = + LCSFactory::GetNClosestContactPairs(*plant_ptr, *context, all_pairs, N); + ASSERT_EQ(selected.size(), N); + + if (N == 0) { + // If N=0, no pairs should be selected + EXPECT_TRUE(selected.empty()); + return; + } + + // Get distances for selected pairs + std::vector selected_distances; + selected_distances.reserve(selected.size()); + for (const auto& pair : selected) { + ASSERT_TRUE(pair_to_distance.count(pair) > 0); + selected_distances.push_back(pair_to_distance.at(pair)); + } + + // Sort selected distances + std::sort(selected_distances.begin(), selected_distances.end()); + + // Get all distances and sort them + std::vector all_distances; + all_distances.reserve(all_pairs.size()); + for (const auto& [pair, distance] : pair_to_distance) { + all_distances.push_back(distance); + } + std::sort(all_distances.begin(), all_distances.end()); + + // Verify selected distances match the N smallest + for (size_t i = 0; i < N; ++i) { + EXPECT_NEAR(selected_distances[i], all_distances[i], 1e-6) + << "Selected distance at index " << i << " is " << selected_distances[i] + << " but expected " << all_distances[i]; + } +} + +// Test with N = 1, 2, 3, 5, 7, 10 (all), and 15 (exceeds total) +INSTANTIATE_TEST_SUITE_P(DifferentSubsetSizes, GetNClosestContactPairsTest, + ::testing::Values(0, 1, 5, 10, 15)); + } // namespace test } // namespace multibody } // namespace c3 From 5fded9afa288348c9b68c29ba8996bdaef7608c9 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Sat, 21 Feb 2026 10:10:45 -0500 Subject: [PATCH 3/4] binding: add python bindings --- bindings/pyc3/c3_multibody_py.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/bindings/pyc3/c3_multibody_py.cc b/bindings/pyc3/c3_multibody_py.cc index 5fdd861..0560d07 100644 --- a/bindings/pyc3/c3_multibody_py.cc +++ b/bindings/pyc3/c3_multibody_py.cc @@ -47,6 +47,10 @@ PYBIND11_MODULE(multibody, m) { py::arg("plant"), py::arg("context"), py::arg("plant_ad"), py::arg("context_ad"), py::arg("contact_geoms"), py::arg("options"), py::arg("state"), py::arg("input")) + .def_static("GetNClosestContactPairs", + &c3::multibody::LCSFactory::GetNClosestContactPairs, + py::arg("plant"), py::arg("context"), + py::arg("contact_pairs"), py::arg("N")) .def_static("FixSomeModes", &c3::multibody::LCSFactory::FixSomeModes, py::arg("other"), py::arg("active_lambda_inds"), py::arg("inactive_lambda_inds")) From eaebc7663be6765e2a51f10ed29912f76ac6f6f0 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Mon, 23 Feb 2026 12:57:39 -0500 Subject: [PATCH 4/4] Reviewer Requests --- multibody/test/multibody_test.cc | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/multibody/test/multibody_test.cc b/multibody/test/multibody_test.cc index d7fb687..dd9d9cc 100644 --- a/multibody/test/multibody_test.cc +++ b/multibody/test/multibody_test.cc @@ -465,7 +465,8 @@ class GetNClosestContactPairsTest plant_ptr->SetDefaultFreeBodyPose(cube_body, cube_pose); // Store height for this object - object_heights[cube_name] = height; + object_ground_distances[cube_name] = + height - 0.05; // Distance from ground is height minus half cube size } // Add 5 spheres at random heights @@ -493,7 +494,8 @@ class GetNClosestContactPairsTest plant_ptr->SetDefaultFreeBodyPose(sphere_body, sphere_pose); // Store height for this object - object_heights[sphere_name] = height; + object_ground_distances[sphere_name] = + height - 0.05; // Distance from ground is height minus radius } // Add ground plane (table) @@ -536,7 +538,7 @@ class GetNClosestContactPairsTest all_pairs.push_back(pair); // Map contact pair to object height (proxy for distance) - pair_to_distance[pair] = object_heights[object_name]; + pair_to_distance[pair] = object_ground_distances[object_name]; } } @@ -550,7 +552,7 @@ class GetNClosestContactPairsTest Context* context; std::vector object_names; - std::map object_heights; + std::map object_ground_distances; std::vector> all_pairs; std::map, double> pair_to_distance; };