fix(bindings) : Python examples not working

.cirrus.yml

@@ -12,17 +12,25 @@ jammy_task:
   test_script:
     - export CC=clang-15
     - export CXX=clang++-15
-    - bazel build
+    - apt update && apt install -y python3-venv
+    - python3 -m venv .venv --system-site-packages
+    - . .venv/bin/activate && python3 -m pip install -r bindings/pyc3/requirements.txt && echo "🍏 Installed Python dependencies in Jammy CI environment $(which python3)"
+    - . .venv/bin/activate && export PYTHONPATH=$(python3 -c "import site; print(':'.join(site.getsitepackages()))") && bazel build
       --local_resources=ram=24000
       --local_resources=cpu=8
       --jobs=8
+      --noallow_analysis_cache_discard
+      --python_path=$(which python3)
       --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST
       //...
-    - bazel test
+    - . .venv/bin/activate && export PYTHONPATH=$(python3 -c "import site; print(':'.join(site.getsitepackages()))") && bazel test
       --local_resources=ram=24000
       --local_resources=cpu=8
       --jobs=8
+      --noallow_analysis_cache_discard
+      --python_path=$(which python3)
       --test_output=all
+      --test_env=PYTHONPATH
       --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST
       //...
   jammy_test_artifacts:
@@ -42,17 +50,25 @@ noble_task:
   test_script:
     - export CC=clang-15
     - export CXX=clang++-15
-    - bazel build
+    - apt update && apt install -y python3-venv
+    - python3 -m venv .venv --system-site-packages
+    - . .venv/bin/activate && python3 -m pip install -r bindings/pyc3/requirements.txt && echo "🍏 Installed Python dependencies in Noble CI environment $(which python3)"
+    - . .venv/bin/activate && bazel build
       --local_resources=ram=24000
       --local_resources=cpu=8
       --jobs=8
+      --noallow_analysis_cache_discard
+      --python_path=$(which python3)
       --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST
       //...
-    - bazel test
+    - . .venv/bin/activate && bazel test
       --local_resources=ram=24000
       --local_resources=cpu=8
       --jobs=8
       --test_output=all
+      --noallow_analysis_cache_discard
+      --python_path=$(which python3)
+      --test_env=PYTHONPATH
       --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST
       //...
   noble_test_artifacts:

.github/workflows/coverage.yml

@@ -37,7 +37,10 @@ jobs:
           --local_resources=ram=24000
           --local_resources=cpu=4
           --jobs=4
-          //...
+          --instrumentation_filter="//core[:/],//systems[:/],//multibody[:/]"
+          //core/...
+          //systems/...
+          //multibody/...
       - name: Report code coverage
         uses: zgosalvez/github-actions-report-lcov@v4.1.26
         with:

.gitignore

