diff --git a/.bazelrc b/.bazelrc index 764b3ec..5197273 100644 --- a/.bazelrc +++ b/.bazelrc @@ -51,4 +51,4 @@ build --action_env=LD_LIBRARY_PATH= # use python3 by default build --python_path=python3 -build --define=WITH_GUROBI=ON \ No newline at end of file +build --define=WITH_GUROBI=ON 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/BUILD.bazel b/BUILD.bazel index a791480..eee0451 100644 --- a/BUILD.bazel +++ b/BUILD.bazel @@ -21,6 +21,7 @@ cc_library( name = "libc3", hdrs = [":c3_headers"], # Changed from srcs to hdrs for headers deps = LIBC3_COMPONENTS + [ + "//lcmtypes:lcmt_c3", "@drake//:drake_shared_library", ], include_prefix = "c3", diff --git a/bindings/pyc3/c3_multibody_py.cc b/bindings/pyc3/c3_multibody_py.cc index f17ebc9..541bfbc 100644 --- a/bindings/pyc3/c3_multibody_py.cc +++ b/bindings/pyc3/c3_multibody_py.cc @@ -26,6 +26,20 @@ PYBIND11_MODULE(multibody, m) { c3::multibody::ContactModel::kFrictionlessSpring) .export_values(); + py::class_(m, "LCSContactDescription") + .def(py::init<>()) + .def_readwrite("witness_point_A", + &c3::multibody::LCSContactDescription::witness_point_A) + .def_readwrite("witness_point_B", + &c3::multibody::LCSContactDescription::witness_point_B) + .def_readwrite("force_basis", + &c3::multibody::LCSContactDescription::force_basis) + .def_readwrite("is_slack", + &c3::multibody::LCSContactDescription::is_slack) + .def_static("CreateSlackVariableDescription", + &c3::multibody::LCSContactDescription:: + CreateSlackVariableDescription); + py::class_(m, "LCSFactory") .def(py::init&, drake::systems::Context&, @@ -37,8 +51,8 @@ PYBIND11_MODULE(multibody, m) { py::arg("plant"), py::arg("context"), py::arg("plant_ad"), py::arg("context_ad"), py::arg("contact_geoms"), py::arg("options")) .def("GenerateLCS", &c3::multibody::LCSFactory::GenerateLCS) - .def("GetContactJacobianAndPoints", - &c3::multibody::LCSFactory::GetContactJacobianAndPoints) + .def("GetContactDescriptions", + &c3::multibody::LCSFactory::GetContactDescriptions) .def("UpdateStateAndInput", &c3::multibody::LCSFactory::UpdateStateAndInput, py::arg("state"), py::arg("input")) @@ -61,6 +75,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 +94,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/bindings/pyc3/c3_systems_py.cc b/bindings/pyc3/c3_systems_py.cc index cc12670..7c397f6 100644 --- a/bindings/pyc3/c3_systems_py.cc +++ b/bindings/pyc3/c3_systems_py.cc @@ -9,6 +9,9 @@ #include "systems/c3_controller_options.h" #include "systems/framework/c3_output.h" #include "systems/framework/timestamped_vector.h" +#include "systems/lcmt_generators/c3_output_generator.h" +#include "systems/lcmt_generators/c3_trajectory_generator.h" +#include "systems/lcmt_generators/contact_force_generator.h" #include "systems/lcs_factory_system.h" #include "systems/lcs_simulator.h" @@ -100,8 +103,8 @@ PYBIND11_MODULE(systems, m) { py::return_value_policy::reference) .def("get_output_port_lcs", &LCSFactorySystem::get_output_port_lcs, py::return_value_policy::reference) - .def("get_output_port_lcs_contact_jacobian", - &LCSFactorySystem::get_output_port_lcs_contact_jacobian, + .def("get_output_port_lcs_contact_descriptions", + &LCSFactorySystem::get_output_port_lcs_contact_descriptions, py::return_value_policy::reference); py::class_(m, "C3Solution") @@ -178,6 +181,88 @@ PYBIND11_MODULE(systems, m) { &C3ControllerOptions::state_prediction_joints); m.def("LoadC3ControllerOptions", &LoadC3ControllerOptions); + + // C3OutputGenerator + py::class_>(m, "C3OutputGenerator") + .def(py::init<>()) + .def("get_input_port_c3_solution", + &lcmt_generators::C3OutputGenerator::get_input_port_c3_solution, + py::return_value_policy::reference) + .def("get_input_port_c3_intermediates", + &lcmt_generators::C3OutputGenerator::get_input_port_c3_intermediates, + py::return_value_policy::reference) + .def("get_output_port_c3_output", + &lcmt_generators::C3OutputGenerator::get_output_port_c3_output, + py::return_value_policy::reference) + .def_static("AddLcmPublisherToBuilder", + &lcmt_generators::C3OutputGenerator::AddLcmPublisherToBuilder, + py::arg("builder"), py::arg("solution_port"), + py::arg("intermediates_port"), py::arg("channel"), + py::arg("lcm"), py::arg("publish_triggers"), + py::arg("publish_period"), py::arg("publish_offset")); + + // C3TrajectoryGenerator + py::class_( + m, "TrajectoryDescriptionIndexRange") + .def(py::init<>()) + .def_readwrite( + "start", &lcmt_generators::TrajectoryDescription::index_range::start) + .def_readwrite("end", + &lcmt_generators::TrajectoryDescription::index_range::end); + py::class_(m, "TrajectoryDescription") + .def(py::init<>()) + .def_readwrite("trajectory_name", + &lcmt_generators::TrajectoryDescription::trajectory_name) + .def_readwrite("variable_type", + &lcmt_generators::TrajectoryDescription::variable_type) + .def_readwrite("indices", + &lcmt_generators::TrajectoryDescription::indices); + py::class_( + m, "C3TrajectoryGeneratorConfig") + .def(py::init<>()) + .def_readwrite( + "trajectories", + &lcmt_generators::C3TrajectoryGeneratorConfig::trajectories); + py::class_>(m, "C3TrajectoryGenerator") + .def(py::init()) + .def("get_input_port_c3_solution", + &lcmt_generators::C3TrajectoryGenerator::get_input_port_c3_solution, + py::return_value_policy::reference) + .def("get_output_port_lcmt_c3_trajectory", + &lcmt_generators::C3TrajectoryGenerator:: + get_output_port_lcmt_c3_trajectory, + py::return_value_policy::reference) + .def_static( + "AddLcmPublisherToBuilder", + &lcmt_generators::C3TrajectoryGenerator::AddLcmPublisherToBuilder, + py::arg("builder"), py::arg("config"), py::arg("solution_port"), + py::arg("channel"), py::arg("lcm"), py::arg("publish_triggers"), + py::arg("publish_period"), py::arg("publish_offset")); + + // ContactForceGenerator + py::class_>(m, "ContactForceGenerator") + .def(py::init<>()) + .def("get_input_port_c3_solution", + &lcmt_generators::ContactForceGenerator::get_input_port_c3_solution, + py::return_value_policy::reference) + .def("get_input_port_lcs_contact_descriptions", + &lcmt_generators::ContactForceGenerator:: + get_input_port_lcs_contact_descriptions, + py::return_value_policy::reference) + .def("get_output_port_contact_force", + &lcmt_generators::ContactForceGenerator:: + get_output_port_contact_force, + py::return_value_policy::reference) + .def_static( + "AddLcmPublisherToBuilder", + &lcmt_generators::ContactForceGenerator::AddLcmPublisherToBuilder, + py::arg("builder"), py::arg("solution_port"), + py::arg("lcs_contact_info_port"), py::arg("channel"), py::arg("lcm"), + py::arg("publish_triggers"), py::arg("publish_period"), + py::arg("publish_offset")); } } // namespace pyc3 } // namespace systems diff --git a/core/BUILD.bazel b/core/BUILD.bazel index 5b80c66..c047adf 100644 --- a/core/BUILD.bazel +++ b/core/BUILD.bazel @@ -29,11 +29,17 @@ filegroup( ], ) +cc_library( + name = "logging", + hdrs = ["common/logging_utils.hpp"], +) + cc_library( name = "c3", srcs = [ "c3.cc", "c3_qp.cc", + "c3_plus.cc", ] + select({ "//tools:with_gurobi": [ ":c3_miqp.cc", @@ -46,6 +52,7 @@ cc_library( "c3.h", "c3_miqp.h", "c3_qp.h", + "c3_plus.h", ], data = [ ":default_solver_options", @@ -55,6 +62,7 @@ cc_library( deps = [ ":lcs", ":options", + ":logging", "@drake//:drake_shared_library", ] + select({ "//tools:with_gurobi": ["@gurobi//:gurobi_cxx"], @@ -82,6 +90,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..bd4fb50 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -4,8 +4,10 @@ #include #include +#include #include +#include "common/logging_utils.hpp" #include "lcs.h" #include "solver_options_io.h" @@ -41,12 +43,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 +59,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 +90,33 @@ C3::C3(const LCS& lcs, const CostMatrices& costs, w_sol_ = std::make_unique>(); delta_sol_ = std::make_unique>(); for (int i = 0; i < N_; ++i) { - z_sol_->push_back(Eigen::VectorXd::Zero(n_x_ + n_lambda_ + n_u_)); 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,13 @@ void C3::UpdateCostMatrices(const CostMatrices& costs) { } const std::vector& C3::GetTargetCost() { - return target_cost_; + return target_costs_; } void C3::Solve(const VectorXd& x0) { + drake::log()->debug("C3::Solve called"); auto start = std::chrono::high_resolution_clock::now(); + // Set the initial state constraint if (initial_state_constraint_) { initial_state_constraint_->UpdateCoefficients( MatrixXd::Identity(n_x_, n_x_), x0); @@ -256,35 +280,62 @@ 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)); - } + drake::log()->debug("C3::Solve starting ADMM iterations."); 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; if (!options_.end_on_qp_step) { + drake::log()->debug("C3::Solve compute a half step."); *z_sol_ = delta; z_sol_->at(0).segment(0, n_x_) = x0; x_sol_->at(0) = x0; @@ -308,24 +359,26 @@ void C3::Solve(const VectorXd& x0) { solve_time_ = std::chrono::duration_cast(elapsed).count() / 1e6; + drake::log()->debug("C3::Solve completed in {} seconds.", solve_time_); } 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]; } + drake::log()->debug("C3::ADMMStep SolveProjection step."); if (cost_matrices_.U[0].isZero(0)) { *delta = SolveProjection(*G, ZW, admm_iteration); } else { @@ -339,105 +392,98 @@ 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_]); +} + +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]); - for (auto& cost : costs_) { - prog_.RemoveCost(cost); + drake::log()->trace( + "C3::StoreQPResults storing solution for time step {}: " + "lambda = {}", + i, EigenToString(lambda_sol_->at(i).transpose())); } - costs_.clear(); + if (!warm_start_) + return; // No warm start, so no need to update warm start parameters + + drake::log()->trace("C3::StoreQPResults storing warm start values."); for (int i = 0; i < N_ + 1; ++i) { if (i < N_) { - 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]); +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); + + drake::log()->trace("C3::SolveQP calling solver."); + try { + MathematicalProgramResult result = osqp_.Solve(prog_); + if (!result.is_success()) { + drake::log()->warn("C3::SolveQP failed to solve the QP with status: {}", + result.get_solution_result()); } - prog_.SetInitialGuess(x_[0], x0); - prog_.SetInitialGuess(x_[N_], warm_start_x_[admm_iteration][N_]); + StoreQPResults(result, admm_iteration, is_final_solve); + } catch (const std::exception& e) { + drake::log()->error("C3::SolveQP failed with exception: {}", e.what()); } - MathematicalProgramResult result = osqp_.Solve(prog_); + return *z_sol_; +} - 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 { - drake::log()->warn("C3::SolveQP failed to solve the QP with status: {}", - result.get_solution_result()); +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)); } - - return *z_sol_; } vector C3::SolveProjection(const vector& U, vector& WZ, int admm_iteration) { - vector deltaProj(N_, VectorXd::Zero(n_x_ + n_lambda_ + n_u_)); - int i; + vector deltaProj(N_, VectorXd::Zero(n_z_)); if (options_.num_threads > 0) { omp_set_dynamic(0); // Explicitly disable dynamic teams @@ -446,8 +492,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..af0d543 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,11 @@ struct C3Options { std::vector g_lambda_t; std::vector g_lambda; std::vector g_u; + std::vector g_eta_vector; + 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 +89,16 @@ struct C3Options { std::vector u_lambda_t; std::vector u_lambda; std::vector u_u; + std::vector u_eta_vector; + 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 +106,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 +128,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 +151,16 @@ 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()); + g_eta_vector = g_eta.value_or(std::vector()); + if (g_eta_vector.empty() && g_eta_slack.has_value()) { + g_eta_vector.insert(g_eta_vector.end(), g_eta_slack->begin(), + g_eta_slack->end()); + g_eta_vector.insert(g_eta_vector.end(), g_eta_n->begin(), g_eta_n->end()); + g_eta_vector.insert(g_eta_vector.end(), g_eta_t->begin(), g_eta_t->end()); + } + g_vector.insert(g_vector.end(), g_eta_vector.begin(), g_eta_vector.end()); + u_vector = std::vector(); u_vector.insert(u_vector.end(), u_x.begin(), u_x.end()); if (u_lambda.empty()) { @@ -117,9 +168,16 @@ 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()); + u_eta_vector = u_eta.value_or(std::vector()); + if (u_eta_vector.empty() && u_eta_slack.has_value()) { + u_eta_vector.insert(u_eta_vector.end(), u_eta_slack->begin(), + u_eta_slack->end()); + u_eta_vector.insert(u_eta_vector.end(), u_eta_n->begin(), u_eta_n->end()); + u_eta_vector.insert(u_eta_vector.end(), u_eta_t->begin(), u_eta_t->end()); + } + u_vector.insert(u_vector.end(), u_eta_vector.begin(), u_eta_vector.end()); Eigen::VectorXd q = Eigen::Map( this->q_vector.data(), this->q_vector.size()); @@ -130,8 +188,6 @@ struct C3Options { Eigen::VectorXd u = Eigen::Map( this->u_vector.data(), this->u_vector.size()); - DRAKE_DEMAND(g.size() == u.size()); - Q = w_Q * q.asDiagonal(); R = w_R * r.asDiagonal(); G = w_G * g.asDiagonal(); @@ -140,7 +196,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..4b36d99 --- /dev/null +++ b/core/c3_plus.cc @@ -0,0 +1,222 @@ +#include "c3_plus.h" + +#include + +#include + +#include "c3_options.h" +#include "common/logging_utils.hpp" +#include "lcs.h" + +#include "drake/common/text_logging.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()) { + if (warm_start_) { + warm_start_eta_.resize(options_.admm_iter + 1); + for (int iter = 0; iter < options_.admm_iter + 1; ++iter) { + warm_start_eta_[iter].resize(N_); + for (int i = 0; i < N_; ++i) { + warm_start_eta_[iter][i] = VectorXd::Zero(n_lambda_); + } + } + } + + // Initialize eta as optimization variables + eta_ = vector(); + 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), z_.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); + drake::log()->trace("C3Plus::StoreQPResults storing eta results."); + for (int i = 0; i < N_; i++) { + if (is_final_solve) { + eta_sol_->at(i) = result.GetSolution(eta_[i]); + } + z_sol_->at(i).segment(n_x_ + n_lambda_ + n_u_, n_lambda_) = + result.GetSolution(eta_[i]); + drake::log()->trace( + "C3Plus::StoreQPResults storing solution for time step {}: " + "eta = {}", + i, EigenToString(eta_sol_->at(i).transpose())); + } + + if (!warm_start_) + return; // No warm start, so no need to update warm start parameters + + drake::log()->trace("C3Plus::StoreQPResults storing warm start eta."); + for (int i = 0; i < N_; ++i) { + warm_start_eta_[admm_iteration][i] = result.GetSolution(eta_[i]); + } +} + +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_); + + drake::log()->trace( + "C3Plus::SolveSingleProjection ADMM iteration {}: pre-projection " + "lambda = {}, eta = {}", + admm_iteration, EigenToString(lambda_c.transpose()), + EigenToString(eta_c.transpose())); + + // Set the smaller of lambda and eta to zero + Eigen::Array 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/common/logging_utils.hpp b/core/common/logging_utils.hpp new file mode 100644 index 0000000..b5d6c6d --- /dev/null +++ b/core/common/logging_utils.hpp @@ -0,0 +1,19 @@ +#include +#include + +#include + +/** + * Converts an Eigen matrix to a string representation. + * This function is useful for logging or debugging purposes. + * + * @tparam Derived The type of the Eigen matrix. + * @param mat The Eigen matrix to convert. + * @return A string representation of the matrix. + */ +template +std::string EigenToString(const Eigen::MatrixBase& mat) { + std::stringstream ss; + ss << mat; + return ss.str(); +} \ No newline at end of file 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/lcmtypes/BUILD.bazel b/lcmtypes/BUILD.bazel new file mode 100644 index 0000000..7e711db --- /dev/null +++ b/lcmtypes/BUILD.bazel @@ -0,0 +1,44 @@ +load( + "@drake//tools/workspace/lcm:lcm.bzl", + "lcm_cc_library", + "lcm_java_library", + "lcm_py_library", +) +load( + "@drake//tools/skylark:drake_java.bzl", + "drake_java_binary", +) +load("@drake//tools/lint:lint.bzl", "add_lint_tests") + +package(default_visibility = ["//visibility:public"]) + +#Lcm libraries +lcm_cc_library( + name = "lcmt_c3", + lcm_package = "c3", + lcm_srcs = glob(["*.lcm"]), +) + +lcm_java_library( + name = "lcmtypes_c3_java", + lcm_package = "c3", + lcm_srcs = glob(["*.lcm"]), +) + +lcm_py_library( + name = "lcmtypes_c3_py", + add_current_package_to_imports = True, # Use //:module_py instead. + extra_srcs = ["__init__.py"], + lcm_package = "c3", + lcm_srcs = glob(["*.lcm"]), +) + +drake_java_binary( + name = "c3-lcm-spy", + main_class = "lcm.spy.Spy", + visibility = ["//visibility:private"], + runtime_deps = [ + ":lcmtypes_c3_java", + "@drake//lcmtypes:lcmtypes_drake_java", + ], +) diff --git a/lcmtypes/__init__.py b/lcmtypes/__init__.py new file mode 100644 index 0000000..4c83c6c --- /dev/null +++ b/lcmtypes/__init__.py @@ -0,0 +1 @@ +# Empty Python module `__init__`, required to make this a module. \ No newline at end of file diff --git a/lcmtypes/lcmt_c3_trajectory.lcm b/lcmtypes/lcmt_c3_trajectory.lcm new file mode 100644 index 0000000..320e0a4 --- /dev/null +++ b/lcmtypes/lcmt_c3_trajectory.lcm @@ -0,0 +1,8 @@ +package c3; + +struct lcmt_c3_trajectory +{ + int64_t utime; + int32_t num_trajectories; + lcmt_trajectory trajectories[num_trajectories]; +} \ No newline at end of file diff --git a/lcmtypes/lcmt_contact_force.lcm b/lcmtypes/lcmt_contact_force.lcm new file mode 100644 index 0000000..ff9a700 --- /dev/null +++ b/lcmtypes/lcmt_contact_force.lcm @@ -0,0 +1,12 @@ +package c3; + +/* lcmtype containing information to visualize forces in meshcat +*/ +struct lcmt_contact_force +{ + int64_t utime; + + // These are all expressed in the world frame. + double contact_point[3]; + double contact_force[3]; +} \ No newline at end of file diff --git a/lcmtypes/lcmt_contact_forces.lcm b/lcmtypes/lcmt_contact_forces.lcm new file mode 100644 index 0000000..04aedc3 --- /dev/null +++ b/lcmtypes/lcmt_contact_forces.lcm @@ -0,0 +1,11 @@ +package c3; + +/* lcmtype containing information to visualize forces in meshcat +*/ +struct lcmt_contact_forces +{ + int64_t utime; + + int32_t num_forces; + lcmt_contact_force forces[num_forces]; +} \ No newline at end of file diff --git a/lcmtypes/lcmt_intermediates.lcm b/lcmtypes/lcmt_intermediates.lcm new file mode 100644 index 0000000..9290df7 --- /dev/null +++ b/lcmtypes/lcmt_intermediates.lcm @@ -0,0 +1,12 @@ +package c3; + +struct lcmt_intermediates +{ + int32_t num_points; + int32_t num_total_variables; + + float time_vec[num_points]; + float z_sol[num_total_variables][num_points]; + float delta_sol[num_total_variables][num_points]; + float w_sol[num_total_variables][num_points]; +} \ No newline at end of file diff --git a/lcmtypes/lcmt_output.lcm b/lcmtypes/lcmt_output.lcm new file mode 100644 index 0000000..bb0ad25 --- /dev/null +++ b/lcmtypes/lcmt_output.lcm @@ -0,0 +1,9 @@ +package c3; + +struct lcmt_output +{ + int64_t utime; + + lcmt_solution solution; + lcmt_intermediates intermediates; +} diff --git a/lcmtypes/lcmt_solution.lcm b/lcmtypes/lcmt_solution.lcm new file mode 100644 index 0000000..02ab156 --- /dev/null +++ b/lcmtypes/lcmt_solution.lcm @@ -0,0 +1,14 @@ +package c3; + +struct lcmt_solution +{ + int32_t num_points; + int32_t num_state_variables; + int32_t num_contact_variables; + int32_t num_input_variables; + + float time_vec[num_points]; + float x_sol[num_state_variables][num_points]; + float lambda_sol[num_contact_variables][num_points]; + float u_sol[num_input_variables][num_points]; +} diff --git a/lcmtypes/lcmt_trajectory.lcm b/lcmtypes/lcmt_trajectory.lcm new file mode 100644 index 0000000..225b72c --- /dev/null +++ b/lcmtypes/lcmt_trajectory.lcm @@ -0,0 +1,11 @@ +package c3; + +struct lcmt_trajectory +{ + string trajectory_name; + + int32_t num_timesteps; + int32_t vector_dim; + float timestamps[num_timesteps]; + float values[vector_dim][num_timesteps]; +} 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..7e767ae 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); + + // 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); + 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 @@ -172,7 +258,7 @@ Eigen::Matrix3d GeomGeomCollider::ComputePlanarForceBasis( template std::pair, VectorX> -GeomGeomCollider::CalcWitnessPoints(const Context& context) { +GeomGeomCollider::CalcWitnessPoints(const Context& context) { // Get common geometry query results const auto query_result = GetGeometryQueryResult(context); @@ -186,6 +272,24 @@ GeomGeomCollider::CalcWitnessPoints(const Context& context) { return std::pair, VectorX>(p_WCa, p_WCb); } +template +Matrix +GeomGeomCollider::CalcForceBasisInWorldFrame( + const Context& context, int num_friction_directions, + const Vector3d& planar_normal) const { + const auto query_result = GetGeometryQueryResult(context); + if (num_friction_directions < 1) { + // Planar contact: basis is constructed from the contact and planar normals. + return ComputePlanarForceBasis(-query_result.nhat_BA_W, planar_normal); + } else { + // 3D contact: build polytope basis and rotate using contact normal. + auto R_WC = drake::math::RotationMatrix::MakeFromOneVector( + -query_result.nhat_BA_W, 0); + return ComputePolytopeForceBasis(num_friction_directions) * + R_WC.matrix().transpose(); + } +} + } // namespace multibody } // namespace c3 diff --git a/multibody/geom_geom_collider.h b/multibody/geom_geom_collider.h index 14ea121..d17a294 100644 --- a/multibody/geom_geom_collider.h +++ b/multibody/geom_geom_collider.h @@ -99,21 +99,44 @@ class GeomGeomCollider { * closest point on geometry B. */ std::pair, drake::VectorX> CalcWitnessPoints( - const drake::systems::Context& context); + const drake::systems::Context& context); + + /** + * @brief Computes a basis for contact forces in the world frame. + * + * Depending on the number of friction directions, this method constructs + * either a planar (2D) or polytope (3D) basis for the contact forces at the + * collision point, expressed in the world frame. For planar contact + * (num_friction_directions < 1), the basis is constructed from the contact + * normal and the provided planar normal. For 3D contact, a polytope basis is + * generated and rotated to align with the contact normal. + * + * @param context The context for the MultibodyPlant. + * @param num_friction_directions The number of friction directions for the + * polytope approximation. If less than 1, a planar basis is used. + * @param planar_normal The normal vector defining the plane for planar + * contact (default: {0, 1, 0}). + * @return A matrix whose rows form an orthonormal basis for the contact + * forces in the world frame. + */ + Eigen::Matrix CalcForceBasisInWorldFrame( + const drake::systems::Context& context, int num_friction_directions, + const Eigen::Vector3d& planar_normal = {0, 1, 0}) const; private: /** * @brief A struct to hold the results of a geometry query. * * This struct contains the signed distance pair, the frame IDs of the two - * geometries, the frames themselves, and the positions of the closest points - * on each geometry, expressed in their respective frames. + * geometries, the frames themselves, and the positions of the closest + * points on each geometry, expressed in their respective frames. */ struct GeometryQueryResult { /** * @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 +240,66 @@ 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. + * + * @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..f690ae2 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,24 +132,9 @@ 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_); - } -} - -std::pair, std::vector> -LCSFactory::FindWitnessPoints() { - std::vector WCa; - std::vector WCb; - - for (int i = 0; i < n_contacts_; i++) { - multibody::GeomGeomCollider collider(plant_, contact_pairs_[i]); - auto [p_WCa, p_WCb] = collider.CalcWitnessPoints(context_); - WCa.push_back(p_WCa); - WCb.push_back(p_WCb); + 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_); } - - return std::make_pair(WCa, WCb); } void LCSFactory::UpdateStateAndInput( @@ -259,60 +296,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 +352,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(); @@ -384,56 +409,66 @@ LCS LCSFactory::LinearizePlantToLCS( return lcs_factory.GenerateLCS(); } -std::pair> -LCSFactory::GetContactJacobianAndPoints() { - VectorXd phi; // Signed distance values for contacts - MatrixXd Jn; // Normal contact Jacobian - MatrixXd Jt; // Tangential contact Jacobian - ComputeContactJacobian(phi, Jn, Jt); - auto [_, contact_points] = FindWitnessPoints(); - - if (frictionless_) { - // if frictionless_, we only need the normal jacobian - return std::make_pair(Jn, contact_points); - } - - 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_); - J_c << Jn, Jt; - return std::make_pair(J_c, contact_points); - } - - // Model is Anitescu - int n_lambda_ = 2 * n_contacts_ * n_friction_directions_; +std::vector LCSFactory::GetContactDescriptions() { + std::vector normal_contact_descriptions; + std::vector tangential_contact_descriptions; - // 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_); + multibody::GeomGeomCollider collider(plant_, contact_pairs_[i]); + auto [p_WCa, p_WCb] = collider.CalcWitnessPoints(context_); + auto force_basis = collider.CalcForceBasisInWorldFrame( + context_, n_friction_directions_per_contact_[i]); + + for (int j = 0; j < force_basis.rows(); j++) { + LCSContactDescription contact_description = { + .witness_point_A = p_WCa, + .witness_point_B = p_WCb, + .force_basis = force_basis.row(j)}; + if (j == 0) + // Normal contact + normal_contact_descriptions.push_back(contact_description); + else + // Tangential contact + tangential_contact_descriptions.push_back(contact_description); + } } - // Apply same friction coefficients to each friction direction - // of the same contact - if (!frictionless_) DRAKE_DEMAND(mu_.size() == (size_t)n_contacts_); - VectorXd muXd = - Eigen::Map(mu_.data(), mu_.size()); - - 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_); + std::vector contact_descriptions; + if (contact_model_ == ContactModel::kFrictionlessSpring) + contact_descriptions.insert(contact_descriptions.end(), + normal_contact_descriptions.begin(), + normal_contact_descriptions.end()); + else if (contact_model_ == ContactModel::kStewartAndTrinkle) { + for (int i = 0; i < n_contacts_; i++) + contact_descriptions.push_back( + LCSContactDescription::CreateSlackVariableDescription()); + contact_descriptions.insert(contact_descriptions.end(), + normal_contact_descriptions.begin(), + normal_contact_descriptions.end()); + contact_descriptions.insert(contact_descriptions.end(), + tangential_contact_descriptions.begin(), + tangential_contact_descriptions.end()); + } else if (contact_model_ == ContactModel::kAnitescu) { + contact_descriptions.insert(contact_descriptions.end(), + tangential_contact_descriptions.begin(), + tangential_contact_descriptions.end()); + for (int normal_index = 0; normal_index < n_contacts_; normal_index++) { + // Jt_row_sizes_ gives number of friction directions per contact + for (int i = 0; i < Jt_row_sizes_(normal_index); i++) { + int tangential_index = Jt_row_sizes_.segment(0, normal_index).sum() + i; + DRAKE_ASSERT( + contact_descriptions.at(tangential_index).witness_point_A == + normal_contact_descriptions.at(normal_index).witness_point_A); + contact_descriptions.at(tangential_index).force_basis = + mu_[normal_index] * + contact_descriptions.at(tangential_index).force_basis + + normal_contact_descriptions.at(normal_index).force_basis; + contact_descriptions.at(tangential_index).force_basis.normalize(); + } + } } - MatrixXd mu_matrix = mu_vector.asDiagonal(); - // Constructing friction bases Jc = EᵀJₙ + μJₜ - MatrixXd J_c = E_t.transpose() * Jn + mu_matrix * Jt; - return std::make_pair(J_c, contact_points); + return contact_descriptions; } LCS LCSFactory::FixSomeModes(const LCS& other, set active_lambda_inds, @@ -542,29 +577,66 @@ LCS LCSFactory::FixSomeModes(const LCS& other, set active_lambda_inds, return LCS(A, B, D, d, E, F, H, c, other.dt()); } -int LCSFactory::GetNumContactVariables(ContactModel contact_model, - int num_contacts, - int num_friction_directions) { +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 if (contact_model == ContactModel::kStewartAndTrinkle) { - return 2 * num_contacts + - 2 * num_contacts * - num_friction_directions; // Compute contact variable count - // for Stewart-Trinkle model - } else if (contact_model == ContactModel::kAnitescu) { - return 2 * num_contacts * - num_friction_directions; // Compute contact variable + } 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(ContactModel contact_model, + int num_contacts, + int num_friction_directions) { + std::vector num_friction_directions_per_contact(num_contacts, + num_friction_directions); + return GetNumContactVariables(contact_model, num_contacts, + num_friction_directions_per_contact); +} + 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..8620d54 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 { @@ -46,6 +47,19 @@ inline const std::map& GetContactModelMap() { return kContactModelMap; } +struct LCSContactDescription { + Eigen::Vector3d witness_point_A; ///< Witness point on geometry A. + Eigen::Vector3d witness_point_B; ///< Witness point on geometry B. + Eigen::Vector3d force_basis; ///< Force basis vector + bool is_slack = false; ///< Indicates if the contact variable associate to + ///< the LCS is a slack variable. + + static LCSContactDescription CreateSlackVariableDescription() { + return {Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), + Eigen::Vector3d::Zero(), true}; + } +}; + /** * @class LCSFactory * @brief Factory class for creating Linear Complementarity Systems (LCS) from @@ -53,6 +67,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. * @@ -84,16 +104,12 @@ class LCSFactory { LCS GenerateLCS(); /** - * @brief Computes the contact Jacobian for a given multibody plant and - * context. - * - * This method calculates the signed distance values and the contact Jacobians - * for normal and tangential forces at the specified contact points. + * @brief Finds the witness points for each contact pair. * - * @return A pair containing the contact Jacobian matrix and a vector of - * contact points. + * @return A pair of vectors containing the witness points on each geometry + * for each contact pair. */ - std::pair> GetContactJacobianAndPoints(); + std::vector GetContactDescriptions(); /** * @brief Updates the state and input vectors in the internal context. @@ -154,21 +170,37 @@ class LCSFactory { /** * @brief Computes the number of contact variables based on the contact model - * and options. + * and number of contacts. * * @param contact_model The contact model to use. * @param num_contacts The number of contact points. - * @param num_friction_directions The number of friction directions. - * @param frictionless Whether the contacts are frictionless. + * @param num_friction_directions The total number of friction directions. * @return int The number of contact variables. */ static int GetNumContactVariables(ContactModel contact_model, int num_contacts, int num_friction_directions); + /** + * @brief Computes the number of contact variables based on the contact model + * and per-contact friction directions. + * + * @param contact_model The contact model to use. + * @param num_contacts The number of contact points. + * @param num_friction_directions_per_contact The number of friction + * directions for each contact. + * @return int The number of contact variables. + */ + 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. * + * This is the preferred overload as it encapsulates all contact model and + * friction configuration in a single options object. + * * @param options The LCS options specifying contact model and friction * properties. * @return int The number of contact variables. @@ -272,36 +304,31 @@ class LCSFactory { */ void ComputeContactJacobian(VectorXd& phi, MatrixXd& Jn, MatrixXd& Jt); - /** - * @brief Finds the witness points for each contact pair. - * - * @return A pair of vectors containing the witness points on each geometry - * for each contact pair. - */ - std::pair, std::vector> FindWitnessPoints(); - // References to the MultibodyPlant and its contexts const drake::multibody::MultibodyPlant& plant_; 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..e27d664 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; + double 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..73f0251 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,283 @@ 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(); - auto [initial_J, initial_contact_points] = - lcs_factory->GetContactJacobianAndPoints(); +// 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_contact_descriptions = + fixture.lcs_factory->GetContactDescriptions(); - 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(); - auto [updated_J, updated_contact_points] = - lcs_factory->GetContactJacobianAndPoints(); + // Generate updated LCS + LCS updated_lcs = fixture.lcs_factory->GenerateLCS(); + auto updated_contact_descriptions = + fixture.lcs_factory->GetContactDescriptions(); 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()); } - 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]); + // Contact Jacobian and points should change + for (size_t i = 0; i < initial_contact_descriptions.size(); ++i) { + if (initial_contact_descriptions[i].is_slack) continue; + EXPECT_NE(initial_contact_descriptions[i].witness_point_A, + updated_contact_descriptions[i].witness_point_A); + EXPECT_NE(initial_contact_descriptions[i].witness_point_B, + updated_contact_descriptions[i].witness_point_B); + EXPECT_NE(initial_contact_descriptions[i].force_basis, + updated_contact_descriptions[i].force_basis); } } -TEST_P(LCSFactoryPivotingTest, ComputeContactJacobian) { - auto [J, contact_points] = lcs_factory->GetContactJacobianAndPoints(); +// Test contact Jacobian computation for different contact models +TEST_P(LCSFactoryParameterizedPivotingTest, CheckContactDescriptionSizes) { + auto contact_descriptions = fixture.lcs_factory->GetContactDescriptions(); + + 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); + + for (size_t i = 0; i < contact_descriptions.size(); ++i) { + if (contact_descriptions[i].is_slack) continue; + EXPECT_FALSE(contact_descriptions[i].witness_point_A.isZero()); + EXPECT_FALSE(contact_descriptions[i].witness_point_B.isZero()); + EXPECT_FALSE(contact_descriptions[i].force_basis.isZero()); + } - 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(contact_descriptions.size(), + 2*n_contacts + n_tangential_directions); break; case ContactModel::kFrictionlessSpring: - EXPECT_EQ(J.rows(), n_contacts); + // Normal directions only + EXPECT_EQ(contact_descriptions.size(), 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(contact_descriptions.size(), 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()); - 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 +361,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 +} // namespace c3 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/BUILD.bazel b/systems/BUILD.bazel index 29eb400..3054a8a 100644 --- a/systems/BUILD.bazel +++ b/systems/BUILD.bazel @@ -12,6 +12,7 @@ cc_library( ":c3_controller", ":lcs_simulator", ":lcs_factory_system", + ":lcmt_generators", ] ) @@ -26,6 +27,7 @@ cc_library( "framework/timestamped_vector.h", ], deps = [ + "//lcmtypes:lcmt_c3", "@drake//:drake_shared_library", ], ) @@ -85,10 +87,29 @@ cc_library( deps = [ "//core:options", "//multibody:options", - "@drake//:drake_shared_library", ] ) +cc_library( + name = "lcmt_generators", + srcs = [ + "lcmt_generators/c3_output_generator.cc", + "lcmt_generators/contact_force_generator.cc", + "lcmt_generators/c3_trajectory_generator.cc", + ], + hdrs = [ + "lcmt_generators/c3_output_generator.h", + "lcmt_generators/contact_force_generator.h", + "lcmt_generators/c3_trajectory_generator.h", + "lcmt_generators/c3_trajectory_generator_config.h", + ], + deps = [ + ":framework", + "//multibody:lcs_factory", + "//lcmtypes:lcmt_c3", + "@drake//:drake_shared_library", + ], +) cc_library( @@ -100,6 +121,7 @@ cc_library( "@drake//:drake_shared_library", ], ) + cc_test( name = "systems_test", srcs = [ @@ -131,11 +153,27 @@ cc_test( ], ) +cc_test( + name = "generators_test", + srcs = [ + "test/generators_test.cc" + ], + deps = [ + ":lcmt_generators", + "//core:c3_cartpole_problem", + "@gtest//:main", + ], +) + filegroup( name = "headers", srcs = glob([ "*.h", "**/*.h", ]), +) + +exports_files( + glob(["**/*.h", "**/*.hpp"]), visibility = ["//visibility:public"], ) diff --git a/systems/c3_controller.cc b/systems/c3_controller.cc index 85d4315..deb081f 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); @@ -106,8 +111,7 @@ C3Controller::C3Controller( .get_index(); c3_intermediates_port_ = this->DeclareAbstractOutputPort( - "intermediates", - C3Output::C3Intermediates(n_x_, n_lambda_, n_u_, N_), + "intermediates", C3Output::C3Intermediates(c3_->GetZSize(), N_), &C3Controller::OutputC3Intermediates) .get_index(); diff --git a/systems/c3_controller.h b/systems/c3_controller.h index 8a8df72..254c379 100644 --- a/systems/c3_controller.h +++ b/systems/c3_controller.h @@ -98,6 +98,10 @@ class C3Controller : public drake::systems::LeafSystem { c3_->AddLinearConstraint(A, lower_bound, upper_bound, constraint); } + void SetSolverOptions(const drake::solvers::SolverOptions& options) { + c3_->SetSolverOptions(options); + } + private: /** * @struct JointDescription diff --git a/systems/c3_controller_options.h b/systems/c3_controller_options.h index 52bdea3..ef272f0 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_vector.size()) == + expected_lambda_size); + DRAKE_DEMAND(static_cast(c3_options.u_eta_vector.size()) == + expected_lambda_size); + } } }; diff --git a/systems/framework/c3_output.cc b/systems/framework/c3_output.cc index 33f52e1..4d4a6d5 100644 --- a/systems/framework/c3_output.cc +++ b/systems/framework/c3_output.cc @@ -9,5 +9,73 @@ namespace systems { C3Output::C3Output(C3Solution c3_sol, C3Intermediates c3_intermediates) : c3_solution_(c3_sol), c3_intermediates_(c3_intermediates) {} + +lcmt_output C3Output::GenerateLcmObject(double time) const { + lcmt_output c3_output; + lcmt_solution c3_solution; + lcmt_intermediates c3_intermediates; + c3_output.utime = static_cast(1e6 * time); + + c3_solution.num_points = c3_solution_.time_vector_.size(); + int knot_points = c3_solution.num_points; + c3_solution.num_state_variables = c3_solution_.x_sol_.rows(); + c3_solution.num_contact_variables = c3_solution_.lambda_sol_.rows(); + c3_solution.num_input_variables = c3_solution_.u_sol_.rows(); + c3_solution.time_vec.reserve(knot_points); + c3_solution.x_sol = vector>(c3_solution.num_state_variables, + vector(knot_points)); + c3_solution.lambda_sol = vector>( + c3_solution.num_contact_variables, vector(knot_points)); + c3_solution.u_sol = vector>(c3_solution.num_input_variables, + vector(knot_points)); + + c3_solution.time_vec = vector( + c3_solution_.time_vector_.data(), + c3_solution_.time_vector_.data() + c3_solution_.time_vector_.size()); + + c3_intermediates.num_total_variables = c3_intermediates_.delta_.rows(); + c3_intermediates.num_points = c3_solution.num_points; + c3_intermediates.time_vec.reserve(c3_intermediates.num_points); + c3_intermediates.z_sol = vector>( + c3_intermediates.num_total_variables, vector(knot_points)); + c3_intermediates.delta_sol = vector>( + c3_intermediates.num_total_variables, vector(knot_points)); + c3_intermediates.w_sol = vector>( + c3_intermediates.num_total_variables, vector(knot_points)); + c3_intermediates.time_vec = c3_solution.time_vec; + + for (int i = 0; i < c3_solution.num_state_variables; ++i) { + VectorXf temp_row = c3_solution_.x_sol_.row(i); + memcpy(c3_solution.x_sol[i].data(), temp_row.data(), + sizeof(float) * knot_points); + } + for (int i = 0; i < c3_solution.num_contact_variables; ++i) { + VectorXf temp_row = c3_solution_.lambda_sol_.row(i); + memcpy(c3_solution.lambda_sol[i].data(), temp_row.data(), + sizeof(float) * knot_points); + } + for (int i = 0; i < c3_solution.num_input_variables; ++i) { + VectorXf temp_row = c3_solution_.u_sol_.row(i); + memcpy(c3_solution.u_sol[i].data(), temp_row.data(), + sizeof(float) * knot_points); + } + for (int i = 0; i < c3_intermediates.num_total_variables; ++i) { + VectorXf temp_z_row = c3_intermediates_.z_.row(i); + VectorXf temp_delta_row = c3_intermediates_.delta_.row(i); + VectorXf temp_w_row = c3_intermediates_.w_.row(i); + memcpy(c3_intermediates.z_sol[i].data(), temp_z_row.data(), + sizeof(float) * knot_points); + memcpy(c3_intermediates.delta_sol[i].data(), temp_delta_row.data(), + sizeof(float) * knot_points); + memcpy(c3_intermediates.w_sol[i].data(), temp_w_row.data(), + sizeof(float) * knot_points); + } + + c3_output.solution = c3_solution; + c3_output.intermediates = c3_intermediates; + + return c3_output; +} + } // namespace systems } // namespace c3 diff --git a/systems/framework/c3_output.h b/systems/framework/c3_output.h index 319fa13..0ac4e74 100644 --- a/systems/framework/c3_output.h +++ b/systems/framework/c3_output.h @@ -7,6 +7,8 @@ #include +#include "c3/lcmt_output.hpp" + using Eigen::MatrixXf; using Eigen::VectorXf; @@ -27,6 +29,14 @@ class C3Output { time_vector_ = VectorXf::Zero(N); }; + Eigen::MatrixXf GetStateSolution() const { return x_sol_; } + + Eigen::MatrixXf GetForceSolution() const { return lambda_sol_; } + + Eigen::MatrixXf GetInputSolution() const { return u_sol_; } + + Eigen::VectorXf GetTimeVector() const { return time_vector_; } + // Shape is (variable_vector_size, knot_points) Eigen::VectorXf time_vector_; Eigen::MatrixXf x_sol_; @@ -42,6 +52,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_; @@ -57,6 +73,8 @@ class C3Output { virtual ~C3Output() = default; + lcmt_output GenerateLcmObject(double time) const; + private: C3Solution c3_solution_; C3Intermediates c3_intermediates_; diff --git a/systems/lcmt_generators/c3_output_generator.cc b/systems/lcmt_generators/c3_output_generator.cc new file mode 100644 index 0000000..ebc5e39 --- /dev/null +++ b/systems/lcmt_generators/c3_output_generator.cc @@ -0,0 +1,75 @@ +#include "systems/lcmt_generators/c3_output_generator.h" + +#include "systems/framework/c3_output.h" + +using drake::systems::Context; +using drake::systems::DiagramBuilder; +using drake::systems::lcm::LcmPublisherSystem; + +namespace c3 { +namespace systems { +namespace lcmt_generators { + +// Publishes C3Output as an LCM message. +C3OutputGenerator::C3OutputGenerator() { + // Declare input port for the C3 solution. + c3_solution_port_ = + this->DeclareAbstractInputPort("c3_solution", + drake::Value{}) + .get_index(); + // Declare input port for C3 intermediates. + c3_intermediates_port_ = + this->DeclareAbstractInputPort("c3_intermediates", + drake::Value{}) + .get_index(); + + this->set_name("c3_output_publisher"); + // Declare output port for the LCM message. + c3_output_port_ = + this->DeclareAbstractOutputPort("lcmt_c3_output", c3::lcmt_output(), + &C3OutputGenerator::DoCalc) + .get_index(); +} + +// Calculates the LCM output message from the input ports. +void C3OutputGenerator::DoCalc(const drake::systems::Context& context, + c3::lcmt_output* output) const { + // Retrieve input values. + const auto& c3_solution = + this->EvalInputValue(context, c3_solution_port_); + const auto& c3_intermediates = + this->EvalInputValue(context, + c3_intermediates_port_); + + // Construct C3Output and generate the LCM message. + C3Output c3_output = C3Output(*c3_solution, *c3_intermediates); + *output = c3_output.GenerateLcmObject(context.get_time()); +} + +// Adds this publisher and an LCM publisher system to the diagram builder. +LcmPublisherSystem* C3OutputGenerator::AddLcmPublisherToBuilder( + DiagramBuilder& builder, + const drake::systems::OutputPort& solution_port, + const drake::systems::OutputPort& intermediates_port, + const std::string& channel, drake::lcm::DrakeLcmInterface* lcm, + const drake::systems::TriggerTypeSet& publish_triggers, + double publish_period, double publish_offset) { + // Add and connect the C3OutputGenerator system. + auto output_publisher = builder.AddSystem(); + builder.Connect(solution_port, + output_publisher->get_input_port_c3_solution()); + builder.Connect(intermediates_port, + output_publisher->get_input_port_c3_intermediates()); + + // Add and connect the LCM publisher system. + auto lcm_output_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + channel, lcm, publish_triggers, publish_period, publish_offset)); + builder.Connect(output_publisher->get_output_port_c3_output(), + lcm_output_publisher->get_input_port()); + return lcm_output_publisher; +} + +} // namespace lcmt_generators +} // namespace systems +} // namespace c3 \ No newline at end of file diff --git a/systems/lcmt_generators/c3_output_generator.h b/systems/lcmt_generators/c3_output_generator.h new file mode 100644 index 0000000..0b7d8fa --- /dev/null +++ b/systems/lcmt_generators/c3_output_generator.h @@ -0,0 +1,93 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include "c3/lcmt_output.hpp" + +namespace c3 { +namespace systems { +namespace lcmt_generators { + +/** + * @class C3OutputGenerator + * @brief Converts OutputVector objects to LCM type lcmt_output for publishing. + * @details + * - Provides input ports for C3 solution and intermediates. + * - Provides an output port for the constructed lcmt_output message. + * - Can be connected to an LCM publisher system for message transmission. + */ +class C3OutputGenerator : public drake::systems::LeafSystem { + public: + /** + * @brief Constructor. Declares input and output ports. + */ + C3OutputGenerator(); + + /** + * @brief Returns the input port for the C3 solution vector. + */ + const drake::systems::InputPort& get_input_port_c3_solution() const { + return this->get_input_port(c3_solution_port_); + } + + /** + * @brief Returns the input port for the C3 intermediates vector. + */ + const drake::systems::InputPort& get_input_port_c3_intermediates() + const { + return this->get_input_port(c3_intermediates_port_); + } + + /** + * @brief Returns the output port for the lcmt_output message. + */ + const drake::systems::OutputPort& get_output_port_c3_output() const { + return this->get_output_port(c3_output_port_); + } + + /** + * @brief Adds an LCM publisher system to the given DiagramBuilder. + * @param builder The diagram builder to add the publisher to. + * @param solution_port Output port for the solution vector. + * @param intermediates_port Output port for the intermediates vector. + * @param channel LCM channel name. + * @param lcm LCM interface pointer. + * @param publish_triggers Set of triggers for publishing. + * @param publish_period Period for periodic publishing (optional). + * @param publish_offset Offset for periodic publishing (optional). + * @return Pointer to the created LcmPublisherSystem. + */ + static drake::systems::lcm::LcmPublisherSystem* AddLcmPublisherToBuilder( + drake::systems::DiagramBuilder& builder, + const drake::systems::OutputPort& solution_port, + const drake::systems::OutputPort& intermediates_port, + const std::string& channel, drake::lcm::DrakeLcmInterface* lcm, + const drake::systems::TriggerTypeSet& publish_triggers, + double publish_period = 0.0, double publish_offset = 0.0); + + private: + /** + * @brief Calculates the lcmt_output message from the input ports. + * @param context The system context. + * @param output The output message to populate. + */ + void DoCalc(const drake::systems::Context& context, + c3::lcmt_output* output) const; + + drake::systems::InputPortIndex + c3_solution_port_; /**< Index for solution input port. */ + drake::systems::InputPortIndex + c3_intermediates_port_; /**< Index for intermediates input port. */ + drake::systems::OutputPortIndex + c3_output_port_; /**< Index for output port. */ +}; + +} // namespace lcmt_generators +} // namespace systems +} // namespace c3 \ No newline at end of file diff --git a/systems/lcmt_generators/c3_trajectory_generator.cc b/systems/lcmt_generators/c3_trajectory_generator.cc new file mode 100644 index 0000000..af4e4a2 --- /dev/null +++ b/systems/lcmt_generators/c3_trajectory_generator.cc @@ -0,0 +1,119 @@ +#include "systems/lcmt_generators/c3_trajectory_generator.h" + +#include "systems/framework/c3_output.h" + +namespace c3 { +namespace systems { +namespace lcmt_generators { + +C3TrajectoryGenerator::C3TrajectoryGenerator(C3TrajectoryGeneratorConfig config) + : config_(config) { + // Declare input ports for solution and intermediates. + c3_solution_input_port_ = + this->DeclareAbstractInputPort("c3_solution", + drake::Value()) + .get_index(); + + // Declare output port for lcmt_trajectory message. + lcmt_c3_trajectory_output_port_ = + this->DeclareAbstractOutputPort( + "lcmt_c3_trajectory", &C3TrajectoryGenerator::GenerateTrajectory) + .get_index(); + + // this->set_name("c3_trajectory_generator"); +} + +void C3TrajectoryGenerator::GenerateTrajectory( + const drake::systems::Context& context, + lcmt_c3_trajectory* output) const { + // Get input data from the input port + const auto* solution = this->EvalInputValue( + context, c3_solution_input_port_); + + Eigen::VectorXf time_vector = solution->GetTimeVector(); + Eigen::MatrixXf solution_values; + output->num_trajectories = config_.trajectories.size(); + output->trajectories.clear(); + + // Iterate over each trajectory description in config + for (const auto& traj_desc : config_.trajectories) { + lcmt_trajectory trajectory; + trajectory.trajectory_name = traj_desc.trajectory_name; + + // Select the appropriate solution matrix based on variable type + if (traj_desc.variable_type == "state") { + solution_values = solution->GetStateSolution(); + } else if (traj_desc.variable_type == "input") { + solution_values = solution->GetInputSolution(); + } else if (traj_desc.variable_type == "force") { + solution_values = solution->GetForceSolution(); + } else { + throw std::runtime_error( + "Unknown variable type in C3 trajectory description."); + } + + DRAKE_ASSERT(time_vector.size() == solution_values.cols()); + + // Copy timestamps for all timesteps + trajectory.num_timesteps = solution_values.cols(); + trajectory.timestamps.reserve(trajectory.num_timesteps); + trajectory.timestamps.assign(time_vector.data(), + time_vector.data() + trajectory.num_timesteps); + + trajectory.values.clear(); + // Extract each index range specified in the trajectory description + auto indices = traj_desc.indices; + if (indices.empty()) { + TrajectoryDescription::index_range default_indices = { + .start = 0, .end = (int)solution_values.rows() - 1}; + indices.push_back(default_indices); + } + + for (const auto& i : indices) { + int start = i.start; + int end = i.end; + if (start < 0 || end >= solution_values.rows()) { + throw std::out_of_range("Index out of range in C3 solution."); + } + + // Copy the relevant rows from the solution matrix + for (int i = start; i <= end; ++i) { + std::vector row(trajectory.num_timesteps); + Eigen::VectorXf solution_row = solution_values.row(i); + memcpy(row.data(), solution_row.data(), + sizeof(float) * trajectory.num_timesteps); + trajectory.values.push_back(row); + } + } + // Set the number of timesteps and vector dimension for the trajectory + trajectory.vector_dim = (int32_t)trajectory.values.size(); + // Add the constructed trajectory to the output message + output->trajectories.push_back(trajectory); + } + + // Set the timestamp for the output message + output->utime = context.get_time() * 1e6; +} + +drake::systems::lcm::LcmPublisherSystem* +C3TrajectoryGenerator::AddLcmPublisherToBuilder( + drake::systems::DiagramBuilder& builder, + const C3TrajectoryGeneratorConfig config, + const drake::systems::OutputPort& c3_solution_port, + const std::string& channel, drake::lcm::DrakeLcmInterface* lcm, + const drake::systems::TriggerTypeSet& publish_triggers, + double publish_period, double publish_offset) { + auto traj_gen = builder.AddSystem(config); + builder.Connect(c3_solution_port, traj_gen->get_input_port_c3_solution()); + + auto lcm_pub = builder.AddSystem( + drake::systems::lcm::LcmPublisherSystem::Make( + channel, lcm, publish_triggers, publish_period, publish_offset)); + builder.Connect(traj_gen->get_output_port_lcmt_c3_trajectory(), + lcm_pub->get_input_port()); + return lcm_pub; +} + +} // namespace lcmt_generators +} // namespace systems +} // namespace c3 \ No newline at end of file diff --git a/systems/lcmt_generators/c3_trajectory_generator.h b/systems/lcmt_generators/c3_trajectory_generator.h new file mode 100644 index 0000000..c783c4d --- /dev/null +++ b/systems/lcmt_generators/c3_trajectory_generator.h @@ -0,0 +1,77 @@ +#include +#include +#include +#include + +#include "c3/lcmt_c3_trajectory.hpp" +#include "c3/lcmt_trajectory.hpp" +#include "systems/lcmt_generators/c3_trajectory_generator_config.h" + +namespace c3 { +namespace systems { +namespace lcmt_generators { + +/** + * @class C3TrajectoryGenerator + * @brief Drake LeafSystem for generating lcmt_trajectory messages from input + * data. + * + * The C3TrajectoryGenerator system takes trajectory solution and intermediate + * data as input, and produces an lcmt_trajectory message as output. It is + * configurable via the TrajectoryGeneratorOptions struct. + */ +class C3TrajectoryGenerator : public drake::systems::LeafSystem { + public: + /** + * @brief Constructor. Declares input and output ports. + * @param config Configuration options for the trajectory generator. + */ + C3TrajectoryGenerator(C3TrajectoryGeneratorConfig config); + + /** + * @brief Returns the input port for the trajectory solution data. + * @return Reference to the input port for c3 solution data. + */ + const drake::systems::InputPort& get_input_port_c3_solution() const { + return this->get_input_port(c3_solution_input_port_); + } + + /** + * @brief Returns the output port for the generated lcmt_trajectory message. + * @return Reference to the output port for lcmt_trajectory. + */ + const drake::systems::OutputPort& get_output_port_lcmt_c3_trajectory() + const { + return this->get_output_port(lcmt_c3_trajectory_output_port_); + } + + static drake::systems::lcm::LcmPublisherSystem* AddLcmPublisherToBuilder( + drake::systems::DiagramBuilder& builder, + const C3TrajectoryGeneratorConfig config, + const drake::systems::OutputPort& c3_solution_port, + const std::string& channel, drake::lcm::DrakeLcmInterface* lcm, + const drake::systems::TriggerTypeSet& publish_triggers, + double publish_period = 0.0, double publish_offset = 0.0); + + private: + // Input port index for the trajectory solution data. + drake::systems::InputPortIndex c3_solution_input_port_; + + // Output port index for the generated lcmt_trajectory message. + drake::systems::OutputPortIndex lcmt_c3_trajectory_output_port_; + + // Options for configuring the trajectory generator. + C3TrajectoryGeneratorConfig config_; + + /** + * @brief Generates the lcmt_trajectory message from input data. + * @param context The system context containing input values. + * @param output Pointer to the lcmt_trajectory message to populate. + */ + void GenerateTrajectory(const drake::systems::Context& context, + lcmt_c3_trajectory* output) const; +}; + +} // namespace lcmt_generators +} // namespace systems +} // namespace c3 \ No newline at end of file diff --git a/systems/lcmt_generators/c3_trajectory_generator_config.h b/systems/lcmt_generators/c3_trajectory_generator_config.h new file mode 100644 index 0000000..a2f2243 --- /dev/null +++ b/systems/lcmt_generators/c3_trajectory_generator_config.h @@ -0,0 +1,46 @@ +#pragma once + +#include +#include + +namespace c3 { +namespace systems { +namespace lcmt_generators { + +struct TrajectoryDescription { + struct index_range { + int start; // Start index of the range + int end; // End index of the range + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(start)); + a->Visit(DRAKE_NVP(end)); + } + }; + std::string trajectory_name; // Name of the trajectory + std::string variable_type; // Type of C3 variable ["state", "input", "force"] + std::vector indices; + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(trajectory_name)); + a->Visit(DRAKE_NVP(variable_type)); + + DRAKE_ASSERT(variable_type == "state" || variable_type == "input" || + variable_type == "force"); + + a->Visit(DRAKE_NVP(indices)); + } +}; + +struct C3TrajectoryGeneratorConfig { + std::vector + trajectories; // List of trajectories to generate + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(trajectories)); + } +}; + +} // namespace lcmt_generators +} // namespace systems +} // namespace c3 \ No newline at end of file diff --git a/systems/lcmt_generators/contact_force_generator.cc b/systems/lcmt_generators/contact_force_generator.cc new file mode 100644 index 0000000..4b31ac2 --- /dev/null +++ b/systems/lcmt_generators/contact_force_generator.cc @@ -0,0 +1,111 @@ +#include "systems/lcmt_generators/contact_force_generator.h" + +#include + +#include "multibody/lcs_factory.h" +#include "systems/framework/c3_output.h" + +// Drake and Eigen namespace usages for convenience. +using drake::systems::Context; +using drake::systems::DiagramBuilder; +using drake::systems::lcm::LcmPublisherSystem; + +using Eigen::MatrixXd; +using Eigen::Quaterniond; +using Eigen::VectorXd; + +namespace c3 { + +using multibody::LCSContactDescription; + +namespace systems { +namespace lcmt_generators { + +// Publishes contact force information to LCM for visualization and logging. +ContactForceGenerator::ContactForceGenerator() { + // Declare input port for the C3 solution. + c3_solution_port_ = this->DeclareAbstractInputPort( + "solution", drake::Value{}) + .get_index(); + // Declare input port for contact Jacobian and contact points. + lcs_contact_info_port_ = + this->DeclareAbstractInputPort( + "contact_descriptions", + drake::Value>()) + .get_index(); + + this->set_name("c3_contact_force_generator"); + // Declare output port for publishing contact forces. + contact_force_output_port_ = + this->DeclareAbstractOutputPort("lcmt_force", lcmt_contact_forces(), + &ContactForceGenerator::DoCalc) + .get_index(); +} + +// Calculates and outputs the contact forces based on the current context. +void ContactForceGenerator::DoCalc(const Context& context, + lcmt_contact_forces* output) const { + // Get Solution from C3 + const auto& solution = + this->EvalInputValue(context, c3_solution_port_); + // Get Contact infromation form LCS Factory + const auto& contact_info = + this->EvalInputValue>( + context, lcs_contact_info_port_); + + output->forces.clear(); + output->num_forces = 0; + // Iterate over all contact points and compute forces. + for (size_t i = 0; i < contact_info->size(); ++i) { + auto force = lcmt_contact_force(); + + if (contact_info->at(i).is_slack) + // If the contact is slack, ignore it. + continue; + + // Set contact point position. + force.contact_point[0] = contact_info->at(i).witness_point_B[0]; + force.contact_point[1] = contact_info->at(i).witness_point_B[1]; + force.contact_point[2] = contact_info->at(i).witness_point_B[2]; + + // If the contact is not slack, compute the force. + force.contact_force[0] = + contact_info->at(i).force_basis[0] * solution->lambda_sol_(i, 0); + force.contact_force[1] = + contact_info->at(i).force_basis[1] * solution->lambda_sol_(i, 0); + force.contact_force[2] = + contact_info->at(i).force_basis[2] * solution->lambda_sol_(i, 0); + + output->forces.push_back(force); + } + // Set output timestamp in microseconds. + output->num_forces = output->forces.size(); + output->utime = context.get_time() * 1e6; +} + +// Adds this publisher and an LCM publisher system to the diagram builder. +LcmPublisherSystem* ContactForceGenerator::AddLcmPublisherToBuilder( + DiagramBuilder& builder, + const drake::systems::OutputPort& solution_port, + const drake::systems::OutputPort& lcs_contact_descriptions_port, + const std::string& channel, drake::lcm::DrakeLcmInterface* lcm, + const drake::systems::TriggerTypeSet& publish_triggers, + double publish_period, double publish_offset) { + // Add and connect the ContactForceGenerator system. + auto force_publisher = builder.AddSystem(); + builder.Connect(solution_port, force_publisher->get_input_port_c3_solution()); + builder.Connect(lcs_contact_descriptions_port, + force_publisher->get_input_port_lcs_contact_descriptions()); + + // Add and connect the LCM publisher system. + auto lcm_force_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + channel, lcm, publish_triggers, publish_period, publish_offset)); + builder.Connect(force_publisher->get_output_port_contact_force(), + lcm_force_publisher->get_input_port()); + return lcm_force_publisher; +} + +} // namespace lcmt_generators +} // namespace systems +} // namespace c3 \ No newline at end of file diff --git a/systems/lcmt_generators/contact_force_generator.h b/systems/lcmt_generators/contact_force_generator.h new file mode 100644 index 0000000..eef266d --- /dev/null +++ b/systems/lcmt_generators/contact_force_generator.h @@ -0,0 +1,110 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include "c3/lcmt_contact_forces.hpp" + +namespace c3 { +namespace systems { +namespace lcmt_generators { + +/** + * @class ContactForceGenerator + * @brief Converts solution vectors and LCS contact information into LCM contact + * force messages for publishing. + * @details + * - Provides input ports for the C3 solution vector and LCS contact + * information. + * - Computes contact forces based on the provided inputs. + * - Offers an output port for the constructed contact force message. + * - Includes a static helper to add an LCM publisher system to a + * DiagramBuilder for message transmission. + */ +class ContactForceGenerator : public drake::systems::LeafSystem { + public: + /** + * @brief Constructs a ContactForceGenerator system. + * + * Declares input and output ports for the solution vector, LCS contact info, + * and contact force output. + */ + ContactForceGenerator(); + + /** + * @brief Returns the input port for the c3 solution vector. + * @return Reference to the input port for the c3 solution. + */ + const drake::systems::InputPort& get_input_port_c3_solution() const { + return this->get_input_port(c3_solution_port_); + } + + /** + * @brief Returns the input port for the LCS contact information. + * @return Reference to the input port for LCS contact info. + */ + const drake::systems::InputPort& + get_input_port_lcs_contact_descriptions() const { + return this->get_input_port(lcs_contact_info_port_); + } + + /** + * @brief Returns the output port for the computed contact forces. + * @return Reference to the output port for contact forces. + */ + const drake::systems::OutputPort& get_output_port_contact_force() + const { + return this->get_output_port(contact_force_output_port_); + } + + /** + * @brief Adds an LCM publisher system to the given DiagramBuilder for + * publishing contact forces. + * + * @param builder The DiagramBuilder to which the publisher will be added. + * @param solution_port Output port providing the solution vector. + * @param lcs_contact_info_port Output port providing LCS contact information. + * @param channel The LCM channel name to publish on. + * @param lcm The LCM interface to use for publishing. + * @param publish_triggers Set of triggers that determine when publishing + * occurs. + * @param publish_period Optional period for periodic publishing (default: + * 0.0). + * @param publish_offset Optional offset for periodic publishing (default: + * 0.0). + * @return Pointer to the created LcmPublisherSystem. + */ + static drake::systems::lcm::LcmPublisherSystem* AddLcmPublisherToBuilder( + drake::systems::DiagramBuilder& builder, + const drake::systems::OutputPort& solution_port, + const drake::systems::OutputPort& lcs_contact_descriptions_port, + const std::string& channel, drake::lcm::DrakeLcmInterface* lcm, + const drake::systems::TriggerTypeSet& publish_triggers, + double publish_period = 0.0, double publish_offset = 0.0); + + private: + /** + * @brief Calculates the contact forces and populates the output message. + * + * @param context The system context containing input values. + * @param c3_forces_output Pointer to the output message to populate. + */ + void DoCalc(const drake::systems::Context& context, + c3::lcmt_contact_forces* c3_forces_output) const; + + drake::systems::InputPortIndex + c3_solution_port_; ///< Index of the c3 solution input port. + drake::systems::InputPortIndex + lcs_contact_info_port_; ///< Index of the LCS contact info input port. + drake::systems::OutputPortIndex + contact_force_output_port_; ///< Index of the contact force output port. +}; + +} // namespace lcmt_generators +} // namespace systems +} // namespace c3 \ No newline at end of file diff --git a/systems/lcs_factory_system.cc b/systems/lcs_factory_system.cc index 40f2417..e4c3599 100644 --- a/systems/lcs_factory_system.cc +++ b/systems/lcs_factory_system.cc @@ -8,6 +8,7 @@ #include "systems/framework/timestamped_vector.h" using c3::LCS; +using c3::multibody::LCSContactDescription; using c3::multibody::LCSFactory; using c3::systems::TimestampedVector; using drake::multibody::ModelInstanceIndex; @@ -22,6 +23,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 +42,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(); @@ -53,12 +70,10 @@ LCSFactorySystem::LCSFactorySystem( &LCSFactorySystem::OutputLCS) .get_index(); - lcs_contact_jacobian_port_ = + lcs_contact_descriptions_output_port_ = this->DeclareAbstractOutputPort( - "J_lcs, p_lcs", - std::pair(Eigen::MatrixXd(n_x_, n_lambda_), - std::vector()), - &LCSFactorySystem::OutputLCSContactJacobian) + "contact_descriptions", std::vector(), + &LCSFactorySystem::OutputLCSContactDescriptions) .get_index(); } @@ -75,9 +90,9 @@ void LCSFactorySystem::OutputLCS(const drake::systems::Context& context, *output_lcs = lcs_factory_->GenerateLCS(); } -void LCSFactorySystem::OutputLCSContactJacobian( +void LCSFactorySystem::OutputLCSContactDescriptions( const drake::systems::Context& context, - std::pair>* output) const { + std::vector* output) const { const auto lcs_x = static_cast*>( this->EvalVectorInput(context, lcs_state_input_port_)); const auto lcs_u = static_cast*>( @@ -86,7 +101,7 @@ void LCSFactorySystem::OutputLCSContactJacobian( DRAKE_DEMAND(lcs_x->get_data().size() == n_x_); DRAKE_DEMAND(lcs_u->get_value().size() == n_u_); lcs_factory_->UpdateStateAndInput(lcs_x->get_data(), lcs_u->get_value()); - *output = lcs_factory_->GetContactJacobianAndPoints(); + *output = lcs_factory_->GetContactDescriptions(); } } // namespace systems diff --git a/systems/lcs_factory_system.h b/systems/lcs_factory_system.h index 15404f5..46edd25 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. * @@ -88,11 +94,13 @@ class LCSFactorySystem : public drake::systems::LeafSystem { * @return A reference to the output port for the LCS contact Jacobian. */ const drake::systems::OutputPort& - get_output_port_lcs_contact_jacobian() const { - return this->get_output_port(lcs_contact_jacobian_port_); + get_output_port_lcs_contact_descriptions() const { + return this->get_output_port(lcs_contact_descriptions_output_port_); } private: + void InitializeSystem(const drake::multibody::MultibodyPlant& plant, + LCSFactoryOptions& options); /** * @brief Computes the LCS based on the current state and inputs. * @@ -109,15 +117,15 @@ class LCSFactorySystem : public drake::systems::LeafSystem { * @param output Pointer to the output pair containing the contact Jacobian * and contact points. */ - void OutputLCSContactJacobian( + void OutputLCSContactDescriptions( const drake::systems::Context& context, - std::pair>* output) const; + std::vector* output) const; // Member variables for input and output port indices drake::systems::InputPortIndex lcs_state_input_port_; drake::systems::InputPortIndex lcs_inputs_input_port_; drake::systems::OutputPortIndex lcs_port_; - drake::systems::OutputPortIndex lcs_contact_jacobian_port_; + drake::systems::OutputPortIndex lcs_contact_descriptions_output_port_; // Convenience variables for system dimensions int n_q_; ///< Number of positions in the plant. diff --git a/systems/test/generators_test.cc b/systems/test/generators_test.cc new file mode 100644 index 0000000..c8d581e --- /dev/null +++ b/systems/test/generators_test.cc @@ -0,0 +1,371 @@ +// generators_test.cc +// Unit tests for LCM generators in the c3 project. + +#include +#include +#include + +#include "core/test/c3_cartpole_problem.hpp" +#include "multibody/lcs_factory.h" +#include "systems/framework/c3_output.h" +#include "systems/lcmt_generators/c3_output_generator.h" +#include "systems/lcmt_generators/c3_trajectory_generator.h" +#include "systems/lcmt_generators/contact_force_generator.h" + +using c3::multibody::LCSContactDescription; +using c3::systems::C3Output; +using c3::systems::lcmt_generators::C3OutputGenerator; +using c3::systems::lcmt_generators::C3TrajectoryGenerator; +using c3::systems::lcmt_generators::C3TrajectoryGeneratorConfig; +using c3::systems::lcmt_generators::ContactForceGenerator; +using c3::systems::lcmt_generators::TrajectoryDescription; +using drake::systems::Context; +using drake::systems::SystemOutput; +using Eigen::MatrixXd; +using Eigen::VectorXd; + +namespace c3 { +namespace systems { +namespace test { + +// Test fixture for ContactForceGenerator +class ContactForceGeneratorTest : public ::testing::Test, + public C3CartpoleProblem { + protected: + ContactForceGeneratorTest() + : C3CartpoleProblem(0.411, 0.978, 0.6, 0.4267, 0.35, -0.35, 100, 9.81) {} + + void SetUp() override { + generator_ = std::make_unique(); + context_ = generator_->CreateDefaultContext(); + output_ = generator_->AllocateOutput(); + + // Create mock C3 solution + solution_ = std::make_unique( + pSystem->num_states(), pSystem->num_lambdas(), pSystem->num_inputs(), + pSystem->N()); + solution_->time_vector_ = VectorXf::LinSpaced(pSystem->N(), 0.0, 1.0); + solution_->x_sol_ = MatrixXf::Random(pSystem->num_states(), pSystem->N()); + solution_->lambda_sol_ = + MatrixXf::Random(pSystem->num_lambdas(), pSystem->N()); + solution_->u_sol_ = MatrixXf::Random(pSystem->num_inputs(), pSystem->N()); + + // Create mock contact descriptions + contact_descriptions_.resize(2); + contact_descriptions_[0].witness_point_A = Eigen::Vector3d(1.0, 0.0, 0.0); + contact_descriptions_[0].witness_point_B = Eigen::Vector3d(0.5, 0.0, 0.0); + contact_descriptions_[0].force_basis = Eigen::Vector3d(1.0, 0.0, 0.0); + contact_descriptions_[0].is_slack = false; + + contact_descriptions_[1].witness_point_A = Eigen::Vector3d(-1.0, 0.0, 0.0); + contact_descriptions_[1].witness_point_B = Eigen::Vector3d(-0.5, 0.0, 0.0); + contact_descriptions_[1].force_basis = Eigen::Vector3d(-1.0, 0.0, 0.0); + contact_descriptions_[1].is_slack = true; // This one should be ignored + + // Set up input ports + generator_->get_input_port_c3_solution().FixValue(context_.get(), + *solution_); + generator_->get_input_port_lcs_contact_descriptions().FixValue( + context_.get(), contact_descriptions_); + } + + std::unique_ptr generator_; + std::unique_ptr> context_; + std::unique_ptr> output_; + std::unique_ptr solution_; + std::vector contact_descriptions_; +}; + +TEST_F(ContactForceGeneratorTest, InputOutputPortsExist) { + // Check that all expected input and output ports exist + EXPECT_NO_THROW(generator_->get_input_port_c3_solution()); + EXPECT_NO_THROW(generator_->get_input_port_lcs_contact_descriptions()); + EXPECT_NO_THROW(generator_->get_output_port_contact_force()); +} + +TEST_F(ContactForceGeneratorTest, GeneratesContactForces) { + // Should not throw when generating contact forces + EXPECT_NO_THROW(generator_->CalcOutput(*context_, output_.get())); + + // Get the output message + const auto& forces_msg = + output_->get_data(0)->get_value(); + + // Should have 1 force (one contact is slack and ignored) + EXPECT_EQ(forces_msg.num_forces, 1); + EXPECT_EQ(forces_msg.forces.size(), 1); + + // Check contact point coordinates + const auto& force = forces_msg.forces[0]; + EXPECT_DOUBLE_EQ(force.contact_point[0], 0.5); + EXPECT_DOUBLE_EQ(force.contact_point[1], 0.0); + EXPECT_DOUBLE_EQ(force.contact_point[2], 0.0); + + // Check force calculation (force_basis * lambda) + double expected_force = 1.0 * solution_->lambda_sol_(0, 0); + EXPECT_DOUBLE_EQ(force.contact_force[0], expected_force); + EXPECT_DOUBLE_EQ(force.contact_force[1], 0.0); + EXPECT_DOUBLE_EQ(force.contact_force[2], 0.0); + + // Check timestamp + EXPECT_DOUBLE_EQ(forces_msg.utime, context_->get_time() * 1e6); +} + +TEST_F(ContactForceGeneratorTest, IgnoresSlackContacts) { + // Mark all contacts as slack + for (auto& contact : contact_descriptions_) { + contact.is_slack = true; + } + generator_->get_input_port_lcs_contact_descriptions().FixValue( + context_.get(), contact_descriptions_); + + generator_->CalcOutput(*context_, output_.get()); + const auto& forces_msg = + output_->get_data(0)->get_value(); + + // Should have no forces when all contacts are slack + EXPECT_EQ(forces_msg.num_forces, 0); + EXPECT_TRUE(forces_msg.forces.empty()); +} + +// Test fixture for C3TrajectoryGenerator +class C3TrajectoryGeneratorTest : public ::testing::Test, + public C3CartpoleProblem { + protected: + C3TrajectoryGeneratorTest() + : C3CartpoleProblem(0.411, 0.978, 0.6, 0.4267, 0.35, -0.35, 100, 9.81) {} + + void SetUp() override { + // Set up trajectory generator config + config_.trajectories.resize(3); + + // State trajectory + config_.trajectories[0].trajectory_name = "states"; + config_.trajectories[0].variable_type = "state"; + config_.trajectories[0].indices = {{.start = 0, .end = 1}}; + + // Input trajectory + config_.trajectories[1].trajectory_name = "inputs"; + config_.trajectories[1].variable_type = "input"; + config_.trajectories[1].indices = {{.start = 0, .end = 0}}; + + // Force trajectory + config_.trajectories[2].trajectory_name = "forces"; + config_.trajectories[2].variable_type = "force"; + // Empty indices should use default (all) + + generator_ = std::make_unique(config_); + context_ = generator_->CreateDefaultContext(); + output_ = generator_->AllocateOutput(); + + // Create mock C3 solution + solution_ = std::make_unique( + pSystem->num_states(), pSystem->num_lambdas(), pSystem->num_inputs(), + pSystem->N()); + solution_->time_vector_ = VectorXf::LinSpaced(pSystem->N(), 0.0, 1.0); + solution_->x_sol_ = MatrixXf::Random(pSystem->num_states(), pSystem->N()); + solution_->lambda_sol_ = + MatrixXf::Random(pSystem->num_lambdas(), pSystem->N()); + solution_->u_sol_ = MatrixXf::Random(pSystem->num_inputs(), pSystem->N()); + + // Set up input port + generator_->get_input_port_c3_solution().FixValue(context_.get(), + *solution_); + } + + C3TrajectoryGeneratorConfig config_; + std::unique_ptr generator_; + std::unique_ptr> context_; + std::unique_ptr> output_; + std::unique_ptr solution_; +}; + +TEST_F(C3TrajectoryGeneratorTest, InputOutputPortsExist) { + // Check that all expected input and output ports exist + EXPECT_NO_THROW(generator_->get_input_port_c3_solution()); + EXPECT_NO_THROW(generator_->get_output_port_lcmt_c3_trajectory()); +} + +TEST_F(C3TrajectoryGeneratorTest, GeneratesTrajectories) { + // Should not throw when generating trajectories + EXPECT_NO_THROW(generator_->CalcOutput(*context_, output_.get())); + + // Get the output message + const auto& traj_msg = + output_->get_data(0)->get_value(); + + // Should have 3 trajectories as configured + EXPECT_EQ(traj_msg.num_trajectories, 3); + EXPECT_EQ(traj_msg.trajectories.size(), 3); + + // Check state trajectory + const auto& state_traj = traj_msg.trajectories[0]; + EXPECT_EQ(state_traj.trajectory_name, "states"); + EXPECT_EQ(state_traj.num_timesteps, pSystem->N()); + EXPECT_EQ(state_traj.vector_dim, 2); // indices 0-1 + EXPECT_EQ(state_traj.timestamps.size(), pSystem->N()); + EXPECT_EQ(state_traj.values.size(), 2); + + // Check input trajectory + const auto& input_traj = traj_msg.trajectories[1]; + EXPECT_EQ(input_traj.trajectory_name, "inputs"); + EXPECT_EQ(input_traj.num_timesteps, pSystem->N()); + EXPECT_EQ(input_traj.vector_dim, 1); // indices 0-0 + EXPECT_EQ(input_traj.values.size(), 1); + + // Check force trajectory (should use all lambda indices) + const auto& force_traj = traj_msg.trajectories[2]; + EXPECT_EQ(force_traj.trajectory_name, "forces"); + EXPECT_EQ(force_traj.num_timesteps, pSystem->N()); + EXPECT_EQ(force_traj.vector_dim, pSystem->num_lambdas()); + EXPECT_EQ(force_traj.values.size(), pSystem->num_lambdas()); + + // Check timestamp + EXPECT_DOUBLE_EQ(traj_msg.utime, context_->get_time() * 1e6); +} + +TEST_F(C3TrajectoryGeneratorTest, ThrowsOnInvalidVariableType) { + // Create config with invalid variable type + C3TrajectoryGeneratorConfig bad_config; + bad_config.trajectories.resize(1); + bad_config.trajectories[0].trajectory_name = "invalid"; + bad_config.trajectories[0].variable_type = "invalid_type"; + + auto bad_generator = std::make_unique(bad_config); + auto bad_context = bad_generator->CreateDefaultContext(); + auto bad_output = bad_generator->AllocateOutput(); + + bad_generator->get_input_port_c3_solution().FixValue(bad_context.get(), + *solution_); + + // Should throw when trying to generate trajectories with invalid type + EXPECT_THROW(bad_generator->CalcOutput(*bad_context, bad_output.get()), + std::runtime_error); +} + +TEST_F(C3TrajectoryGeneratorTest, ThrowsOnOutOfRangeIndices) { + // Create config with out-of-range indices + C3TrajectoryGeneratorConfig bad_config; + bad_config.trajectories.resize(1); + bad_config.trajectories[0].trajectory_name = "out_of_range"; + bad_config.trajectories[0].variable_type = "state"; + bad_config.trajectories[0].indices = {{.start = 0, .end = 999}}; // Too high + + auto bad_generator = std::make_unique(bad_config); + auto bad_context = bad_generator->CreateDefaultContext(); + auto bad_output = bad_generator->AllocateOutput(); + + bad_generator->get_input_port_c3_solution().FixValue(bad_context.get(), + *solution_); + + // Should throw when indices are out of range + EXPECT_THROW(bad_generator->CalcOutput(*bad_context, bad_output.get()), + std::out_of_range); +} + +// Test fixture for C3OutputGenerator +class C3OutputGeneratorTest : public ::testing::Test, public C3CartpoleProblem { + protected: + C3OutputGeneratorTest() + : C3CartpoleProblem(0.411, 0.978, 0.6, 0.4267, 0.35, -0.35, 100, 9.81) {} + + void SetUp() override { + generator_ = std::make_unique(); + context_ = generator_->CreateDefaultContext(); + output_ = generator_->AllocateOutput(); + + // Create mock C3 solution + solution_ = std::make_unique( + pSystem->num_states(), pSystem->num_lambdas(), pSystem->num_inputs(), + pSystem->N()); + solution_->time_vector_ = VectorXf::LinSpaced(pSystem->N(), 0.0, 1.0); + solution_->x_sol_ = MatrixXf::Random(pSystem->num_states(), pSystem->N()); + solution_->lambda_sol_ = + MatrixXf::Random(pSystem->num_lambdas(), pSystem->N()); + solution_->u_sol_ = MatrixXf::Random(pSystem->num_inputs(), pSystem->N()); + + // Create mock C3 intermediates + int total_vars = + pSystem->num_states() + pSystem->num_lambdas() + pSystem->num_inputs(); + intermediates_ = std::make_unique( + pSystem->num_states(), pSystem->num_lambdas(), pSystem->num_inputs(), + pSystem->N()); + intermediates_->time_vector_ = VectorXf::LinSpaced(pSystem->N(), 0.0, 1.0); + intermediates_->z_ = MatrixXf::Random(total_vars, pSystem->N()); + intermediates_->delta_ = MatrixXf::Random(total_vars, pSystem->N()); + intermediates_->w_ = MatrixXf::Random(total_vars, pSystem->N()); + + // Set up input ports + generator_->get_input_port_c3_solution().FixValue(context_.get(), + *solution_); + generator_->get_input_port_c3_intermediates().FixValue(context_.get(), + *intermediates_); + } + + std::unique_ptr generator_; + std::unique_ptr> context_; + std::unique_ptr> output_; + std::unique_ptr solution_; + std::unique_ptr intermediates_; +}; + +TEST_F(C3OutputGeneratorTest, InputOutputPortsExist) { + // Check that all expected input and output ports exist + EXPECT_NO_THROW(generator_->get_input_port_c3_solution()); + EXPECT_NO_THROW(generator_->get_input_port_c3_intermediates()); + EXPECT_NO_THROW(generator_->get_output_port_c3_output()); +} + +TEST_F(C3OutputGeneratorTest, GeneratesOutput) { + // Should not throw when generating output + EXPECT_NO_THROW(generator_->CalcOutput(*context_, output_.get())); + + // Get the output message + const auto& output_msg = output_->get_data(0)->get_value(); + + // Check basic properties + EXPECT_DOUBLE_EQ(output_msg.utime, context_->get_time() * 1e6); + + // Check that solution data is present + EXPECT_EQ(output_msg.solution.time_vec.size(), pSystem->N()); + EXPECT_EQ(output_msg.solution.x_sol.size(), pSystem->num_states()); + EXPECT_EQ(output_msg.solution.x_sol[0].size(), pSystem->N()); + EXPECT_EQ(output_msg.solution.lambda_sol.size(), pSystem->num_lambdas()); + EXPECT_EQ(output_msg.solution.lambda_sol[0].size(), pSystem->N()); + EXPECT_EQ(output_msg.solution.u_sol.size(), pSystem->num_inputs()); + EXPECT_EQ(output_msg.solution.u_sol[0].size(), pSystem->N()); + + // Check that intermediates data is present + int total_vars = + pSystem->num_states() + pSystem->num_lambdas() + pSystem->num_inputs(); + EXPECT_EQ(output_msg.intermediates.z_sol.size(), total_vars); + EXPECT_EQ(output_msg.intermediates.z_sol[0].size(), pSystem->N()); + EXPECT_EQ(output_msg.intermediates.delta_sol.size(), total_vars); + EXPECT_EQ(output_msg.intermediates.delta_sol[0].size(), pSystem->N()); + EXPECT_EQ(output_msg.intermediates.w_sol.size(), total_vars); + EXPECT_EQ(output_msg.intermediates.w_sol[0].size(), pSystem->N()); + EXPECT_EQ(output_msg.intermediates.time_vec.size(), pSystem->N()); +} + +TEST_F(C3OutputGeneratorTest, VerifyDataConsistency) { + generator_->CalcOutput(*context_, output_.get()); + const auto& output_msg = output_->get_data(0)->get_value(); + + // Check that time vectors match + for (int i = 0; i < pSystem->N(); ++i) { + EXPECT_FLOAT_EQ(output_msg.solution.time_vec[i], + solution_->time_vector_(i)); + } + + // Check that solution data matches (spot check first few elements) + for (int i = 0; i < std::min(3, (int)output_msg.solution.x_sol.size()); ++i) { + int row = i % pSystem->num_states(); + int col = i / pSystem->num_states(); + EXPECT_FLOAT_EQ(output_msg.solution.x_sol[row][col], + solution_->x_sol_(row, col)); + } +} + +} // namespace test +} // namespace systems +} // namespace c3 \ No newline at end of file diff --git a/systems/test/systems_test.cc b/systems/test/systems_test.cc index 140225a..ecf862d 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" @@ -13,6 +16,7 @@ #include "systems/lcs_factory_system.h" #include "systems/lcs_simulator.h" +using c3::multibody::LCSContactDescription; using c3::multibody::LCSFactory; using c3::systems::C3Controller; using c3::systems::C3ControllerOptions; @@ -92,25 +96,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 +132,7 @@ class C3ControllerTest : public ::testing::Test, public C3CartpoleProblem { plant->Finalize(); controller_ = - std::make_unique(*plant, cost, controller_options); + std::make_unique(*plant, cost, controller_options_); context_ = controller_->CreateDefaultContext(); output_ = controller_->AllocateOutput(); @@ -145,9 +156,15 @@ class C3ControllerTest : public ::testing::Test, public C3CartpoleProblem { std::unique_ptr> context_; std::unique_ptr> output_; std::unique_ptr> discrete_values_; + C3ControllerOptions controller_options_; }; -TEST_F(C3ControllerTest, CheckInputOutputPorts) { +using projection_types = ::testing::Types; +TYPED_TEST_SUITE(C3ControllerTypedTest, projection_types); + +TYPED_TEST(C3ControllerTypedTest, CheckInputOutputPorts) { + auto& controller_ = this->controller_; + auto& pSystem = this->pSystem; // Check input and output port sizes and existence EXPECT_NO_THROW(controller_->get_input_port_lcs_state()); EXPECT_EQ(controller_->get_input_port_lcs_state().size(), @@ -158,7 +175,12 @@ TEST_F(C3ControllerTest, CheckInputOutputPorts) { EXPECT_NO_THROW(controller_->get_output_port_c3_intermediates()); } -TEST_F(C3ControllerTest, CheckPlannedTrajectory) { +TYPED_TEST(C3ControllerTypedTest, CheckPlannedTrajectory) { + auto& controller_ = this->controller_; + auto& context_ = this->context_; + auto& discrete_values_ = this->discrete_values_; + auto& output_ = this->output_; + auto& pSystem = this->pSystem; // Should not throw when computing plan with valid inputs EXPECT_NO_THROW({ controller_->CalcForcedDiscreteVariableUpdate(*context_, @@ -168,7 +190,7 @@ TEST_F(C3ControllerTest, CheckPlannedTrajectory) { // Check C3 solution output const auto& c3_solution = - output_->get_data(0)->get_value(); + output_->get_data(0)->template get_value(); EXPECT_EQ(c3_solution.time_vector_.size(), pSystem->N()); EXPECT_EQ(c3_solution.x_sol_.rows(), pSystem->num_states()); EXPECT_EQ(c3_solution.lambda_sol_.rows(), pSystem->num_lambdas()); @@ -179,10 +201,14 @@ 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(); + if constexpr (std::is_same::value) { + // C3Plus has additional slack variables + total_vars += pSystem->num_lambdas(); + } EXPECT_EQ(c3_intermediates.z_.rows(), total_vars); EXPECT_EQ(c3_intermediates.delta_.rows(), total_vars); EXPECT_EQ(c3_intermediates.w_.rows(), total_vars); @@ -191,7 +217,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 +237,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 +250,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 +261,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)); } @@ -309,7 +345,8 @@ TEST_F(LCSFactorySystemTest, InputOutputPortSizes) { EXPECT_EQ(lcs_factory_system->get_input_port_lcs_input().size(), plant->num_actuators()); EXPECT_NO_THROW(lcs_factory_system->get_output_port_lcs()); - EXPECT_NO_THROW(lcs_factory_system->get_output_port_lcs_contact_jacobian()); + EXPECT_NO_THROW( + lcs_factory_system->get_output_port_lcs_contact_descriptions()); } TEST_F(LCSFactorySystemTest, OutputLCSIsValid) { @@ -329,14 +366,21 @@ TEST_F(LCSFactorySystemTest, OutputContactJacobianIsValid) { // Check that the contact Jacobian output is valid EXPECT_NO_THROW( { lcs_factory_system->CalcOutput(*lcs_context, lcs_output.get()); }); - auto [J_lcs, p_lcs] = - lcs_output->get_data(1) - ->get_value< - std::pair>>(); - EXPECT_EQ(p_lcs.size(), contact_pairs.size()); // for two pairs of contacts - EXPECT_EQ(p_lcs.at(0).size(), 3); // 3D coordinate point - EXPECT_EQ(J_lcs.cols(), plant->num_velocities()); - EXPECT_EQ(J_lcs.rows(), contact_pairs.size()); // for frictionless spring + auto contact_descriptions = + lcs_output->get_data(1)->get_value>(); + EXPECT_EQ(contact_descriptions.size(), + contact_pairs.size()); // for two pairs of contacts + EXPECT_EQ(contact_descriptions.back().witness_point_A.size(), + 3); // 3D coordinate point + EXPECT_FALSE( + contact_descriptions.back().witness_point_A.isZero()); // Not zero + EXPECT_EQ(contact_descriptions.back().witness_point_B.size(), + 3); // 3D coordinate point + EXPECT_FALSE( + contact_descriptions.back().witness_point_B.isZero()); // Not zero + EXPECT_EQ(contact_descriptions.back().force_basis.size(), + 3); // 3D force basis + EXPECT_FALSE(contact_descriptions.back().force_basis.isZero()); // Not zero } } // namespace test