fix(lcs-factory-options) : breaking python tests using plant for number of contacts resolution

.cirrus.yml

@@ -3,13 +3,20 @@ jammy_task:
   only_if: "$CIRRUS_PR != ''  || $CIRRUS_BRANCH == 'main'" # This condition ensures the task runs only for PRs or on the main branch.
   timeout_in: 120m
   container:
-    image: ghcr.io/dairlab/docker-dair/jammy-dair-base:v1.42
+    image: ghcr.io/dairlab/docker-dair/jammy-dair-base:v1.42.0-2
     cpu: 8
     memory: 24
   format_script:
     - apt update && apt install -y clang-format
     - ./tools/scripts/check_format.sh
   test_script:
+    - | # For PRs, merge the base branch to ensure the latest changes are included in the CI environment. This helps catch merge conflicts early and ensures tests run against the most up-to-date code.
+      if [ -n "$CIRRUS_PR" ]; then
+        git config user.email "ci@ci.com"
+        git config user.name "CI"
+        git fetch origin $CIRRUS_BASE_BRANCH
+        git merge origin/$CIRRUS_BASE_BRANCH --no-edit || (echo "Merge conflict with $CIRRUS_BASE_BRANCH" && exit 1)
+      fi
     - export CC=clang-15
     - export CXX=clang++-15
     - apt update && apt install -y python3-venv
@@ -40,13 +47,20 @@ noble_task:
   only_if: "$CIRRUS_PR != ''  || $CIRRUS_BRANCH == 'main'"
   timeout_in: 120m
   container:
-    image: ghcr.io/dairlab/docker-dair/noble-dair-base:v1.42
+    image: ghcr.io/dairlab/docker-dair/noble-dair-base:v1.42.0-2
     cpu: 8
     memory: 24
   format_script:
     - apt update && apt install -y clang-format
     - ./tools/scripts/check_format.sh
   test_script:
+    - | # For PRs, merge the base branch to ensure the latest changes are included in the CI environment. This helps catch merge conflicts early and ensures tests run against the most up-to-date code.
+      if [ -n "$CIRRUS_PR" ]; then
+        git config user.email "ci@ci.com"
+        git config user.name "CI"
+        git fetch origin $CIRRUS_BASE_BRANCH
+        git merge origin/$CIRRUS_BASE_BRANCH --no-edit || (echo "Merge conflict with $CIRRUS_BASE_BRANCH" && exit 1)
+      fi
     - export CC=clang-15
     - export CXX=clang++-15
     - apt update && apt install -y python3-venv

.github/workflows/coverage.yml

@@ -12,7 +12,7 @@ jobs:
       group: ci-${{ github.ref }}
       cancel-in-progress: true
     container:
-      image: ghcr.io/dairlab/docker-dair/noble-dair-base:v1.42
+      image: ghcr.io/dairlab/docker-dair/noble-dair-base:v1.51.1
       credentials:
         username: ${{ github.actor }}
         password: ${{ secrets.GITHUB_TOKEN }}

bindings/pyc3/BUILD.bazel

