feat: bring output publishers from dairlib to c3

Meow404 · Meow404 · commit 720d12bab40d · 2025-06-27T17:22:18.000-04:00
diff --git a/core/BUILD.bazel b/core/BUILD.bazel
@@ -29,12 +29,10 @@ cc_library(
         "c3.h",
         "c3_miqp.h",
         "c3_qp.h",
+        "configs/solve_options_default.hpp"
     ],
     copts = ["-fopenmp"],
     linkopts = ["-fopenmp"],
-    data = glob([
-        "configs/**",
-    ]),
     deps = [
         ":lcs",
         ":options",
diff --git a/core/c3.cc b/core/c3.cc
@@ -8,6 +8,7 @@
 
 #include "lcs.h"
 #include "solver_options_io.h"
+#include "configs/solve_options_default.hpp"
 
 #include "drake/common/text_logging.h"
 #include "drake/solvers/mathematical_program.h"
@@ -154,8 +155,7 @@ C3::C3(const LCS& lcs, const CostMatrices& costs,
 
   // Set default solver options
   drake::solvers::SolverOptions solver_options =
-      drake::yaml::LoadYamlFile<c3::SolverOptionsFromYaml>(
-          "core/configs/solver_options_default.yaml")
+      drake::yaml::LoadYamlString<c3::SolverOptionsFromYaml>(default_solver_options)
           .GetAsSolverOptions(drake::solvers::OsqpSolver::id());
   SetSolverOptions(solver_options);
 }
diff --git a/core/configs/solve_options_default.hpp b/core/configs/solve_options_default.hpp
@@ -0,0 +1,11 @@
+#pragma once
+#include <string>
+const std::string default_solver_options =
+    "print_to_console: 0\nlog_file_name: \"\"\nint_options:\n  max_iter: "
+    "1000\n  # linsys_solver: 0\n  verbose: 0\n  warm_start: 1\n  polish: 1\n  "
+    "scaled_termination: 1\n  check_termination: 25\n  polish_refine_iter: 3\n "
+    " scaling: 10\n  adaptive_rho: 1\n  adaptive_rho_interval: "
+    "0\n\ndouble_options:\n  rho: 0.0001\n  sigma: 1e-6\n  eps_abs: 1e-5\n  "
+    "eps_rel: 1e-5\n  eps_prim_inf: 1e-5\n  eps_dual_inf: 1e-5\n  alpha: 1.6\n "
+    " delta: 1e-6\n  adaptive_rho_tolerance: 5\n  adaptive_rho_fraction: 0.4\n "
+    " time_limit: 1.0\n\nstring_options: {}";
diff --git a/core/configs/solver_options_default.yaml b/core/configs/solver_options_default.yaml
diff --git a/lcmtypes/BUILD.bazel b/lcmtypes/BUILD.bazel
@@ -0,0 +1,47 @@
+load(
+    "@drake//tools/skylark:drake_lcm.bzl",
+    "drake_lcm_cc_library",
+    "drake_lcm_java_library",
+    "drake_lcm_py_library",
+)
+load(
+    "@drake//tools/skylark:drake_java.bzl",
+    "drake_java_binary",
+)
+load("@drake//tools/lint:lint.bzl", "add_lint_tests")
+
+package(default_visibility = ["//visibility:public"])
+
+#Lcm libraries
+drake_lcm_cc_library(
+    name = "lcmt_c3",
+    lcm_package = "c3",
+    lcm_srcs = glob(["*.lcm"]),
+)
+
+drake_lcm_java_library(
+    name = "lcmtypes_c3_java",
+    lcm_package = "c3",
+    lcm_srcs = glob(["*.lcm"]),
+)
+
+drake_lcm_py_library(
+    name = "lcmtypes_c3_py",
+    add_current_package_to_imports = True,  # Use //:module_py instead.
+    extra_srcs = ["__init__.py"],
+    lcm_package = "c3",
+    lcm_srcs = glob(["*.lcm"]),
+    deps = [
+        "@drake//:module_py",
+    ],
+)
+
+drake_java_binary(
+    name = "c3-lcm-spy",
+    main_class = "lcm.spy.Spy",
+    visibility = ["//visibility:private"],
+    runtime_deps = [
+        ":lcmtypes_c3_java",
+        "@drake//lcmtypes:lcmtypes_drake_java",
+    ],
+)
diff --git a/lcmtypes/__init__.py b/lcmtypes/__init__.py
@@ -0,0 +1 @@
+# Empty Python module `__init__`, required to make this a module.
diff --git a/lcmtypes/lcmt_contact_force.lcm b/lcmtypes/lcmt_contact_force.lcm
@@ -0,0 +1,12 @@
+package c3;
+
+/*  lcmtype containing information to visualize forces in meshcat
+*/
+struct lcmt_contact_force
+{
+  int64_t utime;
+
+  // These are all expressed in the world frame.
+  double contact_point[3];
+  double contact_force[3];
+}
diff --git a/lcmtypes/lcmt_forces.lcm b/lcmtypes/lcmt_forces.lcm
@@ -0,0 +1,11 @@
+package c3;
+
+/*  lcmtype containing information to visualize forces in meshcat
+*/
+struct lcmt_forces
+{
+  int64_t utime;
+
+  int32_t num_forces;
+  lcmt_contact_force forces[num_forces];
+}
diff --git a/lcmtypes/lcmt_intermediates.lcm b/lcmtypes/lcmt_intermediates.lcm
@@ -0,0 +1,12 @@
+package c3;
+
+struct lcmt_intermediates
+{
+  int32_t num_points;
+  int32_t num_total_variables;
+
+  float time_vec[num_points];
+  float z_sol[num_total_variables][num_points];
+  float delta_sol[num_total_variables][num_points];
+  float w_sol[num_total_variables][num_points];
+}
diff --git a/lcmtypes/lcmt_output.lcm b/lcmtypes/lcmt_output.lcm
@@ -0,0 +1,9 @@
+package c3;
+
+struct lcmt_output
+{
+  int64_t utime;
+
+  lcmt_solution solution;
+  lcmt_intermediates intermediates;
+}
diff --git a/lcmtypes/lcmt_solution.lcm b/lcmtypes/lcmt_solution.lcm
@@ -0,0 +1,14 @@
+package c3;
+
+struct lcmt_solution
+{
+  int32_t num_points;
+  int32_t num_state_variables;
+  int32_t num_contact_variables;
+  int32_t num_input_variables;
+
+  float time_vec[num_points];
+  float x_sol[num_state_variables][num_points];
+  float lambda_sol[num_contact_variables][num_points];
+  float u_sol[num_input_variables][num_points];
+}
diff --git a/systems/BUILD.bazel b/systems/BUILD.bazel
@@ -34,6 +34,8 @@ cc_library(
     deps = [
         ":vector",
         ":options",
+        ":c3_output",
+        ":publishers",
         "//core:c3",
         "//core:options",
         "//multibody:lcs_factory",
@@ -49,6 +51,36 @@ cc_library(
     deps = [
         "//core:options",
         "//multibody:options",
+    ]
+)
+
+cc_library(
+    name = "publishers",
+    srcs = [
+        "publishers/output_publisher.cc",
+        "publishers/force_publisher.cc"
+    ],
+    hdrs = [
+        "publishers/output_publisher.h",
+        "publishers/force_publisher.h"
+    ],
+    deps = [
+        ":c3_output",
+        "//lcmtypes:lcmt_c3",
+        "@drake//:drake_shared_library",
+    ],
+)
+
+cc_library(
+    name = "c3_output",
+        srcs = [
+        "framework/c3_output.cc",
+    ],
+    hdrs = [
+        "framework/c3_output.h",
+        ],
+    deps = [
+        "//lcmtypes:lcmt_c3",
         "@drake//:drake_shared_library",
     ]
 )
diff --git a/systems/framework/c3_output.cc b/systems/framework/c3_output.cc
@@ -9,5 +9,73 @@ namespace systems {
 
 C3Output::C3Output(C3Solution c3_sol, C3Intermediates c3_intermediates)
     : c3_solution_(c3_sol), c3_intermediates_(c3_intermediates) {}
+
+lcmt_output C3Output::GenerateLcmObject(double time) const {
+  lcmt_output c3_output;
+  lcmt_solution c3_solution;
+  lcmt_intermediates c3_intermediates;
+  c3_output.utime = static_cast<int64_t>(1e6 * time);
+
+  c3_solution.num_points = c3_solution_.time_vector_.size();
+  int knot_points = c3_solution.num_points;
+  c3_solution.num_state_variables = c3_solution_.x_sol_.rows();
+  c3_solution.num_contact_variables = c3_solution_.lambda_sol_.rows();
+  c3_solution.num_input_variables = c3_solution_.u_sol_.rows();
+  c3_solution.time_vec.reserve(knot_points);
+  c3_solution.x_sol = vector<vector<float>>(c3_solution.num_state_variables,
+                                            vector<float>(knot_points));
+  c3_solution.lambda_sol = vector<vector<float>>(
+      c3_solution.num_contact_variables, vector<float>(knot_points));
+  c3_solution.u_sol = vector<vector<float>>(c3_solution.num_input_variables,
+                                            vector<float>(knot_points));
+
+  c3_solution.time_vec = vector<float>(
+      c3_solution_.time_vector_.data(),
+      c3_solution_.time_vector_.data() + c3_solution_.time_vector_.size());
+
+  c3_intermediates.num_total_variables = c3_intermediates_.delta_.rows();
+  c3_intermediates.num_points = c3_solution.num_points;
+  c3_intermediates.time_vec.reserve(c3_intermediates.num_points);
+  c3_intermediates.z_sol = vector<vector<float>>(
+      c3_intermediates.num_total_variables, vector<float>(knot_points));
+  c3_intermediates.delta_sol = vector<vector<float>>(
+      c3_intermediates.num_total_variables, vector<float>(knot_points));
+  c3_intermediates.w_sol = vector<vector<float>>(
+      c3_intermediates.num_total_variables, vector<float>(knot_points));
+  c3_intermediates.time_vec = c3_solution.time_vec;
+
+  for (int i = 0; i < c3_solution.num_state_variables; ++i) {
+    VectorXf temp_row = c3_solution_.x_sol_.row(i);
+    memcpy(c3_solution.x_sol[i].data(), temp_row.data(),
+           sizeof(float) * knot_points);
+  }
+  for (int i = 0; i < c3_solution.num_contact_variables; ++i) {
+    VectorXf temp_row = c3_solution_.lambda_sol_.row(i);
+    memcpy(c3_solution.lambda_sol[i].data(), temp_row.data(),
+           sizeof(float) * knot_points);
+  }
+  for (int i = 0; i < c3_solution.num_input_variables; ++i) {
+    VectorXf temp_row = c3_solution_.u_sol_.row(i);
+    memcpy(c3_solution.u_sol[i].data(), temp_row.data(),
+           sizeof(float) * knot_points);
+  }
+  for (int i = 0; i < c3_intermediates.num_total_variables; ++i) {
+    VectorXf temp_z_row = c3_intermediates_.z_.row(i);
+    VectorXf temp_delta_row = c3_intermediates_.delta_.row(i);
+    VectorXf temp_w_row = c3_intermediates_.w_.row(i);
+    memcpy(c3_intermediates.z_sol[i].data(), temp_z_row.data(),
+           sizeof(float) * knot_points);
+    memcpy(c3_intermediates.delta_sol[i].data(), temp_delta_row.data(),
+           sizeof(float) * knot_points);
+    memcpy(c3_intermediates.w_sol[i].data(), temp_w_row.data(),
+           sizeof(float) * knot_points);
+  }
+
+  c3_output.solution = c3_solution;
+  c3_output.intermediates = c3_intermediates;
+
+  return c3_output;
+}
+
 }  // namespace systems
 }  // namespace c3
diff --git a/systems/framework/c3_output.h b/systems/framework/c3_output.h
@@ -5,6 +5,8 @@
 #include <unordered_map>
 #include <vector>
 
+#include "c3/lcmt_output.hpp"
+
 #include <Eigen/Dense>
 
 using Eigen::MatrixXf;
@@ -57,6 +59,8 @@ class C3Output {
 
   virtual ~C3Output() = default;
 
+  lcmt_output GenerateLcmObject(double time) const;
+
  private:
   C3Solution c3_solution_;
   C3Intermediates c3_intermediates_;
diff --git a/systems/publishers/force_publisher.cc b/systems/publishers/force_publisher.cc
diff --git a/systems/publishers/force_publisher.h b/systems/publishers/force_publisher.h
diff --git a/systems/publishers/output_publisher.cc b/systems/publishers/output_publisher.cc
diff --git a/systems/publishers/output_publisher.h b/systems/publishers/output_publisher.h

Original file line number	Diff line number	Diff line change
`@@ -0,0 +1 @@`
	`1`	+# Empty Python module `__init__`, required to make this a module.