feat(plate-balancing) : Support plate-balancing example in dairlib

.bazelrc

@@ -51,4 +51,4 @@ build --action_env=LD_LIBRARY_PATH=
 # use python3 by default
 build --python_path=python3
 
-build --define=WITH_GUROBI=ON
+build --define=WITH_GUROBI=ON

.github/workflows/coverage.yml

@@ -1,5 +1,5 @@
 name: C3 Coverage
-on: 
+on:
   push:
     branches:
       - main
@@ -11,11 +11,11 @@ jobs:
     concurrency:
       group: ci-${{ github.ref }}
       cancel-in-progress: true
-    container: 
+    container:
       image: ghcr.io/dairlab/docker-dair/noble-dair-base:v1.42
       credentials:
-       username: ${{ github.actor }}
-       password: ${{ secrets.GITHUB_TOKEN }}
+        username: ${{ github.actor }}
+        password: ${{ secrets.GITHUB_TOKEN }}
       options: --cpus 4
     steps:
       - name: Check out repository code
@@ -30,13 +30,14 @@ jobs:
           key: c3-bazel-cov-cache-${{ hashFiles('**/BUILD.bazel') }}
           restore-keys: |
             c3-bazel-cov-cache-
-      - name: Generate Coverage 
+      - name: Generate Coverage
+        id: generate-coverage
         run: bazel coverage
-              --combined_report=lcov
-              --local_resources=ram=24000
-              --local_resources=cpu=4
-              --jobs=4
-              //...
+          --combined_report=lcov
+          --local_resources=ram=24000
+          --local_resources=cpu=4
+          --jobs=4
+          //...
       - name: Report code coverage
         uses: zgosalvez/github-actions-report-lcov@v4.1.26
         with:
@@ -47,33 +48,33 @@ jobs:
           update-comment: true
       - name: Save coverage cache
         id: c3-cov-cache-save
-        if: always() && !cancelled() && steps.c3-cov-cache-restore.outputs.cache-hit != 'true'
+        if: always() && !cancelled() && steps.c3-cov-cache-restore.outputs.cache-hit != 'true' && steps.generate-coverage.outcome == 'success'
         uses: actions/cache/save@v4
         with:
           key: ${{ steps.c3-cov-cache-restore.outputs.cache-primary-key }}
           path: ~/.cache/bazel
       - run: echo "🍏 This job's status is ${{ job.status }}."
-  # Deploy job
-  deploy:
-    # Add a dependency to the build job
-    needs: coverage
-    if: ${{ github.event_name == 'push' }}
+  # Deploy job [Not currently enabled]
+  # deploy:
+  #   # Add a dependency to the build job
+  #   needs: coverage
+  #   if: ${{ github.event_name == 'push' }}
 
-    # Grant GITHUB_TOKEN the permissions required to make a Pages deployment
-    permissions:
-      pages: write      # to deploy to Pages
-      id-token: write   # to verify the deployment originates from an appropriate source
+  #   # Grant GITHUB_TOKEN the permissions required to make a Pages deployment
+  #   permissions:
+  #     pages: write      # to deploy to Pages
+  #     id-token: write   # to verify the deployment originates from an appropriate source
 
-    # Deploy to the github-pages environment
-    environment:
-      name: github-pages
-      url: ${{ steps.deployment.outputs.page_url }}
+  #   # Deploy to the github-pages environment
+  #   environment:
+  #     name: github-pages
+  #     url: ${{ steps.deployment.outputs.page_url }}
 
-    # Specify runner + deployment step
-    runs-on: ubuntu-latest
-    steps:
-      - name: Deploy to GitHub Pages
-        id: deployment
-        uses: actions/deploy-pages@v4 # or specific "vX.X.X" version tag for this action
-        with:
-          artifact_name: noble-code-coverage-report
+  #   # Specify runner + deployment step
+  #   runs-on: ubuntu-latest
+  #   steps:
+  #     - name: Deploy to GitHub Pages
+  #       id: deployment
+  #       uses: actions/deploy-pages@v4 # or specific "vX.X.X" version tag for this action
+  #       with:
+  #         artifact_name: noble-code-coverage-report

BUILD.bazel

