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 0560d07..c4c5ac9 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")) @@ -60,10 +74,12 @@ PYBIND11_MODULE(multibody, m) { &c3::multibody::LCSFactory::GetNumContactVariables), py::arg("contact_model"), py::arg("num_contacts"), py::arg("num_friction_directions")) - .def_static("GetNumContactVariables", - py::overload_cast( - &c3::multibody::LCSFactory::GetNumContactVariables), - py::arg("options")); + .def_static( + "GetNumContactVariables", + py::overload_cast&, + const c3::LCSFactoryOptions&>( + &c3::multibody::LCSFactory::GetNumContactVariables), + py::arg("plant"), py::arg("options")); py::class_(m, "ContactPairConfig") .def(py::init<>()) diff --git a/bindings/pyc3/c3_systems_py.cc b/bindings/pyc3/c3_systems_py.cc index ebeb288..b49f075 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") @@ -182,6 +185,88 @@ PYBIND11_MODULE(systems, m) { &C3ControllerOptions::quaternion_regularizer_fraction); 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 ec5c92e..7e1bbb0 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 f46c20a..bd4fb50 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" @@ -266,6 +267,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_) { @@ -316,6 +318,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); } @@ -331,6 +335,7 @@ void C3::Solve(const VectorXd& x0) { *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; @@ -354,6 +359,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, @@ -372,6 +378,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 { @@ -416,10 +423,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]); @@ -437,15 +451,18 @@ vector C3::SolveQP(const VectorXd& x0, const vector& G, AddAugmentedCost(G, WD, delta, is_final_solve); 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_; } diff --git a/core/c3_options.h b/core/c3_options.h index 2734960..af0d543 100644 --- a/core/c3_options.h +++ b/core/c3_options.h @@ -76,6 +76,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; @@ -88,6 +89,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; @@ -150,16 +152,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()); @@ -170,16 +170,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()) { - u_vector.insert(u_vector.end(), u_eta_slack->begin(), - u_eta_slack->end()); - u_vector.insert(u_vector.end(), u_eta_n->begin(), u_eta_n->end()); - u_vector.insert(u_vector.end(), u_eta_t->begin(), u_eta_t->end()); - } else { - u_vector.insert(u_vector.end(), u_eta->begin(), u_eta->end()); - } + 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()); @@ -190,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(); diff --git a/core/c3_plus.cc b/core/c3_plus.cc index 2554b2d..4b36d99 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]); } @@ -178,6 +195,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/examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml b/examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml index e25e758..19832e8 100644 --- a/examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml +++ b/examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml @@ -11,10 +11,8 @@ 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 planar_normal_direction: [0, 1, 0] diff --git a/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml b/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml index 7954fd4..185a02f 100644 --- a/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml +++ b/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml @@ -15,10 +15,8 @@ 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 planar_normal_direction: [0, 1, 0] 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/BUILD.bazel b/multibody/BUILD.bazel index f446985..1eb1472 100644 --- a/multibody/BUILD.bazel +++ b/multibody/BUILD.bazel @@ -12,22 +12,45 @@ cc_library( ], ) +cc_library( + name = "geom_geom_collider", + srcs = ["geom_geom_collider.cc"], + hdrs = ["geom_geom_collider.h"], + deps = [ + "@drake//:drake_shared_library", + ], + visibility = ["//visibility:public"], +) + +cc_library( + name = "multibody_utils", + srcs = ["multibody_utils.cc"], + hdrs = ["multibody_utils.h"], + deps = [ + "@drake//:drake_shared_library", + ], + visibility = ["//visibility:public"], +) + cc_library( name = "lcs_factory", - srcs = ["lcs_factory.cc", - "geom_geom_collider.cc", - "multibody_utils.cc"], + srcs = [ + "lcs_factory.cc", + "contact_evaluator.cc" + ], hdrs = [ - "lcs_factory.h", - "geom_geom_collider.h", - "multibody_utils.h", + "lcs_factory.h", + "contact_evaluator.h" ], deps = [ + ":geom_geom_collider", + ":multibody_utils", ":options", '//core:lcs', '//core:options', "@drake//:drake_shared_library", ], + visibility = ["//visibility:public"], ) cc_library( diff --git a/multibody/contact_evaluator.cc b/multibody/contact_evaluator.cc new file mode 100644 index 0000000..4649a7a --- /dev/null +++ b/multibody/contact_evaluator.cc @@ -0,0 +1,12 @@ +#include "multibody/contact_evaluator.h" + +namespace c3 { +namespace multibody { + +// Explicit template instantiations for double precision +template class ContactEvaluator; +template class PolytopeContactEvaluator; +template class PlanarContactEvaluator; + +} // namespace multibody +} // namespace c3 \ No newline at end of file diff --git a/multibody/contact_evaluator.h b/multibody/contact_evaluator.h new file mode 100644 index 0000000..602513d --- /dev/null +++ b/multibody/contact_evaluator.h @@ -0,0 +1,157 @@ +#pragma once + +#include + +#include + +#include "geom_geom_collider.h" + +#include "drake/common/sorted_pair.h" +#include "drake/geometry/geometry_ids.h" +#include "drake/multibody/plant/multibody_plant.h" + +namespace c3 { +namespace multibody { + +/** + * @brief Abstract base class for evaluating contact constraints. + * + * ContactEvaluator provides an interface for computing signed distance and + * contact Jacobians for different friction models (planar vs polytope). + */ +template +class ContactEvaluator { + public: + ContactEvaluator( + const drake::multibody::MultibodyPlant& plant, + const drake::SortedPair& contact_pair) + : collider_(plant, contact_pair) {} + + virtual ~ContactEvaluator() = default; + + /** + * @brief Evaluates the signed distance and contact Jacobian. + * + * @param context The context for the MultibodyPlant. + * @param wrt Whether to compute Jacobian w.r.t. q or v. + * @return Pair of (signed_distance, contact_jacobian) + */ + virtual std::pair> Eval( + const drake::systems::Context& context, + drake::multibody::JacobianWrtVariable wrt = + drake::multibody::JacobianWrtVariable::kV) const = 0; + + /** + * @brief Returns the number of friction directions for this contact model. + */ + virtual int GetNumFrictionDirections() const = 0; + + /** + * @brief Computes the force basis for the contact constraint. + * + * For polytope contacts, returns the friction cone approximation basis. + * For planar contacts, returns the planar friction basis. + * + * @param context The context for the MultibodyPlant. + * @return Matrix where each row is a force direction in world frame. + */ + virtual Eigen::Matrix CalcForceBasis( + const drake::systems::Context& context) const = 0; + + /** + * @brief Computes witness points (closest points on each geometry). + * + * @param context The context for the MultibodyPlant. + * @return Pair of witness points in world frame. + */ + std::pair CalcWitnessPoints( + const drake::systems::Context& context) const { + return collider_.CalcWitnessPoints(context); + } + + protected: + mutable GeomGeomCollider collider_; +}; + +/** + * @brief Contact evaluator for polytope friction cone approximation. + */ +template +class PolytopeContactEvaluator : public ContactEvaluator { + public: + PolytopeContactEvaluator( + const drake::multibody::MultibodyPlant& plant, + const drake::SortedPair& contact_pair, + int num_friction_directions) + : ContactEvaluator(plant, contact_pair), + num_friction_directions_(num_friction_directions) { + DRAKE_DEMAND(num_friction_directions_ > 1); + } + + std::pair> Eval( + const drake::systems::Context& context, + drake::multibody::JacobianWrtVariable wrt = + drake::multibody::JacobianWrtVariable::kV) const override { + return this->collider_.EvalPolytope(context, num_friction_directions_, wrt); + } + + int GetNumFrictionDirections() const override { + return num_friction_directions_; + } + + Eigen::Matrix CalcForceBasis( + const drake::systems::Context& context) const override { + return this->collider_.CalcForceBasisInWorldFrame(context, + num_friction_directions_); + } + + private: + int num_friction_directions_; +}; + +/** + * @brief Contact evaluator for planar (2D) friction. + */ +template +class PlanarContactEvaluator : public ContactEvaluator { + public: + PlanarContactEvaluator( + const drake::multibody::MultibodyPlant& plant, + const drake::SortedPair& contact_pair, + const Eigen::Vector3d& planar_normal) + : ContactEvaluator(plant, contact_pair), + planar_normal_(planar_normal) { + // Validate unit vector + DRAKE_DEMAND(std::abs(planar_normal_.norm() - 1.0) < 1e-6); + } + + std::pair> Eval( + const drake::systems::Context& context, + drake::multibody::JacobianWrtVariable wrt = + drake::multibody::JacobianWrtVariable::kV) const override { + return this->collider_.EvalPlanar(context, planar_normal_, wrt); + } + + int GetNumFrictionDirections() const override { + return 1; // Planar contact has 1 friction direction + } + + Eigen::Matrix CalcForceBasis( + const drake::systems::Context& context) const override { + // For planar contact, pass num_friction_directions = 0 to trigger planar + // mode + return this->collider_.CalcForceBasisInWorldFrame(context, 0, + planar_normal_); + } + + private: + Eigen::Vector3d planar_normal_; +}; + +} // namespace multibody +} // namespace c3 + +// Explicitly instantiate for double +extern template class c3::multibody::ContactEvaluator; +extern template class c3::multibody::PolytopeContactEvaluator; +extern template class c3::multibody::PlanarContactEvaluator; \ No newline at end of file diff --git a/multibody/geom_geom_collider.cc b/multibody/geom_geom_collider.cc index cadbfab..e8e5747 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,47 @@ 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); + + // Compute the force basis in the world frame for contact forces. + // + // IMPORTANT: Force Direction Convention + // ====================================== + // The forces computed here represent forces that object A exerts on object B. + // This is why we use -nhat_BA_W (negative of the normal from B to A). + // + // Recall that nhat_BA_W points from body B towards body A in the world frame. + // For contact forces, we want the force that A applies to B, which acts in + // the direction opposite to nhat_BA_W (i.e., from A towards B). + // + // For friction cone discretization: + // - The first basis vector is the normal force direction: -nhat_BA_W + // - The remaining basis vectors span the tangent plane (friction directions) + + if (num_friction_directions < 1) { + // Planar contact: basis is constructed from the contact and planar normals. + // The planar_normal defines the plane of admissible motion. + // This is typically used for 2D contact scenarios or constrained motion. + return ComputePlanarForceBasis(-query_result.nhat_BA_W, planar_normal); + } else { + // 3D contact: build polytope basis and rotate using contact normal. + // The polytope basis consists of 2*num_friction_directions + 1 vectors: + // - 1 normal force vector + // - 2*num_friction_directions tangential vectors (forming a friction cone) + // We rotate this basis from the contact frame to the world frame using + // R_WC. + 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 ffce8c0..58000c4 100644 --- a/multibody/geom_geom_collider.h +++ b/multibody/geom_geom_collider.h @@ -99,14 +99,36 @@ 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; /** * @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 23537d9..c597ab8 100644 --- a/multibody/lcs_factory.cc +++ b/multibody/lcs_factory.cc @@ -2,7 +2,6 @@ #include -#include "multibody/geom_geom_collider.h" #include "multibody/multibody_utils.h" #include "drake/common/text_logging.h" @@ -29,6 +28,94 @@ using Eigen::VectorXd; namespace c3 { namespace multibody { +// Helper function to expand contact_pair_configs into per-contact arrays +struct ExpandedContactConfig { + std::vector> contact_geoms; + std::vector num_friction_directions_per_contact; + std::vector mu; + std::vector>> + planar_normal_direction_per_contact; + int num_contacts() const { return contact_geoms.size(); } +}; + +ExpandedContactConfig ExpandContactPairConfigs( + const drake::multibody::MultibodyPlant& plant, + const drake::systems::Context& context, + const std::vector& configs) { + ExpandedContactConfig result; + + for (const auto& pair : configs) { + 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)); + + // Resolve indices: use all geometries if not provided or if empty + std::vector body_A_indices = + pair.body_A_collision_geom_indices.value_or(std::vector{}); + std::vector body_B_indices = + pair.body_B_collision_geom_indices.value_or(std::vector{}); + + if (body_A_indices.empty()) { + // Use all geometries for body A + body_A_indices.resize(body_A_collision_geoms.size()); + std::iota(body_A_indices.begin(), body_A_indices.end(), 0); + } else { + // Validate provided indices for body A + for (int idx : body_A_indices) { + DRAKE_DEMAND(idx >= 0 && + idx < static_cast(body_A_collision_geoms.size())); + } + } + + if (body_B_indices.empty()) { + // Use all geometries for body B + body_B_indices.resize(body_B_collision_geoms.size()); + std::iota(body_B_indices.begin(), body_B_indices.end(), 0); + } else { + // Validate provided indices for body B + for (int idx : body_B_indices) { + DRAKE_DEMAND(idx >= 0 && + idx < static_cast(body_B_collision_geoms.size())); + } + } + + // Create contact pairs for all combinations and replicate properties + std::vector> + AB_geometric_pairs; + for (int i : body_A_indices) { + for (int j : body_B_indices) { + AB_geometric_pairs.emplace_back(body_A_collision_geoms[i], + body_B_collision_geoms[j]); + } + } + + // If no limit on the number of contact pairs is specified, or if the limit + // is equal to the total number of pairs, return all pairs. + if (pair.num_active_contact_pairs.has_value() && + pair.num_active_contact_pairs.value() != AB_geometric_pairs.size()) { + AB_geometric_pairs = LCSFactory::GetNClosestContactPairs( + plant, context, AB_geometric_pairs, + pair.num_active_contact_pairs.value()); + } + + result.contact_geoms.insert(result.contact_geoms.end(), + AB_geometric_pairs.begin(), + AB_geometric_pairs.end()); + int n_active_contacts = AB_geometric_pairs.size(); + result.mu.insert(result.mu.end(), n_active_contacts, pair.mu); + result.num_friction_directions_per_contact.insert( + result.num_friction_directions_per_contact.end(), n_active_contacts, + pair.num_friction_directions); + result.planar_normal_direction_per_contact.insert( + result.planar_normal_direction_per_contact.end(), n_active_contacts, + pair.planar_normal_direction.value_or(std::array{})); + } + + return result; +} + +// Constructor 1: Takes only LCSFactoryOptions with contact_pair_configs LCSFactory::LCSFactory( const drake::multibody::MultibodyPlant& plant, drake::systems::Context& context, @@ -49,69 +136,94 @@ LCSFactory::LCSFactory( dt_(options.dt) { DRAKE_DEMAND(options.contact_pair_configs.has_value()); - n_contacts_ = 0; + // Expand contact_pair_configs into per-contact arrays using plant + auto expanded = ExpandContactPairConfigs( + plant, context, options.contact_pair_configs.value()); - mu_.clear(); - n_friction_directions_per_contact_.clear(); - contact_pairs_.clear(); - planar_normal_direction_per_contact_.clear(); + contact_pairs_ = expanded.contact_geoms; + n_contacts_ = contact_pairs_.size(); + mu_ = expanded.mu; - // Process each contact pair configuration to populate contact pairs, friction - // coefficients, and friction directions. This allows for different friction - // properties for different pairs of bodies. - for (auto& pair : options.contact_pair_configs.value()) { - std::vector> collision_geom_pairs; - 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)); - - // If no specific indices are provided, use all collision geometries for - // each body. - if (pair.body_A_collision_geom_indices.empty()) { - pair.body_A_collision_geom_indices.resize(body_A_collision_geoms.size()); - std::iota(pair.body_A_collision_geom_indices.begin(), - pair.body_A_collision_geom_indices.end(), 0); - } - if (pair.body_B_collision_geom_indices.empty()) { - pair.body_B_collision_geom_indices.resize(body_B_collision_geoms.size()); - std::iota(pair.body_B_collision_geom_indices.begin(), - pair.body_B_collision_geom_indices.end(), 0); - } + // Create contact evaluators + InitializeContactEvaluators(expanded.num_friction_directions_per_contact, + expanded.planar_normal_direction_per_contact); +} - for (int i : pair.body_A_collision_geom_indices) { - for (int j : pair.body_B_collision_geom_indices) { - collision_geom_pairs.emplace_back(SortedPair( - body_A_collision_geoms[i], body_B_collision_geoms[j])); - } - } +// Constructor 2: Takes explicit contact_geoms with per-contact arrays +LCSFactory::LCSFactory( + const MultibodyPlant& plant, Context& context, + const MultibodyPlant& plant_ad, Context& context_ad, + const std::vector>& + contact_geoms, + const LCSFactoryOptions& options) + : plant_(plant), + context_(context), + plant_ad_(plant_ad), + context_ad_(context_ad), + contact_pairs_(contact_geoms), + options_(options), + n_contacts_(contact_geoms.size()), + 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) { + // Resolve configuration using options resolver methods + std::vector friction_dirs = options_.ResolveNumFrictionDirections(); + mu_ = options_.ResolveMu(); + std::vector>> planar_normals = + options_.ResolvePlanarNormals(); + + DRAKE_DEMAND(friction_dirs.size() == (size_t)n_contacts_); + DRAKE_DEMAND(mu_.size() == (size_t)n_contacts_); + DRAKE_DEMAND(planar_normals.size() == (size_t)n_contacts_); + + // Create contact evaluators + InitializeContactEvaluators(friction_dirs, planar_normals); +} - // If no limit on the number of contact pairs is specified, or if the limit - // is equal to the total number of pairs, return all pairs. - if (pair.num_active_contact_pairs.has_value() && - pair.num_active_contact_pairs.value() != collision_geom_pairs.size()) - collision_geom_pairs = - GetNClosestContactPairs(plant_, context_, collision_geom_pairs, - pair.num_active_contact_pairs.value()); - - contact_pairs_.insert(contact_pairs_.end(), collision_geom_pairs.begin(), - collision_geom_pairs.end()); - mu_.insert(mu_.end(), collision_geom_pairs.size(), pair.mu); - n_friction_directions_per_contact_.insert( - n_friction_directions_per_contact_.end(), collision_geom_pairs.size(), - pair.num_friction_directions); - planar_normal_direction_per_contact_.insert( - planar_normal_direction_per_contact_.end(), collision_geom_pairs.size(), - pair.planar_normal_direction.value_or(std::array{})); +// Common initialization logic for contact evaluators +void LCSFactory::InitializeContactEvaluators( + const std::vector& friction_dirs, + const std::vector>>& planar_normals) { + DRAKE_DEMAND(friction_dirs.size() == (size_t)n_contacts_); + DRAKE_DEMAND(planar_normals.size() == (size_t)n_contacts_); - n_contacts_ += collision_geom_pairs.size(); + contact_evaluators_.clear(); + contact_evaluators_.reserve(n_contacts_); + + for (int i = 0; i < n_contacts_; ++i) { + if (frictionless_ || friction_dirs[i] == 1) { + // Planar contact + DRAKE_DEMAND(planar_normals[i].has_value()); + Eigen::Vector3d planar_normal = + Eigen::Map(planar_normals[i].value().data()); + contact_evaluators_.push_back( + std::make_unique>( + plant_, contact_pairs_[i], planar_normal)); + } else { + // Polytope contact + contact_evaluators_.push_back( + std::make_unique>( + plant_, contact_pairs_[i], friction_dirs[i])); + } + } + + // Calculate n_lambda_ and Jt_row_sizes_ from evaluators + std::vector n_friction_directions_from_evaluators; + for (const auto& evaluator : contact_evaluators_) { + n_friction_directions_from_evaluators.push_back( + evaluator->GetNumFrictionDirections()); } n_lambda_ = multibody::LCSFactory::GetNumContactVariables( - contact_model_, n_contacts_, n_friction_directions_per_contact_); + contact_model_, n_contacts_, n_friction_directions_from_evaluators); + Jt_row_sizes_ = 2 * Eigen::Map( - n_friction_directions_per_contact_.data(), - n_friction_directions_per_contact_.size()); + n_friction_directions_from_evaluators.data(), + n_friction_directions_from_evaluators.size()); } std::vector> LCSFactory::GetNClosestContactPairs( @@ -148,60 +260,6 @@ std::vector> LCSFactory::GetNClosestContactPairs( return active_contact_pairs; } -LCSFactory::LCSFactory( - const MultibodyPlant& plant, Context& context, - const MultibodyPlant& plant_ad, Context& context_ad, - const std::vector>& - contact_geoms, - const LCSFactoryOptions& options) - : plant_(plant), - context_(context), - plant_ad_(plant_ad), - 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( - contact_model_, n_contacts_, n_friction_directions_per_contact_)), - n_u_(plant_.num_actuators()), - mu_(options.mu.value()), - frictionless_(contact_model_ == ContactModel::kFrictionlessSpring), - dt_(options.dt) { - // Handle planar normal directions for contacts with 1 friction direction - // Initialize all with empty default values - planar_normal_direction_per_contact_.resize(n_contacts_, {}); - - for (int i = 0; i < n_contacts_; ++i) { - if (n_friction_directions_per_contact_[i] == 1) { - // Planar contact - assign the appropriate normal - if (options_.planar_normal_direction.has_value()) { - // Single planar normal provided - use for all planar contacts - DRAKE_DEMAND(options_.planar_normal_direction.value().size() == 3); - planar_normal_direction_per_contact_[i] = - options_.planar_normal_direction.value(); - } else { - // Per-contact planar normals provided - DRAKE_DEMAND(options_.planar_normal_direction_per_contact.has_value()); - DRAKE_DEMAND( - options_.planar_normal_direction_per_contact.value().size() == - (size_t)n_contacts_); - planar_normal_direction_per_contact_[i] = - options_.planar_normal_direction_per_contact.value()[i]; - } - } - // Non-planar contacts keep the default empty value {} - } - 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 @@ -212,15 +270,8 @@ void LCSFactory::ComputeContactJacobian(VectorXd& phi, MatrixXd& Jn, double phi_i; MatrixX J_i; for (int i = 0; i < n_contacts_; i++) { - multibody::GeomGeomCollider collider(plant_, contact_pairs_[i]); - if (frictionless_ || n_friction_directions_per_contact_[i] == 1) { - Eigen::Vector3d planar_normal = - Eigen::Map( - planar_normal_direction_per_contact_[i].data()); - std::tie(phi_i, J_i) = collider.EvalPlanar(context_, planar_normal); - } else - std::tie(phi_i, J_i) = collider.EvalPolytope( - context_, n_friction_directions_per_contact_[i]); + // Use polymorphic Eval method + auto [phi_i, J_i] = contact_evaluators_[i]->Eval(context_); // Signed distance value for contact i phi(i) = phi_i; @@ -236,21 +287,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) { @@ -375,7 +411,8 @@ LCS LCSFactory::GenerateLCS() { } else if (contact_model_ == ContactModel::kFrictionlessSpring) { // Frictionless spring FormulateFrictionlessSpringContactDynamics( - phi, Jn, qdotNv, options_.spring_stiffness, M, D, E, F, H, c); + phi, Jn, qdotNv, options_.spring_stiffness.value_or(1.0), M, D, E, F, H, + c); } else { throw std::out_of_range("Unsupported contact model."); } @@ -523,53 +560,123 @@ 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); - } +std::vector LCSFactory::GetContactDescriptions() { + std::vector normal_contact_descriptions; + std::vector tangential_contact_descriptions; - if (contact_model_ == ContactModel::kStewartAndTrinkle) { - // if Stewart and Trinkle model, concatenate the normal and tangential - // jacobian - MatrixXd J_c = MatrixXd::Zero(n_contacts_ + Jt_row_sizes_.sum(), n_v_); - J_c << Jn, Jt; - return std::make_pair(J_c, contact_points); - } + for (int i = 0; i < n_contacts_; i++) { + auto [p_WCa, p_WCb] = contact_evaluators_[i]->CalcWitnessPoints(context_); + auto force_basis = contact_evaluators_[i]->CalcForceBasis(context_); - // Model is Anitescu - int n_lambda_ = Jt_row_sizes_.sum(); + 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)}; - // 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, Jt_row_sizes_.segment(0, i).sum(), 1, Jt_row_sizes_(i)) = - MatrixXd::Ones(1, Jt_row_sizes_(i)); + if (j == 0) { + normal_contact_descriptions.push_back(contact_description); + } else { + 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(Jt_row_sizes_.segment(0, i).sum(), Jt_row_sizes_(i)) = - muXd(i) * VectorXd::Ones(Jt_row_sizes_(i)); + // =========================================================================== + // Contact Force Variable Stacking Convention + // =========================================================================== + // + // The order in which contact force variables (λ) are stacked depends on the + // contact model. This ordering is crucial for understanding the structure of + // the LCS matrices (D, E, F, H, c). + // + // Stewart-Trinkle Model (n_λ = 2*n_contacts + n_tangential): + // ----------------------------------------------------------- + // λ = [γ₁, γ₂, ..., γₙ, <- Slack variables (n_contacts) + // λₙ₁, λₙ₂, ..., λₙₙ, <- Normal forces (n_contacts) + // λₜ₁₁, λₜ₁₂, ..., λₜ₁ₘ₁, <- Tangential forces for contact 1 + // λₜ₂₁, λₜ₂₂, ..., λₜ₂ₘ₂, <- Tangential forces for contact 2 + // ... + // λₜₙ₁, λₜₙ₂, ..., λₜₙₘₙ] <- Tangential forces for contact n + // + // where: + // - n = n_contacts (number of contact pairs) + // - mᵢ = 2 * num_friction_directions[i] (tangential force components per + // contact) + // - γᵢ are slack variables for the friction cone constraints + // + // Example with 2 contacts, 2 friction directions each: + // λ = [γ₁, γ₂, λₙ₁, λₙ₂, λₜ₁₁, λₜ₁₂, λₜ₁₃, λₜ₁₄, λₜ₂₁, λₜ₂₂, λₜ₂₃, λₜ₂₄] + // ↑─────↑ ↑─────↑ ↑─────────────────────↑ ↑─────────────────────↑ + // slacks normals tangential (contact 1) tangential (contact 2) + // + // Anitescu Model (n_λ = n_tangential): + // ------------------------------------- + // λ = [λc₁₁, λc₁₂, ..., λc₁ₘ₁, <- Combined forces for contact 1 + // λc₂₁, λc₂₂, ..., λc₂ₘ₂, <- Combined forces for contact 2 + // ... + // λcₙ₁, λcₙ₂, ..., λcₙₘₙ] <- Combined forces for contact n + // + // where each λcᵢⱼ represents a combined normal-tangential force: + // λcᵢⱼ acts along the direction: μᵢ * tangent_directionᵢⱼ + + // normal_directionᵢ + // + // Example with 2 contacts, 2 friction directions each: + // λ = [λc₁₁, λc₁₂, λc₁₃, λc₁₄, λc₂₁, λc₂₂, λc₂₃, λc₂₄] + // ↑─────────────────────↑ ↑─────────────────────↑ + // combined (contact 1) combined (contact 2) + // + // Frictionless Spring Model (n_λ = n_contacts): + // ---------------------------------------------- + // λ = [λₙ₁, λₙ₂, ..., λₙₙ] <- Normal forces only + // + // =========================================================================== + + 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) { + // Stack as: [slack variables, normal forces, tangential forces] + 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) { + // Start with tangential basis vectors + contact_descriptions.insert(contact_descriptions.end(), + tangential_contact_descriptions.begin(), + tangential_contact_descriptions.end()); + + // For each contact, modify tangential force bases to include normal + // component resulting in combined friction cone approximation vectors + 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); + + // Combine tangential and normal force directions: + // force_basis = μ * tangent_direction + normal_direction + // This creates the edges of the linearized friction cone + 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, @@ -708,35 +815,33 @@ int LCSFactory::GetNumContactVariables(ContactModel contact_model, num_friction_directions_per_contact); } -int LCSFactory::GetNumContactVariables(const LCSFactoryOptions options) { +int LCSFactory::GetNumContactVariables( + const drake::multibody::MultibodyPlant& plant, + 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()) { + + int n_contacts = options.ResolveNumContacts(); + std::vector n_friction_directions_per_contact = + options.ResolveNumFrictionDirections(); + if (options.contact_pair_configs.has_value()) { + // If contact pair configs are provided, they take precedence over the + // options for number of contacts and friction directions. We can expand the + // contact pair configs to get the actual number of contacts and friction + // directions per contact. + + // Use default context since we only need the geometry query results to + // expand the contact pair configs, and the geometry query results do not + // depend on the state of the plant. + auto context = plant.CreateDefaultContext(); + auto expanded = ExpandContactPairConfigs( + plant, *context, options.contact_pair_configs.value()); + n_contacts = expanded.num_contacts(); 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."); + expanded.num_friction_directions_per_contact; } - DRAKE_DEMAND(n_friction_directions_per_contact.size() == - (size_t)options.num_contacts); - return GetNumContactVariables(contact_model, options.num_contacts, + + return GetNumContactVariables(contact_model, n_contacts, n_friction_directions_per_contact); } diff --git a/multibody/lcs_factory.h b/multibody/lcs_factory.h index f8e13e0..00b4064 100644 --- a/multibody/lcs_factory.h +++ b/multibody/lcs_factory.h @@ -7,6 +7,8 @@ #include #include "core/lcs.h" +#include "multibody/contact_evaluator.h" +#include "multibody/geom_geom_collider.h" #include "multibody/lcs_factory_options.h" #include "drake/common/sorted_pair.h" @@ -47,6 +49,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 @@ -112,16 +127,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. @@ -213,13 +224,32 @@ class LCSFactory { * This is the preferred overload as it encapsulates all contact model and * friction configuration in a single options object. * + * @param plant The MultibodyPlant to analyze for contact information. * @param options The LCS options specifying contact model and friction * properties. * @return int The number of contact variables. */ - static int GetNumContactVariables(const LCSFactoryOptions options); + static int GetNumContactVariables( + const drake::multibody::MultibodyPlant& plant, + const LCSFactoryOptions& options); private: + /** + * @brief Initializes contact evaluators for all contact pairs. + * + * This method creates and configures ContactEvaluator objects for each + * contact pair, setting up the friction directions and planar normal + * constraints as specified. + * + * @param friction_dirs Vector specifying the number of friction directions + * for each contact. + * @param planar_normals Vector of optional planar normal vectors for each + * contact. If specified, constrains the contact normal to lie in a plane. + */ + void InitializeContactEvaluators( + const std::vector& friction_dirs, + const std::vector>>& planar_normals); + /** * @brief Formulates the contact dynamics for the frictionless spring contact * model. @@ -316,14 +346,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_; @@ -335,18 +357,13 @@ class LCSFactory { LCSFactoryOptions options_; int n_contacts_; ///< Number of contact points. - std::vector - n_friction_directions_per_contact_; ///< Number of friction directions. - std::vector> - planar_normal_direction_per_contact_; ///< Optional normal vector for - ///< planar contact for each - ///< contact. - 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>> contact_evaluators_; + 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. diff --git a/multibody/lcs_factory_options.h b/multibody/lcs_factory_options.h index b234e02..3cb8114 100644 --- a/multibody/lcs_factory_options.h +++ b/multibody/lcs_factory_options.h @@ -10,12 +10,12 @@ 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; + std::optional> body_A_collision_geom_indices; + std::optional> body_B_collision_geom_indices; int num_friction_directions; + double mu; // friction coefficient std::optional> planar_normal_direction; // optional normal vector for planar contact - double mu; // friction coefficient std::optional num_active_contact_pairs; // Number of geometry-geometry contact pairs to // consider (e.g., if body_A has 10 geoms and @@ -34,65 +34,187 @@ struct ContactPairConfig { a->Visit(DRAKE_NVP(planar_normal_direction)); a->Visit(DRAKE_NVP(mu)); a->Visit(DRAKE_NVP(num_active_contact_pairs)); + + ValidateContactPairConfig(); + } + + private: + void ValidateContactPairConfig() const { + DRAKE_DEMAND(!body_A.empty()); + DRAKE_DEMAND(!body_B.empty()); + if (body_A_collision_geom_indices.has_value()) { + for (int idx : body_A_collision_geom_indices.value()) { + DRAKE_DEMAND(idx >= 0); + } + } + if (body_B_collision_geom_indices.has_value()) { + for (int idx : body_B_collision_geom_indices.value()) { + DRAKE_DEMAND(idx >= 0); + } + } + DRAKE_DEMAND(num_friction_directions >= 0); + DRAKE_DEMAND(mu >= 0); // Friction coefficient must be non-negative + if (num_friction_directions == 1) { + DRAKE_DEMAND( + planar_normal_direction.has_value()); // For planar contact, the + // normal direction must be + // specified + const auto& normal = planar_normal_direction.value(); + double norm = std::sqrt(normal[0] * normal[0] + normal[1] * normal[1] + + normal[2] * normal[2]); + DRAKE_DEMAND(std::abs(norm - 1) < 1e-6); // Ensure it's a unit vector + } + + if (num_active_contact_pairs.has_value()) { + DRAKE_DEMAND(num_active_contact_pairs.value() > 0); + } } }; struct LCSFactoryOptions { - 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> - planar_normal_direction; // Optional normal vector for planar contact - std::optional>> - planar_normal_direction_per_contact; // Optional normal vector for planar - // contact for each contact + // Contact model type: "stewart_and_trinkle", "anitescu", or + // "frictionless_spring" + std::string contact_model; + int N; // Number of time steps in the prediction horizon + double dt; // Time step size (seconds) + + // Total number of contact points in the system + std::optional num_contacts; + + // Spring stiffness for frictionless_spring contact model (must be positive) + std::optional spring_stiffness; - std::optional> - contact_pair_configs; // Optional detailed contact pair configurations + // Detailed per-contact-pair configurations (alternative to global settings) + std::optional> contact_pair_configs; - int N; // number of time steps in the prediction horizon - double dt; // time step size + // Per-contact friction parameters (must match num_contacts if provided) + std::optional> num_friction_directions_per_contact; + std::optional> mu_per_contact; + std::optional>> + planar_normal_direction_per_contact; + + // Global friction parameters (applied to all contacts if per-contact not + // specified) + std::optional num_friction_directions; + std::optional mu; + std::optional> planar_normal_direction; template void Serialize(Archive* a) { a->Visit(DRAKE_NVP(contact_model)); a->Visit(DRAKE_NVP(num_contacts)); a->Visit(DRAKE_NVP(spring_stiffness)); - a->Visit(DRAKE_NVP(num_friction_directions)); + a->Visit(DRAKE_NVP(contact_pair_configs)); a->Visit(DRAKE_NVP(num_friction_directions_per_contact)); + a->Visit(DRAKE_NVP(mu_per_contact)); + a->Visit(DRAKE_NVP(planar_normal_direction_per_contact)); + a->Visit(DRAKE_NVP(num_friction_directions)); a->Visit(DRAKE_NVP(mu)); a->Visit(DRAKE_NVP(planar_normal_direction)); - a->Visit(DRAKE_NVP(planar_normal_direction_per_contact)); - a->Visit(DRAKE_NVP(contact_pair_configs)); - a->Visit(DRAKE_NVP(N)); a->Visit(DRAKE_NVP(dt)); - // 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()); + if (contact_model == "frictionless_spring") { + DRAKE_DEMAND(spring_stiffness.has_value()); + DRAKE_DEMAND(spring_stiffness.value() > 0); + } + } + + // Returns the total number of contact points. + // Requires: num_contacts must be set and non-negative. + int ResolveNumContacts() const { + DRAKE_DEMAND(num_contacts.has_value()); + DRAKE_DEMAND(num_contacts.value() >= 0); + return num_contacts.value(); + } + + // Resolves the number of friction directions for each contact point. + // Returns a vector of length num_contacts where each element specifies + // the friction direction count for that contact. + // Uses per-contact values if available, otherwise broadcasts the global + // value to all contacts. + // Requires: num_contacts and either num_friction_directions_per_contact + // or num_friction_directions must be set. + std::vector ResolveNumFrictionDirections() const { + if (contact_model == "frictionless_spring") + return std::vector(ResolveNumContacts(), 0); + + DRAKE_DEMAND(num_friction_directions_per_contact.has_value() || + num_friction_directions.has_value()); + + if (num_friction_directions_per_contact.has_value()) { + DRAKE_DEMAND(num_friction_directions_per_contact.value().size() == + (size_t)num_contacts.value()); + for (int n : num_friction_directions_per_contact.value()) { + DRAKE_DEMAND(n >= 0); + } + return num_friction_directions_per_contact.value(); + } + + return std::vector(ResolveNumContacts(), + num_friction_directions.value()); + } + + // Resolves the friction coefficient (mu) for each contact point. + // Returns a vector of length num_contacts where each element specifies + // the friction coefficient for that contact. + // Uses per-contact values if available, otherwise broadcasts the global + // value to all contacts. + // Requires: num_contacts and either mu_per_contact or mu must be set. + std::vector ResolveMu() const { + if (contact_model == "frictionless_spring") + return std::vector(ResolveNumContacts(), 0.0); + + DRAKE_DEMAND(mu_per_contact.has_value() || mu.has_value()); + + if (mu_per_contact.has_value()) { + DRAKE_DEMAND(mu_per_contact.value().size() == + (size_t)num_contacts.value()); + for (double coeff : mu_per_contact.value()) { + DRAKE_DEMAND(coeff >= 0); + } + return mu_per_contact.value(); + } + + return std::vector(ResolveNumContacts(), mu.value()); + } + + // Resolves the planar normal direction for each contact point. + // Returns a vector of length num_contacts where each element is either: + // - A unit normal vector for planar contacts (num_friction_directions==1) + // - std::nullopt for non-planar contacts (num_friction_directions!=1) + // Uses per-contact normals if available, otherwise broadcasts the global + // normal to all planar contacts. + // Requires: num_contacts must be set, and planar contacts must have + // either planar_normal_direction_per_contact or + // planar_normal_direction specified. + std::vector>> ResolvePlanarNormals() + const { + DRAKE_DEMAND(num_contacts.has_value()); + DRAKE_DEMAND(num_contacts.value() >= 0); + + auto n_friction_direction_per_contact = ResolveNumFrictionDirections(); + DRAKE_DEMAND(n_friction_direction_per_contact.size() == + (size_t)num_contacts.value()); + + std::vector>> resolved_normals; + for (size_t i = 0; i < n_friction_direction_per_contact.size(); ++i) { + if (contact_model == "frictionless_spring" || + n_friction_direction_per_contact[i] == 1) { + if (planar_normal_direction_per_contact.has_value()) { + DRAKE_DEMAND(planar_normal_direction_per_contact.value().size() == + (size_t)num_contacts.value()); + resolved_normals.push_back( + planar_normal_direction_per_contact.value()[i]); + } else { + DRAKE_DEMAND(planar_normal_direction.has_value()); + resolved_normals.push_back(planar_normal_direction); + } } else { - DRAKE_DEMAND(num_friction_directions_per_contact.has_value()); - DRAKE_DEMAND(num_friction_directions_per_contact.value().size() == - (size_t)num_contacts); + resolved_normals.push_back(std::nullopt); } } + return resolved_normals; } }; diff --git a/multibody/test/multibody_test.cc b/multibody/test/multibody_test.cc index dd9d9cc..08697d1 100644 --- a/multibody/test/multibody_test.cc +++ b/multibody/test/multibody_test.cc @@ -45,42 +45,6 @@ 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, - 2, 4), - 20); - EXPECT_EQ(LCSFactory::GetNumContactVariables(ContactModel::kAnitescu, 3, 2), - 12); - EXPECT_EQ(LCSFactory::GetNumContactVariables( - ContactModel::kFrictionlessSpring, 3, 0), - 3); - EXPECT_THROW(LCSFactory::GetNumContactVariables(ContactModel::kUnknown, 3, 0), - std::out_of_range); - - // Test with LCSFactoryOptions struct - LCSFactoryOptions options; - options.contact_model = "stewart_and_trinkle"; - options.num_friction_directions = 4; - options.num_contacts = 2; - EXPECT_EQ(LCSFactory::GetNumContactVariables(options), 20); - - options.contact_model = "anitescu"; - options.num_friction_directions = 2; - options.num_contacts = 3; - EXPECT_EQ(LCSFactory::GetNumContactVariables(options), 12); - - options.contact_model = "frictionless_spring"; - options.num_friction_directions = 0; - 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); -} - // Helper struct to hold common test fixture data for cube pivoting scenarios struct PivotingTestFixture { DiagramBuilder plant_builder; @@ -110,8 +74,9 @@ struct PivotingTestFixture { // 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()); + options.num_friction_directions_per_contact = + std::vector(options.ResolveNumContacts(), + options.num_friction_directions.value()); } // Build diagram and create contexts @@ -161,6 +126,43 @@ class LCSFactoryPivotingTest : public testing::Test { PivotingTestFixture fixture; }; +// Test the static method for computing number of contact variables +TEST_F(LCSFactoryPivotingTest, GetNumContactVariables) { + // Test with different contact models and friction properties + EXPECT_EQ(LCSFactory::GetNumContactVariables(ContactModel::kStewartAndTrinkle, + 2, 4), + 20); + EXPECT_EQ(LCSFactory::GetNumContactVariables(ContactModel::kAnitescu, 3, 2), + 12); + EXPECT_EQ(LCSFactory::GetNumContactVariables( + ContactModel::kFrictionlessSpring, 3, 0), + 3); + EXPECT_THROW(LCSFactory::GetNumContactVariables(ContactModel::kUnknown, 3, 0), + std::out_of_range); + + // Test with LCSFactoryOptions struct + LCSFactoryOptions options; + options.contact_model = "stewart_and_trinkle"; + options.num_friction_directions = 4; + options.num_contacts = 2; + EXPECT_EQ(LCSFactory::GetNumContactVariables(*fixture.plant, options), 20); + + options.contact_model = "anitescu"; + options.num_friction_directions = 2; + options.num_contacts = 3; + EXPECT_EQ(LCSFactory::GetNumContactVariables(*fixture.plant, options), 12); + + options.contact_model = "frictionless_spring"; + options.num_friction_directions = 0; + options.num_contacts = 3; + EXPECT_EQ(LCSFactory::GetNumContactVariables(*fixture.plant, options), 3); + + // Test error handling for invalid contact model + options.contact_model = "some_random_contact_model"; + EXPECT_THROW(LCSFactory::GetNumContactVariables(*fixture.plant, options), + std::out_of_range); +} + // Test that contact pairs can be parsed from options instead of explicit list TEST_F(LCSFactoryPivotingTest, ContactPairParsing) { std::unique_ptr contact_pair_parsed_factory; @@ -188,8 +190,8 @@ TEST_F(LCSFactoryPivotingTest, ContactPairParsing) { 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)); + EXPECT_EQ(lcs.num_lambdas(), LCSFactory::GetNumContactVariables( + *fixture.plant, fixture.options)); } // Parameterized test fixture for testing different contact models and friction @@ -207,11 +209,13 @@ class LCSFactoryParameterizedPivotingTest VectorXd::Zero(fixture.plant->num_actuators()); // Set contact model and friction directions from test parameters + fixture.options.contact_pair_configs = std::nullopt; 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())); + fixture.options.planar_normal_direction = std::array({0, 0, 1}); // Create factory with parameterized options fixture.lcs_factory = std::make_unique( @@ -234,8 +238,8 @@ TEST_P(LCSFactoryParameterizedPivotingTest, 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)); + EXPECT_EQ(lcs.num_lambdas(), LCSFactory::GetNumContactVariables( + *fixture.plant, fixture.options)); } // Test static linearization method for different contact models @@ -257,16 +261,16 @@ TEST_P(LCSFactoryParameterizedPivotingTest, LinearizePlantToLCS) { 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)); + EXPECT_EQ(lcs.num_lambdas(), LCSFactory::GetNumContactVariables( + *fixture.plant, fixture.options)); } // Test that updating state and input changes contact-dependent LCS matrices TEST_P(LCSFactoryParameterizedPivotingTest, UpdateStateAndInput) { // Generate initial LCS at zero state LCS initial_lcs = fixture.lcs_factory->GenerateLCS(); - auto [initial_J, initial_contact_points] = - fixture.lcs_factory->GetContactJacobianAndPoints(); + auto initial_contact_descriptions = + fixture.lcs_factory->GetContactDescriptions(); // Update to non-zero state (cube tilted and positioned above platform) drake::VectorX state = VectorXd::Zero( @@ -279,10 +283,9 @@ TEST_P(LCSFactoryParameterizedPivotingTest, UpdateStateAndInput) { // Generate updated LCS LCS updated_lcs = fixture.lcs_factory->GenerateLCS(); - auto [updated_J, updated_contact_points] = - fixture.lcs_factory->GetContactJacobianAndPoints(); + auto updated_contact_descriptions = + fixture.lcs_factory->GetContactDescriptions(); - // Dynamics matrices should remain the same (linearized at same point) EXPECT_EQ(initial_lcs.A(), updated_lcs.A()); EXPECT_EQ(initial_lcs.B(), updated_lcs.B()); EXPECT_EQ(initial_lcs.d(), updated_lcs.d()); @@ -297,15 +300,20 @@ TEST_P(LCSFactoryParameterizedPivotingTest, UpdateStateAndInput) { } // Contact Jacobian and points should change - EXPECT_NE(initial_J, updated_J); - for (size_t i = 0; i < initial_contact_points.size(); ++i) { - EXPECT_NE(initial_contact_points[i], updated_contact_points[i]); + 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 contact Jacobian computation for different contact models -TEST_P(LCSFactoryParameterizedPivotingTest, ComputeContactJacobian) { - auto [J, contact_points] = fixture.lcs_factory->GetContactJacobianAndPoints(); +TEST_P(LCSFactoryParameterizedPivotingTest, CheckContactDescriptionSizes) { + auto contact_descriptions = fixture.lcs_factory->GetContactDescriptions(); int n_contacts = fixture.contact_pairs.size(); auto n_tangential_directions = @@ -313,27 +321,31 @@ TEST_P(LCSFactoryParameterizedPivotingTest, ComputeContactJacobian) { 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()); + } + // Verify Jacobian rows based on contact model switch (fixture.contact_model) { case ContactModel::kStewartAndTrinkle: // Normal + tangential directions for all contacts - EXPECT_EQ(J.rows(), n_contacts + n_tangential_directions); + EXPECT_EQ(contact_descriptions.size(), + 2 * n_contacts + n_tangential_directions); break; case ContactModel::kFrictionlessSpring: // Normal directions only - EXPECT_EQ(J.rows(), n_contacts); + EXPECT_EQ(contact_descriptions.size(), n_contacts); break; case ContactModel::kAnitescu: // Tangential directions only (normal handled differently) - EXPECT_EQ(J.rows(), n_tangential_directions); + EXPECT_EQ(contact_descriptions.size(), n_tangential_directions); break; default: EXPECT_TRUE(false); } - - // Jacobian should map velocities to contact space - EXPECT_EQ(J.cols(), fixture.plant->num_velocities()); - EXPECT_EQ(contact_points.size(), n_contacts); } // Test fixing specific contact modes in the LCS diff --git a/multibody/test/resources/lcs_factory_pivoting_options.yaml b/multibody/test/resources/lcs_factory_pivoting_options.yaml index 1b7b7f6..6f82dd9 100644 --- a/multibody/test/resources/lcs_factory_pivoting_options.yaml +++ b/multibody/test/resources/lcs_factory_pivoting_options.yaml @@ -3,7 +3,7 @@ num_friction_directions: 1 planar_normal_direction: [0, 1, 0] num_contacts: 3 spring_stiffness: 0 -mu: [0.1, 0.1, 0.1] +mu: 0.1 N: 10 dt: 0.01 contact_pair_configs: # Not used but kept to test parsing diff --git a/systems/BUILD.bazel b/systems/BUILD.bazel index 54fe405..59fd10b 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", ], ) @@ -86,10 +88,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( name = "quaternion_error_hessian", @@ -113,6 +134,7 @@ cc_library( "@drake//:drake_shared_library", ], ) + cc_test( name = "systems_test", srcs = [ @@ -164,11 +186,27 @@ cc_test( size = "small", ) +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 338bf32..1b99a58 100644 --- a/systems/c3_controller.cc +++ b/systems/c3_controller.cc @@ -71,7 +71,7 @@ C3Controller::C3Controller( // Determine the size of lambda based on the contact model n_lambda_ = multibody::LCSFactory::GetNumContactVariables( - controller_options_.lcs_factory_options); + plant_, controller_options_.lcs_factory_options); // Placeholder vector for initialization VectorXd zeros = VectorXd::Zero(n_x_ + n_lambda_ + n_u_); @@ -119,8 +119,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 0602dac..da6c609 100644 --- a/systems/c3_controller.h +++ b/systems/c3_controller.h @@ -113,6 +113,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 94e20e8..54457c4 100644 --- a/systems/c3_controller_options.h +++ b/systems/c3_controller_options.h @@ -81,19 +81,6 @@ struct C3ControllerOptions { if (projection_type == "QP") { DRAKE_DEMAND(lcs_factory_options.contact_model == "anitescu"); } - - int expected_lambda_size = - LCSFactory::GetNumContactVariables(lcs_factory_options); - DRAKE_DEMAND(static_cast(c3_options.g_lambda.size()) == - expected_lambda_size); - DRAKE_DEMAND(static_cast(c3_options.u_lambda.size()) == - expected_lambda_size); - if (projection_type == "C3+") { - DRAKE_DEMAND(static_cast(c3_options.g_eta->size()) == - expected_lambda_size); - DRAKE_DEMAND(static_cast(c3_options.u_eta->size()) == - expected_lambda_size); - } } }; diff --git a/systems/framework/c3_output.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..1db767c --- /dev/null +++ b/systems/lcmt_generators/c3_output_generator.cc @@ -0,0 +1,73 @@ +#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(); + // 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..eedd972 --- /dev/null +++ b/systems/lcmt_generators/c3_trajectory_generator.cc @@ -0,0 +1,117 @@ +#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(); +} + +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..b9e93c5 --- /dev/null +++ b/systems/lcmt_generators/c3_trajectory_generator_config.h @@ -0,0 +1,98 @@ +#pragma once + +#include +#include + +namespace c3 { +namespace systems { +namespace lcmt_generators { + +struct TrajectoryDescription { + // =========================================================================== + // Index Range for Extracting Vector Slices + // =========================================================================== + // + // The index_range struct defines a contiguous slice of a vector (state, + // input, or force) to extract and publish as a separate trajectory. + // + // Purpose: + // -------- + // C3 systems typically have large state/input/force vectors. Often, we only + // want to visualize or log a subset of these variables. For example: + // - Extract positions [0:6] from a 14-dimensional state vector + // - Extract a single actuator force [3] from a 10-dimensional input vector + // - Extract contact forces [0:3] from a 20-dimensional force vector + // + // Usage: + // ------ + // Given a vector x of size n, index_range {start, end} extracts: + // x[start], x[start+1], ..., x[end-1] + // + // This follows standard C++ iterator conventions where: + // - start: inclusive (first element to include) + // - end: exclusive (one past the last element to include) + // + // Examples: + // --------- + // 1. Extract full 7-DOF robot joint positions: + // {start: 0, end: 7} -> extracts indices [0, 1, 2, 3, 4, 5, 6] + // + // 2. Extract x, y, z position from state: + // {start: 0, end: 3} -> extracts indices [0, 1, 2] + // + // 3. Extract single element: + // {start: 5, end: 6} -> extracts index [5] + // + // Multiple Ranges: + // ---------------- + // The 'indices' vector allows specifying multiple non-contiguous ranges + // that will be concatenated into a single trajectory. For example: + // indices: [{start: 0, end: 3}, {start: 7, end: 10}] + // -> extracts [0, 1, 2, 7, 8, 9] and concatenates them + // + // This is useful when you want to publish related but non-contiguous + // variables together, such as positions from multiple joints that aren't + // adjacent in the state vector. + // =========================================================================== + struct index_range { + int start; // Start index (inclusive) - first element to extract + int end; // End index (exclusive) - one past the last element to extract + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(start)); + a->Visit(DRAKE_NVP(end)); + } + }; + + std::string trajectory_name; // Name of the trajectory for LCM publishing + std::string + variable_type; // Type of C3 variable: "state", "input", or "force" + std::vector + indices; // List of index ranges to extract and concatenate + + 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 and publish via LCM + + 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..d92f9b3 --- /dev/null +++ b/systems/lcmt_generators/contact_force_generator.cc @@ -0,0 +1,110 @@ +#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(); + + // 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 91bead8..85c28ec 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; @@ -52,7 +53,7 @@ void LCSFactorySystem::InitializeSystem( this->set_name("lcs_factory_system"); n_x_ = plant.num_positions() + plant.num_velocities(); - n_lambda_ = multibody::LCSFactory::GetNumContactVariables(options); + n_lambda_ = multibody::LCSFactory::GetNumContactVariables(plant, options); n_u_ = plant.num_actuators(); lcs_state_input_port_ = @@ -69,12 +70,10 @@ void LCSFactorySystem::InitializeSystem( &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(); } @@ -91,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*>( @@ -102,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 92b7464..46edd25 100644 --- a/systems/lcs_factory_system.h +++ b/systems/lcs_factory_system.h @@ -94,8 +94,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: @@ -117,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/resources/quaternion_cost_test.yaml b/systems/test/resources/quaternion_cost_test.yaml index dfaeeb0..caa08aa 100644 --- a/systems/test/resources/quaternion_cost_test.yaml +++ b/systems/test/resources/quaternion_cost_test.yaml @@ -15,8 +15,7 @@ lcs_factory_options: contact_model: "anitescu" num_friction_directions: 2 num_contacts: 8 - spring_stiffness: 0 - mu: [0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4] + mu: 0.4 N: 10 dt: 0.02 diff --git a/systems/test/systems_test.cc b/systems/test/systems_test.cc index ab81645..6fd56f0 100644 --- a/systems/test/systems_test.cc +++ b/systems/test/systems_test.cc @@ -16,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; @@ -204,6 +205,10 @@ TYPED_TEST(C3ControllerTypedTest, CheckPlannedTrajectory) { 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); @@ -340,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) { @@ -350,8 +356,9 @@ TEST_F(LCSFactorySystemTest, OutputLCSIsValid) { const auto& lcs = lcs_output->get_data(0)->get_value(); 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( - controller_options.lcs_factory_options)); + EXPECT_EQ(lcs.num_lambdas(), + LCSFactory::GetNumContactVariables( + *plant, controller_options.lcs_factory_options)); EXPECT_EQ(lcs.dt(), controller_options.lcs_factory_options.dt); EXPECT_EQ(lcs.N(), controller_options.lcs_factory_options.N); } @@ -360,14 +367,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