feat(lcmt_generators) : Add standard LCM generators for C3 Output and Contact Descriptions

.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

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"))
@@ -60,10 +74,12 @@ PYBIND11_MODULE(multibody, m) {
                       &c3::multibody::LCSFactory::GetNumContactVariables),
                   py::arg("contact_model"), py::arg("num_contacts"),
                   py::arg("num_friction_directions"))
-      .def_static("GetNumContactVariables",
-                  py::overload_cast<const c3::LCSFactoryOptions>(
-                      &c3::multibody::LCSFactory::GetNumContactVariables),
-                  py::arg("options"));
+      .def_static(
+          "GetNumContactVariables",
+          py::overload_cast<const drake::multibody::MultibodyPlant<double>&,
+                            const c3::LCSFactoryOptions&>(
+              &c3::multibody::LCSFactory::GetNumContactVariables),
+          py::arg("plant"), py::arg("options"));
 
   py::class_<ContactPairConfig>(m, "ContactPairConfig")
       .def(py::init<>())

bindings/pyc3/c3_py.cc

@@ -197,16 +197,6 @@ PYBIND11_MODULE(c3, m) {
       .def_property(
           "U", [](C3Options const& self) { return self.U; },
           [](C3Options& self, const Eigen::MatrixXd& val) { self.U = val; })
-      .def_property(
-          "g_vector", [](C3Options const& self) { return self.g_vector; },
-          [](C3Options& self, const std::vector<double>& val) {
-            self.g_vector = val;
-          })
-      .def_property(
-          "u_vector", [](C3Options const& self) { return self.u_vector; },
-          [](C3Options& self, const std::vector<double>& val) {
-            self.u_vector = val;
-          })
       .def_readwrite("warm_start", &C3Options::warm_start)
       .def_readwrite("scale_lcs", &C3Options::scale_lcs)
       .def_readwrite("end_on_qp_step", &C3Options::end_on_qp_step)
@@ -220,4 +210,4 @@ PYBIND11_MODULE(c3, m) {
   m.def("LoadC3Options", &LoadC3Options);
 }
 }  // namespace pyc3
-}  // namespace c3
+}  // namespace c3

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")
@@ -182,6 +185,88 @@ PYBIND11_MODULE(systems, m) {
                      &C3ControllerOptions::quaternion_regularizer_fraction);
 
   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,6 +29,11 @@ filegroup(
     ],
 )
 
