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"
 
@@ -89,7 +90,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_));
@@ -267,6 +267,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_) {
@@ -317,6 +318,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);
   }
@@ -332,6 +335,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;
@@ -355,6 +359,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,
@@ -373,6 +378,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 {
@@ -417,10 +423,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]);
@@ -438,16 +451,17 @@ vector<VectorXd> C3::SolveQP(const VectorXd& x0, const vector<MatrixXd>& G,
   AddAugmentedCost(G, WD, delta, is_final_solve);
   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_;
 }

core/c3_options.h

@@ -76,6 +76,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;
@@ -88,6 +89,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;
@@ -150,16 +152,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());
@@ -170,16 +170,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()) {
-        u_vector.insert(u_vector.end(), u_eta_slack->begin(),
-                        u_eta_slack->end());
-        u_vector.insert(u_vector.end(), u_eta_n->begin(), u_eta_n->end());
-        u_vector.insert(u_vector.end(), u_eta_t->begin(), u_eta_t->end());
-      } else {
-        u_vector.insert(u_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());
@@ -190,8 +188,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]);
   }
@@ -156,6 +173,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

@@ -416,8 +416,8 @@ std::vector<LCSContactDescription> LCSFactory::GetContactDescriptions() {
   for (int i = 0; i < n_contacts_; i++) {
     multibody::GeomGeomCollider collider(plant_, contact_pairs_[i]);
     auto [p_WCa, p_WCb] = collider.CalcWitnessPoints(context_);
-    auto force_basis =
-        collider.CalcForceBasisInWorldFrame(context_, n_friction_directions_);
+    auto force_basis = collider.CalcForceBasisInWorldFrame(
+        context_, n_friction_directions_per_contact_[i]);
 
     for (int j = 0; j < force_basis.rows(); j++) {
       LCSContactDescription contact_description = {
@@ -452,16 +452,19 @@ std::vector<LCSContactDescription> LCSFactory::GetContactDescriptions() {
     contact_descriptions.insert(contact_descriptions.end(),
                                 tangential_contact_descriptions.begin(),
                                 tangential_contact_descriptions.end());
-    DRAKE_ASSERT(n_friction_directions_ > 0);
-    for (int i = 0; i < tangential_contact_descriptions.size(); i++) {
-      int normal_index = i / (2 * n_friction_directions_);
-      DRAKE_ASSERT(
-          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;
+    for (int normal_index = 0; normal_index < n_contacts_; normal_index++) {
+      // Jt_row_sizes_ gives number of friction directions per contact
+      for (int i = 0; i < Jt_row_sizes_(normal_index); i++) {
+        int tangential_index = Jt_row_sizes_.segment(0, normal_index).sum() + i;
+        DRAKE_ASSERT(
+            contact_descriptions.at(tangential_index).witness_point_A ==
+            normal_contact_descriptions.at(normal_index).witness_point_A);
+        contact_descriptions.at(tangential_index).force_basis =
+            mu_[normal_index] *
+                contact_descriptions.at(tangential_index).force_basis +
+            normal_contact_descriptions.at(normal_index).force_basis;
+        contact_descriptions.at(tangential_index).force_basis.normalize();
+      }
     }
   }