@@ -21,6 +21,7 @@ cc_library(
     name = "libc3",
     hdrs = [":c3_headers"],  # Changed from srcs to hdrs for headers
     deps = LIBC3_COMPONENTS + [
+        "//lcmtypes:lcmt_c3",
         "@drake//:drake_shared_library",
     ],
     include_prefix = "c3",

bindings/pyc3/c3_multibody_py.cc

@@ -26,6 +26,20 @@ PYBIND11_MODULE(multibody, m) {
              c3::multibody::ContactModel::kFrictionlessSpring)
       .export_values();
 
+  py::class_<c3::multibody::LCSContactDescription>(m, "LCSContactDescription")
+      .def(py::init<>())
+      .def_readwrite("witness_point_A",
+                     &c3::multibody::LCSContactDescription::witness_point_A)
+      .def_readwrite("witness_point_B",
+                     &c3::multibody::LCSContactDescription::witness_point_B)
+      .def_readwrite("force_basis",
+                     &c3::multibody::LCSContactDescription::force_basis)
+      .def_readwrite("is_slack",
+                     &c3::multibody::LCSContactDescription::is_slack)
+      .def_static("CreateSlackVariableDescription",
+                  &c3::multibody::LCSContactDescription::
+                      CreateSlackVariableDescription);
+
   py::class_<c3::multibody::LCSFactory>(m, "LCSFactory")
       .def(py::init<const drake::multibody::MultibodyPlant<double>&,
                     drake::systems::Context<double>&,
@@ -37,8 +51,8 @@ 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"))
       .def("GenerateLCS", &c3::multibody::LCSFactory::GenerateLCS)
-      .def("GetContactJacobianAndPoints",
-           &c3::multibody::LCSFactory::GetContactJacobianAndPoints)
+      .def("GetContactDescriptions",
+           &c3::multibody::LCSFactory::GetContactDescriptions)
       .def("UpdateStateAndInput",
            &c3::multibody::LCSFactory::UpdateStateAndInput, py::arg("state"),
            py::arg("input"))
@@ -61,19 +75,35 @@ PYBIND11_MODULE(multibody, m) {
                       &c3::multibody::LCSFactory::GetNumContactVariables),
                   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);
+
   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_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("mu", &LCSFactoryOptions::mu)
+      .def_readwrite("contact_pair_configs",
+                     &LCSFactoryOptions::contact_pair_configs);
 
   m.def("LoadLCSFactoryOptions", &LoadLCSFactoryOptions);
 }
 }  // namespace pyc3
 }  // namespace multibody
-}  // namespace c3
+}  // namespace c3

bindings/pyc3/c3_py.cc

@@ -154,7 +154,7 @@ 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"),
-           "Simulate the system for one step")
+           py::arg("regularized") = false, "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

@@ -9,6 +9,9 @@
 #include "systems/c3_controller_options.h"
 #include "systems/framework/c3_output.h"
 #include "systems/framework/timestamped_vector.h"
+#include "systems/lcmt_generators/c3_output_generator.h"
+#include "systems/lcmt_generators/c3_trajectory_generator.h"
+#include "systems/lcmt_generators/contact_force_generator.h"
 #include "systems/lcs_factory_system.h"
 #include "systems/lcs_simulator.h"
 
@@ -100,8 +103,8 @@ PYBIND11_MODULE(systems, m) {
            py::return_value_policy::reference)
       .def("get_output_port_lcs", &LCSFactorySystem::get_output_port_lcs,
            py::return_value_policy::reference)