+cc_library(
+    name = "logging",
+    hdrs = ["common/logging_utils.hpp"],
+)
+
 cc_library(
     name = "c3",
     srcs = [
@@ -57,6 +62,7 @@ cc_library(
     deps = [
         ":lcs",
         ":options",
+        ":logging",
         "@drake//:drake_shared_library",
     ] + select({
          "//tools:with_gurobi": ["@gurobi//:gurobi_cxx"],

core/c3.cc

@@ -7,6 +7,7 @@
 #include <drake/common/find_runfiles.h>
 #include <omp.h>
 
+#include "common/logging_utils.hpp"
 #include "lcs.h"
 #include "solver_options_io.h"
 
@@ -264,6 +265,7 @@ const vector<drake::solvers::QuadraticCost*>& C3::GetTargetCost() {
 }
 
 void C3::Solve(const VectorXd& x0) {
+  drake::log()->debug("C3::Solve called");
   auto start = std::chrono::high_resolution_clock::now();
   // Set the initial state constraint
   if (initial_state_constraint_) {
@@ -315,6 +317,8 @@ void C3::Solve(const VectorXd& x0) {
   vector<VectorXd> w(N_, VectorXd::Zero(n_z_));
   vector<MatrixXd> G = cost_matrices_.G;
 
+  drake::log()->debug("C3::Solve starting ADMM iterations.");
+
   for (int iter = 0; iter < options_.admm_iter; iter++) {
     ADMMStep(x0, &delta, &w, &G, iter);
   }
@@ -330,6 +334,7 @@ void C3::Solve(const VectorXd& x0) {
   *delta_sol_ = delta;
 
   if (!options_.end_on_qp_step) {
+    drake::log()->debug("C3::Solve compute a half step.");
     *z_sol_ = delta;
     z_sol_->at(0).segment(0, n_x_) = x0;
     x_sol_->at(0) = x0;
@@ -353,6 +358,7 @@ void C3::Solve(const VectorXd& x0) {
   solve_time_ =
       std::chrono::duration_cast<std::chrono::microseconds>(elapsed).count() /
       1e6;
+  drake::log()->debug("C3::Solve completed in {} seconds.", solve_time_);
 }
 
 void C3::ADMMStep(const VectorXd& x0, vector<VectorXd>* delta,
@@ -371,6 +377,7 @@ void C3::ADMMStep(const VectorXd& x0, vector<VectorXd>* delta,
     ZW[i] = w->at(i) + z[i];
   }
 
+  drake::log()->debug("C3::ADMMStep SolveProjection step.");
   if (cost_matrices_.U[0].isZero(0)) {
     *delta = SolveProjection(*G, ZW, admm_iteration);
   } else {
@@ -419,6 +426,8 @@ void C3::StoreQPResults(const MathematicalProgramResult& result,
 
   if (!warm_start_)
     return;  // No warm start, so no need to update warm start parameters
+
+  drake::log()->trace("C3::StoreQPResults storing warm start values.");
   for (int i = 0; i < N_ + 1; ++i) {
     if (i < N_) {
       warm_start_x_[admm_iteration][i] = result.GetSolution(x_[i]);
@@ -436,18 +445,49 @@ vector<VectorXd> C3::SolveQP(const VectorXd& x0, const vector<MatrixXd>& G,
   AddAugmentedCost(G, WD, delta, is_final_solve);
   SetInitialGuessQP(x0, admm_iteration);
 
-  MathematicalProgramResult result = osqp_.Solve(prog_);
-
-  if (!result.is_success()) {
-    drake::log()->warn("C3::SolveQP failed to solve the QP with status: {}",
-                       result.get_solution_result());
+  drake::log()->trace("C3::SolveQP calling solver.");
+  try {
+    MathematicalProgramResult result = osqp_.Solve(prog_);
+    if (!result.is_success()) {
+      drake::log()->warn("C3::SolveQP failed to solve the QP with status: {}",
+                         result.get_solution_result());
+      SetFallbackSolution(x0, is_final_solve);
+    } else {
+      StoreQPResults(result, admm_iteration, is_final_solve);
+    }
+  } catch (const std::exception& e) {
+    drake::log()->error("C3::SolveQP failed with exception: {}", e.what());
+    SetFallbackSolution(x0, is_final_solve);
   }
 
-  StoreQPResults(result, admm_iteration, is_final_solve);
-
   return *z_sol_;
 }
 
+/// When the QP solver fails to find a solution (either returns an
+/// unsuccessful status or throws an exception), we fall back to a safe
+/// default: hold the current state and apply zero inputs. This ensures
+/// that downstream consumers always receive a valid solution vector,
+/// and the robot will not execute arbitrary or stale commands.
+/// Specifically:
+///   - x is set to x0 (current robot state) for all time steps
+///   - u is set to zero for all time steps
+///   - lambda is set to zero for all time steps
+void C3::SetFallbackSolution(const VectorXd& x0, bool is_final_solve) {
+  drake::log()->warn(
+      "C3::SetFallbackSolution: Using current state with zero inputs "
+      "as fallback.");
+  for (int i = 0; i < N_; ++i) {
+    if (is_final_solve) {
+      x_sol_->at(i) = x0;
+      lambda_sol_->at(i) = VectorXd::Zero(n_lambda_);
+      u_sol_->at(i) = VectorXd::Zero(n_u_);
+    }
+    z_sol_->at(i).segment(0, n_x_) = x0;
+    z_sol_->at(i).segment(n_x_, n_lambda_) = VectorXd::Zero(n_lambda_);
+    z_sol_->at(i).segment(n_x_ + n_lambda_, n_u_) = VectorXd::Zero(n_u_);
+  }
+}
+
 void C3::AddAugmentedCost(const vector<MatrixXd>& G, const vector<VectorXd>& WD,
                           const vector<VectorXd>& delta, bool is_final_solve) {
   // Remove previous augmented costs

core/c3.h

@@ -393,6 +393,12 @@ class C3 {
       const drake::solvers::MathematicalProgramResult& result,
       int admm_iteration, bool is_final_solve);
 
+  /// Sets a safe fallback solution when the QP solver fails. Sets all states
+  /// to the current state x0 and all inputs/forces to zero, ensuring the
+  /// robot holds position rather than executing stale or undefined commands.
+  virtual void SetFallbackSolution(const Eigen::VectorXd& x0,
+                                   bool is_final_solve);
+
   LCS lcs_;
   double AnDn_ = 1.0;  // Scaling factor for lambdas
   CostMatrices cost_matrices_;