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/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..faeff15 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")) 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 7fa4685..c047adf 100644 --- a/core/BUILD.bazel +++ b/core/BUILD.bazel @@ -29,6 +29,11 @@ filegroup( ], ) +cc_library( + name = "logging", + hdrs = ["common/logging_utils.hpp"], +) + cc_library( name = "c3", srcs = [ @@ -57,6 +62,7 @@ cc_library( deps = [ ":lcs", ":options", + ":logging", "@drake//:drake_shared_library", ] + select({ "//tools:with_gurobi": ["@gurobi//:gurobi_cxx"], diff --git a/core/c3.cc b/core/c3.cc index 7a5164b..bb95c3f 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -7,6 +7,7 @@ #include #include +#include "common/logging_utils.hpp" #include "lcs.h" #include "solver_options_io.h" @@ -90,7 +91,6 @@ 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_z_)); x_sol_->push_back(Eigen::VectorXd::Zero(n_x_)); lambda_sol_->push_back(Eigen::VectorXd::Zero(n_lambda_)); u_sol_->push_back(Eigen::VectorXd::Zero(n_u_)); @@ -269,6 +269,7 @@ const std::vector& C3::GetTargetCost() { } void C3::Solve(const VectorXd& x0) { + drake::log()->debug("C3::Solve called"); auto start = std::chrono::high_resolution_clock::now(); // Set the initial state constraint if (initial_state_constraint_) { @@ -319,6 +320,8 @@ void C3::Solve(const VectorXd& x0) { std::vector w(N_, VectorXd::Zero(n_z_)); vector G = cost_matrices_.G; + drake::log()->debug("C3::Solve starting ADMM iterations."); + for (int iter = 0; iter < options_.admm_iter; iter++) { ADMMStep(x0, &delta, &w, &G, iter); } @@ -328,12 +331,14 @@ void C3::Solve(const VectorXd& x0) { WD.at(i) = delta.at(i) - w.at(i); } + drake::log()->debug("C3::Solve final SolveQP step."); *z_fin_ = SolveQP(x0, G, WD, 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; @@ -357,6 +362,7 @@ 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, @@ -368,6 +374,7 @@ void C3::ADMMStep(const VectorXd& x0, vector* delta, WD.at(i) = delta->at(i) - w->at(i); } + drake::log()->debug("C3::ADMMStep SolveQP step."); vector z = SolveQP(x0, *G, WD, admm_iteration, false); vector ZW(N_, VectorXd::Zero(n_z_)); @@ -375,6 +382,7 @@ void C3::ADMMStep(const VectorXd& x0, vector* delta, ZW[i] = w->at(i) + z[i]; } + drake::log()->debug("C3::ADMMStep SolveProjection step."); if (cost_matrices_.U[0].isZero(0)) { *delta = SolveProjection(*G, ZW, admm_iteration); } else { @@ -419,10 +427,17 @@ void C3::StoreQPResults(const MathematicalProgramResult& result, z_sol_->at(i).segment(0, n_x_) = result.GetSolution(x_[i]); z_sol_->at(i).segment(n_x_, n_lambda_) = result.GetSolution(lambda_[i]); z_sol_->at(i).segment(n_x_ + n_lambda_, n_u_) = result.GetSolution(u_[i]); + + drake::log()->trace( + "C3::StoreQPResults storing solution for time step {}: " + "lambda = {}", + i, EigenToString(lambda_sol_->at(i).transpose())); } if (!warm_start_) return; // No warm start, so no need to update warm start parameters + + drake::log()->trace("C3::StoreQPResults storing warm start values."); for (int i = 0; i < N_ + 1; ++i) { if (i < N_) { warm_start_x_[admm_iteration][i] = result.GetSolution(x_[i]); @@ -436,6 +451,7 @@ void C3::StoreQPResults(const MathematicalProgramResult& result, vector C3::SolveQP(const VectorXd& x0, const vector& G, const vector& WD, int admm_iteration, bool is_final_solve) { + drake::log()->trace("C3::SolveQP Adding augmented costs(G)."); // Add or update augmented costs if (augmented_costs_.size() == 0) { for (int i = 0; i < N_; ++i) @@ -452,15 +468,18 @@ vector C3::SolveQP(const VectorXd& x0, const vector& G, SetInitialGuessQP(x0, admm_iteration); - 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()); + drake::log()->trace("C3::SolveQP calling solver."); + try { + MathematicalProgramResult result = osqp_.Solve(prog_); + if (!result.is_success()) { + drake::log()->warn("C3::SolveQP failed to solve the QP with status: {}", + result.get_solution_result()); + } + StoreQPResults(result, admm_iteration, is_final_solve); + } catch (const std::exception& e) { + drake::log()->error("C3::SolveQP failed with exception: {}", e.what()); } - StoreQPResults(result, admm_iteration, is_final_solve); - return *z_sol_; } @@ -475,8 +494,11 @@ vector C3::SolveProjection(const vector& U, omp_set_schedule(omp_sched_static, 0); } + // clang-format off #pragma omp parallel for num_threads( \ - options_.num_threads) if (use_parallelization_in_projection_) + options_.num_threads) if (use_parallelization_in_projection_) + // clang-format on + for (int i = 0; i < N_; ++i) { if (warm_start_) { if (i == N_ - 1) { diff --git a/core/c3_options.h b/core/c3_options.h index 08f3b6a..8a55ee8 100644 --- a/core/c3_options.h +++ b/core/c3_options.h @@ -67,6 +67,7 @@ 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; @@ -79,6 +80,7 @@ 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; @@ -139,16 +141,14 @@ struct C3Options { } g_vector.insert(g_vector.end(), g_lambda.begin(), g_lambda.end()); g_vector.insert(g_vector.end(), g_u.begin(), g_u.end()); - if (g_eta != std::nullopt || g_eta_slack != std::nullopt) { - if (g_eta == std::nullopt || g_eta->empty()) { - g_vector.insert(g_vector.end(), g_eta_slack->begin(), - g_eta_slack->end()); - g_vector.insert(g_vector.end(), g_eta_n->begin(), g_eta_n->end()); - g_vector.insert(g_vector.end(), g_eta_t->begin(), g_eta_t->end()); - } else { - g_vector.insert(g_vector.end(), g_eta->begin(), g_eta->end()); - } + g_eta_vector = g_eta.value_or(std::vector()); + 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()); @@ -159,16 +159,14 @@ struct C3Options { } u_vector.insert(u_vector.end(), u_lambda.begin(), u_lambda.end()); u_vector.insert(u_vector.end(), u_u.begin(), u_u.end()); - if (u_eta != std::nullopt || u_eta_slack != std::nullopt) { - if (u_eta == std::nullopt || u_eta->empty()) { - g_vector.insert(g_vector.end(), u_eta_slack->begin(), - u_eta_slack->end()); - g_vector.insert(g_vector.end(), u_eta_n->begin(), u_eta_n->end()); - g_vector.insert(g_vector.end(), u_eta_t->begin(), u_eta_t->end()); - } else { - g_vector.insert(g_vector.end(), u_eta->begin(), u_eta->end()); - } + 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()); @@ -179,8 +177,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(); diff --git a/core/c3_plus.cc b/core/c3_plus.cc index 5d927ea..eec2e1c 100644 --- a/core/c3_plus.cc +++ b/core/c3_plus.cc @@ -5,8 +5,11 @@ #include #include "c3_options.h" +#include "common/logging_utils.hpp" #include "lcs.h" +#include "drake/common/text_logging.h" + namespace c3 { using Eigen::MatrixXd; @@ -19,6 +22,16 @@ C3Plus::C3Plus(const LCS& lcs, const CostMatrices& costs, const vector& 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>(); @@ -40,10 +53,7 @@ C3Plus::C3Plus(const LCS& lcs, const CostMatrices& costs, EtaLinEq.block(0, n_x_ + n_lambda_, n_lambda_, n_u_) = lcs_.H().at(i); eta_constraints_[i] = - prog_ - .AddLinearEqualityConstraint( - EtaLinEq, -lcs_.c().at(i), - {x_.at(i), lambda_.at(i), u_.at(i), eta_.at(i)}) + prog_.AddLinearEqualityConstraint(EtaLinEq, -lcs_.c().at(i), z_.at(i)) .evaluator() .get(); } @@ -81,16 +91,23 @@ void C3Plus::SetInitialGuessQP(const Eigen::VectorXd& x0, int admm_iteration) { void C3Plus::StoreQPResults(const MathematicalProgramResult& result, int admm_iteration, bool is_final_solve) { C3::StoreQPResults(result, admm_iteration, is_final_solve); + drake::log()->trace("C3Plus::StoreQPResults storing eta results."); for (int i = 0; i < N_; i++) { if (is_final_solve) { eta_sol_->at(i) = result.GetSolution(eta_[i]); } z_sol_->at(i).segment(n_x_ + n_lambda_ + n_u_, n_lambda_) = result.GetSolution(eta_[i]); + drake::log()->trace( + "C3Plus::StoreQPResults storing solution for time step {}: " + "eta = {}", + i, EigenToString(eta_sol_->at(i).transpose())); } if (!warm_start_) return; // No warm start, so no need to update warm start parameters + + drake::log()->trace("C3Plus::StoreQPResults storing warm start eta."); for (int i = 0; i < N_; ++i) { warm_start_eta_[admm_iteration][i] = result.GetSolution(eta_[i]); } @@ -120,6 +137,12 @@ VectorXd C3Plus::SolveSingleProjection(const MatrixXd& U, VectorXd lambda_c = delta_c.segment(n_x_, n_lambda_); VectorXd eta_c = delta_c.segment(n_x_ + n_lambda_ + n_u_, n_lambda_); + drake::log()->trace( + "C3Plus::SolveSingleProjection ADMM iteration {}: pre-projection " + "lambda = {}, eta = {}", + admm_iteration, EigenToString(lambda_c.transpose()), + EigenToString(eta_c.transpose())); + // Set the smaller of lambda and eta to zero Eigen::Array eta_larger = eta_c.array() * w_eta_vec.array().sqrt() > 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/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/geom_geom_collider.cc b/multibody/geom_geom_collider.cc index cadbfab..7e767ae 100644 --- a/multibody/geom_geom_collider.cc +++ b/multibody/geom_geom_collider.cc @@ -258,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); @@ -272,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 5e6b142..18ccce0 100644 --- a/multibody/geom_geom_collider.h +++ b/multibody/geom_geom_collider.h @@ -99,15 +99,37 @@ 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 { /** diff --git a/multibody/lcs_factory.cc b/multibody/lcs_factory.cc index 9fd65f2..0690665 100644 --- a/multibody/lcs_factory.cc +++ b/multibody/lcs_factory.cc @@ -86,21 +86,6 @@ void LCSFactory::ComputeContactJacobian(VectorXd& phi, MatrixXd& Jn, } } -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); - } - - return std::make_pair(WCa, WCb); -} - void LCSFactory::UpdateStateAndInput( const Eigen::Ref>& state, const Eigen::Ref>& input) { @@ -385,56 +370,63 @@ 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(); +std::vector LCSFactory::GetContactDescriptions() { + std::vector normal_contact_descriptions; + std::vector tangential_contact_descriptions; - 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_; - - // 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_); + + 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()); + DRAKE_ASSERT(n_friction_directions_ > 0); + for (int i = 0; i < tangential_contact_descriptions.size(); i++) { + int normal_index = i / (2 * n_friction_directions_); + DRAKE_ASSERT( + contact_descriptions.at(i).witness_point_A == + normal_contact_descriptions.at(normal_index).witness_point_A); + contact_descriptions.at(i).force_basis = + mu_[normal_index] * contact_descriptions.at(i).force_basis + + normal_contact_descriptions.at(normal_index).force_basis; + contact_descriptions.at(i).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, diff --git a/multibody/lcs_factory.h b/multibody/lcs_factory.h index e5f7940..6c85dc5 100644 --- a/multibody/lcs_factory.h +++ b/multibody/lcs_factory.h @@ -46,6 +46,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 @@ -84,16 +97,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. @@ -272,14 +281,6 @@ 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_; diff --git a/multibody/test/multibody_test.cc b/multibody/test/multibody_test.cc index d2ff762..c491149 100644 --- a/multibody/test/multibody_test.cc +++ b/multibody/test/multibody_test.cc @@ -187,8 +187,7 @@ TEST_P(LCSFactoryPivotingTest, LinearizePlantToLCS) { TEST_P(LCSFactoryPivotingTest, UpdateStateAndInput) { LCS initial_lcs = lcs_factory->GenerateLCS(); - auto [initial_J, initial_contact_points] = - lcs_factory->GetContactJacobianAndPoints(); + auto initial_contact_descriptions = lcs_factory->GetContactDescriptions(); drake::VectorX state = VectorXd::Zero(plant->num_positions() + plant->num_velocities()); @@ -200,8 +199,7 @@ TEST_P(LCSFactoryPivotingTest, UpdateStateAndInput) { lcs_factory->UpdateStateAndInput(state, input); LCS updated_lcs = lcs_factory->GenerateLCS(); - auto [updated_J, updated_contact_points] = - lcs_factory->GetContactJacobianAndPoints(); + auto updated_contact_descriptions = lcs_factory->GetContactDescriptions(); EXPECT_EQ(initial_lcs.A(), updated_lcs.A()); EXPECT_EQ(initial_lcs.B(), updated_lcs.B()); @@ -217,33 +215,28 @@ TEST_P(LCSFactoryPivotingTest, UpdateStateAndInput) { 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]); + 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(); - - int n_contacts = contact_pairs.size(); - // Check for number of force variables (not including slack variables) - switch (contact_model) { - case ContactModel::kStewartAndTrinkle: - EXPECT_EQ(J.rows(), - n_contacts + 2 * n_contacts * options.num_friction_directions); - break; - case ContactModel::kFrictionlessSpring: - EXPECT_EQ(J.rows(), n_contacts); - break; - case ContactModel::kAnitescu: - EXPECT_EQ(J.rows(), 2 * n_contacts * options.num_friction_directions); - break; - default: - EXPECT_TRUE(false); // Something went wrong in parsing the contact model +TEST_P(LCSFactoryPivotingTest, GetContactDescriptions) { + auto contact_descriptions = lcs_factory->GetContactDescriptions(); + + int n_contacts = contact_descriptions.size(); + EXPECT_EQ(LCSFactory::GetNumContactVariables(options), n_contacts); + 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()); } - EXPECT_EQ(J.cols(), plant->num_velocities()); - EXPECT_EQ(contact_points.size(), n_contacts); } TEST_P(LCSFactoryPivotingTest, FixSomeModes) { 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 1ec42d4..deb081f 100644 --- a/systems/c3_controller.cc +++ b/systems/c3_controller.cc @@ -111,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 4bb38b1..ef272f0 100644 --- a/systems/c3_controller_options.h +++ b/systems/c3_controller_options.h @@ -82,9 +82,9 @@ struct C3ControllerOptions { DRAKE_DEMAND(static_cast(c3_options.u_lambda.size()) == expected_lambda_size); if (projection_type == "C3+") { - DRAKE_DEMAND(static_cast(c3_options.g_eta->size()) == + DRAKE_DEMAND(static_cast(c3_options.g_eta_vector.size()) == expected_lambda_size); - DRAKE_DEMAND(static_cast(c3_options.u_eta->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 1be7b00..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_; @@ -63,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..e573c51 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; @@ -53,12 +54,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 +74,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 +85,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..97583ba 100644 --- a/systems/lcs_factory_system.h +++ b/systems/lcs_factory_system.h @@ -88,8 +88,8 @@ 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: @@ -109,15 +109,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..5b3cd3e 100644 --- a/systems/test/systems_test.cc +++ b/systems/test/systems_test.cc @@ -13,6 +13,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; @@ -309,7 +310,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 +331,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