feat(lcs-factory): add planar normal config for lcs factory options

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 = ["."],

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_<LCSFactoryOptions>(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);
 

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

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

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

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<GeometryId> 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,30 @@ LCSFactory::LCSFactory(
       mu_(options.mu.value()),
       frictionless_(contact_model_ == ContactModel::kFrictionlessSpring),
       dt_(options.dt) {
+  // Handle planar normal directions for contacts with 1 friction direction
+  // 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 {
+        // 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<const VectorXi, Eigen::Unaligned>(
                           n_friction_directions_per_contact_.data(),
                           n_friction_directions_per_contact_.size());
@@ -112,14 +145,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<double> 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<const Eigen::Vector3d, Eigen::Unaligned>(
+              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]);
 

multibody/lcs_factory.h

@@ -316,7 +316,11 @@ class LCSFactory {
   int n_contacts_;  ///< Number of contact points.
   std::vector<int>
       n_friction_directions_per_contact_;  ///< Number of friction directions.
-  ContactModel contact_model_;             ///< The contact model being used.
+  std::vector<std::array<double, 3>>
+      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.

multibody/lcs_factory_options.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <optional>
+
 #include "drake/common/yaml/yaml_io.h"
 #include "drake/common/yaml/yaml_read_archive.h"
 
@@ -11,7 +13,9 @@ struct ContactPairConfig {
   std::vector<int> body_A_collision_geom_indices;
   std::vector<int> body_B_collision_geom_indices;
   int num_friction_directions;
-  double mu;  // friction coefficient
+  std::optional<std::array<double, 3>>
+      planar_normal_direction;  // optional normal vector for planar contact
+  double mu;                    // friction coefficient
 
   template <typename Archive>
   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<std::vector<double>>
       mu;  // Friction coefficient for each contact
+  std::optional<std::array<double, 3>>
+      planar_normal_direction;  // Optional normal vector for planar contact
+  std::optional<std::vector<std::array<double, 3>>>
+      planar_normal_direction_per_contact;  // Optional normal vector for planar
+                                            // contact for each contact
 
   std::optional<std::vector<ContactPairConfig>>
       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));

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]