From 9554b92e2d820dc2574d8e598dae986ae9493198 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Fri, 1 Aug 2025 20:06:33 +0000 Subject: [PATCH 01/16] feat: add c3 plus --- core/BUILD.bazel | 5 + core/c3.cc | 249 ++++++++++++++----------- core/c3.h | 31 ++- core/c3_miqp.cc | 177 ++++++++++-------- core/c3_options.h | 52 +++++- core/c3_plus.cc | 141 ++++++++++++++ core/c3_plus.h | 74 ++++++++ core/test/c3_cartpole_problem.hpp | 16 ++ core/test/core_test.cc | 15 +- examples/c3_controller_example.cc | 2 + examples/lcs_factory_system_example.cc | 2 + systems/c3_controller.cc | 5 + systems/c3_controller_options.h | 8 +- 13 files changed, 574 insertions(+), 203 deletions(-) create mode 100644 core/c3_plus.cc create mode 100644 core/c3_plus.h diff --git a/core/BUILD.bazel b/core/BUILD.bazel index 5b80c66..7fa4685 100644 --- a/core/BUILD.bazel +++ b/core/BUILD.bazel @@ -34,6 +34,7 @@ cc_library( srcs = [ "c3.cc", "c3_qp.cc", + "c3_plus.cc", ] + select({ "//tools:with_gurobi": [ ":c3_miqp.cc", @@ -46,6 +47,7 @@ cc_library( "c3.h", "c3_miqp.h", "c3_qp.h", + "c3_plus.h", ], data = [ ":default_solver_options", @@ -82,6 +84,9 @@ cc_library( hdrs = [ "test/c3_cartpole_problem.hpp", ], + deps = [ + ":core", + ], data = [ ":test_data", ] diff --git a/core/c3.cc b/core/c3.cc index 1fe6968..5936658 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -4,6 +4,7 @@ #include #include +#include #include #include "lcs.h" @@ -41,12 +42,14 @@ C3::CostMatrices::CostMatrices(const std::vector& Q, } C3::C3(const LCS& lcs, const CostMatrices& costs, - const vector& x_desired, const C3Options& options) + const vector& x_desired, const C3Options& options, + CalcZSizeFunc calc_z_size) : warm_start_(options.warm_start), N_(lcs.N()), n_x_(lcs.num_states()), n_lambda_(lcs.num_lambdas()), n_u_(lcs.num_inputs()), + n_z_(calc_z_size ? calc_z_size(lcs) : n_x_ + n_lambda_ + n_u_), lcs_(lcs), cost_matrices_(costs), x_desired_(x_desired), @@ -55,20 +58,10 @@ C3::C3(const LCS& lcs, const CostMatrices& costs, prog_(MathematicalProgram()), osqp_(OsqpSolver()) { if (warm_start_) { - warm_start_delta_.resize(options_.admm_iter + 1); - warm_start_binary_.resize(options_.admm_iter + 1); warm_start_x_.resize(options_.admm_iter + 1); warm_start_lambda_.resize(options_.admm_iter + 1); warm_start_u_.resize(options_.admm_iter + 1); for (int iter = 0; iter < options_.admm_iter + 1; ++iter) { - warm_start_delta_[iter].resize(N_); - for (int i = 0; i < N_; ++i) { - warm_start_delta_[iter][i] = VectorXd::Zero(n_x_ + n_lambda_ + n_u_); - } - warm_start_binary_[iter].resize(N_); - for (int i = 0; i < N_; ++i) { - warm_start_binary_[iter][i] = VectorXd::Zero(n_lambda_); - } warm_start_x_[iter].resize(N_ + 1); for (int i = 0; i < N_ + 1; ++i) { warm_start_x_[iter][i] = VectorXd::Zero(n_x_); @@ -96,29 +89,33 @@ C3::C3(const LCS& lcs, const CostMatrices& costs, w_sol_ = std::make_unique>(); delta_sol_ = std::make_unique>(); for (int i = 0; i < N_; ++i) { - z_sol_->push_back(Eigen::VectorXd::Zero(n_x_ + n_lambda_ + n_u_)); + 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_)); - w_sol_->push_back(Eigen::VectorXd::Zero(n_x_ + n_lambda_ + n_u_)); - delta_sol_->push_back(Eigen::VectorXd::Zero(n_x_ + n_lambda_ + n_u_)); + w_sol_->push_back(Eigen::VectorXd::Zero(n_z_)); + delta_sol_->push_back(Eigen::VectorXd::Zero(n_z_)); } + z_.resize(N_); for (int i = 0; i < N_ + 1; ++i) { x_.push_back(prog_.NewContinuousVariables(n_x_, "x" + std::to_string(i))); if (i < N_) { - u_.push_back(prog_.NewContinuousVariables(n_u_, "k" + std::to_string(i))); lambda_.push_back(prog_.NewContinuousVariables( n_lambda_, "lambda" + std::to_string(i))); + u_.push_back(prog_.NewContinuousVariables(n_u_, "k" + std::to_string(i))); + z_.at(i).push_back(x_.back()); + z_.at(i).push_back(lambda_.back()); + z_.at(i).push_back(u_.back()); } } // initialize the constraint bindings initial_state_constraint_ = nullptr; initial_force_constraint_ = nullptr; - dynamics_constraints_.resize(N_); - target_cost_.resize(N_ + 1); + // Add dynamics constraints + dynamics_constraints_.resize(N_); MatrixXd LinEq(n_x_, 2 * n_x_ + n_lambda_ + n_u_); LinEq.block(0, n_x_ + n_lambda_ + n_u_, n_x_, n_x_) = -1 * MatrixXd::Identity(n_x_, n_x_); @@ -135,9 +132,13 @@ C3::C3(const LCS& lcs, const CostMatrices& costs, .evaluator() .get(); } + + // Setup QP costs + target_costs_.resize(N_ + 1); input_costs_.resize(N_); + augmented_costs_.clear(); for (int i = 0; i < N_ + 1; ++i) { - target_cost_[i] = + target_costs_[i] = prog_ .AddQuadraticCost(2 * cost_matrices_.Q.at(i), -2 * cost_matrices_.Q.at(i) * x_desired_.at(i), @@ -153,9 +154,24 @@ C3::C3(const LCS& lcs, const CostMatrices& costs, } // Set default solver options + SetDefaultSolverOptions(); +} + +void C3::SetDefaultSolverOptions() { + // Set default solver options + auto main_runfile = + drake::FindRunfile("_main/core/configs/solver_options_default.yaml"); + auto external_runfile = + drake::FindRunfile("c3/core/configs/solver_options_default.yaml"); + if (main_runfile.abspath.empty() && external_runfile.abspath.empty()) { + throw std::runtime_error(fmt::format( + "Could not find the default solver options YAML file. {}, {}", + main_runfile.error, external_runfile.error)); + } drake::solvers::SolverOptions solver_options = drake::yaml::LoadYamlFile( - "core/configs/solver_options_default.yaml") + main_runfile.abspath.empty() ? external_runfile.abspath + : main_runfile.abspath) .GetAsSolverOptions(drake::solvers::OsqpSolver::id()); SetSolverOptions(solver_options); } @@ -219,7 +235,7 @@ C3::GetDynamicConstraints() { void C3::UpdateTarget(const std::vector& x_des) { x_desired_ = x_des; for (int i = 0; i < N_ + 1; ++i) { - target_cost_[i]->UpdateCoefficients( + target_costs_[i]->UpdateCoefficients( 2 * cost_matrices_.Q.at(i), -2 * cost_matrices_.Q.at(i) * x_desired_.at(i)); } @@ -230,7 +246,7 @@ void C3::UpdateCostMatrices(const CostMatrices& costs) { cost_matrices_ = costs; for (int i = 0; i < N_ + 1; ++i) { - target_cost_[i]->UpdateCoefficients( + target_costs_[i]->UpdateCoefficients( 2 * cost_matrices_.Q.at(i), -2 * cost_matrices_.Q.at(i) * x_desired_.at(i)); if (i < N_) { @@ -241,11 +257,12 @@ void C3::UpdateCostMatrices(const CostMatrices& costs) { } const std::vector& C3::GetTargetCost() { - return target_cost_; + return target_costs_; } void C3::Solve(const VectorXd& x0) { auto start = std::chrono::high_resolution_clock::now(); + // Set the initial state constraint if (initial_state_constraint_) { initial_state_constraint_->UpdateCoefficients( MatrixXd::Identity(n_x_, n_x_), x0); @@ -256,25 +273,49 @@ void C3::Solve(const VectorXd& x0) { x_[0]) .evaluator(); } - VectorXd delta_init = VectorXd::Zero(n_x_ + n_lambda_ + n_u_); + + // Set the initial force constraint + if (h_is_zero_ == 1) { // No dependence on u, so just simulate passive system + drake::solvers::MobyLCPSolver LCPSolver; + VectorXd lambda0; + LCPSolver.SolveLcpLemke(lcs_.F()[0], lcs_.E()[0] * x0 + lcs_.c()[0], + &lambda0); + // Force constraints to be updated before every solve if no dependence on u + if (initial_force_constraint_) { + initial_force_constraint_->UpdateCoefficients( + MatrixXd::Identity(n_lambda_, n_lambda_), lambda0); + } else { + initial_force_constraint_ = + prog_ + .AddLinearEqualityConstraint( + MatrixXd::Identity(n_lambda_, n_lambda_), lambda0, lambda_[0]) + .evaluator(); + } + } + + if (options_.penalize_input_change) { + for (int i = 0; i < N_; ++i) { + // Penalize deviation from previous input solution: input cost is + // (u-u_prev)' * R * (u-u_prev). + input_costs_[i]->UpdateCoefficients( + 2 * cost_matrices_.R.at(i), + -2 * cost_matrices_.R.at(i) * u_sol_->at(i)); + } + } + + VectorXd delta_init = VectorXd::Zero(n_z_); if (options_.delta_option == 1) { delta_init.head(n_x_) = x0; } std::vector delta(N_, delta_init); - std::vector w(N_, VectorXd::Zero(n_x_ + n_lambda_ + n_u_)); + std::vector w(N_, VectorXd::Zero(n_z_)); vector G = cost_matrices_.G; - for (int i = 0; i < N_; ++i) { - input_costs_[i]->UpdateCoefficients( - 2 * cost_matrices_.R.at(i), - -2 * cost_matrices_.R.at(i) * u_sol_->at(i)); - } - for (int iter = 0; iter < options_.admm_iter; iter++) { ADMMStep(x0, &delta, &w, &G, iter); } - vector WD(N_, VectorXd::Zero(n_x_ + n_lambda_ + n_u_)); + vector WD(N_, VectorXd::Zero(n_z_)); for (int i = 0; i < N_; ++i) { WD.at(i) = delta.at(i) - w.at(i); } @@ -313,15 +354,15 @@ void C3::Solve(const VectorXd& x0) { void C3::ADMMStep(const VectorXd& x0, vector* delta, vector* w, vector* G, int admm_iteration) { - vector WD(N_, VectorXd::Zero(n_x_ + n_lambda_ + n_u_)); + vector WD(N_, VectorXd::Zero(n_z_)); for (int i = 0; i < N_; ++i) { WD.at(i) = delta->at(i) - w->at(i); } - vector z = SolveQP(x0, *G, WD, admm_iteration, true); + vector z = SolveQP(x0, *G, WD, admm_iteration, false); - vector ZW(N_, VectorXd::Zero(n_x_ + n_lambda_ + n_u_)); + vector ZW(N_, VectorXd::Zero(n_z_)); for (int i = 0; i < N_; ++i) { ZW[i] = w->at(i) + z[i]; } @@ -339,105 +380,84 @@ void C3::ADMMStep(const VectorXd& x0, vector* delta, } } -vector C3::SolveQP(const VectorXd& x0, const vector& G, - const vector& WD, int admm_iteration, - bool is_final_solve) { - if (h_is_zero_ == 1) { // No dependence on u, so just simulate passive system - drake::solvers::MobyLCPSolver LCPSolver; - VectorXd lambda0; - LCPSolver.SolveLcpLemke(lcs_.F()[0], lcs_.E()[0] * x0 + lcs_.c()[0], - &lambda0); - // Force constraints to be updated before every solve if no dependence on u - if (initial_force_constraint_) { - initial_force_constraint_->UpdateCoefficients( - MatrixXd::Identity(n_lambda_, n_lambda_), lambda0); - } else { - initial_force_constraint_ = - prog_ - .AddLinearEqualityConstraint( - MatrixXd::Identity(n_lambda_, n_lambda_), lambda0, lambda_[0]) - .evaluator(); - } - } +void C3::WarmStartQP(const Eigen::VectorXd& x0, int admm_iteration) { + if (admm_iteration == 0) return; // No warm start for the first iteration + int index = solve_time_ / lcs_.dt(); + double weight = (solve_time_ - index * lcs_.dt()) / lcs_.dt(); + for (int i = 0; i < N_ - 1; ++i) { + prog_.SetInitialGuess( + x_[i], (1 - weight) * warm_start_x_[admm_iteration - 1][i] + + weight * warm_start_x_[admm_iteration - 1][i + 1]); + prog_.SetInitialGuess( + lambda_[i], (1 - weight) * warm_start_lambda_[admm_iteration - 1][i] + + weight * warm_start_lambda_[admm_iteration - 1][i + 1]); + prog_.SetInitialGuess( + u_[i], (1 - weight) * warm_start_u_[admm_iteration - 1][i] + + weight * warm_start_u_[admm_iteration - 1][i + 1]); + } + prog_.SetInitialGuess(x_[N_], warm_start_x_[admm_iteration - 1][N_]); +} - for (auto& cost : costs_) { - prog_.RemoveCost(cost); +void C3::ProcessQPResults(const MathematicalProgramResult& result, + int admm_iteration, bool is_final_solve) { + for (int i = 0; i < N_; ++i) { + if (is_final_solve) { + x_sol_->at(i) = result.GetSolution(x_[i]); + lambda_sol_->at(i) = result.GetSolution(lambda_[i]); + u_sol_->at(i) = result.GetSolution(u_[i]); + } + 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]); } - costs_.clear(); + if (!warm_start_) + return; // No warm start, so no need to update warm start parameters for (int i = 0; i < N_ + 1; ++i) { if (i < N_) { - costs_.push_back(prog_.AddQuadraticCost( - 2 * G.at(i).block(0, 0, n_x_, n_x_), - -2 * G.at(i).block(0, 0, n_x_, n_x_) * WD.at(i).segment(0, n_x_), - x_.at(i), 1)); - costs_.push_back(prog_.AddQuadraticCost( - 2 * G.at(i).block(n_x_, n_x_, n_lambda_, n_lambda_), - -2 * G.at(i).block(n_x_, n_x_, n_lambda_, n_lambda_) * - WD.at(i).segment(n_x_, n_lambda_), - lambda_.at(i), 1)); - costs_.push_back(prog_.AddQuadraticCost( - 2 * G.at(i).block(n_x_ + n_lambda_, n_x_ + n_lambda_, n_u_, n_u_), - -2 * G.at(i).block(n_x_ + n_lambda_, n_x_ + n_lambda_, n_u_, n_u_) * - WD.at(i).segment(n_x_ + n_lambda_, n_u_), - u_.at(i), 1)); + warm_start_x_[admm_iteration][i] = result.GetSolution(x_[i]); + warm_start_lambda_[admm_iteration][i] = result.GetSolution(lambda_[i]); + warm_start_u_[admm_iteration][i] = result.GetSolution(u_[i]); } + warm_start_x_[admm_iteration][N_] = result.GetSolution(x_[N_]); } +} - // /// initialize decision variables to warm start - if (warm_start_) { - int index = solve_time_ / lcs_.dt(); - double weight = (solve_time_ - index * lcs_.dt()) / lcs_.dt(); - for (int i = 0; i < N_ - 1; ++i) { - prog_.SetInitialGuess(x_[i], - (1 - weight) * warm_start_x_[admm_iteration][i] + - weight * warm_start_x_[admm_iteration][i + 1]); - prog_.SetInitialGuess( - lambda_[i], (1 - weight) * warm_start_lambda_[admm_iteration][i] + - weight * warm_start_lambda_[admm_iteration][i + 1]); - prog_.SetInitialGuess(u_[i], - (1 - weight) * warm_start_u_[admm_iteration][i] + - weight * warm_start_u_[admm_iteration][i + 1]); - } - prog_.SetInitialGuess(x_[0], x0); - prog_.SetInitialGuess(x_[N_], warm_start_x_[admm_iteration][N_]); +vector C3::SolveQP(const VectorXd& x0, const vector& G, + const vector& WD, int admm_iteration, + bool is_final_solve) { + // Add or update augmented costs + if (augmented_costs_.size() == 0) { + for (int i = 0; i < N_; ++i) + augmented_costs_.push_back(prog_ + .AddQuadraticCost(2 * G.at(i), + -2 * G.at(i) * WD.at(i), + z_.at(i), 1) + .evaluator()); + } else { + for (int i = 0; i < N_; ++i) + augmented_costs_[i]->UpdateCoefficients(2 * G.at(i), + -2 * G.at(i) * WD.at(i)); } + prog_.SetInitialGuess(x_[0], x0); + if (warm_start_) WarmStartQP(x0, admm_iteration); + MathematicalProgramResult result = osqp_.Solve(prog_); - if (result.is_success()) { - for (int i = 0; i < N_; ++i) { - if (is_final_solve) { - x_sol_->at(i) = result.GetSolution(x_[i]); - lambda_sol_->at(i) = result.GetSolution(lambda_[i]); - u_sol_->at(i) = result.GetSolution(u_[i]); - } - 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]); - - if (warm_start_) { - // update warm start parameters - warm_start_x_[admm_iteration][i] = result.GetSolution(x_[i]); - warm_start_lambda_[admm_iteration][i] = result.GetSolution(lambda_[i]); - warm_start_u_[admm_iteration][i] = result.GetSolution(u_[i]); - } - } - if (warm_start_) { - warm_start_x_[admm_iteration][N_] = result.GetSolution(x_[N_]); - } - } else { + if (!result.is_success()) { drake::log()->warn("C3::SolveQP failed to solve the QP with status: {}", result.get_solution_result()); } + ProcessQPResults(result, admm_iteration, is_final_solve); + return *z_sol_; } vector C3::SolveProjection(const vector& U, vector& WZ, int admm_iteration) { - vector deltaProj(N_, VectorXd::Zero(n_x_ + n_lambda_ + n_u_)); - int i; + vector deltaProj(N_, VectorXd::Zero(n_z_)); if (options_.num_threads > 0) { omp_set_dynamic(0); // Explicitly disable dynamic teams @@ -446,8 +466,9 @@ vector C3::SolveProjection(const vector& U, omp_set_schedule(omp_sched_static, 0); } -#pragma omp parallel for num_threads(options_.num_threads) - for (i = 0; i < N_; ++i) { +#pragma omp parallel for num_threads( \ + options_.num_threads) if (use_parallelization_in_projection_) + for (int i = 0; i < N_; ++i) { if (warm_start_) { if (i == N_ - 1) { deltaProj[i] = diff --git a/core/c3.h b/core/c3.h index 0247f20..1a11594 100644 --- a/core/c3.h +++ b/core/c3.h @@ -13,6 +13,8 @@ namespace c3 { typedef drake::solvers::Binding LinearConstraintBinding; +typedef drake::solvers::Binding + QuadraticCostBinding; enum ConstraintVariable : uint8_t { STATE = 1, INPUT = 2, FORCE = 3 }; @@ -50,6 +52,8 @@ class C3 { std::vector U; }; + using CalcZSizeFunc = std::function; + /*! * @param LCS system dynamics, defined as an LCS (see lcs.h) * @param costs Cost function parameters (see above) @@ -58,7 +62,8 @@ class C3 { * @param options see c3_options.h */ C3(const LCS& LCS, const CostMatrices& costs, - const std::vector& x_desired, const C3Options& options); + const std::vector& x_desired, const C3Options& options, + CalcZSizeFunc calc_z_size = nullptr); virtual ~C3() = default; @@ -72,7 +77,7 @@ class C3 { * Update the dynamics * @param lcs the new LCS */ - void UpdateLCS(const LCS& lcs); + virtual void UpdateLCS(const LCS& lcs); /** * @brief Get a vector dynamic constraints. @@ -205,6 +210,9 @@ class C3 { const int n_x_; // n const int n_lambda_; // m const int n_u_; // k + const int n_z_; // (default) n + m + k + + bool use_parallelization_in_projection_ = true; /*! * Project delta_c onto the LCP constraint. @@ -213,7 +221,8 @@ class C3 { * @param delta_c value to project to the LCP constraint * @param E, F, H, c LCS state, force, input, and constant terms * @param admm_iteration index of the current ADMM iteration - * @param warm_start_index index into cache of warm start variables + * @param warm_start_index index into cache of warm start variables, -1 for no + * warm start * @return */ virtual Eigen::VectorXd SolveSingleProjection( @@ -222,13 +231,22 @@ class C3 { const Eigen::MatrixXd& H, const Eigen::VectorXd& c, const int admm_iteration, const int& warm_start_index) = 0; - private: + virtual void WarmStartQP(const Eigen::VectorXd& x0, int admm_iteration); + virtual void ProcessQPResults( + const drake::solvers::MathematicalProgramResult& result, + int admm_iteration, bool is_final_solve); /*! * Scales the LCS matrices internally to better condition the problem. * This only scales the lambdas. */ void ScaleLCS(); + /*! + * Set the default solver options for the MathematicalProgram + * This is called in the constructor, and can be overridden by the user + */ + void SetDefaultSolverOptions(); + /*! * Solve the projection problem for all time-steps * @param U Similarity cost weights for the projection optimization @@ -279,6 +297,7 @@ class C3 { std::vector x_; std::vector u_; std::vector lambda_; + std::vector z_; // QP step constraints std::shared_ptr @@ -291,8 +310,8 @@ class C3 { /// Projection step variables are defined outside of the MathematicalProgram /// interface - std::vector target_cost_; - std::vector> costs_; + std::vector target_costs_; + std::vector> augmented_costs_; std::vector> input_costs_; // Solutions diff --git a/core/c3_miqp.cc b/core/c3_miqp.cc index 144df7c..b97e41d 100644 --- a/core/c3_miqp.cc +++ b/core/c3_miqp.cc @@ -10,7 +10,22 @@ using std::vector; C3MIQP::C3MIQP(const LCS& LCS, const CostMatrices& costs, const vector& xdesired, const C3Options& options) - : C3(LCS, costs, xdesired, options), M_(options.M) {} + : C3(LCS, costs, xdesired, options), M_(options.M) { + if (warm_start_) { + warm_start_delta_.resize(options_.admm_iter + 1); + warm_start_binary_.resize(options_.admm_iter + 1); + for (int iter = 0; iter < options_.admm_iter + 1; ++iter) { + warm_start_delta_[iter].resize(N_); + for (int i = 0; i < N_; ++i) { + warm_start_delta_[iter][i] = VectorXd::Zero(n_z_); + } + warm_start_binary_[iter].resize(N_); + for (int i = 0; i < N_; ++i) { + warm_start_binary_[iter][i] = VectorXd::Zero(n_lambda_); + } + } + } +} VectorXd C3MIQP::SolveSingleProjection(const MatrixXd& U, const VectorXd& delta_c, @@ -18,100 +33,106 @@ VectorXd C3MIQP::SolveSingleProjection(const MatrixXd& U, const MatrixXd& H, const VectorXd& c, const int admm_iteration, const int& warm_start_index) { - // Create an environment - GRBEnv env(true); - env.set("LogToConsole", "0"); - env.set("OutputFlag", "0"); - env.set("Threads", "0"); - env.start(); - // set up linear term in cost - VectorXd cost_lin = -2 * delta_c.transpose() * U; - - // set up for constraints (Ex + F \lambda + Hu + c >= 0) - MatrixXd Mcons1(n_lambda_, n_x_ + n_lambda_ + n_u_); - Mcons1 << E, F, H; - - // set up for constraints (\lambda >= 0) - MatrixXd MM1 = MatrixXd::Zero(n_lambda_, n_x_); - MatrixXd MM2 = MatrixXd::Identity(n_lambda_, n_lambda_); - MatrixXd MM3 = MatrixXd::Zero(n_lambda_, n_u_); - MatrixXd Mcons2(n_lambda_, n_x_ + n_lambda_ + n_u_); - Mcons2 << MM1, MM2, MM3; - - GRBModel model = GRBModel(env); - - GRBVar delta_k[n_x_ + n_lambda_ + n_u_]; - GRBVar binary[n_lambda_]; - - for (int i = 0; i < n_lambda_; i++) { - binary[i] = model.addVar(0.0, 1.0, 0.0, GRB_BINARY); - if (warm_start_index != -1) { - binary[i].set(GRB_DoubleAttr_Start, - warm_start_binary_[admm_iteration][warm_start_index](i)); + try { + // Create an environment + GRBEnv env(true); + env.set("LogToConsole", "0"); + env.set("OutputFlag", "0"); + env.set("Threads", "0"); + env.start(); + // set up linear term in cost + VectorXd cost_lin = -2 * delta_c.transpose() * U; + + // set up for constraints (Ex + F \lambda + Hu + c >= 0) + MatrixXd Mcons1(n_lambda_, n_x_ + n_lambda_ + n_u_); + Mcons1 << E, F, H; + + // set up for constraints (\lambda >= 0) + MatrixXd MM1 = MatrixXd::Zero(n_lambda_, n_x_); + MatrixXd MM2 = MatrixXd::Identity(n_lambda_, n_lambda_); + MatrixXd MM3 = MatrixXd::Zero(n_lambda_, n_u_); + MatrixXd Mcons2(n_lambda_, n_x_ + n_lambda_ + n_u_); + Mcons2 << MM1, MM2, MM3; + + GRBModel model = GRBModel(env); + + GRBVar delta_k[n_x_ + n_lambda_ + n_u_]; + GRBVar binary[n_lambda_]; + + for (int i = 0; i < n_lambda_; i++) { + binary[i] = model.addVar(0.0, 1.0, 0.0, GRB_BINARY); + if (warm_start_index != -1) { + binary[i].set(GRB_DoubleAttr_Start, + warm_start_binary_[admm_iteration][warm_start_index](i)); + } } - } - for (int i = 0; i < n_x_ + n_lambda_ + n_u_; i++) { - delta_k[i] = - model.addVar(-kVariableBounds, kVariableBounds, 0.0, GRB_CONTINUOUS); - if (warm_start_index != -1) { - delta_k[i].set(GRB_DoubleAttr_Start, - warm_start_delta_[admm_iteration][warm_start_index](i)); + for (int i = 0; i < n_x_ + n_lambda_ + n_u_; i++) { + delta_k[i] = + model.addVar(-kVariableBounds, kVariableBounds, 0.0, GRB_CONTINUOUS); + if (warm_start_index != -1) { + delta_k[i].set(GRB_DoubleAttr_Start, + warm_start_delta_[admm_iteration][warm_start_index](i)); + } } - } - GRBQuadExpr obj = 0; + GRBQuadExpr obj = 0; - for (int i = 0; i < n_x_ + n_lambda_ + n_u_; i++) { - obj.addTerm(cost_lin(i), delta_k[i]); - obj.addTerm(U(i, i), delta_k[i], delta_k[i]); - } + for (int i = 0; i < n_x_ + n_lambda_ + n_u_; i++) { + obj.addTerm(cost_lin(i), delta_k[i]); + obj.addTerm(U(i, i), delta_k[i], delta_k[i]); + } - model.setObjective(obj, GRB_MINIMIZE); + model.setObjective(obj, GRB_MINIMIZE); - double coeff[n_x_ + n_lambda_ + n_u_]; - double coeff2[n_x_ + n_lambda_ + n_u_]; + double coeff[n_x_ + n_lambda_ + n_u_]; + double coeff2[n_x_ + n_lambda_ + n_u_]; - for (int i = 0; i < n_lambda_; i++) { - GRBLinExpr lambda_expr = 0; + for (int i = 0; i < n_lambda_; i++) { + GRBLinExpr lambda_expr = 0; - /// convert VectorXd to double - for (int j = 0; j < n_x_ + n_lambda_ + n_u_; j++) { - coeff[j] = Mcons2(i, j); - } + /// convert VectorXd to double + for (int j = 0; j < n_x_ + n_lambda_ + n_u_; j++) { + coeff[j] = Mcons2(i, j); + } - lambda_expr.addTerms(coeff, delta_k, n_x_ + n_lambda_ + n_u_); - model.addConstr(lambda_expr >= 0); - model.addConstr(lambda_expr <= M_ * (1 - binary[i])); + lambda_expr.addTerms(coeff, delta_k, n_x_ + n_lambda_ + n_u_); + model.addConstr(lambda_expr >= 0); + model.addConstr(lambda_expr <= M_ * (1 - binary[i])); - GRBLinExpr activation_expr = 0; + GRBLinExpr activation_expr = 0; - /// convert VectorXd to double - for (int j = 0; j < n_x_ + n_lambda_ + n_u_; j++) { - coeff2[j] = Mcons1(i, j); - } + /// convert VectorXd to double + for (int j = 0; j < n_x_ + n_lambda_ + n_u_; j++) { + coeff2[j] = Mcons1(i, j); + } - activation_expr.addTerms(coeff2, delta_k, n_x_ + n_lambda_ + n_u_); - model.addConstr(activation_expr + c(i) >= 0); - model.addConstr(activation_expr + c(i) <= M_ * binary[i]); - } + activation_expr.addTerms(coeff2, delta_k, n_x_ + n_lambda_ + n_u_); + model.addConstr(activation_expr + c(i) >= 0); + model.addConstr(activation_expr + c(i) <= M_ * binary[i]); + } - model.optimize(); + model.optimize(); - VectorXd delta_kc(n_x_ + n_lambda_ + n_u_); - VectorXd binaryc(n_lambda_); - for (int i = 0; i < n_x_ + n_lambda_ + n_u_; i++) { - delta_kc(i) = delta_k[i].get(GRB_DoubleAttr_X); - } - for (int i = 0; i < n_lambda_; i++) { - binaryc(i) = binary[i].get(GRB_DoubleAttr_X); - } + VectorXd delta_kc(n_x_ + n_lambda_ + n_u_); + VectorXd binaryc(n_lambda_); + for (int i = 0; i < n_x_ + n_lambda_ + n_u_; i++) { + delta_kc(i) = delta_k[i].get(GRB_DoubleAttr_X); + } + for (int i = 0; i < n_lambda_; i++) { + binaryc(i) = binary[i].get(GRB_DoubleAttr_X); + } - if (warm_start_index != -1) { - warm_start_delta_[admm_iteration][warm_start_index] = delta_kc; - warm_start_binary_[admm_iteration][warm_start_index] = binaryc; + if (warm_start_index != -1) { + warm_start_delta_[admm_iteration][warm_start_index] = delta_kc; + warm_start_binary_[admm_iteration][warm_start_index] = binaryc; + } + return delta_kc; + } catch (GRBException& e) { + std::cerr << "Error code = " << e.getErrorCode() << std::endl; + std::cerr << e.getMessage() << std::endl; + throw std::runtime_error("Gurobi optimization failed."); } - return delta_kc; } } // namespace c3 diff --git a/core/c3_options.h b/core/c3_options.h index a80e122..5583bdb 100644 --- a/core/c3_options.h +++ b/core/c3_options.h @@ -1,5 +1,9 @@ #pragma once +#include + +#include + #include "drake/common/yaml/yaml_io.h" #include "drake/common/yaml/yaml_read_archive.h" @@ -9,6 +13,8 @@ struct C3Options { // Hyperparameters bool warm_start = true; // Use results of current admm iteration as warm start for next + std::optional penalize_input_change = + true; // Penalize change in input between iterations bool end_on_qp_step = true; // If false, Run a half step calculating the state using the LCS bool scale_lcs = @@ -56,6 +62,10 @@ struct C3Options { std::vector g_lambda_t; std::vector g_lambda; std::vector g_u; + std::optional> g_eta_slack; + std::optional> g_eta_n; + std::optional> g_eta_t; + std::optional> g_eta; std::vector u_vector; std::vector u_x; @@ -64,10 +74,15 @@ struct C3Options { std::vector u_lambda_t; std::vector u_lambda; std::vector u_u; + std::optional> u_eta_slack; + std::optional> u_eta_n; + std::optional> u_eta_t; + std::optional> u_eta; template void Serialize(Archive* a) { a->Visit(DRAKE_NVP(warm_start)); + a->Visit(DRAKE_NVP(penalize_input_change)); a->Visit(DRAKE_NVP(end_on_qp_step)); a->Visit(DRAKE_NVP(scale_lcs)); @@ -93,12 +108,20 @@ struct C3Options { a->Visit(DRAKE_NVP(g_lambda_t)); a->Visit(DRAKE_NVP(g_lambda)); a->Visit(DRAKE_NVP(g_u)); + a->Visit(DRAKE_NVP(g_eta_slack)); + a->Visit(DRAKE_NVP(g_eta_n)); + a->Visit(DRAKE_NVP(g_eta_t)); + a->Visit(DRAKE_NVP(g_eta)); a->Visit(DRAKE_NVP(u_x)); a->Visit(DRAKE_NVP(u_gamma)); a->Visit(DRAKE_NVP(u_lambda_n)); a->Visit(DRAKE_NVP(u_lambda_t)); a->Visit(DRAKE_NVP(u_lambda)); a->Visit(DRAKE_NVP(u_u)); + a->Visit(DRAKE_NVP(u_eta_slack)); + a->Visit(DRAKE_NVP(u_eta_n)); + a->Visit(DRAKE_NVP(u_eta_t)); + a->Visit(DRAKE_NVP(u_eta)); g_vector = std::vector(); g_vector.insert(g_vector.end(), g_x.begin(), g_x.end()); @@ -108,8 +131,18 @@ struct C3Options { g_lambda.insert(g_lambda.end(), g_lambda_t.begin(), g_lambda_t.end()); } 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()); + } + } + u_vector = std::vector(); u_vector.insert(u_vector.end(), u_x.begin(), u_x.end()); if (u_lambda.empty()) { @@ -117,9 +150,18 @@ struct C3Options { u_lambda.insert(u_lambda.end(), u_lambda_n.begin(), u_lambda_n.end()); u_lambda.insert(u_lambda.end(), u_lambda_t.begin(), u_lambda_t.end()); } - 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()); + } + } Eigen::VectorXd q = Eigen::Map( this->q_vector.data(), this->q_vector.size()); @@ -140,7 +182,11 @@ struct C3Options { }; inline C3Options LoadC3Options(const std::string& filename) { - auto options = drake::yaml::LoadYamlFile(filename); + auto options = drake::yaml::LoadYamlFile( + filename, std::nullopt, std::nullopt, + drake::yaml::LoadYamlOptions{.allow_yaml_with_no_cpp = false, + .allow_cpp_with_no_yaml = true, + .retain_map_defaults = false}); return options; } diff --git a/core/c3_plus.cc b/core/c3_plus.cc new file mode 100644 index 0000000..eaef732 --- /dev/null +++ b/core/c3_plus.cc @@ -0,0 +1,141 @@ +#include "c3_plus.h" + +#include + +#include + +#include "c3_options.h" +#include "lcs.h" + +namespace c3 { + +using Eigen::MatrixXd; +using Eigen::VectorXd; +using std::vector; + +using drake::solvers::MathematicalProgramResult; + +C3Plus::C3Plus(const LCS& LCS, const CostMatrices& costs, + const vector& xdesired, const C3Options& options) + : C3(LCS, costs, xdesired, options, [](const class LCS& lcs) { + return (lcs.num_states() + 2 * lcs.num_lambdas() + lcs.num_inputs()); + }) { + eta_ = vector(); + eta_sol_ = std::make_unique>(); + for (int i = 0; i < N_; ++i) { + eta_sol_->push_back(Eigen::VectorXd::Zero(n_lambda_)); + eta_.push_back( + prog_.NewContinuousVariables(n_lambda_, "eta" + std::to_string(i))); + z_.at(i).push_back(eta_.back()); + } + MatrixXd EtaLinEq(n_lambda_, n_x_ + 2 * n_lambda_ + n_u_); + EtaLinEq.block(0, n_x_ + n_lambda_ + n_u_, n_lambda_, n_lambda_) = + -1 * MatrixXd::Identity(n_lambda_, n_lambda_); + eta_constraints_.resize(N_); + for (int i = 0; i < N_; ++i) { + EtaLinEq.block(0, 0, n_lambda_, n_x_) = lcs_.E().at(i); + EtaLinEq.block(0, n_x_, n_lambda_, n_lambda_) = lcs_.F().at(i); + 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)}) + .evaluator() + .get(); + } + + // Disable parallelization for C3+ because of the overhead cost + use_parallelization_in_projection_ = false; +} + +void C3Plus::UpdateLCS(const LCS& lcs) { + C3::UpdateLCS(lcs); + MatrixXd EtaLinEq(n_lambda_, n_x_ + 2 * n_lambda_ + n_u_); + EtaLinEq.block(0, n_x_ + n_lambda_ + n_u_, n_lambda_, n_lambda_) = + -1 * MatrixXd::Identity(n_lambda_, n_lambda_); + for (int i = 0; i < N_; ++i) { + EtaLinEq.block(0, 0, n_lambda_, n_x_) = lcs_.E().at(i); + EtaLinEq.block(0, n_x_, n_lambda_, n_lambda_) = lcs_.F().at(i); + EtaLinEq.block(0, n_x_ + n_lambda_, n_lambda_, n_u_) = lcs_.H().at(i); + eta_constraints_[i]->UpdateCoefficients(EtaLinEq, -lcs_.c().at(i)); + } +} + +void C3Plus::WarmStartQP(const Eigen::VectorXd& x0, int admm_iteration) { + if (admm_iteration == 0) return; // No warm start for the first iteration + int index = solve_time_ / lcs_.dt(); + double weight = (solve_time_ - index * lcs_.dt()) / lcs_.dt(); + for (int i = 0; i < N_ - 1; ++i) { + prog_.SetInitialGuess( + x_[i], (1 - weight) * warm_start_x_[admm_iteration - 1][i] + + weight * warm_start_x_[admm_iteration - 1][i + 1]); + prog_.SetInitialGuess( + lambda_[i], (1 - weight) * warm_start_lambda_[admm_iteration - 1][i] + + weight * warm_start_lambda_[admm_iteration - 1][i + 1]); + prog_.SetInitialGuess( + u_[i], (1 - weight) * warm_start_u_[admm_iteration - 1][i] + + weight * warm_start_u_[admm_iteration - 1][i + 1]); + prog_.SetInitialGuess( + eta_[i], (1 - weight) * warm_start_eta_[admm_iteration - 1][i] + + weight * warm_start_eta_[admm_iteration - 1][i + 1]); + } + prog_.SetInitialGuess(x_[N_], warm_start_x_[admm_iteration - 1][N_]); +} + +void C3Plus::ProcessQPResults(const MathematicalProgramResult& result, + int admm_iteration, bool is_final_solve) { + C3::ProcessQPResults(result, admm_iteration, is_final_solve); + 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]); + } +} + +VectorXd C3Plus::SolveSingleProjection(const MatrixXd& U, + const VectorXd& delta_c, + const MatrixXd& E, const MatrixXd& F, + const MatrixXd& H, const VectorXd& c, + const int admm_iteration, + const int& warm_start_index) { + VectorXd delta_proj = delta_c; + + // Handle complementarity constraints for each lambda-eta pair + for (int i = 0; i < n_lambda_; ++i) { + double w_eta = + std::abs(U(n_x_ + n_lambda_ + n_u_ + i, n_x_ + n_lambda_ + n_u_ + i)); + double w_lambda = std::abs(U(n_x_ + i, n_x_ + i)); + + double lambda_val = delta_c(n_x_ + i); + double eta_val = delta_c(n_x_ + n_lambda_ + n_u_ + i); + + if (lambda_val <= 0) { + delta_proj(n_x_ + i) = 0; + delta_proj(n_x_ + n_lambda_ + n_u_ + i) = std::max(0.0, eta_val); + } else { + if (eta_val <= 0) { + delta_proj(n_x_ + i) = lambda_val; + delta_proj(n_x_ + n_lambda_ + n_u_ + i) = 0; + } else { + // If point (lambda, eta) is above the slope sqrt(w_lambda/w_eta), set + // lambda to 0 and keep eta Otherwise, set lambda to lambda and set eta + // to 0 + if (eta_val * std::sqrt(w_eta) > lambda_val * std::sqrt(w_lambda)) { + delta_proj(n_x_ + i) = 0; + delta_proj(n_x_ + n_lambda_ + n_u_ + i) = eta_val; + } else { + delta_proj(n_x_ + i) = lambda_val; + delta_proj(n_x_ + n_lambda_ + n_u_ + i) = 0; + } + } + } + } + + return delta_proj; +} + +} // namespace c3 diff --git a/core/c3_plus.h b/core/c3_plus.h new file mode 100644 index 0000000..76f2a53 --- /dev/null +++ b/core/c3_plus.h @@ -0,0 +1,74 @@ +#pragma once + +#include + +#include + +#include "c3.h" +#include "c3_options.h" +#include "lcs.h" + +namespace c3 { + +// C3+ is a variant of C3 that introduces a new slack variable, η, and a new +// equality constraint, η = E * x + F * λ + H * u + c, to the QP step. +// This allows us to solve the projection step in a more efficient way (without +// solving any optimization problem). +// The z and delta vector now have the following structure [x, λ, u, η]. +// +// In C3+ QP step, we solve the following problem: +// +// min_{x, u, λ, η} g_x ||x - x_des||² + g_u ||u||² + +// g_λ ||λ - λ₀||² + g_η ||η - η₀||² +// s.t. x_next = A * x + B * u + D * λ + d +// η = E * x + F * λ + H * u + c +// u_min ≤ u ≤ u_max +// x_min ≤ x ≤ x_max +// +// In C3+ projection step, we aim to solve the following problem +// +// min_{λ, η} w_λ ||λ - λ₀||² + w_η ||η - η₀||² +// s.t. 0 ≤ λ ⊥ η ≥ 0 +// +// where λ₀ and η₀ are the values of λ and η obtained from the QP step, +// respectively. The solution to this problem is the projection of (λ₀, η₀) +// onto the feasible set defined by the complementarity condition (i.e., λᵢ ηᵢ +// = 0 for all i, with λ ≥ 0 and η ≥ 0). +// +// To get the solution, we can simply perform if-else to handle the following +// cases: +// +// 1. λ₀ <= 0 and η₀ > 0, then λ = 0 and η = η₀ +// 2. λ₀ <= 0 and η₀ <= 0 then λ = 0 and η = 0 +// 3. λ₀ > 0 and η₀ <= 0, then λ = λ₀ and η = 0 +// 4. λ₀ > 0, η₀ > 0, and η₀ > sqrt(w_λ/w_η) * λ₀, then λ = 0 and η = η₀ +// 5. λ₀ > 0, η₀ > 0, and η₀ <= sqrt(w_λ/w_η) * λ₀, then λ = λ₀ and η = 0 +class C3Plus final : public C3 { + public: + C3Plus(const LCS& LCS, const CostMatrices& costs, + const std::vector& xdesired, + const C3Options& options); + ~C3Plus() override = default; + + Eigen::VectorXd SolveSingleProjection( + const Eigen::MatrixXd& U, const Eigen::VectorXd& delta_c, + const Eigen::MatrixXd& E, const Eigen::MatrixXd& F, + const Eigen::MatrixXd& H, const Eigen::VectorXd& c, + const int admm_iteration, const int& warm_start_index = -1) override; + + protected: + std::vector> warm_start_eta_; + + private: + void ProcessQPResults(const drake::solvers::MathematicalProgramResult& result, + int admm_iteration, bool is_final_solve) override; + void UpdateLCS(const LCS& lcs) override; + void WarmStartQP(const Eigen::VectorXd& x0, int admm_iteration) override; + std::vector eta_; + std::unique_ptr> eta_sol_; + + // Store the following constraint η = E * x + F * λ + H * u + c + std::vector eta_constraints_; +}; + +} // namespace c3 \ No newline at end of file diff --git a/core/test/c3_cartpole_problem.hpp b/core/test/c3_cartpole_problem.hpp index d325605..d1a182e 100644 --- a/core/test/c3_cartpole_problem.hpp +++ b/core/test/c3_cartpole_problem.hpp @@ -129,6 +129,22 @@ class C3CartpoleProblem { xdesired.resize(N + 1, xdesiredinit); } + void UseC3Plus() { + MatrixXd Ginit(n + 2 * m + k, n + 2 * m + k); + Ginit = MatrixXd::Zero(n + 2 * m + k, n + 2 * m + k); + Ginit.block(n + m + k, n + m + k, m, m) = MatrixXd::Identity(m, m); + Ginit.block(n, n, m, m) = MatrixXd::Identity(m, m); + + MatrixXd Uinit(n + 2 * m + k, n + 2 * m + k); + Uinit = MatrixXd::Zero(n + 2 * m + k, n + 2 * m + k); + Uinit.block(n, n, m, m) = MatrixXd::Identity(m, m); + Uinit.block(n + m + k, n + m + k, m, m) = 10000 * MatrixXd::Identity(m, m); + + vector G(N, Ginit); + vector U(N, Uinit); + cost = C3::CostMatrices(cost.Q, cost.R, G, U); + } + // Cartpole problem parameters float mp, mc, len_p, len_com, d1, d2, ks, g; diff --git a/core/test/core_test.cc b/core/test/core_test.cc index 3c90db7..5e1fc76 100644 --- a/core/test/core_test.cc +++ b/core/test/core_test.cc @@ -5,6 +5,7 @@ #include #include "core/c3_miqp.h" +#include "core/c3_plus.h" #include "core/c3_qp.h" #include "core/lcs.h" #include "core/test/c3_cartpole_problem.hpp" @@ -40,6 +41,7 @@ using namespace c3; * | Solve | - | * | SetOsqpSolverOptions | - | * | CreatePlaceholderLCS | DONE | + * | WarmStartSmokeTest | DONE | * | # of regression tests | 2 | * * It also has an E2E test for ensuring the "Solve()" function and other @@ -342,17 +344,28 @@ TEST_F(C3CartpoleTest, CreatePlaceholder) { ASSERT_TRUE(placeholder.HasSameDimensionsAs(*pSystem)); } +// Test if the solver works with warm start enabled (smoke test) +TEST_F(C3CartpoleTest, WarmStartSmokeTest) { + // Enable warm start option + options.warm_start = true; + C3MIQP optimizer(*pSystem, cost, xdesired, options); + + // Solver should not throw when called with warm start + ASSERT_NO_THROW(optimizer.Solve(x0)); +} + template class C3CartpoleTypedTest : public testing::Test, public C3CartpoleProblem { protected: C3CartpoleTypedTest() : C3CartpoleProblem(0.411, 0.978, 0.6, 0.4267, 0.35, -0.35, 100, 9.81) { + if (std::is_same::value) UseC3Plus(); pOpt = std::make_unique(*pSystem, cost, xdesired, options); } std::unique_ptr pOpt; }; -using projection_types = ::testing::Types; +using projection_types = ::testing::Types; TYPED_TEST_SUITE(C3CartpoleTypedTest, projection_types); // Test the cartpole example diff --git a/examples/c3_controller_example.cc b/examples/c3_controller_example.cc index 973672f..8fac634 100644 --- a/examples/c3_controller_example.cc +++ b/examples/c3_controller_example.cc @@ -76,6 +76,7 @@ int DoMain() { // Initialize the C3 cartpole problem. auto c3_cartpole_problem = C3CartpoleProblem(); + c3_cartpole_problem.UseC3Plus(); // Use C3+ controller if desired. // Add the LCS simulator. auto lcs_simulator = @@ -101,6 +102,7 @@ int DoMain() { state_zero_order_hold->get_output_port()); // Add the C3 controller. + options.projection_type = "C3+"; // Set projection type to C3+. auto c3_controller = builder.AddSystem( *plant, c3_cartpole_problem.cost, options); diff --git a/examples/lcs_factory_system_example.cc b/examples/lcs_factory_system_example.cc index da4fcda..9bd86b5 100644 --- a/examples/lcs_factory_system_example.cc +++ b/examples/lcs_factory_system_example.cc @@ -114,6 +114,7 @@ int RunCartpoleTest() { // Initialize the C3 cartpole problem. Assuming SDF matches default values in // problem. auto c3_cartpole_problem = C3CartpoleProblem(); + c3_cartpole_problem.UseC3Plus(); // Use C3+ controller. DiagramBuilder plant_builder; auto [plant_for_lcs, scene_graph_for_lcs] = @@ -162,6 +163,7 @@ int RunCartpoleTest() { C3ControllerOptions options = drake::yaml::LoadYamlFile( "examples/resources/cartpole_softwalls/" "c3_controller_cartpole_options.yaml"); + options.projection_type = "C3+"; // Use C3+ controller. std::unique_ptr> plant_diagram_context = plant_diagram->CreateDefaultContext(); diff --git a/systems/c3_controller.cc b/systems/c3_controller.cc index 85d4315..1ec42d4 100644 --- a/systems/c3_controller.cc +++ b/systems/c3_controller.cc @@ -5,6 +5,7 @@ #include #include "core/c3_miqp.h" +#include "core/c3_plus.h" #include "core/c3_qp.h" #include "multibody/lcs_factory.h" @@ -82,6 +83,10 @@ C3Controller::C3Controller( } else if (controller_options_.projection_type == "QP") { c3_ = std::make_unique(lcs_placeholder, costs, x_desired_placeholder, controller_options_.c3_options); + } else if (controller_options_.projection_type == "C3+") { + c3_ = + std::make_unique(lcs_placeholder, costs, x_desired_placeholder, + controller_options_.c3_options); } else { drake::log()->error("Unknown projection type : {}", controller_options_.projection_type); diff --git a/systems/c3_controller_options.h b/systems/c3_controller_options.h index 52bdea3..4bb38b1 100644 --- a/systems/c3_controller_options.h +++ b/systems/c3_controller_options.h @@ -49,7 +49,7 @@ struct C3StatePredictionJoint { }; struct C3ControllerOptions { - std::string projection_type; // "QP" or "MIQP" + std::string projection_type; // "QP" or "MIQP" or "C3+" // C3 optimization options C3Options c3_options; @@ -81,6 +81,12 @@ struct C3ControllerOptions { expected_lambda_size); DRAKE_DEMAND(static_cast(c3_options.u_lambda.size()) == expected_lambda_size); + if (projection_type == "C3+") { + DRAKE_DEMAND(static_cast(c3_options.g_eta->size()) == + expected_lambda_size); + DRAKE_DEMAND(static_cast(c3_options.u_eta->size()) == + expected_lambda_size); + } } }; From c24bf7ce18095b5502ed4454bbc89338a4a771fa Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 5 Aug 2025 21:08:07 +0000 Subject: [PATCH 02/16] fix: add fixes from making push T and jacktoy example work --- core/c3.cc | 15 ++++++-- core/c3.h | 27 ++++++++++++-- core/c3_miqp.cc | 2 +- core/c3_options.h | 9 ++++- core/c3_plus.cc | 70 ++++++++++++++++++----------------- core/c3_qp.cc | 4 +- multibody/lcs_factory.cc | 11 +++--- multibody/lcs_factory.h | 13 ++++--- systems/framework/c3_output.h | 6 +++ 9 files changed, 101 insertions(+), 56 deletions(-) diff --git a/core/c3.cc b/core/c3.cc index 5936658..d36577f 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -43,13 +43,13 @@ C3::CostMatrices::CostMatrices(const std::vector& Q, C3::C3(const LCS& lcs, const CostMatrices& costs, const vector& x_desired, const C3Options& options, - CalcZSizeFunc calc_z_size) + const int z_size) : warm_start_(options.warm_start), N_(lcs.N()), n_x_(lcs.num_states()), n_lambda_(lcs.num_lambdas()), n_u_(lcs.num_inputs()), - n_z_(calc_z_size ? calc_z_size(lcs) : n_x_ + n_lambda_ + n_u_), + n_z_(z_size), lcs_(lcs), cost_matrices_(costs), x_desired_(x_desired), @@ -82,6 +82,7 @@ C3::C3(const LCS& lcs, const CostMatrices& costs, u_ = vector(); lambda_ = vector(); + z_fin_ = std::make_unique>(); z_sol_ = std::make_unique>(); x_sol_ = std::make_unique>(); lambda_sol_ = std::make_unique>(); @@ -93,6 +94,8 @@ C3::C3(const LCS& lcs, const CostMatrices& costs, 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_)); + z_fin_->push_back(Eigen::VectorXd::Zero(n_z_)); + z_sol_->push_back(Eigen::VectorXd::Zero(n_z_)); w_sol_->push_back(Eigen::VectorXd::Zero(n_z_)); delta_sol_->push_back(Eigen::VectorXd::Zero(n_z_)); } @@ -176,6 +179,11 @@ void C3::SetDefaultSolverOptions() { SetSolverOptions(solver_options); } +C3::C3(const LCS& lcs, const CostMatrices& costs, + const vector& x_desired, const C3Options& options) + : C3(lcs, costs, x_desired, options, + lcs.num_states() + lcs.num_lambdas() + lcs.num_inputs()) {} + C3::CostMatrices C3::CreateCostMatricesFromC3Options(const C3Options& options, int N) { std::vector Q; // State cost matrices. @@ -320,7 +328,7 @@ void C3::Solve(const VectorXd& x0) { WD.at(i) = delta.at(i) - w.at(i); } - vector zfin = SolveQP(x0, G, WD, options_.admm_iter, true); + *z_fin_ = SolveQP(x0, G, WD, options_.admm_iter, true); *w_sol_ = w; *delta_sol_ = delta; @@ -381,6 +389,7 @@ void C3::ADMMStep(const VectorXd& x0, vector* delta, } void C3::WarmStartQP(const Eigen::VectorXd& x0, int admm_iteration) { + if (admm_iteration == 0) return; // No warm start for the first iteration int index = solve_time_ / lcs_.dt(); double weight = (solve_time_ - index * lcs_.dt()) / lcs_.dt(); diff --git a/core/c3.h b/core/c3.h index 1a11594..6e69f4f 100644 --- a/core/c3.h +++ b/core/c3.h @@ -52,8 +52,6 @@ class C3 { std::vector U; }; - using CalcZSizeFunc = std::function; - /*! * @param LCS system dynamics, defined as an LCS (see lcs.h) * @param costs Cost function parameters (see above) @@ -62,8 +60,7 @@ class C3 { * @param options see c3_options.h */ C3(const LCS& LCS, const CostMatrices& costs, - const std::vector& x_desired, const C3Options& options, - CalcZSizeFunc calc_z_size = nullptr); + const std::vector& x_desired, const C3Options& options); virtual ~C3() = default; @@ -193,13 +190,32 @@ class C3 { } std::vector GetFullSolution() { return *z_sol_; } + std::vector GetFullQPSolution() { return *z_fin_; } std::vector GetStateSolution() { return *x_sol_; } std::vector GetForceSolution() { return *lambda_sol_; } std::vector GetInputSolution() { return *u_sol_; } std::vector GetDualDeltaSolution() { return *delta_sol_; } std::vector GetDualWSolution() { return *w_sol_; } + int GetZSize() const { return n_z_; } + protected: + /// @param lcs Parameters defining the LCS. + /// @param costs Cost matrices used in the optimization. + /// @param x_des Desired goal state trajectory. + /// @param options Options specific to the C3 formulation. + /// @param z_size Size of the z vector, which depends on the specific C3 + /// variant. + /// For example: + /// - C3MIQP / C3QP: z = [x, u, lambda] + /// - C3Plus: z = [x, u, lambda, eta] + /// + /// This constructor is intended for internal use only. The public constructor + /// delegates to this one, passing in an explicitly computed z vector size. + C3(const LCS& lcs, const CostMatrices& costs, + const std::vector& x_des, const C3Options& options, + int z_size); + std::vector> warm_start_delta_; std::vector> warm_start_binary_; std::vector> warm_start_x_; @@ -320,6 +336,9 @@ class C3 { std::unique_ptr> u_sol_; std::unique_ptr> z_sol_; + std::unique_ptr> + z_fin_; // Final QP solution which may differ from z_sol_ if + // end_on_qp_step is false std::unique_ptr> delta_sol_; std::unique_ptr> w_sol_; }; diff --git a/core/c3_miqp.cc b/core/c3_miqp.cc index b97e41d..51f800e 100644 --- a/core/c3_miqp.cc +++ b/core/c3_miqp.cc @@ -10,7 +10,7 @@ using std::vector; C3MIQP::C3MIQP(const LCS& LCS, const CostMatrices& costs, const vector& xdesired, const C3Options& options) - : C3(LCS, costs, xdesired, options), M_(options.M) { + : C3(LCS, costs, xdesired, options), M_(options.M.value_or(1000)) { if (warm_start_) { warm_start_delta_.resize(options_.admm_iter + 1); warm_start_binary_.resize(options_.admm_iter + 1); diff --git a/core/c3_options.h b/core/c3_options.h index 5583bdb..08f3b6a 100644 --- a/core/c3_options.h +++ b/core/c3_options.h @@ -24,7 +24,12 @@ struct C3Options { int delta_option = 1; // 1 initializes the state value of the delta value with x0 - double M = 1000; // big M value for MIQP + std::optional M = 1000; // big M value for MIQP + + std::optional + qp_projection_alpha; // alpha value for the QP projection + std::optional + qp_projection_scaling; // scaling factor for the QP projection int admm_iter = 3; // total number of ADMM iterations @@ -90,6 +95,8 @@ struct C3Options { a->Visit(DRAKE_NVP(delta_option)); a->Visit(DRAKE_NVP(M)); + a->Visit(DRAKE_NVP(qp_projection_alpha)); + a->Visit(DRAKE_NVP(qp_projection_scaling)); a->Visit(DRAKE_NVP(admm_iter)); diff --git a/core/c3_plus.cc b/core/c3_plus.cc index eaef732..95085a3 100644 --- a/core/c3_plus.cc +++ b/core/c3_plus.cc @@ -15,11 +15,11 @@ using std::vector; using drake::solvers::MathematicalProgramResult; -C3Plus::C3Plus(const LCS& LCS, const CostMatrices& costs, +C3Plus::C3Plus(const LCS& lcs, const CostMatrices& costs, const vector& xdesired, const C3Options& options) - : C3(LCS, costs, xdesired, options, [](const class LCS& lcs) { - return (lcs.num_states() + 2 * lcs.num_lambdas() + lcs.num_inputs()); - }) { + : C3(lcs, costs, xdesired, options, + lcs.num_states() + 2 * lcs.num_lambdas() + lcs.num_inputs()) { + // Initialize eta as optimization variables eta_ = vector(); eta_sol_ = std::make_unique>(); for (int i = 0; i < N_; ++i) { @@ -28,6 +28,8 @@ C3Plus::C3Plus(const LCS& LCS, const CostMatrices& costs, prog_.NewContinuousVariables(n_lambda_, "eta" + std::to_string(i))); z_.at(i).push_back(eta_.back()); } + + // Add eta equality constraints η = E * x + F * λ + H * u + c MatrixXd EtaLinEq(n_lambda_, n_x_ + 2 * n_lambda_ + n_u_); EtaLinEq.block(0, n_x_ + n_lambda_ + n_u_, n_lambda_, n_lambda_) = -1 * MatrixXd::Identity(n_lambda_, n_lambda_); @@ -104,38 +106,38 @@ VectorXd C3Plus::SolveSingleProjection(const MatrixXd& U, const int& warm_start_index) { VectorXd delta_proj = delta_c; - // Handle complementarity constraints for each lambda-eta pair - for (int i = 0; i < n_lambda_; ++i) { - double w_eta = - std::abs(U(n_x_ + n_lambda_ + n_u_ + i, n_x_ + n_lambda_ + n_u_ + i)); - double w_lambda = std::abs(U(n_x_ + i, n_x_ + i)); - - double lambda_val = delta_c(n_x_ + i); - double eta_val = delta_c(n_x_ + n_lambda_ + n_u_ + i); - - if (lambda_val <= 0) { - delta_proj(n_x_ + i) = 0; - delta_proj(n_x_ + n_lambda_ + n_u_ + i) = std::max(0.0, eta_val); - } else { - if (eta_val <= 0) { - delta_proj(n_x_ + i) = lambda_val; - delta_proj(n_x_ + n_lambda_ + n_u_ + i) = 0; - } else { - // If point (lambda, eta) is above the slope sqrt(w_lambda/w_eta), set - // lambda to 0 and keep eta Otherwise, set lambda to lambda and set eta - // to 0 - if (eta_val * std::sqrt(w_eta) > lambda_val * std::sqrt(w_lambda)) { - delta_proj(n_x_ + i) = 0; - delta_proj(n_x_ + n_lambda_ + n_u_ + i) = eta_val; - } else { - delta_proj(n_x_ + i) = lambda_val; - delta_proj(n_x_ + n_lambda_ + n_u_ + i) = 0; - } - } - } + // Extract the weight vectors for lambda and eta from the diagonal of the cost + // matrix U. + VectorXd w_eta_vec = U.block(n_x_ + n_lambda_ + n_u_, n_x_ + n_lambda_ + n_u_, + n_lambda_, n_lambda_) + .diagonal(); + VectorXd w_lambda_vec = U.block(n_x_, n_x_, n_lambda_, n_lambda_).diagonal(); + + // Throw an error if any weights are negative. + if (w_eta_vec.minCoeff() < 0 || w_lambda_vec.minCoeff() < 0) { + throw std::runtime_error( + "Negative weights in the cost matrix U are not allowed."); } + VectorXd lambda_c = delta_c.segment(n_x_, n_lambda_); + VectorXd eta_c = delta_c.segment(n_x_ + n_lambda_ + n_u_, n_lambda_); + + // Set the smaller of lambda and eta to zero + Eigen::Array eta_larger = + eta_c.array() * w_eta_vec.array().sqrt() > + lambda_c.array() * w_lambda_vec.array().sqrt(); + + delta_proj.segment(n_x_, n_lambda_) = + eta_larger.select(VectorXd::Zero(n_lambda_), lambda_c); + delta_proj.segment(n_x_ + n_lambda_ + n_u_, n_lambda_) = + eta_larger.select(eta_c, VectorXd::Zero(n_lambda_)); + + // Clip lambda and eta at 0 + delta_proj.segment(n_x_, n_lambda_) = + delta_proj.segment(n_x_, n_lambda_).cwiseMax(0); + delta_proj.segment(n_x_ + n_lambda_ + n_u_, n_lambda_) = + delta_proj.segment(n_x_ + n_lambda_ + n_u_, n_lambda_).cwiseMax(0); + return delta_proj; } - } // namespace c3 diff --git a/core/c3_qp.cc b/core/c3_qp.cc index df4b3af..1bd18e7 100644 --- a/core/c3_qp.cc +++ b/core/c3_qp.cc @@ -46,8 +46,8 @@ VectorXd C3QP::SolveSingleProjection(const MatrixXd& U, const VectorXd& delta_c, auto ln_ = prog.NewContinuousVariables(n_lambda_, "lambda"); auto un_ = prog.NewContinuousVariables(n_u_, "u"); - double alpha = 0.01; - double scaling = 1000; + double alpha = options_.qp_projection_alpha.value_or(0.01); + double scaling = options_.qp_projection_scaling.value_or(1000); MatrixXd EFH(n_lambda_, n_x_ + n_lambda_ + n_u_); EFH.block(0, 0, n_lambda_, n_x_) = E / scaling; diff --git a/multibody/lcs_factory.cc b/multibody/lcs_factory.cc index 07002cb..9fd65f2 100644 --- a/multibody/lcs_factory.cc +++ b/multibody/lcs_factory.cc @@ -41,17 +41,18 @@ LCSFactory::LCSFactory( context_ad_(context_ad), contact_pairs_(contact_geoms), options_(options), + n_contacts_(contact_geoms.size()), + n_friction_directions_(options_.num_friction_directions), + contact_model_(GetContactModelMap().at(options_.contact_model)), n_q_(plant_.num_positions()), n_v_(plant_.num_velocities()), n_x_(n_q_ + n_v_), - n_lambda_(multibody::LCSFactory::GetNumContactVariables(options)), + n_lambda_(multibody::LCSFactory::GetNumContactVariables( + contact_model_, n_contacts_, n_friction_directions_)), n_u_(plant_.num_actuators()), - n_contacts_(contact_geoms.size()), - n_friction_directions_(options_.num_friction_directions), - contact_model_(GetContactModelMap().at(options.contact_model)), mu_(options.mu), frictionless_(contact_model_ == ContactModel::kFrictionlessSpring), - dt_(options_.dt) {} + dt_(options.dt) {} void LCSFactory::ComputeContactJacobian(VectorXd& phi, MatrixXd& Jn, MatrixXd& Jt) { diff --git a/multibody/lcs_factory.h b/multibody/lcs_factory.h index 0a02b7e..e5f7940 100644 --- a/multibody/lcs_factory.h +++ b/multibody/lcs_factory.h @@ -291,17 +291,18 @@ class LCSFactory { // Configuration options for the LCSFactory LCSFactoryOptions options_; + ContactModel contact_model_; ///< The contact model being used. + int n_contacts_; ///< Number of contact points. + int n_friction_directions_; ///< Number of friction directions.u int n_q_; ///< Number of configuration variables. int n_v_; ///< Number of velocity variables. int n_x_; ///< Number of state variables. int n_lambda_; ///< Number of contact force variables. int n_u_; ///< Number of input variables. - int n_contacts_; ///< Number of contact points. - int n_friction_directions_; ///< Number of friction directions. - ContactModel contact_model_; ///< The contact model being used. - std::vector mu_; ///< Vector of friction coefficients. - bool frictionless_; ///< Flag indicating frictionless contacts. - double dt_; ///< Time step. + + std::vector mu_; ///< Vector of friction coefficients. + bool frictionless_; ///< Flag indicating frictionless contacts. + double dt_; ///< Time step. }; } // namespace multibody diff --git a/systems/framework/c3_output.h b/systems/framework/c3_output.h index 319fa13..1be7b00 100644 --- a/systems/framework/c3_output.h +++ b/systems/framework/c3_output.h @@ -42,6 +42,12 @@ class C3Output { w_ = MatrixXf::Zero(n_x + n_lambda + n_u, N); time_vector_ = VectorXf::Zero(N); }; + C3Intermediates(int n_z, int N) { + z_ = MatrixXf::Zero(n_z, N); + delta_ = MatrixXf::Zero(n_z, N); + w_ = MatrixXf::Zero(n_z, N); + time_vector_ = VectorXf::Zero(N); + }; // Shape is (variable_vector_size, knot_points) Eigen::VectorXf time_vector_; From b56eb515c0248beab1f2069a660509eff57ed0e7 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 12 Aug 2025 11:34:20 -0400 Subject: [PATCH 03/16] fix: update c3 plus --- core/c3.cc | 12 ++++++------ core/c3_plus.cc | 32 +++++++++++++++++++------------- core/c3_plus.h | 11 +++-------- 3 files changed, 28 insertions(+), 27 deletions(-) diff --git a/core/c3.cc b/core/c3.cc index d36577f..e621208 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -148,12 +148,12 @@ C3::C3(const LCS& lcs, const CostMatrices& costs, x_.at(i), 1) .evaluator() .get(); - if (i < N_) { - input_costs_[i] = prog_ - .AddQuadraticCost(2 * cost_matrices_.R.at(i), - VectorXd::Zero(n_u_), u_.at(i), 1) - .evaluator(); - } + // Skip input cost at the (N + 1)th time step + if (i == N_) break; + input_costs_[i] = prog_ + .AddQuadraticCost(2 * cost_matrices_.R.at(i), + VectorXd::Zero(n_u_), u_.at(i), 1) + .evaluator(); } // Set default solver options diff --git a/core/c3_plus.cc b/core/c3_plus.cc index 95085a3..daa5b88 100644 --- a/core/c3_plus.cc +++ b/core/c3_plus.cc @@ -66,24 +66,24 @@ void C3Plus::UpdateLCS(const LCS& lcs) { } void C3Plus::WarmStartQP(const Eigen::VectorXd& x0, int admm_iteration) { - if (admm_iteration == 0) return; // No warm start for the first iteration + // if (admm_iteration == 0) return; // No warm start for the first iteration int index = solve_time_ / lcs_.dt(); double weight = (solve_time_ - index * lcs_.dt()) / lcs_.dt(); for (int i = 0; i < N_ - 1; ++i) { + prog_.SetInitialGuess(x_[i], + (1 - weight) * warm_start_x_[admm_iteration][i] + + weight * warm_start_x_[admm_iteration][i + 1]); prog_.SetInitialGuess( - x_[i], (1 - weight) * warm_start_x_[admm_iteration - 1][i] + - weight * warm_start_x_[admm_iteration - 1][i + 1]); - prog_.SetInitialGuess( - lambda_[i], (1 - weight) * warm_start_lambda_[admm_iteration - 1][i] + - weight * warm_start_lambda_[admm_iteration - 1][i + 1]); - prog_.SetInitialGuess( - u_[i], (1 - weight) * warm_start_u_[admm_iteration - 1][i] + - weight * warm_start_u_[admm_iteration - 1][i + 1]); - prog_.SetInitialGuess( - eta_[i], (1 - weight) * warm_start_eta_[admm_iteration - 1][i] + - weight * warm_start_eta_[admm_iteration - 1][i + 1]); + lambda_[i], (1 - weight) * warm_start_lambda_[admm_iteration][i] + + weight * warm_start_lambda_[admm_iteration][i + 1]); + prog_.SetInitialGuess(u_[i], + (1 - weight) * warm_start_u_[admm_iteration][i] + + weight * warm_start_u_[admm_iteration][i + 1]); + prog_.SetInitialGuess(eta_[i], + (1 - weight) * warm_start_eta_[admm_iteration][i] + + weight * warm_start_eta_[admm_iteration][i + 1]); } - prog_.SetInitialGuess(x_[N_], warm_start_x_[admm_iteration - 1][N_]); + prog_.SetInitialGuess(x_[N_], warm_start_x_[admm_iteration][N_]); } void C3Plus::ProcessQPResults(const MathematicalProgramResult& result, @@ -96,6 +96,12 @@ void C3Plus::ProcessQPResults(const MathematicalProgramResult& result, z_sol_->at(i).segment(n_x_ + n_lambda_ + n_u_, n_lambda_) = result.GetSolution(eta_[i]); } + + if (!warm_start_) + return; // No warm start, so no need to update warm start parameters + for (int i = 0; i < N_; ++i) { + warm_start_eta_[admm_iteration][i] = result.GetSolution(eta_[i]); + } } VectorXd C3Plus::SolveSingleProjection(const MatrixXd& U, diff --git a/core/c3_plus.h b/core/c3_plus.h index 76f2a53..037c913 100644 --- a/core/c3_plus.h +++ b/core/c3_plus.h @@ -35,14 +35,9 @@ namespace c3 { // onto the feasible set defined by the complementarity condition (i.e., λᵢ ηᵢ // = 0 for all i, with λ ≥ 0 and η ≥ 0). // -// To get the solution, we can simply perform if-else to handle the following -// cases: -// -// 1. λ₀ <= 0 and η₀ > 0, then λ = 0 and η = η₀ -// 2. λ₀ <= 0 and η₀ <= 0 then λ = 0 and η = 0 -// 3. λ₀ > 0 and η₀ <= 0, then λ = λ₀ and η = 0 -// 4. λ₀ > 0, η₀ > 0, and η₀ > sqrt(w_λ/w_η) * λ₀, then λ = 0 and η = η₀ -// 5. λ₀ > 0, η₀ > 0, and η₀ <= sqrt(w_λ/w_η) * λ₀, then λ = λ₀ and η = 0 +// To get the solution, we can simply do the following steps: +// 1. If η₀ > sqrt(w_λ/w_η) * λ₀, then λ = 0, else η = 0 +// 2. [λ, η] = max(0, [λ, η]) class C3Plus final : public C3 { public: C3Plus(const LCS& LCS, const CostMatrices& costs, From 204b0af471d586b694ccc77a2a09aa4da528012d Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 12 Aug 2025 11:54:45 -0400 Subject: [PATCH 04/16] feat: add sphere mesh distance calculation to geom geom collider --- multibody/geom_geom_collider.cc | 138 ++++++++++++++++++++++++++------ multibody/geom_geom_collider.h | 64 ++++++++++++++- 2 files changed, 175 insertions(+), 27 deletions(-) diff --git a/multibody/geom_geom_collider.cc b/multibody/geom_geom_collider.cc index aa9e605..df42ae7 100644 --- a/multibody/geom_geom_collider.cc +++ b/multibody/geom_geom_collider.cc @@ -8,7 +8,11 @@ using drake::EigenPtr; using drake::MatrixX; using drake::VectorX; using drake::geometry::GeometryId; +using drake::geometry::GeometrySet; +using drake::geometry::QueryObject; +using drake::geometry::SceneGraphInspector; using drake::geometry::SignedDistancePair; +using drake::geometry::SignedDistanceToPoint; using drake::multibody::JacobianWrtVariable; using drake::multibody::MultibodyPlant; using drake::systems::Context; @@ -26,43 +30,126 @@ GeomGeomCollider::GeomGeomCollider( geometry_id_A_(geometry_pair.first()), geometry_id_B_(geometry_pair.second()) {} +// Determines if the geometry pair consists of a sphere and a mesh. template -typename GeomGeomCollider::GeometryQueryResult -GeomGeomCollider::GetGeometryQueryResult(const Context& context) const { +bool GeomGeomCollider::IsSphereAndMesh( + const SceneGraphInspector& inspector) const { + const auto type_A = inspector.GetShape(geometry_id_A_).type_name(); + const auto type_B = inspector.GetShape(geometry_id_B_).type_name(); + return ((type_A == "Sphere" && type_B == "Mesh") || + (type_A == "Mesh" && type_B == "Sphere")); +} + +// Computes collision information between a sphere and a mesh. +template +void GeomGeomCollider::ComputeSphereMeshDistance(const Context& context, + Vector3d& p_ACa, + Vector3d& p_BCb, + T& distance, + Vector3d& nhat_BA_W) const { // Access the geometry query object from the plant's geometry query port. const auto& query_port = plant_.get_geometry_query_input_port(); - const auto& query_object = - query_port.template Eval>(context); + const auto& query_object = query_port.template Eval>(context); + + // Access the geometry inspector from the query object. + const auto& inspector = query_object.inspector(); + + // Identify which geometry is the mesh and which is the sphere. + bool geometry_A_is_mesh = + (inspector.GetShape(geometry_id_A_).type_name() == "Mesh"); + GeometryId mesh_id = geometry_A_is_mesh ? geometry_id_A_ : geometry_id_B_; + GeometryId sphere_id = geometry_A_is_mesh ? geometry_id_B_ : geometry_id_A_; + + // Get sphere properties. + const auto* sphere = dynamic_cast( + &inspector.GetShape(sphere_id)); + T sphere_radius = sphere->radius(); + + // Compute sphere center in world frame. + auto X_FS = inspector.GetPoseInFrame(sphere_id).template cast(); + auto frame_S_id = inspector.GetFrameId(sphere_id); + auto X_WS = plant_.EvalBodyPoseInWorld( + context, *plant_.GetBodyFromFrameId(frame_S_id)); + auto X_WS_sphere = X_WS * X_FS; + Vector3d sphere_center = X_WS_sphere.translation(); + + // Compute signed distance from sphere center to mesh. + GeometrySet mesh_set; + mesh_set.Add(mesh_id); + const auto sd_set = query_object.ComputeSignedDistanceGeometryToPoint( + sphere_center, mesh_set); + DRAKE_ASSERT(sd_set.size() > 0); + SignedDistanceToPoint sd_to_point = sd_set[0]; + + // Compute contact distance and normal. + distance = sd_to_point.distance - sphere_radius; + nhat_BA_W = sd_to_point.grad_W.normalized(); + + // Compute contact points in local frames. + if (geometry_A_is_mesh) { + nhat_BA_W = -nhat_BA_W; + p_ACa = inspector.GetPoseInFrame(geometry_id_A_).template cast() * + sd_to_point.p_GN; + p_BCb = X_FS.template cast() * (-sphere_radius * nhat_BA_W); + } else { + p_BCb = inspector.GetPoseInFrame(geometry_id_B_).template cast() * + sd_to_point.p_GN; + p_ACa = X_FS.template cast() * (-sphere_radius * nhat_BA_W); + } +} + +// Computes collision information for general geometry pairs (non-sphere-mesh). +template +void GeomGeomCollider::ComputeGeneralGeometryDistance( + const Context& context, Vector3d& p_ACa, Vector3d& p_BCb, T& distance, + Vector3d& nhat_BA_W) const { + // Access the geometry query object from the plant's geometry query port. + const auto& query_port = plant_.get_geometry_query_input_port(); + const auto& query_object = query_port.template Eval>(context); + + // Access the geometry inspector from the query object. + const auto& inspector = query_object.inspector(); - // Compute the signed distance pair between the two geometries. const SignedDistancePair signed_distance_pair = query_object.ComputeSignedDistancePairClosestPoints(geometry_id_A_, geometry_id_B_); + distance = signed_distance_pair.distance; + nhat_BA_W = signed_distance_pair.nhat_BA_W; + p_ACa = inspector.GetPoseInFrame(geometry_id_A_).template cast() * + signed_distance_pair.p_ACa; + p_BCb = inspector.GetPoseInFrame(geometry_id_B_).template cast() * + signed_distance_pair.p_BCb; +} + +// Computes and returns all relevant geometry query results for the collider +// pair. +template +typename GeomGeomCollider::GeometryQueryResult +GeomGeomCollider::GetGeometryQueryResult(const Context& context) const { + // Access the geometry query object from the plant's geometry query port. + const auto& query_port = plant_.get_geometry_query_input_port(); + const auto& query_object = query_port.template Eval>(context); // Access the geometry inspector from the query object. const auto& inspector = query_object.inspector(); const auto frame_A_id = inspector.GetFrameId(geometry_id_A_); const auto frame_B_id = inspector.GetFrameId(geometry_id_B_); - // Get the frames associated with the geometry ids + // Get the frames associated with the geometry ids. const auto& frameA = plant_.GetBodyFromFrameId(frame_A_id)->body_frame(); const auto& frameB = plant_.GetBodyFromFrameId(frame_B_id)->body_frame(); - // Get the poses of the contact points in their respective frames. - const Vector3d& p_ACa = - inspector.GetPoseInFrame(geometry_id_A_).template cast() * - signed_distance_pair.p_ACa; - const Vector3d& p_BCb = - inspector.GetPoseInFrame(geometry_id_B_).template cast() * - signed_distance_pair.p_BCb; - - return GeometryQueryResult{signed_distance_pair, - frame_A_id, - frame_B_id, - frameA, - frameB, - p_ACa, - p_BCb}; + // Compute distance and contact points. + T distance; + Vector3d nhat_BA_W, p_ACa, p_BCb; + if (IsSphereAndMesh(inspector)) { + ComputeSphereMeshDistance(context, p_ACa, p_BCb, distance, nhat_BA_W); + } else { + ComputeGeneralGeometryDistance(context, p_ACa, p_BCb, distance, nhat_BA_W); + } + + return GeometryQueryResult{distance, nhat_BA_W, frame_A_id, frame_B_id, + frameA, frameB, p_ACa, p_BCb}; } template @@ -88,8 +175,7 @@ std::pair> GeomGeomCollider::DoEval( // Compute final Jacobian: J = force_basis * R_WC^T * (Jv_WCa - Jv_WCb) auto J = force_basis * R_WC.matrix().transpose() * (Jv_WCa - Jv_WCb); - return std::pair>(query_result.signed_distance_pair.distance, - J); + return std::pair>(query_result.distance, J); } template @@ -111,7 +197,7 @@ std::pair> GeomGeomCollider::EvalPolytope( // Create rotation matrix from contact normal auto R_WC = drake::math::RotationMatrix::MakeFromOneVector( - query_result.signed_distance_pair.nhat_BA_W, 0); + query_result.nhat_BA_W, 0); return DoEval(context, query_result, polytope_force_bases, wrt, R_WC); } @@ -141,8 +227,8 @@ std::pair> GeomGeomCollider::EvalPlanar( const auto query_result = GetGeometryQueryResult(context); // Compute the planar force basis using the contact normal and planar normal - auto planar_force_basis = ComputePlanarForceBasis( - query_result.signed_distance_pair.nhat_BA_W, planar_normal); + auto planar_force_basis = + ComputePlanarForceBasis(query_result.nhat_BA_W, planar_normal); // For planar case, use identity rotation since force basis is already in // world frame diff --git a/multibody/geom_geom_collider.h b/multibody/geom_geom_collider.h index 14ea121..5e6b142 100644 --- a/multibody/geom_geom_collider.h +++ b/multibody/geom_geom_collider.h @@ -113,7 +113,8 @@ class GeomGeomCollider { /** * @brief The signed distance pair between the two geometries. */ - drake::geometry::SignedDistancePair signed_distance_pair; + T distance; + Eigen::Vector3 nhat_BA_W; /** * @brief The FrameId of the first frame. */ @@ -217,6 +218,67 @@ class GeomGeomCollider { GeometryQueryResult GetGeometryQueryResult( const drake::systems::Context& context) const; + /** + * @brief Determines if the geometry pair consists of a sphere and a mesh. + * + * This method inspects the two geometries in the collider pair to identify + * whether one is a sphere and the other is a mesh (in either order). This + * classification is used to select the appropriate distance computation + * algorithm. + * + * @param inspector The SceneGraphInspector providing access to geometry + * shape information. + * @return true if one geometry is a sphere and the other is a mesh, + * false otherwise. + */ + bool IsSphereAndMesh( + const drake::geometry::SceneGraphInspector& inspector) const; + + /** + * @brief Computes collision information for sphere-mesh geometry pairs. + * + * This method provides specialized collision detection for sphere-mesh + * pairs that can handle non-convex meshes. This implementation uses + * ComputeSignedDistanceGeometryToPoint to accurately compute distances from a + * point to any mesh (convex or concave). + * + * The method determines which geometry is the sphere and which is the mesh, + * computes the sphere's world-frame center, finds the closest point on the + * mesh surface, and calculates the resulting contact information. + * + * @param context The context for the MultibodyPlant. + * @param[out] p_ACa Contact point on geometry A expressed in frame A. + * @param[out] p_BCb Contact point on geometry B expressed in frame B. + * @param[out] distance Signed distance between the geometries (negative + * indicates penetration). + * @param[out] nhat_BA_W Unit normal vector pointing from geometry B to + * geometry A, expressed in world frame. + */ + void ComputeSphereMeshDistance(const drake::systems::Context& context, + Eigen::Vector3d& p_ACa, Eigen::Vector3d& p_BCb, + T& distance, Eigen::Vector3d& nhat_BA_W) const; + + /** + * @brief Computes collision information for general geometry pairs. + * + * This method handles collision detection for arbitrary geometry pairs + * using Drake's standard ComputeSignedDistancePairClosestPoints algorithm. + * It works reliably for convex geometries and convex hulls of meshes, but + * may not provide accurate results for non-convex mesh surfaces. + * + * @param context The context for the MultibodyPlant. + * @param[out] p_ACa Contact point on geometry A expressed in frame A. + * @param[out] p_BCb Contact point on geometry B expressed in frame B. + * @param[out] distance Signed distance between the geometries (negative + * indicates penetration). + * @param[out] nhat_BA_W Unit normal vector pointing from geometry B to + * geometry A, expressed in world frame. + */ + void ComputeGeneralGeometryDistance(const drake::systems::Context& context, + Eigen::Vector3d& p_ACa, + Eigen::Vector3d& p_BCb, T& distance, + Eigen::Vector3d& nhat_BA_W) const; + /** * @brief A reference to the MultibodyPlant containing the geometries. */ From 2bd13495c3306cf7d53992e2495774d1ad454cca Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 12 Aug 2025 16:26:49 +0000 Subject: [PATCH 05/16] feat: reorganize --- core/c3.cc | 18 +++++++++--------- core/c3.h | 4 ++-- core/c3_plus.cc | 26 +++++++++----------------- core/c3_plus.h | 7 ++++--- multibody/geom_geom_collider.cc | 6 +++--- 5 files changed, 27 insertions(+), 34 deletions(-) diff --git a/core/c3.cc b/core/c3.cc index e621208..7a5164b 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -388,9 +388,10 @@ void C3::ADMMStep(const VectorXd& x0, vector* delta, } } -void C3::WarmStartQP(const Eigen::VectorXd& x0, int admm_iteration) { - - if (admm_iteration == 0) return; // No warm start for the first iteration +void C3::SetInitialGuessQP(const Eigen::VectorXd& x0, int admm_iteration) { + prog_.SetInitialGuess(x_[0], x0); + if (!warm_start_ || admm_iteration == 0) + return; // No warm start for the first iteration int index = solve_time_ / lcs_.dt(); double weight = (solve_time_ - index * lcs_.dt()) / lcs_.dt(); for (int i = 0; i < N_ - 1; ++i) { @@ -407,8 +408,8 @@ void C3::WarmStartQP(const Eigen::VectorXd& x0, int admm_iteration) { prog_.SetInitialGuess(x_[N_], warm_start_x_[admm_iteration - 1][N_]); } -void C3::ProcessQPResults(const MathematicalProgramResult& result, - int admm_iteration, bool is_final_solve) { +void C3::StoreQPResults(const MathematicalProgramResult& result, + int admm_iteration, bool is_final_solve) { for (int i = 0; i < N_; ++i) { if (is_final_solve) { x_sol_->at(i) = result.GetSolution(x_[i]); @@ -449,8 +450,7 @@ vector C3::SolveQP(const VectorXd& x0, const vector& G, -2 * G.at(i) * WD.at(i)); } - prog_.SetInitialGuess(x_[0], x0); - if (warm_start_) WarmStartQP(x0, admm_iteration); + SetInitialGuessQP(x0, admm_iteration); MathematicalProgramResult result = osqp_.Solve(prog_); @@ -459,7 +459,7 @@ vector C3::SolveQP(const VectorXd& x0, const vector& G, result.get_solution_result()); } - ProcessQPResults(result, admm_iteration, is_final_solve); + StoreQPResults(result, admm_iteration, is_final_solve); return *z_sol_; } @@ -476,7 +476,7 @@ vector C3::SolveProjection(const vector& U, } #pragma omp parallel for num_threads( \ - options_.num_threads) if (use_parallelization_in_projection_) + options_.num_threads) if (use_parallelization_in_projection_) for (int i = 0; i < N_; ++i) { if (warm_start_) { if (i == N_ - 1) { diff --git a/core/c3.h b/core/c3.h index 6e69f4f..b31c359 100644 --- a/core/c3.h +++ b/core/c3.h @@ -247,8 +247,8 @@ class C3 { const Eigen::MatrixXd& H, const Eigen::VectorXd& c, const int admm_iteration, const int& warm_start_index) = 0; - virtual void WarmStartQP(const Eigen::VectorXd& x0, int admm_iteration); - virtual void ProcessQPResults( + virtual void SetInitialGuessQP(const Eigen::VectorXd& x0, int admm_iteration); + virtual void StoreQPResults( const drake::solvers::MathematicalProgramResult& result, int admm_iteration, bool is_final_solve); /*! diff --git a/core/c3_plus.cc b/core/c3_plus.cc index daa5b88..5d927ea 100644 --- a/core/c3_plus.cc +++ b/core/c3_plus.cc @@ -65,30 +65,22 @@ void C3Plus::UpdateLCS(const LCS& lcs) { } } -void C3Plus::WarmStartQP(const Eigen::VectorXd& x0, int admm_iteration) { - // if (admm_iteration == 0) return; // No warm start for the first iteration +void C3Plus::SetInitialGuessQP(const Eigen::VectorXd& x0, int admm_iteration) { + C3::SetInitialGuessQP(x0, admm_iteration); + if (!warm_start_ || admm_iteration == 0) + return; // No warm start for the first iteration int index = solve_time_ / lcs_.dt(); double weight = (solve_time_ - index * lcs_.dt()) / lcs_.dt(); for (int i = 0; i < N_ - 1; ++i) { - prog_.SetInitialGuess(x_[i], - (1 - weight) * warm_start_x_[admm_iteration][i] + - weight * warm_start_x_[admm_iteration][i + 1]); prog_.SetInitialGuess( - lambda_[i], (1 - weight) * warm_start_lambda_[admm_iteration][i] + - weight * warm_start_lambda_[admm_iteration][i + 1]); - prog_.SetInitialGuess(u_[i], - (1 - weight) * warm_start_u_[admm_iteration][i] + - weight * warm_start_u_[admm_iteration][i + 1]); - prog_.SetInitialGuess(eta_[i], - (1 - weight) * warm_start_eta_[admm_iteration][i] + - weight * warm_start_eta_[admm_iteration][i + 1]); + eta_[i], (1 - weight) * warm_start_eta_[admm_iteration - 1][i] + + weight * warm_start_eta_[admm_iteration - 1][i + 1]); } - prog_.SetInitialGuess(x_[N_], warm_start_x_[admm_iteration][N_]); } -void C3Plus::ProcessQPResults(const MathematicalProgramResult& result, - int admm_iteration, bool is_final_solve) { - C3::ProcessQPResults(result, admm_iteration, is_final_solve); +void C3Plus::StoreQPResults(const MathematicalProgramResult& result, + int admm_iteration, bool is_final_solve) { + C3::StoreQPResults(result, admm_iteration, is_final_solve); for (int i = 0; i < N_; i++) { if (is_final_solve) { eta_sol_->at(i) = result.GetSolution(eta_[i]); diff --git a/core/c3_plus.h b/core/c3_plus.h index 037c913..2905da8 100644 --- a/core/c3_plus.h +++ b/core/c3_plus.h @@ -55,10 +55,11 @@ class C3Plus final : public C3 { std::vector> warm_start_eta_; private: - void ProcessQPResults(const drake::solvers::MathematicalProgramResult& result, - int admm_iteration, bool is_final_solve) override; + void StoreQPResults(const drake::solvers::MathematicalProgramResult& result, + int admm_iteration, bool is_final_solve) override; void UpdateLCS(const LCS& lcs) override; - void WarmStartQP(const Eigen::VectorXd& x0, int admm_iteration) override; + void SetInitialGuessQP(const Eigen::VectorXd& x0, + int admm_iteration) override; std::vector eta_; std::unique_ptr> eta_sol_; diff --git a/multibody/geom_geom_collider.cc b/multibody/geom_geom_collider.cc index df42ae7..cadbfab 100644 --- a/multibody/geom_geom_collider.cc +++ b/multibody/geom_geom_collider.cc @@ -78,7 +78,7 @@ void GeomGeomCollider::ComputeSphereMeshDistance(const Context& context, mesh_set.Add(mesh_id); const auto sd_set = query_object.ComputeSignedDistanceGeometryToPoint( sphere_center, mesh_set); - DRAKE_ASSERT(sd_set.size() > 0); + DRAKE_DEMAND(sd_set.size() == 1); SignedDistanceToPoint sd_to_point = sd_set[0]; // Compute contact distance and normal. @@ -90,11 +90,11 @@ void GeomGeomCollider::ComputeSphereMeshDistance(const Context& context, nhat_BA_W = -nhat_BA_W; p_ACa = inspector.GetPoseInFrame(geometry_id_A_).template cast() * sd_to_point.p_GN; - p_BCb = X_FS.template cast() * (-sphere_radius * nhat_BA_W); + p_BCb = X_FS.template cast() * (-1 * sphere_radius * nhat_BA_W); } else { p_BCb = inspector.GetPoseInFrame(geometry_id_B_).template cast() * sd_to_point.p_GN; - p_ACa = X_FS.template cast() * (-sphere_radius * nhat_BA_W); + p_ACa = X_FS.template cast() * (-1 * sphere_radius * nhat_BA_W); } } From 24c419acb94a202006c50ce5592b9367c28af5c9 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 19 Aug 2025 10:52:46 -0400 Subject: [PATCH 06/16] feat: add regularized lcp solve for lcs factory --- .github/workflows/coverage.yml | 42 +++++++++++++++++----------------- bindings/pyc3/c3_py.cc | 2 +- core/c3.cc | 3 +++ core/lcs.cc | 10 ++++++-- core/lcs.h | 4 ++-- multibody/lcs_factory.h | 4 ++-- 6 files changed, 37 insertions(+), 28 deletions(-) diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml index fbc5698..0dab8c9 100644 --- a/.github/workflows/coverage.yml +++ b/.github/workflows/coverage.yml @@ -53,27 +53,27 @@ jobs: 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 diff --git a/bindings/pyc3/c3_py.cc b/bindings/pyc3/c3_py.cc index 55927b9..703c58f 100644 --- a/bindings/pyc3/c3_py.cc +++ b/bindings/pyc3/c3_py.cc @@ -154,7 +154,7 @@ PYBIND11_MODULE(c3, m) { py::arg("dt")) .def(py::init(), 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) diff --git a/core/c3.cc b/core/c3.cc index 7a5164b..090f057 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -475,8 +475,11 @@ vector C3::SolveProjection(const vector& 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) { diff --git a/core/lcs.cc b/core/lcs.cc index 2bf74b6..3f1ba54 100644 --- a/core/lcs.cc +++ b/core/lcs.cc @@ -57,11 +57,17 @@ double LCS::ScaleComplementarityDynamics() { return scale; } -const VectorXd LCS::Simulate(VectorXd& x_init, VectorXd& u) const { +const VectorXd LCS::Simulate(VectorXd& x_init, VectorXd& u, + bool regularized) const { VectorXd x_final; VectorXd force; drake::solvers::MobyLCPSolver LCPSolver; - LCPSolver.SolveLcpLemke(F_[0], E_[0] * x_init + c_[0] + H_[0] * u, &force); + if (regularized) { + LCPSolver.SolveLcpLemkeRegularized( + F_[0], E_[0] * x_init + c_[0] + H_[0] * u, &force); + } else { + LCPSolver.SolveLcpLemke(F_[0], E_[0] * x_init + c_[0] + H_[0] * u, &force); + } x_final = A_[0] * x_init + B_[0] * u + D_[0] * force + d_[0]; return x_final; } diff --git a/core/lcs.h b/core/lcs.h index ba2dd13..af80b7c 100644 --- a/core/lcs.h +++ b/core/lcs.h @@ -50,8 +50,8 @@ class LCS { * * @return The state at the next timestep */ - const Eigen::VectorXd Simulate(Eigen::VectorXd& x_init, - Eigen::VectorXd& u) const; + const Eigen::VectorXd Simulate(Eigen::VectorXd& x_init, Eigen::VectorXd& u, + bool regularized = false) const; /*! * Accessors dynamics terms diff --git a/multibody/lcs_factory.h b/multibody/lcs_factory.h index e5f7940..c140de9 100644 --- a/multibody/lcs_factory.h +++ b/multibody/lcs_factory.h @@ -291,9 +291,9 @@ class LCSFactory { // Configuration options for the LCSFactory LCSFactoryOptions options_; - ContactModel contact_model_; ///< The contact model being used. int n_contacts_; ///< Number of contact points. - int n_friction_directions_; ///< Number of friction directions.u + int n_friction_directions_; ///< Number of friction directions. + ContactModel contact_model_; ///< The contact model being used. int n_q_; ///< Number of configuration variables. int n_v_; ///< Number of velocity variables. int n_x_; ///< Number of state variables. From a2030a9312b58e11c1ede25ec6721645c36aeecb Mon Sep 17 00:00:00 2001 From: Meow404 Date: Wed, 1 Oct 2025 21:42:34 +0000 Subject: [PATCH 07/16] feat: add ability to use difference friction directions for each contact pair --- examples/lcs_factory_system_example.cc | 33 +-- .../c3_controller_pivoting_options.yaml | 22 +- multibody/lcs_factory.cc | 192 +++++++++++++----- multibody/lcs_factory.h | 36 +++- multibody/lcs_factory_options.h | 61 +++++- systems/lcs_factory_system.cc | 22 +- systems/lcs_factory_system.h | 8 + 7 files changed, 263 insertions(+), 111 deletions(-) diff --git a/examples/lcs_factory_system_example.cc b/examples/lcs_factory_system_example.cc index 9bd86b5..c004185 100644 --- a/examples/lcs_factory_system_example.cc +++ b/examples/lcs_factory_system_example.cc @@ -282,37 +282,6 @@ int RunPivotingTest() { // Build the plant diagram. auto plant_diagram = plant_builder.Build(); - // Retrieve collision geometries for relevant bodies. - std::vector platform_collision_geoms = - plant_for_lcs.GetCollisionGeometriesForBody( - plant_for_lcs.GetBodyByName("platform")); - std::vector cube_collision_geoms = - plant_for_lcs.GetCollisionGeometriesForBody( - plant_for_lcs.GetBodyByName("cube")); - std::vector left_finger_collision_geoms = - plant_for_lcs.GetCollisionGeometriesForBody( - plant_for_lcs.GetBodyByName("left_finger")); - std::vector right_finger_collision_geoms = - plant_for_lcs.GetCollisionGeometriesForBody( - plant_for_lcs.GetBodyByName("right_finger")); - - // Map collision geometries to their respective components. - std::unordered_map> - contact_geoms; - contact_geoms["PLATFORM"] = platform_collision_geoms; - contact_geoms["CUBE"] = cube_collision_geoms; - contact_geoms["LEFT_FINGER"] = left_finger_collision_geoms; - contact_geoms["RIGHT_FINGER"] = right_finger_collision_geoms; - - // Define contact pairs for the LCS system. - std::vector> contact_pairs; - contact_pairs.emplace_back(contact_geoms["CUBE"][0], - contact_geoms["LEFT_FINGER"][0]); - contact_pairs.emplace_back(contact_geoms["CUBE"][0], - contact_geoms["PLATFORM"][0]); - contact_pairs.emplace_back(contact_geoms["CUBE"][0], - contact_geoms["RIGHT_FINGER"][0]); - // Build the main diagram. DiagramBuilder builder; auto [plant, scene_graph] = AddMultibodyPlantSceneGraph(&builder, 0.01); @@ -340,7 +309,7 @@ int RunPivotingTest() { // Add the LCS factory system. auto lcs_factory_system = builder.AddSystem( plant_for_lcs, plant_for_lcs_context, *plant_autodiff, - *plant_context_autodiff, contact_pairs, options.lcs_factory_options); + *plant_context_autodiff, options.lcs_factory_options); // Add the C3 controller. auto c3_controller = diff --git a/examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml b/examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml index 50b49bf..8e83e9c 100644 --- a/examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml +++ b/examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml @@ -9,17 +9,35 @@ lcs_factory_options: #options are 'stewart_and_trinkle' or 'anitescu' # contact_model : 'stewart_and_trinkle' contact_model: "stewart_and_trinkle" - num_friction_directions: 1 num_contacts: 3 spring_stiffness: 0 - mu: [ 0.1, 0.1, 0.1 ] N: 10 dt: 0.01 + contact_pair_configs: + - body_A: "cube" + body_A_collision_geom_indices: [0] + body_B: "left_finger" + body_B_collision_geom_indices: [0] + mu: 0.1 + num_friction_directions: 1 + - body_A: "cube" + body_A_collision_geom_indices: [0] + body_B: "right_finger" + body_B_collision_geom_indices: [0] + mu: 0.1 + num_friction_directions: 1 + - body_A: "cube" + body_A_collision_geom_indices: [0] + body_B: "platform" + body_B_collision_geom_indices: [0] + mu: 0.1 + num_friction_directions: 1 c3_options: warm_start: false end_on_qp_step: true scale_lcs: false + penalize_input_change: false num_threads: 10 delta_option: 1 diff --git a/multibody/lcs_factory.cc b/multibody/lcs_factory.cc index 9fd65f2..dd915d5 100644 --- a/multibody/lcs_factory.cc +++ b/multibody/lcs_factory.cc @@ -29,6 +29,52 @@ using Eigen::VectorXd; namespace c3 { namespace multibody { +LCSFactory::LCSFactory( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context& context, + const drake::multibody::MultibodyPlant& plant_ad, + drake::systems::Context& context_ad, + LCSFactoryOptions& options) + : plant_(plant), + context_(context), + plant_ad_(plant_ad), + context_ad_(context_ad), + options_(options), + contact_model_(GetContactModelMap().at(options_.contact_model)), + n_q_(plant_.num_positions()), + n_v_(plant_.num_velocities()), + n_x_(n_q_ + n_v_), + n_u_(plant_.num_actuators()), + frictionless_(contact_model_ == ContactModel::kFrictionlessSpring), + dt_(options.dt) { + DRAKE_DEMAND(options.contact_pair_configs.has_value()); + n_contacts_ = options.contact_pair_configs.value().size(); + + mu_.clear(); + n_friction_directions_per_contact_.clear(); + contact_pairs_.clear(); + for (auto& pair : options.contact_pair_configs.value()) { + std::vector body_A_collision_geoms = + plant.GetCollisionGeometriesForBody(plant.GetBodyByName(pair.body_A)); + std::vector body_B_collision_geoms = + plant.GetCollisionGeometriesForBody(plant.GetBodyByName(pair.body_B)); + for (int i : pair.body_A_collision_geom_indices) { + for (int j : pair.body_B_collision_geom_indices) { + contact_pairs_.emplace_back(SortedPair( + body_A_collision_geoms[i], body_B_collision_geoms[j])); + mu_.push_back(pair.mu); + n_friction_directions_per_contact_.push_back( + pair.num_friction_directions); + } + } + } + n_lambda_ = multibody::LCSFactory::GetNumContactVariables( + contact_model_, n_contacts_, n_friction_directions_per_contact_); + Jt_row_sizes_ = 2 * Eigen::Map( + n_friction_directions_per_contact_.data(), + n_friction_directions_per_contact_.size()); +} + LCSFactory::LCSFactory( const MultibodyPlant& plant, Context& context, const MultibodyPlant& plant_ad, Context& context_ad, @@ -42,23 +88,28 @@ LCSFactory::LCSFactory( contact_pairs_(contact_geoms), options_(options), n_contacts_(contact_geoms.size()), - n_friction_directions_(options_.num_friction_directions), + n_friction_directions_per_contact_( + options_.num_friction_directions_per_contact.value()), contact_model_(GetContactModelMap().at(options_.contact_model)), n_q_(plant_.num_positions()), n_v_(plant_.num_velocities()), n_x_(n_q_ + n_v_), n_lambda_(multibody::LCSFactory::GetNumContactVariables( - contact_model_, n_contacts_, n_friction_directions_)), + contact_model_, n_contacts_, n_friction_directions_per_contact_)), n_u_(plant_.num_actuators()), - mu_(options.mu), + mu_(options.mu.value()), frictionless_(contact_model_ == ContactModel::kFrictionlessSpring), - dt_(options.dt) {} + dt_(options.dt) { + Jt_row_sizes_ = 2 * Eigen::Map( + n_friction_directions_per_contact_.data(), + n_friction_directions_per_contact_.size()); +} void LCSFactory::ComputeContactJacobian(VectorXd& phi, MatrixXd& Jn, MatrixXd& Jt) { phi.resize(n_contacts_); // Signed distance values for contacts Jn.resize(n_contacts_, n_v_); // Normal contact Jacobian - Jt.resize(2 * n_contacts_ * n_friction_directions_, + Jt.resize(Jt_row_sizes_.sum(), n_v_); // Tangential contact Jacobian Eigen::Vector3d planar_normal = {0, 1, 0}; @@ -66,11 +117,11 @@ void LCSFactory::ComputeContactJacobian(VectorXd& phi, MatrixXd& Jn, MatrixX J_i; for (int i = 0; i < n_contacts_; i++) { multibody::GeomGeomCollider collider(plant_, contact_pairs_[i]); - if (frictionless_ || n_friction_directions_ == 1) + if (frictionless_ || n_friction_directions_per_contact_[i] == 1) std::tie(phi_i, J_i) = collider.EvalPlanar(context_, planar_normal); else - std::tie(phi_i, J_i) = - collider.EvalPolytope(context_, n_friction_directions_); + std::tie(phi_i, J_i) = collider.EvalPolytope( + context_, n_friction_directions_per_contact_[i]); // Signed distance value for contact i phi(i) = phi_i; @@ -81,8 +132,8 @@ void LCSFactory::ComputeContactJacobian(VectorXd& phi, MatrixXd& Jn, Jn.row(i) = J_i.row(0); if (frictionless_) continue; // If frictionless_, we only need the normal force - Jt.block(2 * i * n_friction_directions_, 0, 2 * n_friction_directions_, - n_v_) = J_i.block(1, 0, 2 * n_friction_directions_, n_v_); + Jt.block(Jt_row_sizes_.segment(0, i).sum(), 0, Jt_row_sizes_(i), n_v_) = + J_i.block(1, 0, Jt_row_sizes_(i), n_v_); } } @@ -260,60 +311,50 @@ void LCSFactory::FormulateStewartTrinkleContactDynamics( // Eₜ = blkdiag(e,.., e), e ∈ 1ⁿᵉ // (ne) number of directions in firctional cone - MatrixXd E_t = - MatrixXd::Zero(n_contacts_, 2 * n_contacts_ * n_friction_directions_); + MatrixXd E_t = MatrixXd::Zero(n_contacts_, Jt_row_sizes_.sum()); for (int i = 0; i < n_contacts_; i++) { - E_t.block(i, i * (2 * n_friction_directions_), 1, - 2 * n_friction_directions_) = - MatrixXd::Ones(1, 2 * n_friction_directions_); + E_t.block(i, Jt_row_sizes_.segment(0, i).sum(), 1, Jt_row_sizes_(i)) = + MatrixXd::Ones(1, Jt_row_sizes_(i)); } // Formulate D matrix (state-force) - D.block(0, 2 * n_contacts_, n_q_, 2 * n_contacts_ * n_friction_directions_) = + D.block(0, 2 * n_contacts_, n_q_, Jt_row_sizes_.sum()) = dt_ * dt_ * qdotNv * MinvJ_t_T; - D.block(n_q_, 2 * n_contacts_, n_v_, - 2 * n_contacts_ * n_friction_directions_) = dt_ * MinvJ_t_T; + D.block(n_q_, 2 * n_contacts_, n_v_, Jt_row_sizes_.sum()) = dt_ * MinvJ_t_T; D.block(0, n_contacts_, n_q_, n_contacts_) = dt_ * dt_ * qdotNv * MinvJ_n_T; D.block(n_q_, n_contacts_, n_v_, n_contacts_) = dt_ * MinvJ_n_T; // Formulate E matrix (force-state) E.block(n_contacts_, 0, n_contacts_, n_q_) = dt_ * dt_ * Jn * Jf_q + Jn * vNqdot; - E.block(2 * n_contacts_, 0, 2 * n_contacts_ * n_friction_directions_, n_q_) = - dt_ * Jt * Jf_q; + E.block(2 * n_contacts_, 0, Jt_row_sizes_.sum(), n_q_) = dt_ * Jt * Jf_q; E.block(n_contacts_, n_q_, n_contacts_, n_v_) = dt_ * Jn + dt_ * dt_ * Jn * Jf_v; - E.block(2 * n_contacts_, n_q_, 2 * n_contacts_ * n_friction_directions_, - n_v_) = Jt + dt_ * Jt * Jf_v; + E.block(2 * n_contacts_, n_q_, Jt_row_sizes_.sum(), n_v_) = + Jt + dt_ * Jt * Jf_v; // Formulate F matrix (force-force) F.block(0, n_contacts_, n_contacts_, n_contacts_) = mu_.asDiagonal(); - F.block(0, 2 * n_contacts_, n_contacts_, - 2 * n_contacts_ * n_friction_directions_) = -E_t; + F.block(0, 2 * n_contacts_, n_contacts_, Jt_row_sizes_.sum()) = -E_t; F.block(n_contacts_, n_contacts_, n_contacts_, n_contacts_) = dt_ * dt_ * Jn * MinvJ_n_T; - F.block(n_contacts_, 2 * n_contacts_, n_contacts_, - 2 * n_contacts_ * n_friction_directions_) = + F.block(n_contacts_, 2 * n_contacts_, n_contacts_, Jt_row_sizes_.sum()) = dt_ * dt_ * Jn * MinvJ_t_T; - F.block(2 * n_contacts_, 0, 2 * n_contacts_ * n_friction_directions_, - n_contacts_) = E_t.transpose(); - F.block(2 * n_contacts_, n_contacts_, - 2 * n_contacts_ * n_friction_directions_, n_contacts_) = + F.block(2 * n_contacts_, 0, Jt_row_sizes_.sum(), n_contacts_) = + E_t.transpose(); + F.block(2 * n_contacts_, n_contacts_, Jt_row_sizes_.sum(), n_contacts_) = dt_ * Jt * MinvJ_n_T; - F.block(2 * n_contacts_, 2 * n_contacts_, - 2 * n_contacts_ * n_friction_directions_, - 2 * n_contacts_ * n_friction_directions_) = dt_ * Jt * MinvJ_t_T; + F.block(2 * n_contacts_, 2 * n_contacts_, Jt_row_sizes_.sum(), + Jt_row_sizes_.sum()) = dt_ * Jt * MinvJ_t_T; // Formulate H matrix (force-input) H.block(n_contacts_, 0, n_contacts_, n_u_) = dt_ * dt_ * Jn * Jf_u; - H.block(2 * n_contacts_, 0, 2 * n_contacts_ * n_friction_directions_, n_u_) = - dt_ * Jt * Jf_u; + H.block(2 * n_contacts_, 0, Jt_row_sizes_.sum(), n_u_) = dt_ * Jt * Jf_u; // Formulate c vector c.segment(n_contacts_, n_contacts_) = phi + dt_ * dt_ * Jn * d_v - Jn * vNqdot * plant_.GetPositions(context_); - c.segment(2 * n_contacts_, 2 * n_contacts_ * n_friction_directions_) = - Jt * dt_ * d_v; + c.segment(2 * n_contacts_, Jt_row_sizes_.sum()) = Jt * dt_ * d_v; } void LCSFactory::FormulateAnitescuContactDynamics( @@ -326,21 +367,19 @@ void LCSFactory::FormulateAnitescuContactDynamics( // Eₜ = blkdiag(e,.., e), e ∈ 1ⁿᵉ // (ne) number of directions in firctional cone - MatrixXd E_t = - MatrixXd::Zero(n_contacts_, 2 * n_contacts_ * n_friction_directions_); + MatrixXd E_t = MatrixXd::Zero(n_contacts_, Jt_row_sizes_.sum()); for (int i = 0; i < n_contacts_; i++) { - E_t.block(i, i * (2 * n_friction_directions_), 1, - 2 * n_friction_directions_) = - MatrixXd::Ones(1, 2 * n_friction_directions_); + E_t.block(i, Jt_row_sizes_.segment(0, i).sum(), 1, Jt_row_sizes_(i)) = + MatrixXd::Ones(1, Jt_row_sizes_(i)); } // Apply same friction coefficients to each friction direction for same // contact VectorXd anitescu_mu_vec = VectorXd::Zero(n_lambda_); for (int i = 0; i < mu_.rows(); i++) { - anitescu_mu_vec.segment((2 * n_friction_directions_) * i, - 2 * n_friction_directions_) = - mu_(i) * VectorXd::Ones(2 * n_friction_directions_); + anitescu_mu_vec.segment(Jt_row_sizes_.segment(0, i).sum(), + Jt_row_sizes_(i)) = + mu_(i) * VectorXd::Ones(Jt_row_sizes_(i)); } MatrixXd anitescu_mu_matrix = anitescu_mu_vec.asDiagonal(); @@ -401,21 +440,19 @@ LCSFactory::GetContactJacobianAndPoints() { if (contact_model_ == ContactModel::kStewartAndTrinkle) { // if Stewart and Trinkle model, concatenate the normal and tangential // jacobian - MatrixXd J_c = MatrixXd::Zero( - n_contacts_ + 2 * n_contacts_ * n_friction_directions_, n_v_); + MatrixXd J_c = MatrixXd::Zero(n_contacts_ + Jt_row_sizes_.sum(), n_v_); J_c << Jn, Jt; return std::make_pair(J_c, contact_points); } // Model is Anitescu - int n_lambda_ = 2 * n_contacts_ * n_friction_directions_; + int n_lambda_ = Jt_row_sizes_.sum(); // Eₜ = blkdiag(e,.., e), e ∈ 1ⁿᵉ MatrixXd E_t = MatrixXd::Zero(n_contacts_, n_lambda_); for (int i = 0; i < n_contacts_; i++) { - E_t.block(i, i * (2 * n_friction_directions_), 1, - 2 * n_friction_directions_) = - MatrixXd::Ones(1, 2 * n_friction_directions_); + E_t.block(i, Jt_row_sizes_.segment(0, i).sum(), 1, Jt_row_sizes_(i)) = + MatrixXd::Ones(1, Jt_row_sizes_(i)); } // Apply same friction coefficients to each friction direction @@ -426,9 +463,8 @@ LCSFactory::GetContactJacobianAndPoints() { VectorXd mu_vector = VectorXd::Zero(n_lambda_); for (int i = 0; i < muXd.rows(); i++) { - mu_vector.segment((2 * n_friction_directions_) * i, - 2 * n_friction_directions_) = - muXd(i) * VectorXd::Ones(2 * n_friction_directions_); + mu_vector.segment(Jt_row_sizes_.segment(0, i).sum(), Jt_row_sizes_(i)) = + muXd(i) * VectorXd::Ones(Jt_row_sizes_(i)); } MatrixXd mu_matrix = mu_vector.asDiagonal(); @@ -561,11 +597,57 @@ int LCSFactory::GetNumContactVariables(ContactModel contact_model, throw std::out_of_range("Unknown contact model."); } +int LCSFactory::GetNumContactVariables( + ContactModel contact_model, int num_contacts, + std::vector num_friction_directions_per_contact) { + if (contact_model == ContactModel::kFrictionlessSpring) { + return num_contacts; // Only normal forces + } else { + int num_tangential_lambda = + 2 * std::accumulate(num_friction_directions_per_contact.begin(), + num_friction_directions_per_contact.end(), 0); + if (contact_model == ContactModel::kStewartAndTrinkle) { + return 2 * num_contacts + + num_tangential_lambda; // Compute contact variable count + // for Stewart-Trinkle model + } else if (contact_model == ContactModel::kAnitescu) { + return num_tangential_lambda; // Compute contact variable + // count for Anitescu model + } + } + throw std::out_of_range("Unknown contact model."); +} + int LCSFactory::GetNumContactVariables(const LCSFactoryOptions options) { multibody::ContactModel contact_model = GetContactModelMap().at(options.contact_model); + std::vector n_friction_directions_per_contact; + if (options.num_friction_directions_per_contact.has_value()) { + n_friction_directions_per_contact = + options.num_friction_directions_per_contact.value(); + } else if (options.contact_pair_configs.has_value()) { + for (auto& pair_config : options.contact_pair_configs.value()) { + std::vector n_friction_directions_for_contact( + pair_config.body_A_collision_geom_indices.size() * + pair_config.body_B_collision_geom_indices.size(), + pair_config.num_friction_directions); + n_friction_directions_per_contact.insert( + n_friction_directions_per_contact.end(), + n_friction_directions_for_contact.begin(), + n_friction_directions_for_contact.end()); + } + } else if (options.num_friction_directions.has_value()) { + n_friction_directions_per_contact = std::vector( + options.num_contacts, options.num_friction_directions.value()); + } else { + throw std::runtime_error( + "LCSFactoryOptions must specify num_friction_directions_per_contact, " + "num_friction_directions, or contact_pair_configs."); + } + DRAKE_DEMAND(n_friction_directions_per_contact.size() == + (size_t)options.num_contacts); return GetNumContactVariables(contact_model, options.num_contacts, - options.num_friction_directions); + n_friction_directions_per_contact); } } // namespace multibody diff --git a/multibody/lcs_factory.h b/multibody/lcs_factory.h index c140de9..e6f802e 100644 --- a/multibody/lcs_factory.h +++ b/multibody/lcs_factory.h @@ -18,6 +18,7 @@ using drake::AutoDiffXd; using drake::MatrixX; using Eigen::MatrixXd; using Eigen::VectorXd; +using Eigen::VectorXi; namespace c3 { namespace multibody { @@ -53,6 +54,12 @@ inline const std::map& GetContactModelMap() { */ class LCSFactory { public: + LCSFactory( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context& context, + const drake::multibody::MultibodyPlant& plant_ad, + drake::systems::Context& context_ad, + LCSFactoryOptions& options); /** * @brief Constructor for the LCSFactory class. * @@ -166,8 +173,13 @@ class LCSFactory { int num_contacts, int num_friction_directions); + static int GetNumContactVariables( + ContactModel contact_model, int num_contacts, + std::vector num_friction_directions_per_contact); + /** - * @brief Computes the number of contact variables based on the LCS options. + * @brief Computes the number of contact variables based on the LCS + * options. * * @param options The LCS options specifying contact model and friction * properties. @@ -285,24 +297,26 @@ class LCSFactory { drake::systems::Context& context_; const drake::multibody::MultibodyPlant& plant_ad_; drake::systems::Context& context_ad_; - const std::vector> - contact_pairs_; + std::vector> contact_pairs_; // Configuration options for the LCSFactory LCSFactoryOptions options_; - int n_contacts_; ///< Number of contact points. - int n_friction_directions_; ///< Number of friction directions. - ContactModel contact_model_; ///< The contact model being used. - int n_q_; ///< Number of configuration variables. - int n_v_; ///< Number of velocity variables. - int n_x_; ///< Number of state variables. - int n_lambda_; ///< Number of contact force variables. - int n_u_; ///< Number of input variables. + int n_contacts_; ///< Number of contact points. + std::vector + n_friction_directions_per_contact_; ///< Number of friction directions. + ContactModel contact_model_; ///< The contact model being used. + int n_q_; ///< Number of configuration variables. + int n_v_; ///< Number of velocity variables. + int n_x_; ///< Number of state variables. + int n_lambda_; ///< Number of contact force variables. + int n_u_; ///< Number of input variables. std::vector mu_; ///< Vector of friction coefficients. bool frictionless_; ///< Flag indicating frictionless contacts. double dt_; ///< Time step. + + VectorXi Jt_row_sizes_; ///< Row sizes for tangential Jacobian blocks. }; } // namespace multibody diff --git a/multibody/lcs_factory_options.h b/multibody/lcs_factory_options.h index 295a135..3f50602 100644 --- a/multibody/lcs_factory_options.h +++ b/multibody/lcs_factory_options.h @@ -5,13 +5,39 @@ namespace c3 { +struct ContactPairConfig { + std::string body_A; + std::string body_B; + std::vector body_A_collision_geom_indices; + std::vector body_B_collision_geom_indices; + int num_friction_directions; + float mu; // friction coefficient + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(body_A)); + a->Visit(DRAKE_NVP(body_B)); + a->Visit(DRAKE_NVP(body_A_collision_geom_indices)); + a->Visit(DRAKE_NVP(body_B_collision_geom_indices)); + a->Visit(DRAKE_NVP(num_friction_directions)); + a->Visit(DRAKE_NVP(mu)); + } +}; + struct LCSFactoryOptions { - std::string contact_model; // "stewart_and_trinkle" or "anitescu" or - // "frictionless_spring" - int num_friction_directions; // Number of friction directions per contact - int num_contacts; // Number of contacts in the system - double spring_stiffness; // Spring stiffness for frictionless spring model - std::vector mu; // Friction coefficient for each contact + std::string contact_model; // "stewart_and_trinkle" or "anitescu" or + // "frictionless_spring" + int num_contacts; // Number of contacts in the system + double spring_stiffness; // Spring stiffness for frictionless spring model + std::optional + num_friction_directions; // Number of friction directions per contact + std::optional> + num_friction_directions_per_contact; // Number of friction directions per + std::optional> + mu; // Friction coefficient for each contact + + std::optional> + contact_pair_configs; // Optional detailed contact pair configurations int N; // number of time steps in the prediction horizon double dt; // time step size @@ -19,15 +45,34 @@ struct LCSFactoryOptions { template void Serialize(Archive* a) { a->Visit(DRAKE_NVP(contact_model)); - a->Visit(DRAKE_NVP(num_friction_directions)); a->Visit(DRAKE_NVP(num_contacts)); a->Visit(DRAKE_NVP(spring_stiffness)); + a->Visit(DRAKE_NVP(num_friction_directions)); + a->Visit(DRAKE_NVP(num_friction_directions_per_contact)); a->Visit(DRAKE_NVP(mu)); + a->Visit(DRAKE_NVP(contact_pair_configs)); a->Visit(DRAKE_NVP(N)); a->Visit(DRAKE_NVP(dt)); - DRAKE_DEMAND(mu.size() == (size_t)num_contacts); + // Contact Pair Information contained in contact_pair_configs + if (contact_pair_configs.has_value()) { + DRAKE_DEMAND(contact_pair_configs.value().size() == (size_t)num_contacts); + } else { + // ensure mu and num_friction_directions are properly specified + DRAKE_DEMAND(mu.has_value()); + DRAKE_DEMAND(mu.value().size() == (size_t)num_contacts); + // if a single num_friction_directions is provided, use it for all + // contacts + if (num_friction_directions.has_value()) { + num_friction_directions_per_contact = + std::vector(num_contacts, num_friction_directions.value()); + } else { + DRAKE_DEMAND(num_friction_directions_per_contact.has_value()); + DRAKE_DEMAND(num_friction_directions_per_contact.value().size() == + (size_t)num_contacts); + } + } } }; diff --git a/systems/lcs_factory_system.cc b/systems/lcs_factory_system.cc index 40f2417..91bead8 100644 --- a/systems/lcs_factory_system.cc +++ b/systems/lcs_factory_system.cc @@ -22,6 +22,17 @@ namespace c3 { namespace systems { +LCSFactorySystem::LCSFactorySystem( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context& context, + const drake::multibody::MultibodyPlant& plant_ad, + drake::systems::Context& context_ad, + LCSFactoryOptions options) { + InitializeSystem(plant, options); + lcs_factory_ = std::make_unique( + plant, context, plant_ad, context_ad, options); +} + LCSFactorySystem::LCSFactorySystem( const drake::multibody::MultibodyPlant& plant, drake::systems::Context& context, @@ -30,15 +41,20 @@ LCSFactorySystem::LCSFactorySystem( const std::vector> contact_geoms, LCSFactoryOptions options) { + InitializeSystem(plant, options); + lcs_factory_ = std::make_unique( + plant, context, plant_ad, context_ad, contact_geoms, options); +} + +void LCSFactorySystem::InitializeSystem( + const drake::multibody::MultibodyPlant& plant, + LCSFactoryOptions& options) { this->set_name("lcs_factory_system"); n_x_ = plant.num_positions() + plant.num_velocities(); n_lambda_ = multibody::LCSFactory::GetNumContactVariables(options); n_u_ = plant.num_actuators(); - lcs_factory_ = std::make_unique( - plant, context, plant_ad, context_ad, contact_geoms, options); - lcs_state_input_port_ = this->DeclareVectorInputPort("x_lcs", TimestampedVector(n_x_)) .get_index(); diff --git a/systems/lcs_factory_system.h b/systems/lcs_factory_system.h index 15404f5..92b7464 100644 --- a/systems/lcs_factory_system.h +++ b/systems/lcs_factory_system.h @@ -33,6 +33,12 @@ namespace systems { */ class LCSFactorySystem : public drake::systems::LeafSystem { public: + explicit LCSFactorySystem( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context& context, + const drake::multibody::MultibodyPlant& plant_ad, + drake::systems::Context& context_ad, + LCSFactoryOptions options); /** * @brief Constructs an LCSFactorySystem. * @@ -93,6 +99,8 @@ class LCSFactorySystem : public drake::systems::LeafSystem { } private: + void InitializeSystem(const drake::multibody::MultibodyPlant& plant, + LCSFactoryOptions& options); /** * @brief Computes the LCS based on the current state and inputs. * From 50d017f2211639c7ac3ca7a4e2ac92a5567d939c Mon Sep 17 00:00:00 2001 From: Meow404 Date: Fri, 3 Oct 2025 16:04:17 -0400 Subject: [PATCH 08/16] feat: add scaling for last solve --- core/c3.cc | 39 ++++++++++++++++++++------------------- core/c3.h | 30 +++++++++++++++--------------- core/c3_options.h | 11 +++++++++++ core/c3_plus.cc | 36 ++++++++++++++++++++++++++++++++++++ core/c3_plus.h | 4 ++++ 5 files changed, 86 insertions(+), 34 deletions(-) diff --git a/core/c3.cc b/core/c3.cc index 090f057..7693c29 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -82,7 +82,6 @@ C3::C3(const LCS& lcs, const CostMatrices& costs, u_ = vector(); lambda_ = vector(); - z_fin_ = std::make_unique>(); z_sol_ = std::make_unique>(); x_sol_ = std::make_unique>(); lambda_sol_ = std::make_unique>(); @@ -94,7 +93,6 @@ C3::C3(const LCS& lcs, const CostMatrices& costs, 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_)); - z_fin_->push_back(Eigen::VectorXd::Zero(n_z_)); z_sol_->push_back(Eigen::VectorXd::Zero(n_z_)); w_sol_->push_back(Eigen::VectorXd::Zero(n_z_)); delta_sol_->push_back(Eigen::VectorXd::Zero(n_z_)); @@ -328,7 +326,7 @@ void C3::Solve(const VectorXd& x0) { WD.at(i) = delta.at(i) - w.at(i); } - *z_fin_ = SolveQP(x0, G, WD, options_.admm_iter, true); + SolveQP(x0, G, WD, delta, options_.admm_iter, true); *w_sol_ = w; *delta_sol_ = delta; @@ -368,7 +366,7 @@ void C3::ADMMStep(const VectorXd& x0, vector* delta, WD.at(i) = delta->at(i) - w->at(i); } - vector z = SolveQP(x0, *G, WD, admm_iteration, false); + vector z = SolveQP(x0, *G, WD, *delta, admm_iteration, false); vector ZW(N_, VectorXd::Zero(n_z_)); for (int i = 0; i < N_; ++i) { @@ -434,22 +432,10 @@ void C3::StoreQPResults(const MathematicalProgramResult& result, } vector C3::SolveQP(const VectorXd& x0, const vector& G, - const vector& WD, int admm_iteration, + const vector& WD, + const vector& delta, int admm_iteration, bool is_final_solve) { - // Add or update augmented costs - if (augmented_costs_.size() == 0) { - for (int i = 0; i < N_; ++i) - augmented_costs_.push_back(prog_ - .AddQuadraticCost(2 * G.at(i), - -2 * G.at(i) * WD.at(i), - z_.at(i), 1) - .evaluator()); - } else { - for (int i = 0; i < N_; ++i) - augmented_costs_[i]->UpdateCoefficients(2 * G.at(i), - -2 * G.at(i) * WD.at(i)); - } - + AddAugmentedCost(G, WD, delta, is_final_solve); SetInitialGuessQP(x0, admm_iteration); MathematicalProgramResult result = osqp_.Solve(prog_); @@ -464,6 +450,21 @@ vector C3::SolveQP(const VectorXd& x0, const vector& G, return *z_sol_; } +void C3::AddAugmentedCost(const vector& G, const vector& WD, + const vector& delta, bool is_final_solve) { + // Remove previous augmented costs + for (auto& cost : augmented_costs_) { + prog_.RemoveCost(cost); + } + augmented_costs_.clear(); + // Add or update augmented costs + for (int i = 0; i < N_; ++i) { + DRAKE_ASSERT(G.at(i).cols() == WD.at(i).rows()); + augmented_costs_.push_back(prog_.AddQuadraticCost( + 2 * G.at(i), -2 * G.at(i) * WD.at(i), z_.at(i), 1)); + } +} + vector C3::SolveProjection(const vector& U, vector& WZ, int admm_iteration) { vector deltaProj(N_, VectorXd::Zero(n_z_)); diff --git a/core/c3.h b/core/c3.h index b31c359..e844ede 100644 --- a/core/c3.h +++ b/core/c3.h @@ -190,7 +190,6 @@ class C3 { } std::vector GetFullSolution() { return *z_sol_; } - std::vector GetFullQPSolution() { return *z_fin_; } std::vector GetStateSolution() { return *x_sol_; } std::vector GetForceSolution() { return *lambda_sol_; } std::vector GetInputSolution() { return *u_sol_; } @@ -246,11 +245,6 @@ class C3 { const Eigen::MatrixXd& E, const Eigen::MatrixXd& F, const Eigen::MatrixXd& H, const Eigen::VectorXd& c, const int admm_iteration, const int& warm_start_index) = 0; - - virtual void SetInitialGuessQP(const Eigen::VectorXd& x0, int admm_iteration); - virtual void StoreQPResults( - const drake::solvers::MathematicalProgramResult& result, - int admm_iteration, bool is_final_solve); /*! * Scales the LCS matrices internally to better condition the problem. * This only scales the lambdas. @@ -293,11 +287,20 @@ class C3 { * @param admm_iteration Index of the current ADMM iteration * @param is_final_solve Whether this is the final ADMM iteration */ - std::vector SolveQP(const Eigen::VectorXd& x0, - const std::vector& G, - const std::vector& WD, - int admm_iteration, - bool is_final_solve = false); + std::vector SolveQP( + const Eigen::VectorXd& x0, const std::vector& G, + const std::vector& WD, + const std::vector& delta, int admm_iteration, + bool is_final_solve = false); + + virtual void AddAugmentedCost(const std::vector& G, + const std::vector& WD, + const std::vector& delta, + bool is_final_solve); + virtual void SetInitialGuessQP(const Eigen::VectorXd& x0, int admm_iteration); + virtual void StoreQPResults( + const drake::solvers::MathematicalProgramResult& result, + int admm_iteration, bool is_final_solve); LCS lcs_; double AnDn_ = 1.0; // Scaling factor for lambdas @@ -327,7 +330,7 @@ class C3 { /// interface std::vector target_costs_; - std::vector> augmented_costs_; + std::vector augmented_costs_; std::vector> input_costs_; // Solutions @@ -336,9 +339,6 @@ class C3 { std::unique_ptr> u_sol_; std::unique_ptr> z_sol_; - std::unique_ptr> - z_fin_; // Final QP solution which may differ from z_sol_ if - // end_on_qp_step is false std::unique_ptr> delta_sol_; std::unique_ptr> w_sol_; }; diff --git a/core/c3_options.h b/core/c3_options.h index 08f3b6a..8fe16f5 100644 --- a/core/c3_options.h +++ b/core/c3_options.h @@ -31,6 +31,15 @@ struct C3Options { std::optional qp_projection_scaling; // scaling factor for the QP projection + std::optional + final_augmented_cost_contact_scaling; // scaling factor for the final + // augmented cost matrix for C3+ + // projection + std::optional> + final_augmented_cost_contact_indices; // vector for scaling the final + // augmented cost matrix for C3+ + // projection + int admm_iter = 3; // total number of ADMM iterations // See comments below for how we parse the .yaml into the cost matrices @@ -97,6 +106,8 @@ struct C3Options { a->Visit(DRAKE_NVP(M)); a->Visit(DRAKE_NVP(qp_projection_alpha)); a->Visit(DRAKE_NVP(qp_projection_scaling)); + a->Visit(DRAKE_NVP(final_augmented_cost_contact_scaling)); + a->Visit(DRAKE_NVP(final_augmented_cost_contact_indices)); a->Visit(DRAKE_NVP(admm_iter)); diff --git a/core/c3_plus.cc b/core/c3_plus.cc index 5d927ea..a60a14d 100644 --- a/core/c3_plus.cc +++ b/core/c3_plus.cc @@ -96,6 +96,42 @@ void C3Plus::StoreQPResults(const MathematicalProgramResult& result, } } +void C3Plus::AddAugmentedCost(const vector& G, + const vector& WD, + const vector& delta, + bool is_final_solve) { + if (is_final_solve) { + for (auto& cost : augmented_costs_) { + prog_.RemoveCost(cost); + } + augmented_costs_.clear(); + + std::vector GFinal = G; + for (int i = 0; i < N_; ++i) { + Eigen::VectorXd GScaling = + Eigen::VectorXd::Ones(n_z_); // Default scaling is 1 + if (options_.final_augmented_cost_contact_scaling.has_value() && + options_.final_augmented_cost_contact_indices.has_value()) { + for (int index : + options_.final_augmented_cost_contact_indices.value()) { + if (delta.at(i)[n_x_ + index] == 0) + GScaling(n_x_ + index) *= + options_.final_augmented_cost_contact_scaling.value(); + else + GScaling(n_x_ + n_lambda_ + n_u_ + index) *= + options_.final_augmented_cost_contact_scaling.value(); + } + } + auto GiFinal = GFinal.at(i).array().colwise() * GScaling.array(); + augmented_costs_.push_back(prog_.AddQuadraticCost( + 2 * GiFinal.matrix(), -2 * GiFinal.matrix() * delta.at(i), z_.at(i), + 1)); + } + } else { + C3::AddAugmentedCost(G, WD, delta, is_final_solve); + } +} + VectorXd C3Plus::SolveSingleProjection(const MatrixXd& U, const VectorXd& delta_c, const MatrixXd& E, const MatrixXd& F, diff --git a/core/c3_plus.h b/core/c3_plus.h index 2905da8..40f8dd2 100644 --- a/core/c3_plus.h +++ b/core/c3_plus.h @@ -55,6 +55,10 @@ class C3Plus final : public C3 { std::vector> warm_start_eta_; private: + void AddAugmentedCost(const std::vector& G, + const std::vector& WD, + const std::vector& delta, + bool is_final_solve) override; void StoreQPResults(const drake::solvers::MathematicalProgramResult& result, int admm_iteration, bool is_final_solve) override; void UpdateLCS(const LCS& lcs) override; From bd48c371d46f62a03120b7118cd5023a39f0ebc1 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Wed, 3 Dec 2025 23:01:35 -0500 Subject: [PATCH 09/16] test: fix breaking tests --- multibody/test/multibody_test.cc | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/multibody/test/multibody_test.cc b/multibody/test/multibody_test.cc index d2ff762..6efcca3 100644 --- a/multibody/test/multibody_test.cc +++ b/multibody/test/multibody_test.cc @@ -139,7 +139,8 @@ class LCSFactoryPivotingTest options.contact_model = std::get<0>(GetParam()); contact_model = GetContactModelMap().at(options.contact_model); - options.num_friction_directions = std::get<1>(GetParam()); + options.num_friction_directions_per_contact = + std::vector(contact_pairs.size(), std::get<1>(GetParam())); lcs_factory = std::make_unique( *plant, plant_context, *plant_autodiff, *plant_context_autodiff, contact_pairs, options); @@ -227,17 +228,20 @@ TEST_P(LCSFactoryPivotingTest, ComputeContactJacobian) { auto [J, contact_points] = lcs_factory->GetContactJacobianAndPoints(); int n_contacts = contact_pairs.size(); + auto n_tangential_directions = + 2 * std::accumulate(options.num_friction_directions_per_contact->begin(), + options.num_friction_directions_per_contact->end(), + 0); // Check for number of force variables (not including slack variables) switch (contact_model) { case ContactModel::kStewartAndTrinkle: - EXPECT_EQ(J.rows(), - n_contacts + 2 * n_contacts * options.num_friction_directions); + EXPECT_EQ(J.rows(), n_contacts + n_tangential_directions); break; case ContactModel::kFrictionlessSpring: EXPECT_EQ(J.rows(), n_contacts); break; case ContactModel::kAnitescu: - EXPECT_EQ(J.rows(), 2 * n_contacts * options.num_friction_directions); + EXPECT_EQ(J.rows(), n_tangential_directions); break; default: EXPECT_TRUE(false); // Something went wrong in parsing the contact model From da860103fbc90201ca5e248afb4ea6e7549953fb Mon Sep 17 00:00:00 2001 From: Meow404 Date: Wed, 3 Dec 2025 23:44:09 -0500 Subject: [PATCH 10/16] doc: add readme for LCS factory options and bindings --- bindings/pyc3/c3_multibody_py.cc | 18 ++- multibody/README.md | 210 +++++++++++++++++++++++++++++++ 2 files changed, 227 insertions(+), 1 deletion(-) create mode 100644 multibody/README.md diff --git a/bindings/pyc3/c3_multibody_py.cc b/bindings/pyc3/c3_multibody_py.cc index f17ebc9..ef63fe1 100644 --- a/bindings/pyc3/c3_multibody_py.cc +++ b/bindings/pyc3/c3_multibody_py.cc @@ -61,6 +61,18 @@ PYBIND11_MODULE(multibody, m) { &c3::multibody::LCSFactory::GetNumContactVariables), py::arg("options")); + py::class_(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_(m, "LCSFactoryOptions") .def(py::init<>()) .def_readwrite("dt", &LCSFactoryOptions::dt) @@ -68,9 +80,13 @@ PYBIND11_MODULE(multibody, m) { .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); } diff --git a/multibody/README.md b/multibody/README.md new file mode 100644 index 0000000..5f3ce0e --- /dev/null +++ b/multibody/README.md @@ -0,0 +1,210 @@ +# C3 Multibody Module + +This module provides tools for creating Linear Complementarity Systems (LCS) from Drake MultibodyPlant models, enabling the use of C3 for contact-rich manipulation and locomotion tasks. + +## Overview + +The multibody module bridges Drake's rigid body dynamics with C3's complementarity-based control framework. It linearizes contact dynamics around operating points and formulates them as LCS problems. + +## Key Components + +### LCSFactory + +The `LCSFactory` class linearizes a Drake `MultibodyPlant` into an LCS representation. It supports multiple contact models: + +- **Stewart and Trinkle**: Full friction cone with normal and tangential forces +- **Anitescu**: Linearized friction cone approximation +- **Frictionless Spring**: Simple normal-only contact with spring compliance + +### Contact Models + +#### Stewart and Trinkle Model +Uses explicit complementarity constraints for normal and tangential contact forces with a polyhedral friction cone approximation. + +#### Anitescu Model +Combines normal and tangential forces into a single set of variables, resulting in a more compact formulation. + +#### Frictionless Spring Model +Models contacts as compliant springs without friction, useful for simple contact scenarios. + +## LCS Factory Options Configuration + +The `LCSFactoryOptions` struct controls how the multibody plant is linearized into an LCS. These options are typically specified in a YAML configuration file. + +### Basic Structure + +```yaml +contact_model: "stewart_and_trinkle" # Contact model type +num_contacts: 3 # Number of contact pairs +spring_stiffness: 0 # Spring stiffness (for frictionless_spring model) +num_friction_directions: 1 # Friction cone approximation resolution +mu: [0.1, 0.1, 0.1] # Friction coefficients per contact +N: 10 # Prediction horizon length +dt: 0.01 # Time step size (seconds) +``` + +### Configuration Fields + +#### Required Fields + +- **`contact_model`** (string): Contact dynamics formulation + - Options: `"stewart_and_trinkle"`, `"anitescu"`, `"frictionless_spring"` + - Default: None (must be specified) + +- **`num_contacts`** (int): Total number of contact pairs in the system + - Must match the actual number of contacts or contact pair configurations + +- **`N`** (int): Number of time steps in the prediction horizon + - Typical values: 5-20 depending on task and computational budget + +- **`dt`** (double): Time step duration in seconds + - Typical values: 0.01-0.05 + - Smaller values give better accuracy but increase computation + +#### Contact-Specific Fields + +- **`spring_stiffness`** (double): Spring constant for `frictionless_spring` model + - Only used when `contact_model: "frictionless_spring"` + - Units: N/m + - Higher values = stiffer contact + +- **`num_friction_directions`** (int, optional): Number of friction cone edges per contact + - Used with `stewart_and_trinkle` or `anitescu` models + - Higher values = better friction cone approximation, more variables + - Typical values: 1-4 + - If specified, applies to all contacts uniformly + +- **`num_friction_directions_per_contact`** (vector, optional): Per-contact friction directions + - Allows different friction cone resolutions for different contacts + - Length must equal `num_contacts` + - Example: `[1, 2, 1]` for 3 contacts with varying resolutions + +- **`mu`** (vector, optional): Friction coefficients + - Length must equal `num_contacts` + - Typical values: 0.1-1.0 + - Required unless using `contact_pair_configs` + +#### Advanced Contact Configuration + +- **`contact_pair_configs`** (vector, optional): Detailed contact specifications + +When using `contact_pair_configs`, each entry specifies: + +```yaml +contact_pair_configs: + - body_A: "cube" # First body name + body_B: "platform" # Second body name + body_A_collision_geom_indices: [0] # Collision geometry indices on body A + body_B_collision_geom_indices: [0] # Collision geometry indices on body B + num_friction_directions: 1 # Friction cone edges for this pair + mu: 0.1 # Friction coefficient for this pair +``` + +**ContactPairConfig Fields:** +- `body_A` (string): Name of first body in contact pair +- `body_B` (string): Name of second body in contact pair +- `body_A_collision_geom_indices` (vector): Indices of collision geometries on body A +- `body_B_collision_geom_indices` (vector): Indices of collision geometries on body B +- `num_friction_directions` (int): Friction cone approximation for this pair +- `mu` (float): Friction coefficient for this pair + +### Configuration Examples + +#### Example 1: Simple Frictionless Contact + +```yaml +# Simple spring-based contact (e.g., cartpole with soft walls) +contact_model: "frictionless_spring" +num_contacts: 2 +spring_stiffness: 100 +N: 5 +dt: 0.01 +``` + +#### Example 2: Uniform Friction Contacts + +```yaml +# Multiple contacts with same friction properties +contact_model: "stewart_and_trinkle" +num_contacts: 3 +num_friction_directions: 1 +mu: [0.1, 0.1, 0.1] +spring_stiffness: 0 +N: 10 +dt: 0.01 +``` + +#### Example 3: Heterogeneous Contact Configuration + +```yaml +# Cube manipulation with different contact properties +contact_model: "stewart_and_trinkle" +num_contacts: 3 +spring_stiffness: 0 +N: 10 +dt: 0.01 + +contact_pair_configs: + - body_A: "cube" + body_A_collision_geom_indices: [0] + body_B: "left_finger" + body_B_collision_geom_indices: [0] + mu: 0.1 + num_friction_directions: 1 + + - body_A: "cube" + body_A_collision_geom_indices: [0] + body_B: "right_finger" + body_B_collision_geom_indices: [0] + mu: 0.1 + num_friction_directions: 1 + + - body_A: "cube" + body_A_collision_geom_indices: [0] + body_B: "platform" + body_B_collision_geom_indices: [0] + mu: 0.5 # Higher friction with platform + num_friction_directions: 2 # Finer friction cone approximation +``` + +#### Example 4: Anitescu Model + +```yaml +# Compact friction formulation +contact_model: "anitescu" +num_contacts: 2 +num_friction_directions: 2 +mu: [0.2, 0.3] +spring_stiffness: 0 +N: 8 +dt: 0.02 +``` + +### Loading Options from YAML + +In C++: +```cpp +#include "multibody/lcs_factory_options.h" + +// Load from file +auto options = c3::LoadLCSFactoryOptions("path/to/config.yaml"); + +// Use with LCSFactory +c3::multibody::LCSFactory factory(plant, context, plant_ad, context_ad, options); +factory.UpdateStateAndInput(state, input); +auto lcs = factory.GenerateLCS(); +``` + +In Python: +```python +from pydrake.all import yaml_load_typed +from pyc3 import LCSFactoryOptions + +# Create options from yaml +options = yaml_load_typed("path/to/config.yaml", schema=LCSFactoryOptions) +``` + +## See Also + +- **Examples**: Check the examples directory for complete working examples with different contact models +- **C3 Options**: See c3_options.h for solver configuration \ No newline at end of file From 47baedc2eeb9fae011817139e9dcd79b3cb922c4 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Thu, 4 Dec 2025 18:54:25 -0500 Subject: [PATCH 11/16] coverage: improve coverage --- core/c3_options.h | 8 +- core/test/core_test.cc | 74 ++-- .../c3Plus_controller_cartpole_options.yaml | 53 +++ multibody/test/multibody_test.cc | 390 +++++++++++++----- .../lcs_factory_pivoting_options.yaml | 19 + multibody/test/resources/mesh.obj | 254 ++++++++++++ multibody/test/resources/sphere-and-mesh.sdf | 78 ++++ systems/test/systems_test.cc | 78 ++-- 8 files changed, 782 insertions(+), 172 deletions(-) create mode 100644 examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml create mode 100644 multibody/test/resources/mesh.obj create mode 100644 multibody/test/resources/sphere-and-mesh.sdf diff --git a/core/c3_options.h b/core/c3_options.h index 8fe16f5..94d21ab 100644 --- a/core/c3_options.h +++ b/core/c3_options.h @@ -172,12 +172,12 @@ struct C3Options { 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_vector.insert(u_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()); + 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 { - g_vector.insert(g_vector.end(), u_eta->begin(), u_eta->end()); + u_vector.insert(u_vector.end(), u_eta->begin(), u_eta->end()); } } diff --git a/core/test/core_test.cc b/core/test/core_test.cc index 5e1fc76..18815c4 100644 --- a/core/test/core_test.cc +++ b/core/test/core_test.cc @@ -223,40 +223,6 @@ TEST_F(C3CartpoleTest, UpdateCostMatrix) { } } -// Test if user can update the LCS for the C3 problem -TEST_F(C3CartpoleTest, UpdateLCSTest) { - std::vector - pre_dynamic_constraints = pOpt->GetDynamicConstraints(); - vector pre_Al(N); - for (int i = 0; i < N; ++i) { - pre_Al[i] = pre_dynamic_constraints[i]->GetDenseA(); - } - - // Dynamics - vector An(N, MatrixXd::Identity(n, n)); - vector Bn(N, MatrixXd::Identity(n, k)); - vector Dn(N, MatrixXd::Identity(n, m)); - vector dn(N, VectorXd::Ones(n)); - - // Complimentary constraints - vector En(N, MatrixXd::Identity(m, n)); - vector Fn(N, MatrixXd::Identity(m, m)); - vector cn(N, VectorXd::Ones(m)); - vector Hn(N, MatrixXd::Identity(m, k)); - - LCS TestSystem(An, Bn, Dn, dn, En, Fn, Hn, cn, dt); - - pOpt->UpdateLCS(TestSystem); - - std::vector - pst_dynamic_constraints = pOpt->GetDynamicConstraints(); - for (int i = 0; i < N; ++i) { - // Linear Equality A matrix should be updated - MatrixXd pst_Al = pst_dynamic_constraints[i]->GetDenseA(); - EXPECT_EQ(pre_Al[i].isApprox(pst_Al), false); - } -} - class C3CartpoleTestParameterizedScalingLCSTest : public C3CartpoleTest, public ::testing::WithParamInterface> {}; @@ -368,6 +334,46 @@ class C3CartpoleTypedTest : public testing::Test, public C3CartpoleProblem { using projection_types = ::testing::Types; TYPED_TEST_SUITE(C3CartpoleTypedTest, projection_types); +// Test if user can update the LCS for the C3 problem +TYPED_TEST(C3CartpoleTypedTest, UpdateLCSTest) { + c3::C3* pOpt = this->pOpt.get(); + auto dt = this->dt; + std::vector + pre_dynamic_constraints = pOpt->GetDynamicConstraints(); + auto& N = this->N; + auto n = this->n; + auto k = this->k; + auto m = this->m; + vector pre_Al(N); + for (int i = 0; i < N; ++i) { + pre_Al[i] = pre_dynamic_constraints[i]->GetDenseA(); + } + + // Dynamics + vector An(N, MatrixXd::Identity(n, n)); + vector Bn(N, MatrixXd::Identity(n, k)); + vector Dn(N, MatrixXd::Identity(n, m)); + vector dn(N, VectorXd::Ones(n)); + + // Complimentary constraints + vector En(N, MatrixXd::Identity(m, n)); + vector Fn(N, MatrixXd::Identity(m, m)); + vector cn(N, VectorXd::Ones(m)); + vector Hn(N, MatrixXd::Identity(m, k)); + + LCS TestSystem(An, Bn, Dn, dn, En, Fn, Hn, cn, dt); + + pOpt->UpdateLCS(TestSystem); + + std::vector + pst_dynamic_constraints = pOpt->GetDynamicConstraints(); + for (int i = 0; i < N; ++i) { + // Linear Equality A matrix should be updated + MatrixXd pst_Al = pst_dynamic_constraints[i]->GetDenseA(); + EXPECT_EQ(pre_Al[i].isApprox(pst_Al), false); + } +} + // Test the cartpole example // This test will take some time to complete ~30s TYPED_TEST(C3CartpoleTypedTest, End2EndCartpoleTest) { diff --git a/examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml b/examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml new file mode 100644 index 0000000..e6b4972 --- /dev/null +++ b/examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml @@ -0,0 +1,53 @@ +projection_type: "C3+" + +solve_time_filter_alpha: 0.0 +#set to 0 to publish as fast as possible +publish_frequency: 100 +state_prediction_joints: [] + # - name : "CartSlider" + # max_acceleration : 10 + +lcs_factory_options: + #options are 'stewart_and_trinkle' or 'anitescu' + # contact_model : 'stewart_and_trinkle' + contact_model: "frictionless_spring" + num_friction_directions: 0 + num_contacts: 2 + spring_stiffness: 100 + mu: [0, 0] + N: 5 + dt: 0.01 + +c3_options: + warm_start: false + end_on_qp_step: true + scale_lcs: false + + num_threads: 5 + delta_option: 0 + + M: 1000 + admm_iter: 10 + + gamma: 1.0 + rho_scale: 2 #matrix scaling + w_Q: 1 + w_R: 1 #Penalty on all decision variables, assuming scalar + w_G: 1 #Penalty on all decision variables, assuming scalar + w_U: 1 #State Tracking Error, assuming diagonal + q_vector: [10, 2, 1, 1] #Penalty on efforts, assuming diagonal + r_vector: [1] #Penalty on matching projected variables + g_x: [0, 0, 0, 0] + g_gamma: [] + g_lambda_n: [] + g_lambda_t: [] + g_lambda: [1, 1] + g_eta: [1, 1] + g_u: [0] #Penalty on matching the QP variables + u_x: [0, 0, 0, 0] + u_gamma: [] + u_lambda_n: [] + u_lambda_t: [] + u_lambda: [1, 1] + u_eta: [10000, 10000] + u_u: [0] diff --git a/multibody/test/multibody_test.cc b/multibody/test/multibody_test.cc index 6efcca3..9f448b1 100644 --- a/multibody/test/multibody_test.cc +++ b/multibody/test/multibody_test.cc @@ -1,5 +1,3 @@ - - #include #include "multibody/geom_geom_collider.h" @@ -12,22 +10,23 @@ #include "drake/systems/framework/context.h" /** - * @file lcs_factory_test.cc - * @brief Tests for the LCSFactory class, which generates Linear Complementarity - * Systems (LCS) for multibody systems with contact. + * @file multibody_test.cc + * @brief Tests for multibody dynamics components including LCSFactory and + * GeomGeomCollider. * - * The tests cover various aspects of the LCSFactory, including: - * - Calculating the number of contact variables for different contact models. - * - Generating an LCS from a MultibodyPlant. - * - Linearizing a MultibodyPlant to an LCS. - * - Updating the state and input of an LCSFactory. - * - Computing the contact Jacobian. - * - Fixing modes in an LCS. + * The tests cover: + * - LCSFactory: Generating Linear Complementarity Systems (LCS) for multibody + * systems with contact, including: + * - Calculating the number of contact variables for different contact models + * - Generating and linearizing an LCS from a MultibodyPlant + * - Updating state and input + * - Computing contact Jacobians + * - Fixing modes in an LCS + * - GeomGeomCollider: Computing distances and Jacobians between geometry pairs * - * The tests use a cube pivoting example to verify the correctness of the LCS - * generation and manipulation. Different contact models (Stewart-Trinkle, - * Anitescu, and Frictionless Spring) are tested to ensure the LCSFactory works - * correctly under various conditions. + * The tests use a cube pivoting example and sphere-mesh collision scenarios + * to verify correctness. Different contact models (Stewart-Trinkle, Anitescu, + * and Frictionless Spring) are tested. */ namespace c3 { namespace multibody { @@ -46,6 +45,7 @@ using drake::systems::System; using Eigen::MatrixXd; using Eigen::VectorXd; +// Test the static method for computing number of contact variables GTEST_TEST(LCSFactoryTest, GetNumContactVariables) { // Test with different contact models and friction properties EXPECT_EQ(LCSFactory::GetNumContactVariables(ContactModel::kStewartAndTrinkle, @@ -59,7 +59,7 @@ GTEST_TEST(LCSFactoryTest, GetNumContactVariables) { EXPECT_THROW(LCSFactory::GetNumContactVariables(ContactModel::kUnknown, 3, 0), std::out_of_range); - // Test with LCSFactoryOptions + // Test with LCSFactoryOptions struct LCSFactoryOptions options; options.contact_model = "stewart_and_trinkle"; options.num_friction_directions = 4; @@ -76,184 +76,275 @@ GTEST_TEST(LCSFactoryTest, GetNumContactVariables) { options.num_contacts = 3; EXPECT_EQ(LCSFactory::GetNumContactVariables(options), 3); + // Test error handling for invalid contact model options.contact_model = "some_random_contact_model"; EXPECT_THROW(LCSFactory::GetNumContactVariables(options), std::out_of_range); } -class LCSFactoryPivotingTest - : public ::testing::TestWithParam> { - protected: - void SetUp() override { +// Helper struct to hold common test fixture data for cube pivoting scenarios +struct PivotingTestFixture { + DiagramBuilder plant_builder; + MultibodyPlant* plant; + SceneGraph* scene_graph; + std::unique_ptr> plant_autodiff; + std::unique_ptr> plant_context_autodiff; + std::unique_ptr> plant_diagram; + std::unique_ptr> plant_diagram_context; + std::vector> contact_pairs; + LCSFactoryOptions options; + std::unique_ptr lcs_factory; + ContactModel contact_model = ContactModel::kUnknown; + + // Initialize the multibody plant and LCS factory for cube pivoting tests + void Initialize() { + // Create plant and scene graph std::tie(plant, scene_graph) = AddMultibodyPlantSceneGraph(&plant_builder, 0.0); Parser parser(plant, scene_graph); parser.AddModels("examples/resources/cube_pivoting/cube_pivoting.sdf"); plant->Finalize(); - // Load controller options from YAML file. + // Load LCS factory options from YAML configuration options = drake::yaml::LoadYamlFile( "multibody/test/resources/lcs_factory_pivoting_options.yaml"); + + // Initialize friction directions per contact if not set in config + if (!options.num_friction_directions_per_contact) { + options.num_friction_directions_per_contact = std::vector( + options.num_contacts, options.num_friction_directions.value()); + } + + // Build diagram and create contexts plant_diagram = plant_builder.Build(); plant_diagram_context = plant_diagram->CreateDefaultContext(); - // Get the context for the plant within the diagram. auto& plant_context = plant_diagram->GetMutableSubsystemContext( *plant, plant_diagram_context.get()); - // Convert the plant to AutoDiffXd for automatic differentiation. + // Create autodiff version for automatic differentiation plant_autodiff = drake::systems::System::ToAutoDiffXd(*plant); plant_context_autodiff = plant_autodiff->CreateDefaultContext(); - // Retrieve collision geometries for relevant bodies. - std::vector platform_collision_geoms = + // Retrieve collision geometries for all bodies + auto platform_geoms = plant->GetCollisionGeometriesForBody(plant->GetBodyByName("platform")); - std::vector cube_collision_geoms = + auto cube_geoms = plant->GetCollisionGeometriesForBody(plant->GetBodyByName("cube")); - std::vector left_finger_collision_geoms = - plant->GetCollisionGeometriesForBody( - plant->GetBodyByName("left_finger")); - std::vector right_finger_collision_geoms = - plant->GetCollisionGeometriesForBody( - plant->GetBodyByName("right_finger")); - - // Map collision geometries to their respective components. - std::unordered_map> contact_geoms; - contact_geoms["PLATFORM"] = platform_collision_geoms; - contact_geoms["CUBE"] = cube_collision_geoms; - contact_geoms["LEFT_FINGER"] = left_finger_collision_geoms; - contact_geoms["RIGHT_FINGER"] = right_finger_collision_geoms; - - // Define contact pairs for the LCS system. - contact_pairs.emplace_back(contact_geoms["CUBE"][0], - contact_geoms["LEFT_FINGER"][0]); - contact_pairs.emplace_back(contact_geoms["CUBE"][0], - contact_geoms["PLATFORM"][0]); - contact_pairs.emplace_back(contact_geoms["CUBE"][0], - contact_geoms["RIGHT_FINGER"][0]); - - // Set initial positions, velocities, and inputs for both double and - // AutoDiffXd plants. + auto left_finger_geoms = plant->GetCollisionGeometriesForBody( + plant->GetBodyByName("left_finger")); + auto right_finger_geoms = plant->GetCollisionGeometriesForBody( + plant->GetBodyByName("right_finger")); + + // Define contact pairs between cube and other bodies + contact_pairs.emplace_back(cube_geoms[0], left_finger_geoms[0]); + contact_pairs.emplace_back(cube_geoms[0], platform_geoms[0]); + contact_pairs.emplace_back(cube_geoms[0], right_finger_geoms[0]); + + // Initialize state and input vectors to zero drake::VectorX state = VectorXd::Zero(plant->num_positions() + plant->num_velocities()); drake::VectorX input = VectorXd::Zero(plant->num_actuators()); - options.contact_model = std::get<0>(GetParam()); - contact_model = GetContactModelMap().at(options.contact_model); - options.num_friction_directions_per_contact = - std::vector(contact_pairs.size(), std::get<1>(GetParam())); + // Create LCS factory with contact pairs lcs_factory = std::make_unique( *plant, plant_context, *plant_autodiff, *plant_context_autodiff, contact_pairs, options); lcs_factory->UpdateStateAndInput(state, input); } +}; - DiagramBuilder plant_builder; - MultibodyPlant* plant; - SceneGraph* scene_graph; - std::unique_ptr> plant_autodiff; - std::unique_ptr> plant_context_autodiff; - std::unique_ptr> plant_diagram; - std::unique_ptr> plant_diagram_context; - std::vector> contact_pairs; - LCSFactoryOptions options; - std::unique_ptr lcs_factory; - ContactModel contact_model = ContactModel::kUnknown; +// Test fixture for non-parameterized LCS factory tests +class LCSFactoryPivotingTest : public testing::Test { + protected: + void SetUp() override { fixture.Initialize(); } + + PivotingTestFixture fixture; +}; + +// Test that contact pairs can be parsed from options instead of explicit list +TEST_F(LCSFactoryPivotingTest, ContactPairParsing) { + std::unique_ptr contact_pair_parsed_factory; + + // Create factory without explicit contact pairs (parsed from options) + EXPECT_NO_THROW({ + contact_pair_parsed_factory = std::make_unique( + *fixture.plant, + fixture.plant_diagram->GetMutableSubsystemContext( + *fixture.plant, fixture.plant_diagram_context.get()), + *fixture.plant_autodiff, *fixture.plant_context_autodiff, + fixture.options); + }); + EXPECT_NE(contact_pair_parsed_factory.get(), nullptr); + + // Update factory with zero state and input + drake::VectorX state = VectorXd::Zero( + fixture.plant->num_positions() + fixture.plant->num_velocities()); + drake::VectorX input = VectorXd::Zero(fixture.plant->num_actuators()); + contact_pair_parsed_factory->UpdateStateAndInput(state, input); + + // Generate LCS and verify dimensions + LCS lcs = contact_pair_parsed_factory->GenerateLCS(); + + EXPECT_EQ(lcs.num_states(), + fixture.plant->num_positions() + fixture.plant->num_velocities()); + EXPECT_EQ(lcs.num_inputs(), fixture.plant->num_actuators()); + EXPECT_EQ(lcs.num_lambdas(), + LCSFactory::GetNumContactVariables(fixture.options)); +} + +// Parameterized test fixture for testing different contact models and friction +// directions +class LCSFactoryParameterizedPivotingTest + : public ::testing::TestWithParam> { + protected: + void SetUp() override { + fixture.Initialize(); + + // Initialize state and input to zero + drake::VectorX state = VectorXd::Zero( + fixture.plant->num_positions() + fixture.plant->num_velocities()); + drake::VectorX input = + VectorXd::Zero(fixture.plant->num_actuators()); + + // Set contact model and friction directions from test parameters + fixture.options.contact_model = std::get<0>(GetParam()); + fixture.contact_model = + GetContactModelMap().at(fixture.options.contact_model); + fixture.options.num_friction_directions_per_contact = + std::vector(fixture.contact_pairs.size(), std::get<1>(GetParam())); + + // Create factory with parameterized options + fixture.lcs_factory = std::make_unique( + *fixture.plant, + fixture.plant_diagram->GetMutableSubsystemContext( + *fixture.plant, fixture.plant_diagram_context.get()), + *fixture.plant_autodiff, *fixture.plant_context_autodiff, + fixture.contact_pairs, fixture.options); + fixture.lcs_factory->UpdateStateAndInput(state, input); + } + + PivotingTestFixture fixture; }; -TEST_P(LCSFactoryPivotingTest, GenerateLCS) { - LCS lcs = lcs_factory->GenerateLCS(); +// Test LCS generation for different contact models +TEST_P(LCSFactoryParameterizedPivotingTest, GenerateLCS) { + LCS lcs = fixture.lcs_factory->GenerateLCS(); - EXPECT_EQ(lcs.num_states(), plant->num_positions() + plant->num_velocities()); - EXPECT_EQ(lcs.num_inputs(), plant->num_actuators()); - EXPECT_EQ(lcs.num_lambdas(), LCSFactory::GetNumContactVariables(options)); + // Verify LCS dimensions match plant configuration + EXPECT_EQ(lcs.num_states(), + fixture.plant->num_positions() + fixture.plant->num_velocities()); + EXPECT_EQ(lcs.num_inputs(), fixture.plant->num_actuators()); + EXPECT_EQ(lcs.num_lambdas(), + LCSFactory::GetNumContactVariables(fixture.options)); } -TEST_P(LCSFactoryPivotingTest, LinearizePlantToLCS) { - // Get the context for the plant within the diagram. - auto& plant_context = plant_diagram->GetMutableSubsystemContext( - *plant, plant_diagram_context.get()); +// Test static linearization method for different contact models +TEST_P(LCSFactoryParameterizedPivotingTest, LinearizePlantToLCS) { + auto& plant_context = fixture.plant_diagram->GetMutableSubsystemContext( + *fixture.plant, fixture.plant_diagram_context.get()); - drake::VectorX state = - VectorXd::Zero(plant->num_positions() + plant->num_velocities()); - drake::VectorX input = VectorXd::Zero(plant->num_actuators()); + drake::VectorX state = VectorXd::Zero( + fixture.plant->num_positions() + fixture.plant->num_velocities()); + drake::VectorX input = VectorXd::Zero(fixture.plant->num_actuators()); + // Use static method to linearize plant directly to LCS LCS lcs = LCSFactory::LinearizePlantToLCS( - *plant, plant_context, *plant_autodiff, *plant_context_autodiff, - contact_pairs, options, state, input); - - EXPECT_EQ(lcs.num_states(), plant->num_positions() + plant->num_velocities()); - EXPECT_EQ(lcs.num_inputs(), plant->num_actuators()); - EXPECT_EQ(lcs.num_lambdas(), LCSFactory::GetNumContactVariables(options)); + *fixture.plant, plant_context, *fixture.plant_autodiff, + *fixture.plant_context_autodiff, fixture.contact_pairs, fixture.options, + state, input); + + // Verify linearized LCS dimensions + EXPECT_EQ(lcs.num_states(), + fixture.plant->num_positions() + fixture.plant->num_velocities()); + EXPECT_EQ(lcs.num_inputs(), fixture.plant->num_actuators()); + EXPECT_EQ(lcs.num_lambdas(), + LCSFactory::GetNumContactVariables(fixture.options)); } -TEST_P(LCSFactoryPivotingTest, UpdateStateAndInput) { - LCS initial_lcs = lcs_factory->GenerateLCS(); +// Test that updating state and input changes contact-dependent LCS matrices +TEST_P(LCSFactoryParameterizedPivotingTest, UpdateStateAndInput) { + // Generate initial LCS at zero state + LCS initial_lcs = fixture.lcs_factory->GenerateLCS(); auto [initial_J, initial_contact_points] = - lcs_factory->GetContactJacobianAndPoints(); + fixture.lcs_factory->GetContactJacobianAndPoints(); - drake::VectorX state = - VectorXd::Zero(plant->num_positions() + plant->num_velocities()); + // Update to non-zero state (cube tilted and positioned above platform) + drake::VectorX state = VectorXd::Zero( + fixture.plant->num_positions() + fixture.plant->num_velocities()); state << 0, 0.75, 0.785, -0.5, 0.5, 0.5, 0.5, 0, 0, 0, 0, 0, 0, 0; - drake::VectorX input = VectorXd::Zero(plant->num_actuators()); + drake::VectorX input = VectorXd::Zero(fixture.plant->num_actuators()); input << 0, 0, 0, 0; - // Update the LCS factory with the state and input. - lcs_factory->UpdateStateAndInput(state, input); + fixture.lcs_factory->UpdateStateAndInput(state, input); - LCS updated_lcs = lcs_factory->GenerateLCS(); + // Generate updated LCS + LCS updated_lcs = fixture.lcs_factory->GenerateLCS(); auto [updated_J, updated_contact_points] = - lcs_factory->GetContactJacobianAndPoints(); + fixture.lcs_factory->GetContactJacobianAndPoints(); + // Dynamics matrices should remain the same (linearized at same point) EXPECT_EQ(initial_lcs.A(), updated_lcs.A()); EXPECT_EQ(initial_lcs.B(), updated_lcs.B()); EXPECT_EQ(initial_lcs.d(), updated_lcs.d()); + // Contact-dependent matrices should change due to different configuration EXPECT_NE(initial_lcs.D(), updated_lcs.D()); - // Except for frictionless spring, the contact model would generate different - // matrices - if (contact_model != ContactModel::kFrictionlessSpring) { + if (fixture.contact_model != ContactModel::kFrictionlessSpring) { EXPECT_NE(initial_lcs.E(), updated_lcs.E()); EXPECT_NE(initial_lcs.F(), updated_lcs.F()); EXPECT_NE(initial_lcs.H(), updated_lcs.H()); EXPECT_NE(initial_lcs.c(), updated_lcs.c()); } + // Contact Jacobian and points should change EXPECT_NE(initial_J, updated_J); for (size_t i = 0; i < initial_contact_points.size(); ++i) { EXPECT_NE(initial_contact_points[i], updated_contact_points[i]); } } -TEST_P(LCSFactoryPivotingTest, ComputeContactJacobian) { - auto [J, contact_points] = lcs_factory->GetContactJacobianAndPoints(); +// Test contact Jacobian computation for different contact models +TEST_P(LCSFactoryParameterizedPivotingTest, ComputeContactJacobian) { + auto [J, contact_points] = fixture.lcs_factory->GetContactJacobianAndPoints(); - int n_contacts = contact_pairs.size(); + int n_contacts = fixture.contact_pairs.size(); auto n_tangential_directions = - 2 * std::accumulate(options.num_friction_directions_per_contact->begin(), - options.num_friction_directions_per_contact->end(), - 0); - // Check for number of force variables (not including slack variables) - switch (contact_model) { + 2 * std::accumulate( + fixture.options.num_friction_directions_per_contact->begin(), + fixture.options.num_friction_directions_per_contact->end(), 0); + + // Verify Jacobian rows based on contact model + switch (fixture.contact_model) { case ContactModel::kStewartAndTrinkle: + // Normal + tangential directions for all contacts EXPECT_EQ(J.rows(), n_contacts + n_tangential_directions); break; case ContactModel::kFrictionlessSpring: + // Normal directions only EXPECT_EQ(J.rows(), n_contacts); break; case ContactModel::kAnitescu: + // Tangential directions only (normal handled differently) EXPECT_EQ(J.rows(), n_tangential_directions); break; default: - EXPECT_TRUE(false); // Something went wrong in parsing the contact model + EXPECT_TRUE(false); } - EXPECT_EQ(J.cols(), plant->num_velocities()); + + // Jacobian should map velocities to contact space + EXPECT_EQ(J.cols(), fixture.plant->num_velocities()); EXPECT_EQ(contact_points.size(), n_contacts); } -TEST_P(LCSFactoryPivotingTest, FixSomeModes) { - LCS lcs = lcs_factory->GenerateLCS(); +// Test fixing specific contact modes in the LCS +TEST_P(LCSFactoryParameterizedPivotingTest, FixSomeModes) { + LCS lcs = fixture.lcs_factory->GenerateLCS(); + + // Fix modes at indices 0 and 1 (removes 2 lambda variables) LCS new_lcs = LCSFactory::FixSomeModes(lcs, {0}, {1}); - int updated_num_lambda = lcs.num_lambdas() - 2; // 1 active, 1 inactive + int updated_num_lambda = lcs.num_lambdas() - 2; + + // Verify all contact-related matrices have reduced dimensions EXPECT_EQ(new_lcs.D()[0].cols(), updated_num_lambda); EXPECT_EQ(new_lcs.E()[0].rows(), updated_num_lambda); EXPECT_EQ(new_lcs.F()[0].cols(), updated_num_lambda); @@ -262,13 +353,90 @@ TEST_P(LCSFactoryPivotingTest, FixSomeModes) { EXPECT_EQ(new_lcs.c()[0].rows(), updated_num_lambda); } -INSTANTIATE_TEST_SUITE_P(ContactModelTests, LCSFactoryPivotingTest, +// Instantiate parameterized tests with different contact models and friction +// directions +INSTANTIATE_TEST_SUITE_P(ContactModelTests, LCSFactoryParameterizedPivotingTest, ::testing::Values(std::tuple("frictionless_spring", 0), std::tuple("stewart_and_trinkle", 1), std::tuple("stewart_and_trinkle", 2), std::tuple("anitescu", 1), std::tuple("anitescu", 2))); +// Test distance computation between sphere and mesh geometries +GTEST_TEST(GeomGeomColliderTest, SphereMeshDistance) { + auto MESH_HEIGHT = 0.015; // Approximate height of mesh above z=0 plane + auto SPHERE_RADIUS = 0.01; + auto NO_CONTACT_HEIGHT = 0.1; // Height where sphere is not in contact + + // Setup plant with sphere and C-shaped mesh + DiagramBuilder plant_builder; + auto [plant, scene_graph] = AddMultibodyPlantSceneGraph(&plant_builder, 0.0); + Parser parser(&plant, &scene_graph); + parser.AddModels("multibody/test/resources/sphere-and-mesh.sdf"); + + // Position sphere well above mesh (no contact) + plant.SetDefaultFreeBodyPose( + plant.GetBodyByName("sphere"), + drake::math::RigidTransformd( + Eigen::Quaterniond::Identity(), + Eigen::Vector3d(0.0, 0.0, NO_CONTACT_HEIGHT))); + plant.Finalize(); + + auto diagram = plant_builder.Build(); + auto diagram_context = diagram->CreateDefaultContext(); + auto& context = + diagram->GetMutableSubsystemContext(plant, diagram_context.get()); + + // Get collision geometries for sphere and mesh + auto sphere_geoms = + plant.GetCollisionGeometriesForBody(plant.GetBodyByName("sphere")); + auto mesh_geoms = + plant.GetCollisionGeometriesForBody(plant.GetBodyByName("mesh")); + + ASSERT_FALSE(sphere_geoms.empty()); + ASSERT_FALSE(mesh_geoms.empty()); + + auto geom_pair = SortedPair(sphere_geoms[0], mesh_geoms[0]); + + // Test sphere not in contact (positive distance) + GeomGeomCollider collider(plant, geom_pair); + auto [distance_no_contact, J] = collider.EvalPolytope(context, 1); + +<<<<<<< Updated upstream + EXPECT_NEAR( + distance_no_contact, + NO_CONTACT_HEIGHT - MESH_HEIGHT - SPHERE_RADIUS, 1e-2); // Expected distance + EXPECT_EQ(J.rows(), 3); +======= + EXPECT_NEAR(distance_no_contact, + NO_CONTACT_HEIGHT - MESH_HEIGHT - SPHERE_RADIUS, + 1e-2); // Expected clearance distance + EXPECT_EQ(J.rows(), 3); // 3D contact Jacobian +>>>>>>> Stashed changes + EXPECT_EQ(J.cols(), plant.num_velocities()); + EXPECT_TRUE(J.allFinite()); + + // Test sphere in contact (sphere center at radius height) + plant.SetFreeBodyPose( + &context, plant.GetBodyByName("sphere"), + drake::math::RigidTransformd(Eigen::Quaterniond::Identity(), + Eigen::Vector3d(0.0, 0.0, SPHERE_RADIUS))); + + GeomGeomCollider collider_contact(plant, geom_pair); + auto [distance_contact, J_contact] = + collider_contact.EvalPolytope(context, 1); + +<<<<<<< Updated upstream + EXPECT_NEAR(distance_contact, -MESH_HEIGHT, 1e-2); // Expect penetration or zero distance +======= + EXPECT_NEAR(distance_contact, -MESH_HEIGHT, + 1e-2); // Negative distance indicates penetration +>>>>>>> Stashed changes + EXPECT_EQ(J_contact.rows(), 3); + EXPECT_EQ(J_contact.cols(), plant.num_velocities()); + EXPECT_TRUE(J_contact.allFinite()); +} + } // namespace test } // namespace multibody } // namespace c3 \ No newline at end of file diff --git a/multibody/test/resources/lcs_factory_pivoting_options.yaml b/multibody/test/resources/lcs_factory_pivoting_options.yaml index 766a033..5b8f128 100644 --- a/multibody/test/resources/lcs_factory_pivoting_options.yaml +++ b/multibody/test/resources/lcs_factory_pivoting_options.yaml @@ -5,3 +5,22 @@ spring_stiffness: 0 mu: [0.1, 0.1, 0.1] N: 10 dt: 0.01 +contact_pair_configs: # Not used but kept to test parsing + - body_A: "cube" + body_A_collision_geom_indices: [0] + body_B: "left_finger" + body_B_collision_geom_indices: [0] + mu: 0.1 + num_friction_directions: 1 + - body_A: "cube" + body_A_collision_geom_indices: [0] + body_B: "right_finger" + body_B_collision_geom_indices: [0] + mu: 0.1 + num_friction_directions: 1 + - body_A: "cube" + body_A_collision_geom_indices: [0] + body_B: "platform" + body_B_collision_geom_indices: [0] + mu: 0.1 + num_friction_directions: 1 diff --git a/multibody/test/resources/mesh.obj b/multibody/test/resources/mesh.obj new file mode 100644 index 0000000..acb3eab --- /dev/null +++ b/multibody/test/resources/mesh.obj @@ -0,0 +1,254 @@ +# https://github.com/mikedh/trimesh +v 0.06519866 0.02141543 0.01714994 +v 0.07274961 -0.00734628 0.01162284 +v 0.07172014 0.02114829 -0.00741718 +v 0.06128203 0.01611072 -0.01577658 +v 0.05313613 -0.01479354 0.01276985 +v 0.06910122 -0.01397309 -0.01450029 +v 0.05475922 0.02157423 0.01076651 +v 0.07175731 0.01900272 0.01342541 +v 0.06586316 -0.01384402 0.01697034 +v 0.07406599 -0.00320264 -0.00757992 +v 0.06938140 -0.00323612 -0.01553922 +v 0.05306509 -0.01477510 -0.01150585 +v 0.05547499 0.02207334 0.01461318 +v 0.06543886 0.02150060 -0.00758191 +v 0.07160662 0.02111678 0.01156906 +v 0.07174875 0.00209975 0.01629878 +v 0.07068870 -0.01372951 0.01255670 +v 0.07393898 0.00941963 0.01064069 +v 0.06481122 0.00515537 -0.01640418 +v 0.06932339 -0.00959835 -0.01499343 +v 0.07388845 0.01258756 -0.00738789 +v 0.05512646 0.01675363 -0.00898146 +v 0.05180865 -0.01470985 -0.00006813 +v 0.05925437 -0.01446938 0.01740083 +v 0.06884903 0.02109069 0.01592420 +v 0.07023885 -0.00513966 0.01702776 +v 0.07196230 -0.01384041 -0.00758209 +v 0.07402152 -0.00214073 0.00848970 +v 0.07282597 -0.00332095 0.01464406 +v 0.07275805 0.01056275 0.01465904 +v 0.07269802 0.01693826 0.01059463 +v 0.05934093 -0.01444015 -0.01635044 +v 0.07396351 0.01273832 0.00430948 +v 0.05889974 0.01653078 -0.01456530 +v 0.05183209 -0.01461544 -0.00944174 +v 0.05735817 0.02186266 0.01629871 +v 0.06802781 -0.01384247 0.01592390 +v 0.07054385 0.02103128 0.01439799 +v 0.06813600 0.01800577 0.01713327 +v 0.07173503 -0.01280988 0.00848986 +v 0.07279629 -0.01056807 -0.00747383 +v 0.07388782 0.00428740 0.01162990 +v 0.07164522 -0.00778705 0.01511473 +v 0.07270535 0.01480824 0.01261721 +v 0.07196899 0.00493236 0.01607566 +v 0.06505701 -0.01411943 -0.01618707 +v 0.07281297 0.01905305 0.00641215 +v 0.05707044 -0.01439157 -0.01546418 +v 0.05971172 0.01647743 -0.01508571 +v 0.06126090 0.02162319 0.01724260 +v 0.05408845 -0.01457373 0.01461682 +v 0.07171309 0.01702678 0.01450573 +v 0.07022066 0.00625902 0.01697505 +v 0.07162112 -0.01157474 0.01149340 +v 0.07168993 -0.01375950 0.00209073 +v 0.07281261 -0.00835625 0.00973872 +v 0.07281933 -0.01065109 -0.00331026 +v 0.07395278 0.00104269 0.01064494 +v 0.07389382 0.00843565 0.01165083 +v 0.07270155 -0.00527522 0.01372881 +v 0.06956554 -0.01258597 0.01538250 +v 0.07093295 -0.00738439 0.01600034 +v 0.07093729 0.01265149 0.01607118 +v 0.06797937 -0.01392691 -0.01518426 +vn 0.11695968 0.69546876 0.70897365 +vn 0.95881934 -0.20954529 0.19171916 +vn 0.59754568 0.70503999 -0.38191330 +vn 0.03718511 0.42767918 -0.90316543 +vn -0.70599122 -0.67203365 0.22348864 +vn 0.55468441 -0.68017280 -0.47926002 +vn -0.75969591 0.65010028 -0.01522335 +vn 0.90892791 0.24811587 0.33509488 +vn 0.19018416 -0.61531605 0.76499421 +vn 0.96679320 -0.06384799 -0.24745573 +vn 0.57647001 0.01429083 -0.81699333 +vn -0.58469394 -0.66646121 -0.46256074 +vn -0.63470539 0.68166081 0.36398847 +vn -0.08682475 0.95195924 -0.29365807 +vn 0.64294440 0.75480187 0.12998707 +vn 0.63476156 -0.00987629 0.77264496 +vn 0.61157300 -0.76048960 0.21825224 +vn 0.99169442 0.08651209 0.09517266 +vn 0.24316202 0.11712185 -0.96288873 +vn 0.67324847 -0.08895800 -0.73404562 +vn 0.92114919 0.14070696 -0.36288528 +vn -0.79884175 0.51229105 -0.31529311 +vn -0.76034791 -0.64886587 0.02905422 +vn -0.13758075 -0.65687062 0.74134508 +vn 0.40551398 0.69561462 0.59302505 +vn 0.40147036 -0.07408010 0.91287112 +vn 0.66567024 -0.73482965 -0.13003277 +vn 0.99327303 -0.09184323 0.07052312 +vn 0.88618116 -0.10128743 0.45213251 +vn 0.85190507 0.09868098 0.51431491 +vn 0.97175169 0.18489714 0.14666871 +vn -0.11613439 -0.65941025 -0.74275900 +vn 0.99473237 0.10038779 0.02073150 +vn -0.57214895 0.52691630 -0.62849406 +vn -0.80371889 -0.55557036 -0.21302000 +vn -0.31893759 0.64393285 0.69543461 +vn 0.37308349 -0.75175050 0.54376456 +vn 0.56233224 0.71148504 0.42139233 +vn 0.32476979 0.11521381 0.93874936 +vn 0.90883458 -0.40769478 0.08834409 +vn 0.95973650 -0.19840555 -0.19884941 +vn 0.97967464 -0.00743069 0.20045544 +vn 0.83615512 -0.23559539 0.49531751 +vn 0.95931849 0.15992693 0.23266158 +vn 0.69724222 0.02668999 0.71633856 +vn 0.14899926 -0.66417381 -0.73257926 +vn 0.94701243 0.31881364 0.03905525 +vn -0.44651292 -0.55755657 -0.69982633 +vn -0.34104427 0.53129856 -0.77550670 +vn -0.04238508 0.66522941 0.74543500 +vn -0.56704692 -0.57825572 0.58658171 +vn 0.80278770 0.17696901 0.56939782 +vn 0.30798546 0.03126921 0.95087706 +vn 0.93737334 -0.28042443 0.20662371 +vn 0.62921146 -0.77669407 0.02897010 +vn 0.96994274 -0.22129445 0.10119213 +vn 0.97383971 -0.22663541 0.01651072 +vn 0.99135580 -0.03589682 0.12619470 +vn 0.97827784 0.04309952 0.20276807 +vn 0.95242839 -0.18742133 0.24031940 +vn 0.65892176 -0.37747413 0.65064229 +vn 0.63438170 -0.19459194 0.74812689 +vn 0.58973336 0.10486127 0.80076131 +vn 0.35110351 -0.55582204 -0.75351721 +f 13//13 7//7 5//5 +f 14//14 3//3 4//4 +f 14//14 13//13 3//3 +f 14//14 7//7 13//13 +f 15//15 13//13 1//1 +f 15//15 3//3 13//13 +f 19//19 4//4 3//3 +f 19//19 3//3 11//11 +f 20//20 11//11 10//10 +f 20//20 10//10 6//6 +f 21//21 11//11 3//3 +f 21//21 10//10 11//11 +f 22//22 7//7 14//14 +f 23//23 12//12 5//5 +f 23//23 5//5 7//7 +f 26//26 24//24 9//9 +f 27//27 17//17 24//24 +f 27//27 24//24 5//5 +f 27//27 12//12 6//6 +f 27//27 5//5 12//12 +f 29//29 16//16 26//26 +f 31//31 15//15 8//8 +f 32//32 4//4 19//19 +f 32//32 6//6 12//12 +f 33//33 10//10 21//21 +f 33//33 28//28 10//10 +f 33//33 18//18 28//28 +f 34//34 12//12 22//22 +f 34//34 22//22 14//14 +f 35//35 22//22 12//12 +f 35//35 12//12 23//23 +f 35//35 23//23 7//7 +f 35//35 7//7 22//22 +f 37//37 24//24 17//17 +f 37//37 9//9 24//24 +f 37//37 26//26 9//9 +f 38//38 15//15 1//1 +f 38//38 1//1 25//25 +f 38//38 8//8 15//15 +f 39//39 25//25 1//1 +f 39//39 24//24 26//26 +f 41//41 27//27 6//6 +f 41//41 6//6 10//10 +f 44//44 8//8 30//30 +f 44//44 31//31 8//8 +f 44//44 18//18 31//31 +f 45//45 16//16 29//29 +f 45//45 29//29 30//30 +f 46//46 6//6 32//32 +f 46//46 32//32 19//19 +f 46//46 19//19 11//11 +f 47//47 31//31 18//18 +f 47//47 18//18 33//33 +f 47//47 15//15 31//31 +f 47//47 3//3 15//15 +f 47//47 33//33 21//21 +f 47//47 21//21 3//3 +f 48//48 32//32 12//12 +f 48//48 12//12 34//34 +f 48//48 4//4 32//32 +f 49//49 34//34 14//14 +f 49//49 14//14 4//4 +f 49//49 48//48 34//34 +f 49//49 4//4 48//48 +f 50//50 36//36 24//24 +f 50//50 1//1 13//13 +f 50//50 13//13 36//36 +f 50//50 39//39 1//1 +f 50//50 24//24 39//39 +f 51//51 36//36 13//13 +f 51//51 13//13 5//5 +f 51//51 5//5 24//24 +f 51//51 24//24 36//36 +f 52//52 30//30 8//8 +f 52//52 8//8 38//38 +f 52//52 38//38 25//25 +f 53//53 39//39 26//26 +f 53//53 26//26 16//16 +f 53//53 16//16 45//45 +f 54//54 17//17 40//40 +f 54//54 43//43 17//17 +f 54//54 2//2 43//43 +f 55//55 40//40 17//17 +f 55//55 17//17 27//27 +f 56//56 28//28 2//2 +f 56//56 54//54 40//40 +f 56//56 2//2 54//54 +f 57//57 41//41 10//10 +f 57//57 10//10 28//28 +f 57//57 28//28 56//56 +f 57//57 56//56 40//40 +f 57//57 40//40 55//55 +f 57//57 55//55 27//27 +f 57//57 27//27 41//41 +f 58//58 42//42 29//29 +f 58//58 29//29 28//28 +f 58//58 28//28 18//18 +f 59//59 30//30 29//29 +f 59//59 29//29 42//42 +f 59//59 44//44 30//30 +f 59//59 18//18 44//44 +f 59//59 58//58 18//18 +f 59//59 42//42 58//58 +f 60//60 43//43 2//2 +f 60//60 29//29 43//43 +f 60//60 2//2 28//28 +f 60//60 28//28 29//29 +f 61//61 37//37 17//17 +f 61//61 17//17 43//43 +f 61//61 26//26 37//37 +f 62//62 43//43 29//29 +f 62//62 29//29 26//26 +f 62//62 61//61 43//43 +f 62//62 26//26 61//61 +f 63//63 45//45 30//30 +f 63//63 53//53 45//45 +f 63//63 39//39 53//53 +f 63//63 30//30 52//52 +f 63//63 52//52 25//25 +f 63//63 25//25 39//39 +f 64//64 20//20 6//6 +f 64//64 6//6 46//46 +f 64//64 46//46 11//11 +f 64//64 11//11 20//20 + diff --git a/multibody/test/resources/sphere-and-mesh.sdf b/multibody/test/resources/sphere-and-mesh.sdf new file mode 100644 index 0000000..f49a085 --- /dev/null +++ b/multibody/test/resources/sphere-and-mesh.sdf @@ -0,0 +1,78 @@ + + + + + + 0 0 0.0 0 0 0 + + + 1.0 + + 0.004 + 0.004 + 0.004 + 0.0 + 0.0 + 0.0 + + + + + + 0.01 + + + + + + + 0.01 + + + + 1 0 0 1 + + + + + + + + -0.06 0 0.0 0 0 0 + + + 0 0 0 0 0 0 + 1.0 + + 0.003 + 0.003 + 0.006 + 0.0 + 0.0 + 0.0 + + + + + 0 0 0 0 0 0 + + + mesh.obj + + + + 0 0 0 1 + + + + 0 0 0 0 0 0 + + + mesh.obj + + + + + + + \ No newline at end of file diff --git a/systems/test/systems_test.cc b/systems/test/systems_test.cc index 140225a..99d71b3 100644 --- a/systems/test/systems_test.cc +++ b/systems/test/systems_test.cc @@ -5,6 +5,9 @@ #include #include +#include "core/c3_miqp.h" +#include "core/c3_plus.h" +#include "core/c3_qp.h" #include "core/test/c3_cartpole_problem.hpp" #include "multibody/lcs_factory.h" #include "systems/c3_controller.h" @@ -92,25 +95,33 @@ TEST_F(LCSSimulatorTest, MissingLCS) { } // Test fixture for C3Controller -class C3ControllerTest : public ::testing::Test, public C3CartpoleProblem { +template +class C3ControllerTypedTest : public ::testing::Test, public C3CartpoleProblem { protected: - C3ControllerTest() + C3ControllerTypedTest() : C3CartpoleProblem(0.411, 0.978, 0.6, 0.4267, 0.35, -0.35, 100, 9.81, 10, - 0.1) {} + 0.1) { + // Load controller options from YAML + if (std::is_same::value){ + controller_options_ = drake::yaml::LoadYamlFile( + "examples/resources/cartpole_softwalls/" + "c3Plus_controller_cartpole_options.yaml"); + UseC3Plus(); + } + else + controller_options_ = drake::yaml::LoadYamlFile( + "examples/resources/cartpole_softwalls/" + "c3_controller_cartpole_options.yaml"); + } void SetUp() override { - // Load controller options from YAML - C3ControllerOptions controller_options = - drake::yaml::LoadYamlFile( - "examples/resources/cartpole_softwalls/" - "c3_controller_cartpole_options.yaml"); - controller_options.publish_frequency = 0; // Forced Update + controller_options_.publish_frequency = 0; // Forced Update // Add prediction for the slider joint - controller_options.state_prediction_joints.push_back( + controller_options_.state_prediction_joints.push_back( {.name = "CartSlider", .max_acceleration = 10.0}); - controller_options.lcs_factory_options.N = 10; - controller_options.lcs_factory_options.dt = 0.1; + controller_options_.lcs_factory_options.N = 10; + controller_options_.lcs_factory_options.dt = 0.1; // Build plant and scene graph std::tie(plant, scene_graph) = @@ -121,7 +132,7 @@ class C3ControllerTest : public ::testing::Test, public C3CartpoleProblem { plant->Finalize(); controller_ = - std::make_unique(*plant, cost, controller_options); + std::make_unique(*plant, cost, controller_options_); context_ = controller_->CreateDefaultContext(); output_ = controller_->AllocateOutput(); @@ -145,9 +156,15 @@ class C3ControllerTest : public ::testing::Test, public C3CartpoleProblem { std::unique_ptr> context_; std::unique_ptr> output_; std::unique_ptr> discrete_values_; + C3ControllerOptions controller_options_; }; -TEST_F(C3ControllerTest, CheckInputOutputPorts) { +using projection_types = ::testing::Types; +TYPED_TEST_SUITE(C3ControllerTypedTest, projection_types); + +TYPED_TEST(C3ControllerTypedTest, CheckInputOutputPorts) { + auto& controller_ = this->controller_; + auto& pSystem = this->pSystem; // Check input and output port sizes and existence EXPECT_NO_THROW(controller_->get_input_port_lcs_state()); EXPECT_EQ(controller_->get_input_port_lcs_state().size(), @@ -158,7 +175,12 @@ TEST_F(C3ControllerTest, CheckInputOutputPorts) { EXPECT_NO_THROW(controller_->get_output_port_c3_intermediates()); } -TEST_F(C3ControllerTest, CheckPlannedTrajectory) { +TYPED_TEST(C3ControllerTypedTest, CheckPlannedTrajectory) { + auto& controller_ = this->controller_; + auto& context_ = this->context_; + auto& discrete_values_ = this->discrete_values_; + auto& output_ = this->output_; + auto& pSystem = this->pSystem; // Should not throw when computing plan with valid inputs EXPECT_NO_THROW({ controller_->CalcForcedDiscreteVariableUpdate(*context_, @@ -168,7 +190,7 @@ TEST_F(C3ControllerTest, CheckPlannedTrajectory) { // Check C3 solution output const auto& c3_solution = - output_->get_data(0)->get_value(); + output_->get_data(0)->template get_value(); EXPECT_EQ(c3_solution.time_vector_.size(), pSystem->N()); EXPECT_EQ(c3_solution.x_sol_.rows(), pSystem->num_states()); EXPECT_EQ(c3_solution.lambda_sol_.rows(), pSystem->num_lambdas()); @@ -179,7 +201,7 @@ TEST_F(C3ControllerTest, CheckPlannedTrajectory) { // Check C3 intermediates output const auto& c3_intermediates = - output_->get_data(1)->get_value(); + output_->get_data(1)->template get_value(); EXPECT_EQ(c3_intermediates.time_vector_.size(), pSystem->N()); int total_vars = pSystem->num_states() + pSystem->num_lambdas() + pSystem->num_inputs(); @@ -191,7 +213,11 @@ TEST_F(C3ControllerTest, CheckPlannedTrajectory) { EXPECT_EQ(c3_intermediates.w_.cols(), pSystem->N()); } -TEST_F(C3ControllerTest, ThrowsOnMissingLCS) { +TYPED_TEST(C3ControllerTypedTest, ThrowsOnMissingLCS) { + auto& controller_ = this->controller_; + auto& plant = this->plant; + auto& x0 = this->x0; + auto& discrete_values_ = this->discrete_values_; // Remove LCS input and expect a runtime error auto new_context = controller_->CreateDefaultContext(); auto state_vec = c3::systems::TimestampedVector( @@ -207,7 +233,12 @@ TEST_F(C3ControllerTest, ThrowsOnMissingLCS) { std::runtime_error); } -TEST_F(C3ControllerTest, TestJointPrediction) { +TYPED_TEST(C3ControllerTypedTest, TestJointPrediction) { + auto& controller_ = this->controller_; + auto& context_ = this->context_; + auto& discrete_values_ = this->discrete_values_; + auto& output_ = this->output_; + auto& x0 = this->x0; // Test that joint prediction modifies the first state in the planned // trajectory double eps = 1e-8; @@ -215,8 +246,9 @@ TEST_F(C3ControllerTest, TestJointPrediction) { discrete_values_.get()); controller_->CalcOutput(*context_, output_.get()); const auto& pre_solution = - output_->get_data(0)->get_value(); - Eigen::VectorXd first_state = pre_solution.x_sol_.col(0).cast(); + output_->get_data(0)->template get_value(); + Eigen::VectorXd first_state = + pre_solution.x_sol_.col(0).template cast(); EXPECT_TRUE(first_state.isApprox(x0, eps)); // Update context with predicted state and check that it changes @@ -225,9 +257,9 @@ TEST_F(C3ControllerTest, TestJointPrediction) { *context_, discrete_values_.get())); controller_->CalcOutput(*context_, output_.get()); const auto& post_solution = - output_->get_data(0)->get_value(); + output_->get_data(0)->template get_value(); Eigen::VectorXd predicted_first_state = - post_solution.x_sol_.col(0).cast(); + post_solution.x_sol_.col(0).template cast(); EXPECT_FALSE(predicted_first_state.isApprox(x0, eps)); } From dc2ab64dccf8afe8752c330193d7e2700d5f60c2 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Thu, 4 Dec 2025 19:00:30 -0500 Subject: [PATCH 12/16] fix: add missing line --- bindings/pyc3/c3_multibody_py.cc | 2 +- multibody/test/multibody_test.cc | 17 +++-------------- 2 files changed, 4 insertions(+), 15 deletions(-) diff --git a/bindings/pyc3/c3_multibody_py.cc b/bindings/pyc3/c3_multibody_py.cc index ef63fe1..ca7abea 100644 --- a/bindings/pyc3/c3_multibody_py.cc +++ b/bindings/pyc3/c3_multibody_py.cc @@ -92,4 +92,4 @@ PYBIND11_MODULE(multibody, m) { } } // namespace pyc3 } // namespace multibody -} // namespace c3 \ No newline at end of file +} // namespace c3 diff --git a/multibody/test/multibody_test.cc b/multibody/test/multibody_test.cc index 9f448b1..d368546 100644 --- a/multibody/test/multibody_test.cc +++ b/multibody/test/multibody_test.cc @@ -402,17 +402,10 @@ GTEST_TEST(GeomGeomColliderTest, SphereMeshDistance) { GeomGeomCollider collider(plant, geom_pair); auto [distance_no_contact, J] = collider.EvalPolytope(context, 1); -<<<<<<< Updated upstream - EXPECT_NEAR( - distance_no_contact, - NO_CONTACT_HEIGHT - MESH_HEIGHT - SPHERE_RADIUS, 1e-2); // Expected distance - EXPECT_EQ(J.rows(), 3); -======= EXPECT_NEAR(distance_no_contact, NO_CONTACT_HEIGHT - MESH_HEIGHT - SPHERE_RADIUS, - 1e-2); // Expected clearance distance - EXPECT_EQ(J.rows(), 3); // 3D contact Jacobian ->>>>>>> Stashed changes + 1e-2); // Expected distance + EXPECT_EQ(J.rows(), 3); EXPECT_EQ(J.cols(), plant.num_velocities()); EXPECT_TRUE(J.allFinite()); @@ -426,12 +419,8 @@ GTEST_TEST(GeomGeomColliderTest, SphereMeshDistance) { auto [distance_contact, J_contact] = collider_contact.EvalPolytope(context, 1); -<<<<<<< Updated upstream - EXPECT_NEAR(distance_contact, -MESH_HEIGHT, 1e-2); // Expect penetration or zero distance -======= EXPECT_NEAR(distance_contact, -MESH_HEIGHT, - 1e-2); // Negative distance indicates penetration ->>>>>>> Stashed changes + 1e-2); // Expect penetration or zero distance EXPECT_EQ(J_contact.rows(), 3); EXPECT_EQ(J_contact.cols(), plant.num_velocities()); EXPECT_TRUE(J_contact.allFinite()); From 514d821855636cf529f75180a6fcc8c5495b2128 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Fri, 5 Dec 2025 11:35:29 -0500 Subject: [PATCH 13/16] coverage: do not update cache if build failed --- .github/workflows/coverage.yml | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml index 0dab8c9..49e887e 100644 --- a/.github/workflows/coverage.yml +++ b/.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,7 +48,7 @@ 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 }} From 92d0acdc56c520cc7f874a3725cb27bfcf99f869 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Mon, 8 Dec 2025 09:41:00 -0500 Subject: [PATCH 14/16] format: clang formatting --- systems/test/systems_test.cc | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/systems/test/systems_test.cc b/systems/test/systems_test.cc index 99d71b3..ab81645 100644 --- a/systems/test/systems_test.cc +++ b/systems/test/systems_test.cc @@ -102,13 +102,12 @@ class C3ControllerTypedTest : public ::testing::Test, public C3CartpoleProblem { : C3CartpoleProblem(0.411, 0.978, 0.6, 0.4267, 0.35, -0.35, 100, 9.81, 10, 0.1) { // Load controller options from YAML - if (std::is_same::value){ + if (std::is_same::value) { controller_options_ = drake::yaml::LoadYamlFile( "examples/resources/cartpole_softwalls/" "c3Plus_controller_cartpole_options.yaml"); - UseC3Plus(); - } - else + UseC3Plus(); + } else controller_options_ = drake::yaml::LoadYamlFile( "examples/resources/cartpole_softwalls/" "c3_controller_cartpole_options.yaml"); From ab5618775ef894725a08df5557e033b2e42583d8 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Sun, 21 Dec 2025 21:26:41 -0500 Subject: [PATCH 15/16] feat: add options for regularized lcs solve --- core/lcs.cc | 10 ++++++---- core/lcs.h | 19 +++++++++++++++++-- 2 files changed, 23 insertions(+), 6 deletions(-) diff --git a/core/lcs.cc b/core/lcs.cc index 3f1ba54..3756c0b 100644 --- a/core/lcs.cc +++ b/core/lcs.cc @@ -58,15 +58,17 @@ double LCS::ScaleComplementarityDynamics() { } const VectorXd LCS::Simulate(VectorXd& x_init, VectorXd& u, - bool regularized) const { + const LCSSimulateConfig& config) const { VectorXd x_final; VectorXd force; drake::solvers::MobyLCPSolver LCPSolver; - if (regularized) { + if (config.regularized) { LCPSolver.SolveLcpLemkeRegularized( - F_[0], E_[0] * x_init + c_[0] + H_[0] * u, &force); + F_[0], E_[0] * x_init + c_[0] + H_[0] * u, &force, config.min_exp, + config.step_exp, config.max_exp, config.piv_tol, config.zero_tol); } else { - LCPSolver.SolveLcpLemke(F_[0], E_[0] * x_init + c_[0] + H_[0] * u, &force); + LCPSolver.SolveLcpLemke(F_[0], E_[0] * x_init + c_[0] + H_[0] * u, &force, + config.piv_tol, config.zero_tol); } x_final = A_[0] * x_init + B_[0] * u + D_[0] * force + d_[0]; return x_final; diff --git a/core/lcs.h b/core/lcs.h index af80b7c..8de9c14 100644 --- a/core/lcs.h +++ b/core/lcs.h @@ -7,6 +7,20 @@ #include "drake/common/drake_assert.h" namespace c3 { +struct LCSSimulateConfig { + // default values found from + // https://drake.mit.edu/doxygen_cxx/classdrake_1_1solvers_1_1_moby_lcp_solver.html + bool regularized = false; + double piv_tol = -1; + double zero_tol = -1; + // Only used for regularized solves + int min_exp = -20; + int step_exp = 1; + int max_exp = 1; + + LCSSimulateConfig() = default; +}; + class LCS { public: LCS() = default; @@ -50,8 +64,9 @@ class LCS { * * @return The state at the next timestep */ - const Eigen::VectorXd Simulate(Eigen::VectorXd& x_init, Eigen::VectorXd& u, - bool regularized = false) const; + const Eigen::VectorXd Simulate( + Eigen::VectorXd& x_init, Eigen::VectorXd& u, + const LCSSimulateConfig& config = LCSSimulateConfig()) const; /*! * Accessors dynamics terms From af38053552ce76a52af9f8f99cf00ae5371b1d0e Mon Sep 17 00:00:00 2001 From: Meow404 Date: Fri, 9 Jan 2026 13:47:18 -0500 Subject: [PATCH 16/16] fix: apply augmented costs only for lambda and eta for c3+ --- core/c3.h | 85 +++++++++++++++++++++++++++++++++++++++++++++++++ core/c3_plus.cc | 76 +++++++++++++++++++++++++++---------------- 2 files changed, 134 insertions(+), 27 deletions(-) diff --git a/core/c3.h b/core/c3.h index e844ede..1fe98ff 100644 --- a/core/c3.h +++ b/core/c3.h @@ -293,11 +293,96 @@ class C3 { const std::vector& delta, int admm_iteration, bool is_final_solve = false); + /*! + * @brief Add the augmented Lagrangian cost to the QP problem. + * + * This function adds (or updates) the quadratic cost terms for the augmented + * Lagrangian formulation of the ADMM algorithm. The cost has the form: + * ||G(z - (w - delta))||^2 + * + * For C3Plus subclass, this function also handles special scaling of contact + * forces during the final solve iteration. + * + * @param G Vector of augmented Lagrangian weight matrices, one per timestep. + * Dimensions: N x (n_z x n_z) + * @param WD Vector of (w - delta) terms, one per timestep. + * Dimensions: N x n_z + * @param delta Vector of copy variables from the previous ADMM iteration, + * one per timestep. Dimensions: N x n_z + * @param is_final_solve Boolean flag indicating if this is the final QP solve + * after all ADMM iterations complete. Used by + * subclasses to apply special handling (e.g., contact scaling). + * + * @note The cost matrices are stored in augmented_costs_ and removed/re-added + * on each call to ensure the QP remains well-posed. + * + * @see ADMMStep, SolveQP + */ virtual void AddAugmentedCost(const std::vector& G, const std::vector& WD, const std::vector& delta, bool is_final_solve); + + /*! + * @brief Set the initial guess (warm start) for the QP solver. + * + * This function provides Drake's OSQP solver with an initial guess for the + * decision variables to accelerate convergence. The warm start strategy + * interpolates between solutions from previous ADMM iterations if available. + * + * For non-first ADMM iterations, the function performs linear interpolation + * between consecutive warm-start solutions based on the solve_time_ and + * the LCS time step dt. This allows smooth transitions across multiple + * planning horizons. + * + * @param x0 The initial state at the current planning iteration. + * Used to initialize x_[0] = x0. + * @param admm_iteration The current ADMM iteration index (0-based). + * For iteration 0, no warm start is applied. + * + * @note Warm starting is only performed if: + * - warm_start_ flag is enabled in C3Options + * - admm_iteration > 0 (skip first iteration) + * + * The interpolation formula used is: + * z_guess[i] = (1 - weight) * warm_start[iter-1][i] + weight * + * warm_start[iter-1][i+1] where weight = (solve_time_ mod dt) / dt + * + * @see SetInitialGuessQP, warm_start_x_, warm_start_lambda_, warm_start_u_ + */ virtual void SetInitialGuessQP(const Eigen::VectorXd& x0, int admm_iteration); + + /*! + * @brief Extract and store the QP solution results. + * + * This function retrieves the optimized decision variables from the OSQP + * solver result and stores them in the solution containers (x_sol_, + * lambda_sol_, u_sol_, z_sol_). Additionally, it updates warm-start caches + * to accelerate future ADMM iterations. + * + * The function handles two scenarios: + * 1. Intermediate ADMM iterations: Stores results in z_sol_ for the z-update + * step and caches them for warm starting. + * 2. Final QP solve: Also stores individual solutions in x_sol_, lambda_sol_, + * and u_sol_ for final output. + * + * Solution storage layout in z_sol_[i]: + * z_sol_[i] = [x_[i]; lambda_[i]; u_[i]; ...] + * where [...] may include additional variables (e.g., eta in C3Plus) + * + * @param result The MathematicalProgramResult from the OSQP solver containing + * the optimized decision variables. + * @param admm_iteration The current ADMM iteration index (0-based). + * @param is_final_solve Boolean flag indicating if this is the final QP + * solve. If true, also populate x_sol_, lambda_sol_, u_sol_. + * + * @note For warm starting: + * - warm_start_x_[admm_iteration][i] = x_[i] solution + * - warm_start_lambda_[admm_iteration][i] = lambda_[i] solution + * - warm_start_u_[admm_iteration][i] = u_[i] solution + * + * @see SolveQP, z_sol_, x_sol_, lambda_sol_, u_sol_ + */ virtual void StoreQPResults( const drake::solvers::MathematicalProgramResult& result, int admm_iteration, bool is_final_solve); diff --git a/core/c3_plus.cc b/core/c3_plus.cc index a60a14d..2554b2d 100644 --- a/core/c3_plus.cc +++ b/core/c3_plus.cc @@ -100,35 +100,57 @@ void C3Plus::AddAugmentedCost(const vector& G, const vector& WD, const vector& delta, bool is_final_solve) { - if (is_final_solve) { - for (auto& cost : augmented_costs_) { - prog_.RemoveCost(cost); - } - augmented_costs_.clear(); - - std::vector GFinal = G; - for (int i = 0; i < N_; ++i) { - Eigen::VectorXd GScaling = - Eigen::VectorXd::Ones(n_z_); // Default scaling is 1 - if (options_.final_augmented_cost_contact_scaling.has_value() && - options_.final_augmented_cost_contact_indices.has_value()) { - for (int index : - options_.final_augmented_cost_contact_indices.value()) { - if (delta.at(i)[n_x_ + index] == 0) - GScaling(n_x_ + index) *= - options_.final_augmented_cost_contact_scaling.value(); - else - GScaling(n_x_ + n_lambda_ + n_u_ + index) *= - options_.final_augmented_cost_contact_scaling.value(); - } + // Remove previous augmented costs + for (auto& cost : augmented_costs_) { + prog_.RemoveCost(cost); + } + augmented_costs_.clear(); + + // Add augmented costs for each time step + for (int i = 0; i < N_; i++) { + Eigen::VectorXd GScaling = Eigen::VectorXd::Ones(n_z_); + + // Apply contact scaling to final solve if specified in options + if (is_final_solve && + options_.final_augmented_cost_contact_scaling.has_value() && + options_.final_augmented_cost_contact_indices.has_value()) { + for (int index : options_.final_augmented_cost_contact_indices.value()) { + // Scale lambda or eta based on whether the corresponding delta is zero + if (delta.at(i)[n_x_ + index] == 0) + GScaling(n_x_ + index) *= + options_.final_augmented_cost_contact_scaling.value(); + else + GScaling(n_x_ + n_lambda_ + n_u_ + index) *= + options_.final_augmented_cost_contact_scaling.value(); } - auto GiFinal = GFinal.at(i).array().colwise() * GScaling.array(); - augmented_costs_.push_back(prog_.AddQuadraticCost( - 2 * GiFinal.matrix(), -2 * GiFinal.matrix() * delta.at(i), z_.at(i), - 1)); } - } else { - C3::AddAugmentedCost(G, WD, delta, is_final_solve); + + // Apply scaling to contact forces in final solve to encourage + // complementarity constraints. For final solve, scale G matrix by GScaling; + // otherwise use unscaled G matrix. This helps satisfy complementarity by + // encouraging some contact forces to match previous projection step values, + // improving dynamic feasibility. Applied selectively to avoid + // over-constraining the QP solver. + const MatrixXd& G_i = + is_final_solve ? (G.at(i).array().colwise() * GScaling.array()).matrix() + : G.at(i); + const VectorXd& WD_i = is_final_solve ? delta.at(i) : WD.at(i); + + // Add quadratic cost for lambda + augmented_costs_.push_back(prog_.AddQuadraticCost( + 2 * G_i.block(n_x_, n_x_, n_lambda_, n_lambda_), + -2 * G_i.block(n_x_, n_x_, n_lambda_, n_lambda_) * + WD_i.segment(n_x_, n_lambda_), + lambda_.at(i), 1)); + // Add quadratic cost for eta + augmented_costs_.push_back(prog_.AddQuadraticCost( + 2 * G_i.block(n_x_ + n_lambda_ + n_u_, n_x_ + n_lambda_ + n_u_, + n_lambda_, n_lambda_), + -2 * + G_i.block(n_x_ + n_lambda_ + n_u_, n_x_ + n_lambda_ + n_u_, + n_lambda_, n_lambda_) * + WD_i.segment(n_x_ + n_lambda_ + n_u_, n_lambda_), + eta_.at(i), 1)); } }