fixes after rebase

Author: Meow404

.bazelrc

@@ -52,5 +52,3 @@ build --action_env=LD_LIBRARY_PATH=
 build --python_path=python3
 
 build --define=WITH_GUROBI=ON
-
-build --local_resources=cpu=8

core/BUILD.bazel

@@ -29,6 +29,11 @@ filegroup(
     ],
 )
 
+cc_library(
+    name = "logging",
+    hdrs = ["common/logging_utils.hpp"],
+)
+
 cc_library(
     name = "c3",
     srcs = [
@@ -49,11 +54,15 @@ cc_library(
         "c3_qp.h",
         "c3_plus.h",
     ],
+    data = [
+        ":default_solver_options",
+    ],
     copts = ["-fopenmp"],
     linkopts = ["-fopenmp"],
     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"
 
@@ -90,7 +91,6 @@ C3::C3(const LCS& lcs, const CostMatrices& costs,
   w_sol_ = std::make_unique<std::vector<VectorXd>>();
   delta_sol_ = std::make_unique<std::vector<VectorXd>>();
   for (int i = 0; i < N_; ++i) {
-    z_sol_->push_back(Eigen::VectorXd::Zero(n_z_));
     x_sol_->push_back(Eigen::VectorXd::Zero(n_x_));
     lambda_sol_->push_back(Eigen::VectorXd::Zero(n_lambda_));
     u_sol_->push_back(Eigen::VectorXd::Zero(n_u_));
@@ -269,6 +269,7 @@ const std::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_) {
@@ -319,6 +320,8 @@ void C3::Solve(const VectorXd& x0) {
   std::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);
   }
@@ -328,12 +331,14 @@ void C3::Solve(const VectorXd& x0) {
     WD.at(i) = delta.at(i) - w.at(i);
   }
 
+  drake::log()->debug("C3::Solve final SolveQP step.");
   *z_fin_ = SolveQP(x0, G, WD, options_.admm_iter, true);
 
   *w_sol_ = w;
   *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;
@@ -357,6 +362,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,
@@ -368,13 +374,15 @@ void C3::ADMMStep(const VectorXd& x0, vector<VectorXd>* delta,
     WD.at(i) = delta->at(i) - w->at(i);
   }
 
+  drake::log()->debug("C3::ADMMStep SolveQP step.");
   vector<VectorXd> z = SolveQP(x0, *G, WD, admm_iteration, false);
 
   vector<VectorXd> ZW(N_, VectorXd::Zero(n_z_));
   for (int i = 0; i < N_; ++i) {
     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,10 +427,17 @@ void C3::StoreQPResults(const MathematicalProgramResult& result,
     z_sol_->at(i).segment(0, n_x_) = result.GetSolution(x_[i]);
     z_sol_->at(i).segment(n_x_, n_lambda_) = result.GetSolution(lambda_[i]);
     z_sol_->at(i).segment(n_x_ + n_lambda_, n_u_) = result.GetSolution(u_[i]);
+
+    drake::log()->trace(
+        "C3::StoreQPResults storing solution for time step {}:  "
+        "lambda = {}",
+        i, EigenToString(lambda_sol_->at(i).transpose()));
   }
 
   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,6 +451,7 @@ void C3::StoreQPResults(const MathematicalProgramResult& result,
 vector<VectorXd> C3::SolveQP(const VectorXd& x0, const vector<MatrixXd>& G,
                              const vector<VectorXd>& WD, int admm_iteration,
                              bool is_final_solve) {
+  drake::log()->trace("C3::SolveQP Adding augmented costs(G).");
   // Add or update augmented costs
   if (augmented_costs_.size() == 0) {
     for (int i = 0; i < N_; ++i)
@@ -452,16 +468,17 @@ vector<VectorXd> C3::SolveQP(const VectorXd& x0, const vector<MatrixXd>& G,
 
   SetInitialGuessQP(x0, admm_iteration);
 
+  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());
+    }
+    StoreQPResults(result, admm_iteration, is_final_solve);
   } catch (const std::exception& e) {
     drake::log()->error("C3::SolveQP failed with exception: {}", e.what());
   }
-  if (!result.is_success()) {
-    drake::log()->warn("C3::SolveQP failed to solve the QP with status: {}",
-                       result.get_solution_result());
-  }
-  StoreQPResults(result, admm_iteration, is_final_solve);
 
   return *z_sol_;
 }
@@ -477,8 +494,11 @@ vector<VectorXd> C3::SolveProjection(const vector<MatrixXd>& U,
     omp_set_schedule(omp_sched_static, 0);
   }
 
+  // clang-format off
 #pragma omp parallel for num_threads( \
         options_.num_threads) if (use_parallelization_in_projection_)
+  // clang-format on
+
   for (int i = 0; i < N_; ++i) {
     if (warm_start_) {
       if (i == N_ - 1) {

core/c3_options.h

@@ -67,6 +67,7 @@ struct C3Options {
   std::vector<double> g_lambda_t;
   std::vector<double> g_lambda;
   std::vector<double> g_u;
+  std::vector<double> g_eta_vector;
   std::optional<std::vector<double>> g_eta_slack;
   std::optional<std::vector<double>> g_eta_n;
   std::optional<std::vector<double>> g_eta_t;
@@ -79,6 +80,7 @@ struct C3Options {
   std::vector<double> u_lambda_t;
   std::vector<double> u_lambda;
   std::vector<double> u_u;
+  std::vector<double> u_eta_vector;
   std::optional<std::vector<double>> u_eta_slack;
   std::optional<std::vector<double>> u_eta_n;
   std::optional<std::vector<double>> u_eta_t;
@@ -139,16 +141,14 @@ struct C3Options {
     }
     g_vector.insert(g_vector.end(), g_lambda.begin(), g_lambda.end());
     g_vector.insert(g_vector.end(), g_u.begin(), g_u.end());
-    if (g_eta != std::nullopt || g_eta_slack != std::nullopt) {
-      if (g_eta == std::nullopt || g_eta->empty()) {
-        g_vector.insert(g_vector.end(), g_eta_slack->begin(),
-                        g_eta_slack->end());
-        g_vector.insert(g_vector.end(), g_eta_n->begin(), g_eta_n->end());
-        g_vector.insert(g_vector.end(), g_eta_t->begin(), g_eta_t->end());
-      } else {
-        g_vector.insert(g_vector.end(), g_eta->begin(), g_eta->end());
-      }
+    g_eta_vector = g_eta.value_or(std::vector<double>());
+    if (g_eta_vector.empty() && g_eta_slack.has_value()) {
+      g_eta_vector.insert(g_eta_vector.end(), g_eta_slack->begin(),
+                          g_eta_slack->end());
+      g_eta_vector.insert(g_eta_vector.end(), g_eta_n->begin(), g_eta_n->end());
+      g_eta_vector.insert(g_eta_vector.end(), g_eta_t->begin(), g_eta_t->end());
     }
+    g_vector.insert(g_vector.end(), g_eta_vector.begin(), g_eta_vector.end());
 
     u_vector = std::vector<double>();
     u_vector.insert(u_vector.end(), u_x.begin(), u_x.end());
@@ -159,16 +159,14 @@ struct C3Options {
     }
     u_vector.insert(u_vector.end(), u_lambda.begin(), u_lambda.end());
     u_vector.insert(u_vector.end(), u_u.begin(), u_u.end());
-    if (u_eta != std::nullopt || u_eta_slack != std::nullopt) {
-      if (u_eta == std::nullopt || u_eta->empty()) {
-        g_vector.insert(g_vector.end(), u_eta_slack->begin(),
-                        u_eta_slack->end());
-        g_vector.insert(g_vector.end(), u_eta_n->begin(), u_eta_n->end());
-        g_vector.insert(g_vector.end(), u_eta_t->begin(), u_eta_t->end());
-      } else {
-        g_vector.insert(g_vector.end(), u_eta->begin(), u_eta->end());
-      }
+    u_eta_vector = u_eta.value_or(std::vector<double>());
+    if (u_eta_vector.empty() && u_eta_slack.has_value()) {
+      u_eta_vector.insert(u_eta_vector.end(), u_eta_slack->begin(),
+                          u_eta_slack->end());
+      u_eta_vector.insert(u_eta_vector.end(), u_eta_n->begin(), u_eta_n->end());
+      u_eta_vector.insert(u_eta_vector.end(), u_eta_t->begin(), u_eta_t->end());
     }
+    u_vector.insert(u_vector.end(), u_eta_vector.begin(), u_eta_vector.end());
 
     Eigen::VectorXd q = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
         this->q_vector.data(), this->q_vector.size());
@@ -179,8 +177,6 @@ struct C3Options {
     Eigen::VectorXd u = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
         this->u_vector.data(), this->u_vector.size());
 
-    DRAKE_DEMAND(g.size() == u.size());
-
     Q = w_Q * q.asDiagonal();
     R = w_R * r.asDiagonal();
     G = w_G * g.asDiagonal();

core/c3_plus.cc

@@ -5,8 +5,11 @@
 #include <Eigen/Dense>
 
 #include "c3_options.h"
+#include "common/logging_utils.hpp"
 #include "lcs.h"
 
+#include "drake/common/text_logging.h"
+
 namespace c3 {
 
 using Eigen::MatrixXd;
@@ -19,6 +22,16 @@ C3Plus::C3Plus(const LCS& lcs, const CostMatrices& costs,
                const vector<VectorXd>& xdesired, const C3Options& options)
     : C3(lcs, costs, xdesired, options,
          lcs.num_states() + 2 * lcs.num_lambdas() + lcs.num_inputs()) {
+  if (warm_start_) {
+    warm_start_eta_.resize(options_.admm_iter + 1);
+    for (int iter = 0; iter < options_.admm_iter + 1; ++iter) {
+      warm_start_eta_[iter].resize(N_);
+      for (int i = 0; i < N_; ++i) {
+        warm_start_eta_[iter][i] = VectorXd::Zero(n_lambda_);
+      }
+    }
+  }
+
   // Initialize eta as optimization variables
   eta_ = vector<drake::solvers::VectorXDecisionVariable>();
   eta_sol_ = std::make_unique<std::vector<VectorXd>>();
@@ -40,10 +53,7 @@ C3Plus::C3Plus(const LCS& lcs, const CostMatrices& costs,
     EtaLinEq.block(0, n_x_ + n_lambda_, n_lambda_, n_u_) = lcs_.H().at(i);
 
     eta_constraints_[i] =
-        prog_
-            .AddLinearEqualityConstraint(
-                EtaLinEq, -lcs_.c().at(i),
-                {x_.at(i), lambda_.at(i), u_.at(i), eta_.at(i)})
+        prog_.AddLinearEqualityConstraint(EtaLinEq, -lcs_.c().at(i), z_.at(i))
             .evaluator()
             .get();
   }
@@ -81,16 +91,23 @@ void C3Plus::SetInitialGuessQP(const Eigen::VectorXd& x0, int admm_iteration) {
 void C3Plus::StoreQPResults(const MathematicalProgramResult& result,
                             int admm_iteration, bool is_final_solve) {
   C3::StoreQPResults(result, admm_iteration, is_final_solve);
+  drake::log()->trace("C3Plus::StoreQPResults storing eta results.");
   for (int i = 0; i < N_; i++) {
     if (is_final_solve) {
       eta_sol_->at(i) = result.GetSolution(eta_[i]);
     }
     z_sol_->at(i).segment(n_x_ + n_lambda_ + n_u_, n_lambda_) =
         result.GetSolution(eta_[i]);
+    drake::log()->trace(
+        "C3Plus::StoreQPResults storing solution for time step {}:  "
+        "eta = {}",
+        i, EigenToString(eta_sol_->at(i).transpose()));
   }
 
   if (!warm_start_)
     return;  // No warm start, so no need to update warm start parameters
+
+  drake::log()->trace("C3Plus::StoreQPResults storing warm start eta.");
   for (int i = 0; i < N_; ++i) {
     warm_start_eta_[admm_iteration][i] = result.GetSolution(eta_[i]);
   }
@@ -120,6 +137,12 @@ VectorXd C3Plus::SolveSingleProjection(const MatrixXd& U,
   VectorXd lambda_c = delta_c.segment(n_x_, n_lambda_);
   VectorXd eta_c = delta_c.segment(n_x_ + n_lambda_ + n_u_, n_lambda_);
 
+  drake::log()->trace(
+      "C3Plus::SolveSingleProjection ADMM iteration {}: pre-projection "
+      "lambda = {}, eta = {}",
+      admm_iteration, EigenToString(lambda_c.transpose()),
+      EigenToString(eta_c.transpose()));
+
   // Set the smaller of lambda and eta to zero
   Eigen::Array<bool, Eigen::Dynamic, 1> eta_larger =
       eta_c.array() * w_eta_vec.array().sqrt() >

core/common/logging_utils.hpp

@@ -0,0 +1,19 @@
+#include <sstream>
+#include <string>
+
+#include <Eigen/Dense>
+
+/**
+ * Converts an Eigen matrix to a string representation.
+ * This function is useful for logging or debugging purposes.
+ *
+ * @tparam Derived The type of the Eigen matrix.
+ * @param mat The Eigen matrix to convert.
+ * @return A string representation of the matrix.
+ */
+template <typename Derived>
+std::string EigenToString(const Eigen::MatrixBase<Derived>& mat) {
+  std::stringstream ss;
+  ss << mat;
+  return ss.str();
+}

multibody/geom_geom_collider.cc

@@ -280,12 +280,11 @@ GeomGeomCollider<T>::CalcForceBasisInWorldFrame(
   const auto query_result = GetGeometryQueryResult(context);
   if (num_friction_directions < 1) {
     // Planar contact: basis is constructed from the contact and planar normals.
-    return ComputePlanarForceBasis(-query_result.signed_distance_pair.nhat_BA_W,
-                                   planar_normal);
+    return ComputePlanarForceBasis(-query_result.nhat_BA_W, planar_normal);
   } else {
     // 3D contact: build polytope basis and rotate using contact normal.
     auto R_WC = drake::math::RotationMatrix<T>::MakeFromOneVector(
-        -query_result.signed_distance_pair.nhat_BA_W, 0);
+        -query_result.nhat_BA_W, 0);
     return ComputePolytopeForceBasis(num_friction_directions) *
            R_WC.matrix().transpose();
   }

multibody/lcs_factory.cc

@@ -420,9 +420,9 @@ std::vector<LCSContactDescription> LCSFactory::GetContactDescriptions() {
           contact_descriptions.at(i).witness_point_A ==
           normal_contact_descriptions.at(normal_index).witness_point_A);
       contact_descriptions.at(i).force_basis =
-          contact_descriptions.at(i).force_basis +
-          mu_[normal_index] *
-              normal_contact_descriptions.at(normal_index).force_basis;
+          mu_[normal_index] * contact_descriptions.at(i).force_basis +
+          normal_contact_descriptions.at(normal_index).force_basis;
+      contact_descriptions.at(i).force_basis.normalize();
     }
   }
 

systems/c3_controller.cc

@@ -111,8 +111,7 @@ C3Controller::C3Controller(
           .get_index();
   c3_intermediates_port_ =
       this->DeclareAbstractOutputPort(
-              "intermediates",
-              C3Output::C3Intermediates(n_x_, n_lambda_, n_u_, N_),
+              "intermediates", C3Output::C3Intermediates(c3_->GetZSize(), N_),
               &C3Controller::OutputC3Intermediates)
           .get_index();
 

systems/c3_controller_options.h

@@ -82,9 +82,9 @@ struct C3ControllerOptions {
     DRAKE_DEMAND(static_cast<int>(c3_options.u_lambda.size()) ==
                  expected_lambda_size);
     if (projection_type == "C3+") {
-      DRAKE_DEMAND(static_cast<int>(c3_options.g_eta->size()) ==
+      DRAKE_DEMAND(static_cast<int>(c3_options.g_eta_vector.size()) ==
                    expected_lambda_size);
-      DRAKE_DEMAND(static_cast<int>(c3_options.u_eta->size()) ==
+      DRAKE_DEMAND(static_cast<int>(c3_options.u_eta_vector.size()) ==
                    expected_lambda_size);
     }
   }