@@ -30,9 +30,7 @@ pybind_py_library(
 
 pybind_py_library(
     name = "traj_eval_py",
-    cc_deps = ["//core:lcs",
-               "//core:traj_eval",
-               "//core:c3"],
+    cc_deps = ["//core:traj_eval"],
     cc_so_name = "traj_eval",
     cc_srcs = ["traj_eval_py.cc"],
     py_deps = [

bindings/pyc3/__init__.py

@@ -1,6 +1,5 @@
 # Importing everything in this directory to this package
 import pydrake
-from . import *
 from .c3 import *
 from .traj_eval import *
 from .systems import *

bindings/pyc3/c3_multibody_py.cc

@@ -17,14 +17,32 @@ PYBIND11_MODULE(multibody, m) {
   m.doc() = "C3 Multibody Utilities";
 
   // LCSFactory Class and ContactModel enum
-  py::enum_<c3::multibody::ContactModel>(m, "ContactModel")
-      .value("Unknown", c3::multibody::ContactModel::kUnknown)
-      .value("StewartAndTrinkle",
-             c3::multibody::ContactModel::kStewartAndTrinkle)
-      .value("Anitescu", c3::multibody::ContactModel::kAnitescu)
-      .value("FrictionlessSpring",
-             c3::multibody::ContactModel::kFrictionlessSpring)
-      .export_values();
+  auto contact_model_enum =
+      py::enum_<c3::multibody::ContactModel>(m, "ContactModel")
+          .value("Unknown", c3::multibody::ContactModel::kUnknown)
+          .value("StewartAndTrinkle",
+                 c3::multibody::ContactModel::kStewartAndTrinkle)
+          .value("Anitescu", c3::multibody::ContactModel::kAnitescu)
+          .value("FrictionlessSpring",
+                 c3::multibody::ContactModel::kFrictionlessSpring)
+          .export_values();
+
+  contact_model_enum.attr("__str__") = py::cpp_function(
+      [](c3::multibody::ContactModel
+             model) {  // Iterate through the map to find the string for the
+                       // given enum value
+        for (const auto& pair : GetContactModelMap()) {
+          if (pair.second == model) {
+            return pair.first;
+          }
+        }
+        return std::string("unknown");
+      },
+      py::name("__str__"), py::is_method(contact_model_enum));
+
+  // Add a binding for the map itself
+  m.def("GetContactModelMap", &GetContactModelMap,
+        "Returns a map from contact model names to enum values.");
 
   py::class_<c3::multibody::LCSContactDescription>(m, "LCSContactDescription")
       .def(py::init<>())
@@ -40,6 +58,91 @@ PYBIND11_MODULE(multibody, m) {
                   &c3::multibody::LCSContactDescription::
                       CreateSlackVariableDescription);
 
+  py::class_<ContactPairConfig>(m, "ContactPairConfig")
+      .def(py::init<>())
+      .def_readwrite("body_A", &ContactPairConfig::body_A)
+      .def_readwrite("body_B", &ContactPairConfig::body_B)
+      .def_readwrite("body_A_collision_geom_indices",
+                     &ContactPairConfig::body_A_collision_geom_indices)
+      .def_readwrite("body_B_collision_geom_indices",
+                     &ContactPairConfig::body_B_collision_geom_indices)
+      .def_readwrite("mu", &ContactPairConfig::mu)
+      .def_readwrite("num_friction_directions",
+                     &ContactPairConfig::num_friction_directions)
+      .def_readwrite("planar_normal_direction",
+                     &ContactPairConfig::planar_normal_direction)
+      .def_readwrite("num_active_contact_pairs",
+                     &ContactPairConfig::num_active_contact_pairs);
+
+  py::class_<LCSFactoryOptions>(m, "LCSFactoryOptions")
+      .def(py::init<>())
+      .def_readwrite("dt", &LCSFactoryOptions::dt)
+      .def_readwrite("N", &LCSFactoryOptions::N)
+      .def_readwrite("contact_model", &LCSFactoryOptions::contact_model)
+      .def_property(
+          "num_contacts",
+          [](const LCSFactoryOptions& self) {
+            return self.num_contacts.has_value()
+                       ? py::cast(self.num_contacts.value())
+                       : py::none();
+          },
+          [](LCSFactoryOptions& self, py::object val) {
+            if (val.is_none()) {
+              self.num_contacts.reset();
+            } else {
+              self.num_contacts = py::cast<int>(val);
+            }
+          })
+      .def_property(
+          "spring_stiffness",
+          [](const LCSFactoryOptions& self) {
+            return self.spring_stiffness.has_value()
+                       ? py::cast(self.spring_stiffness.value())
+                       : py::none();
+          },
+          [](LCSFactoryOptions& self, py::object val) {
+            if (val.is_none()) {
+              self.spring_stiffness.reset();
+            } else {
+              self.spring_stiffness = py::cast<double>(val);
+            }
+          })
+      .def_property(
+          "num_friction_directions",
+          [](const LCSFactoryOptions& self) {
+            return self.num_friction_directions.has_value()
+                       ? py::cast(self.num_friction_directions.value())
+                       : py::none();
+          },
+          [](LCSFactoryOptions& self, py::object val) {
+            if (val.is_none()) {
+              self.num_friction_directions.reset();
+            } else {
+              self.num_friction_directions = py::cast<int>(val);
+            }
+          })
+      .def_property(
+          "mu",
+          [](const LCSFactoryOptions& self) {
+            return self.mu.has_value() ? py::cast(self.mu.value()) : py::none();
+          },
+          [](LCSFactoryOptions& self, py::object val) {
+            if (val.is_none()) {
+              self.mu.reset();
+            } else {
+              self.mu = py::cast<double>(val);
+            }
+          })
+      .def_readwrite("num_friction_directions_per_contact",
+                     &LCSFactoryOptions::num_friction_directions_per_contact)
+      .def_readwrite("mu_per_contact", &LCSFactoryOptions::mu_per_contact)
+      .def_readwrite("planar_normal_direction_per_contact",
+                     &LCSFactoryOptions::planar_normal_direction_per_contact)
+      .def_readwrite("planar_normal_direction",
+                     &LCSFactoryOptions::planar_normal_direction)
+      .def_readwrite("contact_pair_configs",
+                     &LCSFactoryOptions::contact_pair_configs);
+
   py::class_<c3::multibody::LCSFactory>(m, "LCSFactory")
       .def(py::init<const drake::multibody::MultibodyPlant<double>&,
                     drake::systems::Context<double>&,
@@ -76,80 +179,35 @@ PYBIND11_MODULE(multibody, m) {
                   py::arg("other"), py::arg("active_lambda_inds"),
                   py::arg("inactive_lambda_inds"))
       // Overload the function GetNumContactVariables
+      .def("GetNumContactVariablesInstance",
+           py::overload_cast<>(
+               &c3::multibody::LCSFactory::GetNumContactVariables, py::const_))
+      .def_static(
+          "GetNumContactVariables",
+          [](const c3::LCSFactoryOptions& options) {
+            return c3::multibody::LCSFactory::GetNumContactVariables(options,
+                                                                     nullptr);
+          },
+          py::arg("options"))
+      .def_static(
+          "GetNumContactVariables",
+          [](const c3::LCSFactoryOptions& options,
+             const drake::multibody::MultibodyPlant<double>* plant) {
+            return c3::multibody::LCSFactory::GetNumContactVariables(options,
+                                                                     plant);
+          },
+          py::arg("options"), py::arg("plant"))
       .def_static("GetNumContactVariables",
                   py::overload_cast<c3::multibody::ContactModel, int, int>(
                       &c3::multibody::LCSFactory::GetNumContactVariables),
                   py::arg("contact_model"), py::arg("num_contacts"),
                   py::arg("num_friction_directions"))
       .def_static(
           "GetNumContactVariables",
-          py::overload_cast<const drake::multibody::MultibodyPlant<double>&,
-                            const c3::LCSFactoryOptions&>(
+          py::overload_cast<c3::multibody::ContactModel, int, std::vector<int>>(
               &c3::multibody::LCSFactory::GetNumContactVariables),
-          py::arg("plant"), py::arg("options"));
-
-  py::class_<ContactPairConfig>(m, "ContactPairConfig")
-      .def(py::init<>())
-      .def_readwrite("body_A", &ContactPairConfig::body_A)
-      .def_readwrite("body_B", &ContactPairConfig::body_B)
-      .def_readwrite("body_A_collision_geom_indices",
-                     &ContactPairConfig::body_A_collision_geom_indices)
-      .def_readwrite("body_B_collision_geom_indices",
-                     &ContactPairConfig::body_B_collision_geom_indices)
-      .def_readwrite("mu", &ContactPairConfig::mu)
-      .def_readwrite("num_friction_directions",
-                     &ContactPairConfig::num_friction_directions)
-      .def_readwrite("planar_normal_direction",
-                     &ContactPairConfig::planar_normal_direction)
-      .def_readwrite("num_active_contact_pairs",
-                     &ContactPairConfig::num_active_contact_pairs);
-
-  py::class_<LCSFactoryOptions>(m, "LCSFactoryOptions")
-      .def(py::init<>())
-      .def_readwrite("dt", &LCSFactoryOptions::dt)
-      .def_readwrite("N", &LCSFactoryOptions::N)
-      .def_property(
-          "contact_model",
-          [](const LCSFactoryOptions& self) {
-            // Convert string back to enum for Python
-            if (self.contact_model == "stewart_and_trinkle")
-              return c3::multibody::ContactModel::kStewartAndTrinkle;
-            if (self.contact_model == "anitescu")
-              return c3::multibody::ContactModel::kAnitescu;
-            if (self.contact_model == "frictionless_spring")
-              return c3::multibody::ContactModel::kFrictionlessSpring;
-            return c3::multibody::ContactModel::kUnknown;
-          },
-          [](LCSFactoryOptions& self, c3::multibody::ContactModel val) {
-            // Convert enum to the string the C++ struct expects
-            switch (val) {
-              case c3::multibody::ContactModel::kStewartAndTrinkle:
-                self.contact_model = "stewart_and_trinkle";
-                break;
-              case c3::multibody::ContactModel::kAnitescu:
-                self.contact_model = "anitescu";
-                break;
-              case c3::multibody::ContactModel::kFrictionlessSpring:
-                self.contact_model = "frictionless_spring";
-                break;
-              default:
-                self.contact_model = "unknown";
-                break;
-            }
-          })
-      .def_readwrite("num_friction_directions",
-                     &LCSFactoryOptions::num_friction_directions)
-      .def_readwrite("num_friction_directions_per_contact",
-                     &LCSFactoryOptions::num_friction_directions_per_contact)
-      .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);
+          py::arg("contact_model"), py::arg("num_contacts"),
+          py::arg("num_friction_directions_per_contact"));
 
   m.def("LoadLCSFactoryOptions", &LoadLCSFactoryOptions);
 }

bindings/pyc3/c3_systems_py.cc

@@ -38,7 +38,6 @@ namespace systems {
 namespace pyc3 {
 PYBIND11_MODULE(systems, m) {
   py::module::import("pydrake.systems.framework");
-  py::module::import("multibody");  // ensure LCSFactoryOptions is registered
   py::class_<C3Controller, LeafSystem<double>>(m, "C3Controller")
       .def(py::init<const MultibodyPlant<double>&, const C3::CostMatrices,
                     C3ControllerOptions>(),

bindings/pyc3/test/test_c3.py

@@ -99,8 +99,6 @@ def make_cartpole_options_and_costs(lcs, N=5, c3plus=False):
     opts.R = R_mat
     opts.G = G_mat
     opts.U = U_mat
-    opts.g_vector = [0.1] * n_lambda + [0.0] * n_u
-    opts.u_vector = [1.0] * n_lambda + [0.0] * n_u
     opts.warm_start = False
     opts.scale_lcs = False
     opts.end_on_qp_step = True
@@ -137,8 +135,6 @@ def make_options(n_x=4, n_u=2, n_lambda=2, is_c3plus=False):
     n_z = n_x + n_u + n_lambda + (n_lambda if is_c3plus else 0)
     opts.G = np.ones((n_z, n_z))
     opts.U = np.ones((n_z, n_z))
-    opts.g_vector = [1.0] * n_lambda
-    opts.u_vector = [1.0] * n_u
     opts.warm_start = False
     opts.scale_lcs = False
     opts.end_on_qp_step = False

bindings/pyc3/test/test_multibody.py

@@ -29,18 +29,18 @@ def test_fields(self):
         opts.N = 3
         opts.num_contacts = 2
         # mu is list[float] per binding
-        opts.mu = [0.5]
+        opts.mu = 0.5
         opts.spring_stiffness = 100.0
         opts.num_friction_directions = 4
         self.assertAlmostEqual(opts.dt, 0.01)
         self.assertEqual(opts.N, 3)
         self.assertEqual(opts.num_contacts, 2)
-        self.assertAlmostEqual(opts.mu[0], 0.5)
+        self.assertAlmostEqual(opts.mu, 0.5)
 
     def test_contact_model(self):
         opts = multibody.LCSFactoryOptions()
-        opts.contact_model = multibody.ContactModel.StewartAndTrinkle
-        self.assertEqual(opts.contact_model, multibody.ContactModel.StewartAndTrinkle)
+        opts.contact_model = str(multibody.ContactModel.StewartAndTrinkle)
+        self.assertEqual(opts.contact_model, "stewart_and_trinkle")
 
     def test_contact_pair_configs(self):
         opts = multibody.LCSFactoryOptions()
@@ -83,7 +83,7 @@ def test_with_options(self):
         opts = multibody.LCSFactoryOptions()
         opts.num_contacts = 2
         opts.num_friction_directions = 4
-        opts.contact_model = multibody.ContactModel.StewartAndTrinkle
+        opts.contact_model = "stewart_and_trinkle"
         n = multibody.LCSFactory.GetNumContactVariables(opts)
         self.assertGreater(n, 0)
 
@@ -96,9 +96,9 @@ def test_load(self):
         self.assertEqual(opts.N, 10)
         self.assertAlmostEqual(opts.dt, 0.01)
         self.assertEqual(opts.num_contacts, 3)
-        self.assertEqual(opts.contact_model, multibody.ContactModel.StewartAndTrinkle)
+        self.assertEqual(opts.contact_model, "stewart_and_trinkle")
         self.assertEqual(opts.num_friction_directions, 1)
-        self.assertAlmostEqual(opts.mu[0], 0.1)
+        self.assertAlmostEqual(opts.mu, 0.1)
         self.assertEqual(len(opts.contact_pair_configs), 3)
         self.assertEqual(opts.contact_pair_configs[0].body_A, "cube")
         self.assertEqual(opts.contact_pair_configs[0].body_B, "left_finger")
@@ -107,6 +107,9 @@ def test_get_num_contact_variables_from_loaded_options(self):
         opts = multibody.LoadLCSFactoryOptions(
             "multibody/test/resources/lcs_factory_pivoting_options.yaml"
         )
+        opts.contact_pair_configs = (
+            None  # test that GetNumContactVariables doesn't require this field
+        )
         n = multibody.LCSFactory.GetNumContactVariables(opts)
         self.assertGreater(n, 0)
 

bindings/pyc3/test/test_traj_eval.py

@@ -95,8 +95,6 @@ def test_compute_quadratic_trajectory_cost_with_c3(self):
         opts.R = self.R_matrix
         opts.G = np.eye(self.n_x + self.n_u + self.n_lambda)
         opts.U = np.eye(self.n_x + self.n_u + self.n_lambda)
-        opts.g_vector = [1.0] * self.n_lambda + [0.0] * self.n_u
-        opts.u_vector = [1.0] * self.n_lambda + [0.0] * self.n_u
         opts.warm_start = False
         opts.scale_lcs = False
         opts.end_on_qp_step = True