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/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..090f057 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, + 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_(z_size), 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_); @@ -89,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>(); @@ -96,29 +90,35 @@ 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_)); + 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_)); } + 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,31 +135,55 @@ 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), 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 + 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); } +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. @@ -219,7 +243,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 +254,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 +265,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,30 +281,54 @@ 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); } - 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; @@ -313,15 +362,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 +388,85 @@ 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::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) { + 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::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]); + 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)); } + SetInitialGuessQP(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()); } + StoreQPResults(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 +475,12 @@ 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) { + // 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) { deltaProj[i] = diff --git a/core/c3.h b/core/c3.h index 0247f20..b31c359 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 }; @@ -72,7 +74,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. @@ -188,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_; @@ -205,6 +226,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 +237,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 +247,22 @@ class C3 { const Eigen::MatrixXd& H, const Eigen::VectorXd& c, const int admm_iteration, const int& warm_start_index) = 0; - private: + 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. */ 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 +313,7 @@ class C3 { std::vector x_; std::vector u_; std::vector lambda_; + std::vector z_; // QP step constraints std::shared_ptr @@ -291,8 +326,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 @@ -301,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 144df7c..51f800e 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.value_or(1000)) { + 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..08f3b6a 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 = @@ -18,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 @@ -56,6 +67,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 +79,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)); @@ -75,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)); @@ -93,12 +115,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 +138,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 +157,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 +189,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..5d927ea --- /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, + 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) { + 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()); + } + + // 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_); + 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::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( + eta_[i], (1 - weight) * warm_start_eta_[admm_iteration - 1][i] + + weight * warm_start_eta_[admm_iteration - 1][i + 1]); + } +} + +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]); + } + 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, + 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; + + // 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_plus.h b/core/c3_plus.h new file mode 100644 index 0000000..2905da8 --- /dev/null +++ b/core/c3_plus.h @@ -0,0 +1,70 @@ +#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 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, + 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 StoreQPResults(const drake::solvers::MathematicalProgramResult& result, + int admm_iteration, bool is_final_solve) override; + void UpdateLCS(const LCS& lcs) override; + void SetInitialGuessQP(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/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/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/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/multibody/geom_geom_collider.cc b/multibody/geom_geom_collider.cc index aa9e605..cadbfab 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_DEMAND(sd_set.size() == 1); + 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() * (-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() * (-1 * 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. */ 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..c140de9 100644 --- a/multibody/lcs_factory.h +++ b/multibody/lcs_factory.h @@ -291,17 +291,18 @@ class LCSFactory { // 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. - 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/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); + } } }; 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_;