@@ -7,4 +7,5 @@ MODULE.bazel.lock
 **/__pycache__/** 
 .vscode
 *.ps
-genhtml/*
+genhtml/*
+.venv/*

bindings/pyc3/BUILD.bazel

@@ -15,11 +15,15 @@ load("@rules_python//python:packaging.bzl", "py_wheel")
 
 pybind_py_library(
     name = "c3_py",
-    cc_deps = ["//core:c3"],
+    cc_deps = [
+        "//core:c3",
+        "@drake//:drake_shared_library",
+    ],
     cc_so_name = "c3",
     cc_srcs = ["c3_py.cc"],
     py_deps = [
         ":module_py",
+        "@drake//bindings/pydrake",
     ],
     py_imports = ["."],
 )

bindings/pyc3/c3_multibody_py.cc

@@ -36,6 +36,13 @@ PYBIND11_MODULE(multibody, m) {
                     const c3::LCSFactoryOptions&>(),
            py::arg("plant"), py::arg("context"), py::arg("plant_ad"),
            py::arg("context_ad"), py::arg("contact_geoms"), py::arg("options"))
+      .def(py::init<const drake::multibody::MultibodyPlant<double>&,
+                    drake::systems::Context<double>&,
+                    const drake::multibody::MultibodyPlant<drake::AutoDiffXd>&,
+                    drake::systems::Context<drake::AutoDiffXd>&,
+                    c3::LCSFactoryOptions&>(),
+           py::arg("plant"), py::arg("context"), py::arg("plant_ad"),
+           py::arg("context_ad"), py::arg("options"))
       .def("GenerateLCS", &c3::multibody::LCSFactory::GenerateLCS)
       .def("GetContactJacobianAndPoints",
            &c3::multibody::LCSFactory::GetContactJacobianAndPoints)
@@ -77,13 +84,43 @@ PYBIND11_MODULE(multibody, m) {
       .def_readwrite("num_friction_directions",
                      &ContactPairConfig::num_friction_directions)
       .def_readwrite("planar_normal_direction",
-                     &ContactPairConfig::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(
+          "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",

bindings/pyc3/c3_py.cc

@@ -6,6 +6,7 @@
 #include "core/c3.h"
 #include "core/c3_miqp.h"
 #include "core/c3_options.h"
+#include "core/c3_plus.h"
 #include "core/c3_qp.h"
 #include "core/lcs.h"
 
@@ -86,13 +87,24 @@ PYBIND11_MODULE(c3, m) {
              return self.Solve(x0);
            })
       .def("UpdateLCS", &C3::UpdateLCS, py::arg("lcs"))
-      .def("GetDynamicConstraints", &C3::GetDynamicConstraints,
-           py::return_value_policy::copy)
+      .def(
+          "GetDynamicConstraints",
+          [](C3& self) {
+            py::module_::import("pydrake.solvers");
+            return self.GetDynamicConstraints();
+          },
+          py::return_value_policy::reference_internal)
       .def("UpdateTarget", &C3::UpdateTarget, py::arg("x_des"))
       .def("UpdateCostMatrices", &C3::UpdateCostMatrices, py::arg("costs"))
       .def("GetCostMatrices", &C3::GetCostMatrices,
            py::return_value_policy::copy)
-      .def("GetTargetCost", &C3::GetTargetCost, py::return_value_policy::copy)
+      .def(
+          "GetTargetCost",
+          [](C3& self) {
+            py::module_::import("pydrake.solvers");
+            return self.GetTargetCost();
+          },
+          py::return_value_policy::reference_internal)
       .def("AddLinearConstraint",
            static_cast<void (C3::*)(
                const Eigen::MatrixXd&, const Eigen::VectorXd&,
@@ -107,8 +119,13 @@ PYBIND11_MODULE(c3, m) {
            py::arg("A"), py::arg("lower_bound"), py::arg("upper_bound"),
            py::arg("constraint_type"))
       .def("RemoveConstraints", &C3::RemoveConstraints)
-      .def("GetLinearConstraints", &C3::GetLinearConstraints,
-           py::return_value_policy::copy)
+      .def(
+          "GetLinearConstraints",
+          [](C3& self) {
+            py::module_::import("pydrake.solvers");
+            return self.GetLinearConstraints();
+          },
+          py::return_value_policy::reference_internal)
       .def("SetSolverOptions", &C3::SetSolverOptions, py::arg("options"))
       .def("GetFullSolution", &C3::GetFullSolution)
       .def("GetStateSolution", &C3::GetStateSolution)
@@ -132,6 +149,21 @@ PYBIND11_MODULE(c3, m) {
       py::arg("LCS"), py::arg("costs"), py::arg("x_desired"),
       py::arg("options"));
 
+  py::class_<C3Plus, C3>(m, "C3Plus")
+      .def(py::init<const LCS&, const C3::CostMatrices&,
+                    const std::vector<Eigen::VectorXd>&, const C3Options&>(),
+           py::arg("LCS"), py::arg("costs"), py::arg("x_desired"),
+           py::arg("options"));
+
+  py::class_<LCSSimulateConfig>(m, "LCSSimulateConfig")
+      .def(py::init<>())
+      .def_readwrite("regularized", &LCSSimulateConfig::regularized)
+      .def_readwrite("piv_tol", &LCSSimulateConfig::piv_tol)
+      .def_readwrite("zero_tol", &LCSSimulateConfig::zero_tol)
+      .def_readwrite("min_exp", &LCSSimulateConfig::min_exp)
+      .def_readwrite("step_exp", &LCSSimulateConfig::step_exp)
+      .def_readwrite("max_exp", &LCSSimulateConfig::max_exp);
+
   py::class_<LCS>(m, "LCS")
       .def(py::init<const std::vector<Eigen::MatrixXd>&,
                     const std::vector<Eigen::MatrixXd>&,
@@ -154,7 +186,8 @@ PYBIND11_MODULE(c3, m) {
            py::arg("dt"))
       .def(py::init<const LCS&>(), py::arg("other"))
       .def("Simulate", &LCS::Simulate, py::arg("x_init"), py::arg("u"),
-           py::arg("regularized") = false, "Simulate the system for one step")
+           py::arg("simulate_config") = LCSSimulateConfig(),
+           "Simulate the system for one step")
       .def("A", &LCS::A, py::return_value_policy::copy)
       .def("B", &LCS::B, py::return_value_policy::copy)
       .def("D", &LCS::D, py::return_value_policy::copy)

bindings/pyc3/c3_systems_py.cc

@@ -35,6 +35,7 @@ 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>(),
@@ -92,6 +93,13 @@ PYBIND11_MODULE(systems, m) {
               LCSFactoryOptions>(),
           py::arg("plant"), py::arg("context"), py::arg("plant_ad"),
           py::arg("context_ad"), py::arg("contact_geoms"), py::arg("options"))
+      .def(py::init<const MultibodyPlant<double>&,
+                    drake::systems::Context<double>&,
+                    const MultibodyPlant<drake::AutoDiffXd>&,
+                    drake::systems::Context<drake::AutoDiffXd>&,
+                    LCSFactoryOptions&>(),
+           py::arg("plant"), py::arg("context"), py::arg("plant_ad"),
+           py::arg("context_ad"), py::arg("options"))
       .def("get_input_port_lcs_state",
            &LCSFactorySystem::get_input_port_lcs_state,
            py::return_value_policy::reference)

bindings/pyc3/requirements.txt

@@ -1,2 +1,4 @@
 drake
+numpy
+scipy
 

bindings/pyc3/test/BUILD.bazel

@@ -0,0 +1,61 @@
+# -*- python -*-
+package(default_visibility = ["//visibility:public"])
+
+py_test(
+    name = "test_c3",
+    srcs = ["test_c3.py"],
+    deps = [
+        "//bindings/pyc3:c3_py",
+        "@rules_python//python/runfiles",
+    ],
+    data = [
+        "//core:test_data",
+        "//examples:example_data",
+    ],
+    python_version = "PY3",
+    tags = ["smoke"],
+    env_inherit = [
+        "GUROBI_HOME",
+        "GRB_LICENSE_FILE",
+    ],
+)
+
+py_test(
+    name = "test_systems",
+    srcs = ["test_systems.py"],
+    deps = [
+        "//bindings/pyc3:c3_py",
+        "//bindings/pyc3:c3_systems_py",
+        "//bindings/pyc3:c3_multibody_py",
+        "@rules_python//python/runfiles",
+    ],
+    data = [
+        "//examples:example_data",
+    ],
+    python_version = "PY3",
+    tags = ["smoke"],
+    env_inherit = [
+        "GUROBI_HOME",
+        "GRB_LICENSE_FILE",
+    ],
+)
+
+py_test(
+    name = "test_multibody",
+    srcs = ["test_multibody.py"],
+    deps = [
+        "//bindings/pyc3:c3_py",
+        "//bindings/pyc3:c3_multibody_py",
+        "@rules_python//python/runfiles",
+    ],
+    data = [
+        "//multibody:test_data",
+        "//examples:example_data",
+    ],
+    python_version = "PY3",
+    tags = ["smoke"],
+    env_inherit = [
+        "GUROBI_HOME",
+        "GRB_LICENSE_FILE",
+    ],
+)