From 0e068f03ea39ae973bec80c25861396e3ecc9994 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Sat, 7 Feb 2026 17:14:22 -0500 Subject: [PATCH 1/2] feat: add planar normal config for lcs factory options --- bindings/pyc3/BUILD.bazel | 2 + bindings/pyc3/c3_multibody_py.cc | 8 ++- .../c3Plus_controller_cartpole_options.yaml | 1 + .../c3_controller_cartpole_options.yaml | 1 + .../c3_controller_pivoting_options.yaml | 3 ++ multibody/lcs_factory.cc | 52 +++++++++++++++++-- multibody/lcs_factory.h | 6 ++- multibody/lcs_factory_options.h | 14 ++++- .../lcs_factory_pivoting_options.yaml | 4 ++ 9 files changed, 85 insertions(+), 6 deletions(-) diff --git a/bindings/pyc3/BUILD.bazel b/bindings/pyc3/BUILD.bazel index 789b30d..ced0886 100644 --- a/bindings/pyc3/BUILD.bazel +++ b/bindings/pyc3/BUILD.bazel @@ -47,10 +47,12 @@ pybind_py_library( "@drake//bindings/pydrake/common:default_scalars_pybind", "@drake//bindings/pydrake/common:value_pybind", "@drake//bindings/pydrake/common:sorted_pair_pybind", + "@drake//bindings/pydrake/common:type_pack", ], cc_so_name = "systems", cc_srcs = ["c3_systems_py.cc"], py_deps = [ + "@drake//bindings/pydrake", # TODO: This may break pip install when building pyc3_wheel ":module_py", ], py_imports = ["."], diff --git a/bindings/pyc3/c3_multibody_py.cc b/bindings/pyc3/c3_multibody_py.cc index ca7abea..5fdd861 100644 --- a/bindings/pyc3/c3_multibody_py.cc +++ b/bindings/pyc3/c3_multibody_py.cc @@ -71,7 +71,9 @@ PYBIND11_MODULE(multibody, m) { &ContactPairConfig::body_B_collision_geom_indices) .def_readwrite("mu", &ContactPairConfig::mu) .def_readwrite("num_friction_directions", - &ContactPairConfig::num_friction_directions); + &ContactPairConfig::num_friction_directions) + .def_readwrite("planar_normal_direction", + &ContactPairConfig::planar_normal_direction); py::class_(m, "LCSFactoryOptions") .def(py::init<>()) @@ -85,6 +87,10 @@ PYBIND11_MODULE(multibody, m) { .def_readwrite("num_contacts", &LCSFactoryOptions::num_contacts) .def_readwrite("spring_stiffness", &LCSFactoryOptions::spring_stiffness) .def_readwrite("mu", &LCSFactoryOptions::mu) + .def_readwrite("planar_normal_direction", + &LCSFactoryOptions::planar_normal_direction) + .def_readwrite("planar_normal_direction_per_contact", + &LCSFactoryOptions::planar_normal_direction_per_contact) .def_readwrite("contact_pair_configs", &LCSFactoryOptions::contact_pair_configs); diff --git a/examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml b/examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml index e6b4972..e25e758 100644 --- a/examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml +++ b/examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml @@ -17,6 +17,7 @@ lcs_factory_options: mu: [0, 0] N: 5 dt: 0.01 + planar_normal_direction: [0, 1, 0] c3_options: warm_start: false diff --git a/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml b/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml index 1f886fa..ced5cf6 100644 --- a/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml +++ b/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml @@ -17,6 +17,7 @@ lcs_factory_options: mu: [ 0, 0 ] N: 5 dt: 0.01 + planar_normal_direction: [0, 1, 0] c3_options: warm_start: false diff --git a/examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml b/examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml index 8e83e9c..7c8b2b9 100644 --- a/examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml +++ b/examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml @@ -20,18 +20,21 @@ lcs_factory_options: body_B_collision_geom_indices: [0] mu: 0.1 num_friction_directions: 1 + planar_normal_direction: [0, 1, 0] - body_A: "cube" body_A_collision_geom_indices: [0] body_B: "right_finger" body_B_collision_geom_indices: [0] mu: 0.1 num_friction_directions: 1 + planar_normal_direction: [0, 1, 0] - body_A: "cube" body_A_collision_geom_indices: [0] body_B: "platform" body_B_collision_geom_indices: [0] mu: 0.1 num_friction_directions: 1 + planar_normal_direction: [0, 1, 0] c3_options: warm_start: false diff --git a/multibody/lcs_factory.cc b/multibody/lcs_factory.cc index 6d40095..40beda2 100644 --- a/multibody/lcs_factory.cc +++ b/multibody/lcs_factory.cc @@ -53,6 +53,7 @@ LCSFactory::LCSFactory( mu_.clear(); n_friction_directions_per_contact_.clear(); contact_pairs_.clear(); + planar_normal_direction_per_contact_.clear(); for (auto& pair : options.contact_pair_configs.value()) { std::vector body_A_collision_geoms = plant.GetCollisionGeometriesForBody(plant.GetBodyByName(pair.body_A)); @@ -67,6 +68,14 @@ LCSFactory::LCSFactory( 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({}); + } } n_lambda_ = multibody::LCSFactory::GetNumContactVariables( contact_model_, n_contacts_, n_friction_directions_per_contact_); @@ -100,6 +109,41 @@ LCSFactory::LCSFactory( mu_(options.mu.value()), frictionless_(contact_model_ == ContactModel::kFrictionlessSpring), dt_(options.dt) { + // Handle planar normal directions for contacts with 1 friction direction + if (options_.planar_normal_direction.has_value()) { + // Single planar normal provided - replicate for all planar contacts + DRAKE_DEMAND(options_.planar_normal_direction.value().size() == 3); + planar_normal_direction_per_contact_.clear(); + + for (int i = 0; i < n_contacts_; ++i) { + if (n_friction_directions_per_contact_[i] == 1) { + // Planar contact - use the provided normal + planar_normal_direction_per_contact_.push_back( + options_.planar_normal_direction.value()); + } else { + // Non-planar contact - push empty array (unused) + planar_normal_direction_per_contact_.push_back({}); + } + } + } else { + // Per-contact planar normals provided - validate them + DRAKE_DEMAND(options_.planar_normal_direction_per_contact.has_value()); + DRAKE_DEMAND(options_.planar_normal_direction_per_contact.value().size() == + (size_t)n_contacts_); + + // Validate that planar contacts (num_friction_directions == 1) have + // valid 3D normal vectors + for (size_t i = 0; i < n_contacts_; ++i) { + if (n_friction_directions_per_contact_[i] == 1) { + planar_normal_direction_per_contact_.push_back( + options_.planar_normal_direction_per_contact.value()[i]); + } else { + // Non-planar contact - ensure normal vector is empty (unused) + planar_normal_direction_per_contact_.push_back( + {0, 0, 1}); // Default normal (unused) + } + } + } Jt_row_sizes_ = 2 * Eigen::Map( n_friction_directions_per_contact_.data(), n_friction_directions_per_contact_.size()); @@ -112,14 +156,16 @@ void LCSFactory::ComputeContactJacobian(VectorXd& phi, MatrixXd& Jn, Jt.resize(Jt_row_sizes_.sum(), n_v_); // Tangential contact Jacobian - Eigen::Vector3d planar_normal = {0, 1, 0}; double phi_i; MatrixX J_i; for (int i = 0; i < n_contacts_; i++) { multibody::GeomGeomCollider collider(plant_, contact_pairs_[i]); - if (frictionless_ || n_friction_directions_per_contact_[i] == 1) + if (frictionless_ || n_friction_directions_per_contact_[i] == 1) { + Eigen::Vector3d planar_normal = + Eigen::Map( + planar_normal_direction_per_contact_[i].data()); std::tie(phi_i, J_i) = collider.EvalPlanar(context_, planar_normal); - else + } else std::tie(phi_i, J_i) = collider.EvalPolytope( context_, n_friction_directions_per_contact_[i]); diff --git a/multibody/lcs_factory.h b/multibody/lcs_factory.h index 40916e7..57274db 100644 --- a/multibody/lcs_factory.h +++ b/multibody/lcs_factory.h @@ -316,7 +316,11 @@ class LCSFactory { int n_contacts_; ///< Number of contact points. std::vector n_friction_directions_per_contact_; ///< Number of friction directions. - ContactModel contact_model_; ///< The contact model being used. + std::vector> + planar_normal_direction_per_contact_; ///< Optional normal vector for + ///< planar contact for each + ///< contact. + ContactModel contact_model_; ///< The contact model being used. int n_q_; ///< Number of configuration variables. int n_v_; ///< Number of velocity variables. int n_x_; ///< Number of state variables. diff --git a/multibody/lcs_factory_options.h b/multibody/lcs_factory_options.h index e27d664..b85597c 100644 --- a/multibody/lcs_factory_options.h +++ b/multibody/lcs_factory_options.h @@ -1,5 +1,7 @@ #pragma once +#include + #include "drake/common/yaml/yaml_io.h" #include "drake/common/yaml/yaml_read_archive.h" @@ -11,7 +13,9 @@ struct ContactPairConfig { std::vector body_A_collision_geom_indices; std::vector body_B_collision_geom_indices; int num_friction_directions; - double mu; // friction coefficient + std::optional> + planar_normal_direction; // optional normal vector for planar contact + double mu; // friction coefficient template void Serialize(Archive* a) { @@ -20,6 +24,7 @@ struct ContactPairConfig { a->Visit(DRAKE_NVP(body_A_collision_geom_indices)); a->Visit(DRAKE_NVP(body_B_collision_geom_indices)); a->Visit(DRAKE_NVP(num_friction_directions)); + a->Visit(DRAKE_NVP(planar_normal_direction)); a->Visit(DRAKE_NVP(mu)); } }; @@ -35,6 +40,11 @@ struct LCSFactoryOptions { num_friction_directions_per_contact; // Number of friction directions per std::optional> mu; // Friction coefficient for each contact + std::optional> + planar_normal_direction; // Optional normal vector for planar contact + std::optional>> + planar_normal_direction_per_contact; // Optional normal vector for planar + // contact for each contact std::optional> contact_pair_configs; // Optional detailed contact pair configurations @@ -50,6 +60,8 @@ struct LCSFactoryOptions { a->Visit(DRAKE_NVP(num_friction_directions)); a->Visit(DRAKE_NVP(num_friction_directions_per_contact)); a->Visit(DRAKE_NVP(mu)); + a->Visit(DRAKE_NVP(planar_normal_direction)); + a->Visit(DRAKE_NVP(planar_normal_direction_per_contact)); a->Visit(DRAKE_NVP(contact_pair_configs)); a->Visit(DRAKE_NVP(N)); diff --git a/multibody/test/resources/lcs_factory_pivoting_options.yaml b/multibody/test/resources/lcs_factory_pivoting_options.yaml index 5b8f128..1b7b7f6 100644 --- a/multibody/test/resources/lcs_factory_pivoting_options.yaml +++ b/multibody/test/resources/lcs_factory_pivoting_options.yaml @@ -1,5 +1,6 @@ contact_model: "stewart_and_trinkle" num_friction_directions: 1 +planar_normal_direction: [0, 1, 0] num_contacts: 3 spring_stiffness: 0 mu: [0.1, 0.1, 0.1] @@ -12,15 +13,18 @@ contact_pair_configs: # Not used but kept to test parsing body_B_collision_geom_indices: [0] mu: 0.1 num_friction_directions: 1 + planar_normal_direction: [0, 1, 0] - body_A: "cube" body_A_collision_geom_indices: [0] body_B: "right_finger" body_B_collision_geom_indices: [0] mu: 0.1 num_friction_directions: 1 + planar_normal_direction: [0, 1, 0] - body_A: "cube" body_A_collision_geom_indices: [0] body_B: "platform" body_B_collision_geom_indices: [0] mu: 0.1 num_friction_directions: 1 + planar_normal_direction: [0, 1, 0] From af98d68496b1769eea5cc340bc8bf2965e4a1e57 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Wed, 11 Feb 2026 07:11:50 -0500 Subject: [PATCH 2/2] fix: lcs_factory planar normaks loop --- multibody/lcs_factory.cc | 49 ++++++++++++++++------------------------ 1 file changed, 19 insertions(+), 30 deletions(-) diff --git a/multibody/lcs_factory.cc b/multibody/lcs_factory.cc index 40beda2..0ae0c3d 100644 --- a/multibody/lcs_factory.cc +++ b/multibody/lcs_factory.cc @@ -110,39 +110,28 @@ LCSFactory::LCSFactory( frictionless_(contact_model_ == ContactModel::kFrictionlessSpring), dt_(options.dt) { // Handle planar normal directions for contacts with 1 friction direction - if (options_.planar_normal_direction.has_value()) { - // Single planar normal provided - replicate for all planar contacts - DRAKE_DEMAND(options_.planar_normal_direction.value().size() == 3); - planar_normal_direction_per_contact_.clear(); - - for (int i = 0; i < n_contacts_; ++i) { - if (n_friction_directions_per_contact_[i] == 1) { - // Planar contact - use the provided normal - planar_normal_direction_per_contact_.push_back( - options_.planar_normal_direction.value()); + // Initialize all with empty default values + planar_normal_direction_per_contact_.resize(n_contacts_, {}); + + for (int i = 0; i < n_contacts_; ++i) { + if (n_friction_directions_per_contact_[i] == 1) { + // Planar contact - assign the appropriate normal + if (options_.planar_normal_direction.has_value()) { + // Single planar normal provided - use for all planar contacts + DRAKE_DEMAND(options_.planar_normal_direction.value().size() == 3); + planar_normal_direction_per_contact_[i] = + options_.planar_normal_direction.value(); } else { - // Non-planar contact - push empty array (unused) - planar_normal_direction_per_contact_.push_back({}); - } - } - } else { - // Per-contact planar normals provided - validate them - DRAKE_DEMAND(options_.planar_normal_direction_per_contact.has_value()); - DRAKE_DEMAND(options_.planar_normal_direction_per_contact.value().size() == - (size_t)n_contacts_); - - // Validate that planar contacts (num_friction_directions == 1) have - // valid 3D normal vectors - for (size_t i = 0; i < n_contacts_; ++i) { - if (n_friction_directions_per_contact_[i] == 1) { - planar_normal_direction_per_contact_.push_back( - options_.planar_normal_direction_per_contact.value()[i]); - } else { - // Non-planar contact - ensure normal vector is empty (unused) - planar_normal_direction_per_contact_.push_back( - {0, 0, 1}); // Default normal (unused) + // Per-contact planar normals provided + DRAKE_DEMAND(options_.planar_normal_direction_per_contact.has_value()); + DRAKE_DEMAND( + options_.planar_normal_direction_per_contact.value().size() == + (size_t)n_contacts_); + planar_normal_direction_per_contact_[i] = + options_.planar_normal_direction_per_contact.value()[i]; } } + // Non-planar contacts keep the default empty value {} } Jt_row_sizes_ = 2 * Eigen::Map( n_friction_directions_per_contact_.data(),