diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml index fbc5698..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,33 +48,33 @@ 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 }} 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_multibody_py.cc b/bindings/pyc3/c3_multibody_py.cc index f17ebc9..ca7abea 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,12 +80,16 @@ 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); } } // namespace pyc3 } // namespace multibody -} // namespace c3 \ No newline at end of file +} // namespace c3 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..7693c29 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_); @@ -96,29 +89,34 @@ 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_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 +133,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 +241,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 +252,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 +263,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 +279,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); + SolveQP(x0, G, WD, delta, options_.admm_iter, true); *w_sol_ = w; *delta_sol_ = delta; @@ -313,15 +360,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, *delta, 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 +386,88 @@ 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, + const vector& delta, int admm_iteration, + bool is_final_solve) { + AddAugmentedCost(G, WD, delta, is_final_solve); + 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_; } +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_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 +476,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..1fe98ff 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. @@ -194,7 +196,25 @@ class C3 { 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 +225,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 +236,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( @@ -221,14 +245,18 @@ 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; - - private: /*! * 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 @@ -259,11 +287,105 @@ 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); + + /*! + * @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); LCS lcs_; double AnDn_ = 1.0; // Scaling factor for lambdas @@ -279,6 +401,7 @@ class C3 { std::vector x_; std::vector u_; std::vector lambda_; + std::vector z_; // QP step constraints std::shared_ptr @@ -291,8 +414,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..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..94d21ab 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,21 @@ 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 + + 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 @@ -56,6 +76,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 +88,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 +104,10 @@ 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(final_augmented_cost_contact_scaling)); + a->Visit(DRAKE_NVP(final_augmented_cost_contact_indices)); a->Visit(DRAKE_NVP(admm_iter)); @@ -93,12 +126,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 +149,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 +168,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()) { + u_vector.insert(u_vector.end(), u_eta_slack->begin(), + u_eta_slack->end()); + u_vector.insert(u_vector.end(), u_eta_n->begin(), u_eta_n->end()); + u_vector.insert(u_vector.end(), u_eta_t->begin(), u_eta_t->end()); + } else { + u_vector.insert(u_vector.end(), u_eta->begin(), u_eta->end()); + } + } Eigen::VectorXd q = Eigen::Map( this->q_vector.data(), this->q_vector.size()); @@ -140,7 +200,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..2554b2d --- /dev/null +++ b/core/c3_plus.cc @@ -0,0 +1,199 @@ +#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]); + } +} + +void C3Plus::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 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(); + } + } + + // 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)); + } +} + +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..40f8dd2 --- /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 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 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; + 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..3756c0b 100644 --- a/core/lcs.cc +++ b/core/lcs.cc @@ -57,11 +57,19 @@ double LCS::ScaleComplementarityDynamics() { return scale; } -const VectorXd LCS::Simulate(VectorXd& x_init, VectorXd& u) const { +const VectorXd LCS::Simulate(VectorXd& x_init, VectorXd& u, + const LCSSimulateConfig& config) 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 (config.regularized) { + LCPSolver.SolveLcpLemkeRegularized( + 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, + 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 ba2dd13..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) const; + const Eigen::VectorXd Simulate( + Eigen::VectorXd& x_init, Eigen::VectorXd& u, + const LCSSimulateConfig& config = LCSSimulateConfig()) 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..18815c4 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 @@ -221,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> {}; @@ -342,19 +310,70 @@ 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 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/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..c004185 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(); @@ -280,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); @@ -338,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/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/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/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 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..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, @@ -41,23 +87,29 @@ LCSFactory::LCSFactory( context_ad_(context_ad), contact_pairs_(contact_geoms), options_(options), + n_contacts_(contact_geoms.size()), + 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(options)), + n_lambda_(multibody::LCSFactory::GetNumContactVariables( + contact_model_, n_contacts_, n_friction_directions_per_contact_)), 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), + 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}; @@ -65,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; @@ -80,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_); } } @@ -259,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( @@ -325,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(); @@ -400,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 @@ -425,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(); @@ -560,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 0a02b7e..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,23 +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_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. + 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/multibody/test/multibody_test.cc b/multibody/test/multibody_test.cc index d2ff762..d368546 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,180 +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 = 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 = fixture.contact_pairs.size(); + auto n_tangential_directions = + 2 * std::accumulate( + fixture.options.num_friction_directions_per_contact->begin(), + fixture.options.num_friction_directions_per_contact->end(), 0); - int n_contacts = contact_pairs.size(); - // Check for number of force variables (not including slack variables) - switch (contact_model) { + // Verify Jacobian rows based on contact model + switch (fixture.contact_model) { case ContactModel::kStewartAndTrinkle: - EXPECT_EQ(J.rows(), - n_contacts + 2 * n_contacts * options.num_friction_directions); + // 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: - EXPECT_EQ(J.rows(), 2 * n_contacts * options.num_friction_directions); + // 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); @@ -258,13 +353,79 @@ 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); + + EXPECT_NEAR(distance_no_contact, + NO_CONTACT_HEIGHT - MESH_HEIGHT - SPHERE_RADIUS, + 1e-2); // Expected distance + EXPECT_EQ(J.rows(), 3); + 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); + + EXPECT_NEAR(distance_contact, -MESH_HEIGHT, + 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()); +} + } // 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/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_; 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. * diff --git a/systems/test/systems_test.cc b/systems/test/systems_test.cc index 140225a..ab81645 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,32 @@ 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 +131,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 +155,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 +174,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 +189,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 +200,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 +212,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 +232,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 +245,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 +256,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)); }