-      .def("get_output_port_lcs_contact_jacobian",
-           &LCSFactorySystem::get_output_port_lcs_contact_jacobian,
+      .def("get_output_port_lcs_contact_descriptions",
+           &LCSFactorySystem::get_output_port_lcs_contact_descriptions,
            py::return_value_policy::reference);
 
   py::class_<C3Output::C3Solution>(m, "C3Solution")
@@ -178,6 +181,88 @@ PYBIND11_MODULE(systems, m) {
                      &C3ControllerOptions::state_prediction_joints);
 
   m.def("LoadC3ControllerOptions", &LoadC3ControllerOptions);
+
+  // C3OutputGenerator
+  py::class_<lcmt_generators::C3OutputGenerator,
+             drake::systems::LeafSystem<double>>(m, "C3OutputGenerator")
+      .def(py::init<>())
+      .def("get_input_port_c3_solution",
+           &lcmt_generators::C3OutputGenerator::get_input_port_c3_solution,
+           py::return_value_policy::reference)
+      .def("get_input_port_c3_intermediates",
+           &lcmt_generators::C3OutputGenerator::get_input_port_c3_intermediates,
+           py::return_value_policy::reference)
+      .def("get_output_port_c3_output",
+           &lcmt_generators::C3OutputGenerator::get_output_port_c3_output,
+           py::return_value_policy::reference)
+      .def_static("AddLcmPublisherToBuilder",
+                  &lcmt_generators::C3OutputGenerator::AddLcmPublisherToBuilder,
+                  py::arg("builder"), py::arg("solution_port"),
+                  py::arg("intermediates_port"), py::arg("channel"),
+                  py::arg("lcm"), py::arg("publish_triggers"),
+                  py::arg("publish_period"), py::arg("publish_offset"));
+
+  // C3TrajectoryGenerator
+  py::class_<lcmt_generators::TrajectoryDescription::index_range>(
+      m, "TrajectoryDescriptionIndexRange")
+      .def(py::init<>())
+      .def_readwrite(
+          "start", &lcmt_generators::TrajectoryDescription::index_range::start)
+      .def_readwrite("end",
+                     &lcmt_generators::TrajectoryDescription::index_range::end);
+  py::class_<lcmt_generators::TrajectoryDescription>(m, "TrajectoryDescription")
+      .def(py::init<>())
+      .def_readwrite("trajectory_name",
+                     &lcmt_generators::TrajectoryDescription::trajectory_name)
+      .def_readwrite("variable_type",
+                     &lcmt_generators::TrajectoryDescription::variable_type)
+      .def_readwrite("indices",
+                     &lcmt_generators::TrajectoryDescription::indices);
+  py::class_<lcmt_generators::C3TrajectoryGeneratorConfig>(
+      m, "C3TrajectoryGeneratorConfig")
+      .def(py::init<>())
+      .def_readwrite(
+          "trajectories",
+          &lcmt_generators::C3TrajectoryGeneratorConfig::trajectories);
+  py::class_<lcmt_generators::C3TrajectoryGenerator,
+             drake::systems::LeafSystem<double>>(m, "C3TrajectoryGenerator")
+      .def(py::init<lcmt_generators::C3TrajectoryGeneratorConfig>())
+      .def("get_input_port_c3_solution",
+           &lcmt_generators::C3TrajectoryGenerator::get_input_port_c3_solution,
+           py::return_value_policy::reference)
+      .def("get_output_port_lcmt_c3_trajectory",
+           &lcmt_generators::C3TrajectoryGenerator::
+               get_output_port_lcmt_c3_trajectory,
+           py::return_value_policy::reference)
+      .def_static(
+          "AddLcmPublisherToBuilder",
+          &lcmt_generators::C3TrajectoryGenerator::AddLcmPublisherToBuilder,
+          py::arg("builder"), py::arg("config"), py::arg("solution_port"),
+          py::arg("channel"), py::arg("lcm"), py::arg("publish_triggers"),
+          py::arg("publish_period"), py::arg("publish_offset"));
+
+  // ContactForceGenerator
+  py::class_<lcmt_generators::ContactForceGenerator,
+             drake::systems::LeafSystem<double>>(m, "ContactForceGenerator")
+      .def(py::init<>())
+      .def("get_input_port_c3_solution",
+           &lcmt_generators::ContactForceGenerator::get_input_port_c3_solution,
+           py::return_value_policy::reference)
+      .def("get_input_port_lcs_contact_descriptions",
+           &lcmt_generators::ContactForceGenerator::
+               get_input_port_lcs_contact_descriptions,
+           py::return_value_policy::reference)
+      .def("get_output_port_contact_force",
+           &lcmt_generators::ContactForceGenerator::
+               get_output_port_contact_force,
+           py::return_value_policy::reference)
+      .def_static(
+          "AddLcmPublisherToBuilder",
+          &lcmt_generators::ContactForceGenerator::AddLcmPublisherToBuilder,
+          py::arg("builder"), py::arg("solution_port"),
+          py::arg("lcs_contact_info_port"), py::arg("channel"), py::arg("lcm"),
+          py::arg("publish_triggers"), py::arg("publish_period"),
+          py::arg("publish_offset"));
 }
 }  // namespace pyc3
 }  // namespace systems

core/BUILD.bazel

@@ -29,11 +29,17 @@ filegroup(
     ],
 )
 
+cc_library(
+    name = "logging",
+    hdrs = ["common/logging_utils.hpp"],
+)
+
 cc_library(
     name = "c3",
     srcs = [
         "c3.cc",
         "c3_qp.cc",
+        "c3_plus.cc",
     ] + select({
         "//tools:with_gurobi": [
             ":c3_miqp.cc",
@@ -46,6 +52,7 @@ cc_library(
         "c3.h",
         "c3_miqp.h",
         "c3_qp.h",
+        "c3_plus.h",
     ],
     data = [
         ":default_solver_options",
@@ -55,6 +62,7 @@ cc_library(
     deps = [
         ":lcs",
         ":options",
+        ":logging",
         "@drake//:drake_shared_library",
     ] + select({
          "//tools:with_gurobi": ["@gurobi//:gurobi_cxx"],
@@ -82,6 +90,9 @@ cc_library(
     hdrs = [
         "test/c3_cartpole_problem.hpp",
     ],
+    deps = [
+        ":core",
+    ],
     data = [
         ":test_data",
     ]