From bc0976aa9f21267021fb68e3550f658e0517a0f2 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Mon, 23 Jun 2025 12:39:30 -0400 Subject: [PATCH 01/13] fix: add ability to update solver options for c3 controller --- systems/c3_controller.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/systems/c3_controller.h b/systems/c3_controller.h index 8a8df72..254c379 100644 --- a/systems/c3_controller.h +++ b/systems/c3_controller.h @@ -98,6 +98,10 @@ class C3Controller : public drake::systems::LeafSystem { c3_->AddLinearConstraint(A, lower_bound, upper_bound, constraint); } + void SetSolverOptions(const drake::solvers::SolverOptions& options) { + c3_->SetSolverOptions(options); + } + private: /** * @struct JointDescription From 77c5fc4cb5f4e1ff2f7e0fdea7497969eaf62b3d Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 24 Jun 2025 10:22:47 -0400 Subject: [PATCH 02/13] feat: bring output publishers from dairlib to c3 --- core/c3.cc | 1 + core/configs/solve_options_default.hpp | 11 +++ lcmtypes/BUILD.bazel | 47 ++++++++++ lcmtypes/__init__.py | 1 + lcmtypes/lcmt_contact_force.lcm | 12 +++ lcmtypes/lcmt_forces.lcm | 11 +++ lcmtypes/lcmt_intermediates.lcm | 12 +++ lcmtypes/lcmt_output.lcm | 9 ++ lcmtypes/lcmt_solution.lcm | 14 +++ systems/BUILD.bazel | 31 +++++++ systems/framework/c3_output.cc | 68 ++++++++++++++ systems/framework/c3_output.h | 4 + systems/publishers/force_publisher.cc | 119 +++++++++++++++++++++++++ systems/publishers/force_publisher.h | 55 ++++++++++++ systems/publishers/output_publisher.cc | 65 ++++++++++++++ systems/publishers/output_publisher.h | 55 ++++++++++++ 16 files changed, 515 insertions(+) create mode 100644 core/configs/solve_options_default.hpp create mode 100644 lcmtypes/BUILD.bazel create mode 100644 lcmtypes/__init__.py create mode 100644 lcmtypes/lcmt_contact_force.lcm create mode 100644 lcmtypes/lcmt_forces.lcm create mode 100644 lcmtypes/lcmt_intermediates.lcm create mode 100644 lcmtypes/lcmt_output.lcm create mode 100644 lcmtypes/lcmt_solution.lcm create mode 100644 systems/publishers/force_publisher.cc create mode 100644 systems/publishers/force_publisher.h create mode 100644 systems/publishers/output_publisher.cc create mode 100644 systems/publishers/output_publisher.h diff --git a/core/c3.cc b/core/c3.cc index 7693c29..f7848d9 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -9,6 +9,7 @@ #include "lcs.h" #include "solver_options_io.h" +#include "configs/solve_options_default.hpp" #include "drake/common/text_logging.h" #include "drake/solvers/mathematical_program.h" diff --git a/core/configs/solve_options_default.hpp b/core/configs/solve_options_default.hpp new file mode 100644 index 0000000..1890e19 --- /dev/null +++ b/core/configs/solve_options_default.hpp @@ -0,0 +1,11 @@ +#pragma once +#include +const std::string default_solver_options = + "print_to_console: 0\nlog_file_name: \"\"\nint_options:\n max_iter: " + "1000\n # linsys_solver: 0\n verbose: 0\n warm_start: 1\n polish: 1\n " + "scaled_termination: 1\n check_termination: 25\n polish_refine_iter: 3\n " + " scaling: 10\n adaptive_rho: 1\n adaptive_rho_interval: " + "0\n\ndouble_options:\n rho: 0.0001\n sigma: 1e-6\n eps_abs: 1e-5\n " + "eps_rel: 1e-5\n eps_prim_inf: 1e-5\n eps_dual_inf: 1e-5\n alpha: 1.6\n " + " delta: 1e-6\n adaptive_rho_tolerance: 5\n adaptive_rho_fraction: 0.4\n " + " time_limit: 1.0\n\nstring_options: {}"; diff --git a/lcmtypes/BUILD.bazel b/lcmtypes/BUILD.bazel new file mode 100644 index 0000000..75494ac --- /dev/null +++ b/lcmtypes/BUILD.bazel @@ -0,0 +1,47 @@ +load( + "@drake//tools/skylark:drake_lcm.bzl", + "drake_lcm_cc_library", + "drake_lcm_java_library", + "drake_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 +drake_lcm_cc_library( + name = "lcmt_c3", + lcm_package = "c3", + lcm_srcs = glob(["*.lcm"]), +) + +drake_lcm_java_library( + name = "lcmtypes_c3_java", + lcm_package = "c3", + lcm_srcs = glob(["*.lcm"]), +) + +drake_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"]), + deps = [ + "@drake//:module_py", + ], +) + +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_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_forces.lcm b/lcmtypes/lcmt_forces.lcm new file mode 100644 index 0000000..c888776 --- /dev/null +++ b/lcmtypes/lcmt_forces.lcm @@ -0,0 +1,11 @@ +package c3; + +/* lcmtype containing information to visualize forces in meshcat +*/ +struct lcmt_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/systems/BUILD.bazel b/systems/BUILD.bazel index 29eb400..802f1d7 100644 --- a/systems/BUILD.bazel +++ b/systems/BUILD.bazel @@ -40,6 +40,7 @@ cc_library( name = "c3_controller", deps = [ ":options", ":framework", + ":publishers", "//core:c3", "//core:options", "//multibody:lcs_factory", @@ -85,6 +86,36 @@ cc_library( deps = [ "//core:options", "//multibody:options", + ] +) + +cc_library( + name = "publishers", + srcs = [ + "publishers/output_publisher.cc", + "publishers/force_publisher.cc" + ], + hdrs = [ + "publishers/output_publisher.h", + "publishers/force_publisher.h" + ], + deps = [ + ":c3_output", + "//lcmtypes:lcmt_c3", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "c3_output", + srcs = [ + "framework/c3_output.cc", + ], + hdrs = [ + "framework/c3_output.h", + ], + deps = [ + "//lcmtypes:lcmt_c3", "@drake//:drake_shared_library", ] ) 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..9ac77df 100644 --- a/systems/framework/c3_output.h +++ b/systems/framework/c3_output.h @@ -5,6 +5,8 @@ #include #include +#include "c3/lcmt_output.hpp" + #include using Eigen::MatrixXf; @@ -63,6 +65,8 @@ class C3Output { virtual ~C3Output() = default; + lcmt_output GenerateLcmObject(double time) const; + private: C3Solution c3_solution_; C3Intermediates c3_intermediates_; diff --git a/systems/publishers/force_publisher.cc b/systems/publishers/force_publisher.cc new file mode 100644 index 0000000..8662a1e --- /dev/null +++ b/systems/publishers/force_publisher.cc @@ -0,0 +1,119 @@ +#include "force_publisher.h" + +#include "systems/framework/c3_output.h" + +using drake::systems::Context; +using drake::systems::DiagramBuilder; +using drake::systems::lcm::LcmPublisherSystem; + +using Eigen::MatrixXd; +using Eigen::Quaterniond; +using Eigen::VectorXd; + +namespace c3 { +namespace systems { +namespace publishers { +ContactForcePublisher::ContactForcePublisher() { + c3_solution_port_ = this->DeclareAbstractInputPort( + "solution", drake::Value{}) + .get_index(); + lcs_contact_info_port_ = + this->DeclareAbstractInputPort( + "J_lcs, p_lcs", + drake::Value< + std::pair>>()) + .get_index(); + + this->set_name("c3_contact_force_publisher"); + contact_force_output_port_ = + this->DeclareAbstractOutputPort("lcmt_force", lcmt_forces(), + &ContactForcePublisher::DoCalc) + .get_index(); +} + +void ContactForcePublisher::DoCalc(const Context& context, + lcmt_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< + std::pair>>( + context, lcs_contact_info_port_); + // Contact Jacobian + MatrixXd J_c = contact_info->first; + int contact_force_start = solution->lambda_sol_.rows() - J_c.rows(); + bool using_stewart_and_trinkle_model = contact_force_start > 0; + + auto contact_points = contact_info->second; + int forces_per_contact = contact_info->first.rows() / contact_points.size(); + + output->num_forces = forces_per_contact * contact_points.size(); + output->forces.resize(output->num_forces); + + int contact_var_start; + int force_index; + for (int contact_index = 0; contact_index < (int)contact_points.size(); + ++contact_index) { + contact_var_start = + contact_force_start + forces_per_contact * contact_index; + force_index = forces_per_contact * contact_index; + for (int i = 0; i < forces_per_contact; ++i) { + int contact_jacobian_row = force_index + i; // index for anitescu model + int contact_var_index = contact_var_start + i; + if (using_stewart_and_trinkle_model) { // index for stweart and trinkle + // model + if (i == 0) { + contact_jacobian_row = contact_index; + contact_var_index = contact_force_start + contact_index; + } else { + contact_jacobian_row = contact_points.size() + + (forces_per_contact - 1) * contact_index + i - + 1; + contact_var_index = contact_force_start + contact_points.size() + + (forces_per_contact - 1) * contact_index + i - 1; + } + } + auto force = lcmt_contact_force(); + force.contact_point[0] = contact_points.at(contact_index)[0]; + force.contact_point[1] = contact_points.at(contact_index)[1]; + force.contact_point[2] = contact_points.at(contact_index)[2]; + // TODO(yangwill): find a cleaner way to figure out the equivalent forces + // VISUALIZING FORCES FOR THE FIRST KNOT POINT + // 6, 7, 8 are the indices for the x,y,z components of the tray + // expressed in the world frame + force.contact_force[0] = solution->lambda_sol_(contact_var_index, 0) * + J_c.row(contact_jacobian_row)(6); + force.contact_force[1] = solution->lambda_sol_(contact_var_index, 0) * + J_c.row(contact_jacobian_row)(7); + force.contact_force[2] = solution->lambda_sol_(contact_var_index, 0) * + J_c.row(contact_jacobian_row)(8); + output->forces[force_index + i] = force; + } + } + output->utime = context.get_time() * 1e6; +} + +LcmPublisherSystem* ContactForcePublisher::AddLcmPublisherToBuilder( + DiagramBuilder& builder, + const drake::systems::OutputPort& solution_port, + const drake::systems::OutputPort& lcs_contact_info_port, + const std::string& channel, drake::lcm::DrakeLcmInterface* lcm, + const drake::systems::TriggerTypeSet& publish_triggers, + double publish_period, double publish_offset) { + auto force_publisher = builder.AddSystem(); + builder.Connect(solution_port, force_publisher->get_input_port_c3_solution()); + builder.Connect(lcs_contact_info_port, + force_publisher->get_input_port_lcs_contact_info()); + + 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 publishers +} // namespace systems +} // namespace c3 \ No newline at end of file diff --git a/systems/publishers/force_publisher.h b/systems/publishers/force_publisher.h new file mode 100644 index 0000000..63dad9c --- /dev/null +++ b/systems/publishers/force_publisher.h @@ -0,0 +1,55 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include "c3/lcmt_forces.hpp" + +namespace c3 { +namespace systems { +namespace publishers { +class ContactForcePublisher : public drake::systems::LeafSystem { + public: + ContactForcePublisher(); + + const drake::systems::InputPort& get_input_port_c3_solution() const { + return this->get_input_port(c3_solution_port_); + } + + const drake::systems::InputPort& get_input_port_lcs_contact_info() + const { + return this->get_input_port(lcs_contact_info_port_); + } + + // LCS contact force, not actor input forces + const drake::systems::OutputPort& get_output_port_contact_force() + const { + return this->get_output_port(contact_force_output_port_); + } + + static drake::systems::lcm::LcmPublisherSystem* + AddLcmPublisherToBuilder( + drake::systems::DiagramBuilder& builder, + const drake::systems::OutputPort& solution_port, + const drake::systems::OutputPort& lcs_contact_info_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: + void DoCalc(const drake::systems::Context& context, + c3::lcmt_forces* c3_forces_output) const; + + drake::systems::InputPortIndex c3_solution_port_; + drake::systems::InputPortIndex lcs_contact_info_port_; + drake::systems::OutputPortIndex contact_force_output_port_; +}; + +} // namespace publishers +} // namespace systems +} // namespace c3 \ No newline at end of file diff --git a/systems/publishers/output_publisher.cc b/systems/publishers/output_publisher.cc new file mode 100644 index 0000000..25b5f9b --- /dev/null +++ b/systems/publishers/output_publisher.cc @@ -0,0 +1,65 @@ +#include "output_publisher.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 publishers { + +C3OutputPublisher::C3OutputPublisher() { + c3_solution_port_ = + this->DeclareAbstractInputPort("c3_solution", + drake::Value{}) + .get_index(); + c3_intermediates_port_ = + this->DeclareAbstractInputPort("c3_intermediates", + drake::Value{}) + .get_index(); + + this->set_name("c3_output_publisher"); + c3_output_port_ = + this->DeclareAbstractOutputPort("lcmt_c3_output", c3::lcmt_output(), + &C3OutputPublisher::DoCalc) + .get_index(); +} + +void C3OutputPublisher::DoCalc(const drake::systems::Context& context, + c3::lcmt_output* output) const { + const auto& c3_solution = + this->EvalInputValue(context, c3_solution_port_); + const auto& c3_intermediates = + this->EvalInputValue(context, + c3_intermediates_port_); + + C3Output c3_output = C3Output(*c3_solution, *c3_intermediates); + *output = c3_output.GenerateLcmObject(context.get_time()); +} + +LcmPublisherSystem* C3OutputPublisher::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) { + 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()); + + 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 publishers +} // namespace systems +} // namespace c3 \ No newline at end of file diff --git a/systems/publishers/output_publisher.h b/systems/publishers/output_publisher.h new file mode 100644 index 0000000..9c3c88d --- /dev/null +++ b/systems/publishers/output_publisher.h @@ -0,0 +1,55 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include "c3/lcmt_output.hpp" + +namespace c3 { +namespace systems { +namespace publishers { +/// Converts a OutputVector object to LCM type lcmt_robot_output +class C3OutputPublisher : public drake::systems::LeafSystem { + public: + C3OutputPublisher(); + + const drake::systems::InputPort& get_input_port_c3_solution() const { + return this->get_input_port(c3_solution_port_); + } + + const drake::systems::InputPort& get_input_port_c3_intermediates() + const { + return this->get_input_port(c3_intermediates_port_); + } + + const drake::systems::OutputPort& get_output_port_c3_output() const { + return this->get_output_port(c3_output_port_); + } + + 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: + void DoCalc(const drake::systems::Context& context, + c3::lcmt_output* output) const; + + // void OutputNextC3Input(const drake::systems::Context& context, + // drake::systems::BasicVector* u_next) const; + + drake::systems::InputPortIndex c3_solution_port_; + drake::systems::InputPortIndex c3_intermediates_port_; + drake::systems::OutputPortIndex c3_output_port_; +}; +} // namespace publishers +} // namespace systems +} // namespace c3 \ No newline at end of file From 5a1657826ee5dddea25d5f709f0a3d80e67dc5c0 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Mon, 30 Jun 2025 09:30:48 -0400 Subject: [PATCH 03/13] doc: add documentation for publishers --- systems/publishers/force_publisher.cc | 16 ++++++ systems/publishers/force_publisher.h | 67 +++++++++++++++++++++++--- systems/publishers/output_publisher.cc | 10 ++++ systems/publishers/output_publisher.h | 52 +++++++++++++++++--- 4 files changed, 132 insertions(+), 13 deletions(-) diff --git a/systems/publishers/force_publisher.cc b/systems/publishers/force_publisher.cc index 8662a1e..616585a 100644 --- a/systems/publishers/force_publisher.cc +++ b/systems/publishers/force_publisher.cc @@ -2,6 +2,7 @@ #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; @@ -13,10 +14,14 @@ using Eigen::VectorXd; namespace c3 { namespace systems { namespace publishers { + +// Publishes contact force information to LCM for visualization and logging. ContactForcePublisher::ContactForcePublisher() { + // 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( "J_lcs, p_lcs", @@ -25,12 +30,14 @@ ContactForcePublisher::ContactForcePublisher() { .get_index(); this->set_name("c3_contact_force_publisher"); + // Declare output port for publishing contact forces. contact_force_output_port_ = this->DeclareAbstractOutputPort("lcmt_force", lcmt_forces(), &ContactForcePublisher::DoCalc) .get_index(); } +// Calculates and outputs the contact forces based on the current context. void ContactForcePublisher::DoCalc(const Context& context, lcmt_forces* output) const { // Get Solution from C3 @@ -53,6 +60,7 @@ void ContactForcePublisher::DoCalc(const Context& context, int contact_var_start; int force_index; + // Iterate over all contact points and compute forces. for (int contact_index = 0; contact_index < (int)contact_points.size(); ++contact_index) { contact_var_start = @@ -75,6 +83,7 @@ void ContactForcePublisher::DoCalc(const Context& context, } } auto force = lcmt_contact_force(); + // Set contact point position. force.contact_point[0] = contact_points.at(contact_index)[0]; force.contact_point[1] = contact_points.at(contact_index)[1]; force.contact_point[2] = contact_points.at(contact_index)[2]; @@ -82,6 +91,9 @@ void ContactForcePublisher::DoCalc(const Context& context, // VISUALIZING FORCES FOR THE FIRST KNOT POINT // 6, 7, 8 are the indices for the x,y,z components of the tray // expressed in the world frame + // Compute force vector in world frame. + std::cout << J_c << std::endl; + exit(0); force.contact_force[0] = solution->lambda_sol_(contact_var_index, 0) * J_c.row(contact_jacobian_row)(6); force.contact_force[1] = solution->lambda_sol_(contact_var_index, 0) * @@ -91,9 +103,11 @@ void ContactForcePublisher::DoCalc(const Context& context, output->forces[force_index + i] = force; } } + // Set output timestamp in microseconds. output->utime = context.get_time() * 1e6; } +// Adds this publisher and an LCM publisher system to the diagram builder. LcmPublisherSystem* ContactForcePublisher::AddLcmPublisherToBuilder( DiagramBuilder& builder, const drake::systems::OutputPort& solution_port, @@ -101,11 +115,13 @@ LcmPublisherSystem* ContactForcePublisher::AddLcmPublisherToBuilder( const std::string& channel, drake::lcm::DrakeLcmInterface* lcm, const drake::systems::TriggerTypeSet& publish_triggers, double publish_period, double publish_offset) { + // Add and connect the ContactForcePublisher system. auto force_publisher = builder.AddSystem(); builder.Connect(solution_port, force_publisher->get_input_port_c3_solution()); builder.Connect(lcs_contact_info_port, force_publisher->get_input_port_lcs_contact_info()); + // Add and connect the LCM publisher system. auto lcm_force_publisher = builder.AddSystem(LcmPublisherSystem::Make( channel, lcm, publish_triggers, publish_period, publish_offset)); diff --git a/systems/publishers/force_publisher.h b/systems/publishers/force_publisher.h index 63dad9c..f47ac44 100644 --- a/systems/publishers/force_publisher.h +++ b/systems/publishers/force_publisher.h @@ -13,27 +13,73 @@ namespace c3 { namespace systems { namespace publishers { + +/** + * @class ContactForcePublisher + * @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 ContactForcePublisher : public drake::systems::LeafSystem { public: + /** + * @brief Constructs a ContactForcePublisher system. + * + * Declares input and output ports for the solution vector, LCS contact info, + * and contact force output. + */ ContactForcePublisher(); + /** + * @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_info() const { return this->get_input_port(lcs_contact_info_port_); } - // LCS contact force, not actor input forces + /** + * @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_); } - static drake::systems::lcm::LcmPublisherSystem* - AddLcmPublisherToBuilder( + /** + * @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_info_port, @@ -42,12 +88,21 @@ class ContactForcePublisher : public drake::systems::LeafSystem { 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_forces* c3_forces_output) const; - drake::systems::InputPortIndex c3_solution_port_; - drake::systems::InputPortIndex lcs_contact_info_port_; - drake::systems::OutputPortIndex contact_force_output_port_; + 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 publishers diff --git a/systems/publishers/output_publisher.cc b/systems/publishers/output_publisher.cc index 25b5f9b..c18fd96 100644 --- a/systems/publishers/output_publisher.cc +++ b/systems/publishers/output_publisher.cc @@ -10,35 +10,43 @@ namespace c3 { namespace systems { namespace publishers { +// Publishes C3Output as an LCM message. C3OutputPublisher::C3OutputPublisher() { + // Declare input port for the C3 solution. c3_solution_port_ = this->DeclareAbstractInputPort("c3_solution", drake::Value{}) .get_index(); + // Declare input port for C3 intermediates. c3_intermediates_port_ = this->DeclareAbstractInputPort("c3_intermediates", drake::Value{}) .get_index(); this->set_name("c3_output_publisher"); + // Declare output port for the LCM message. c3_output_port_ = this->DeclareAbstractOutputPort("lcmt_c3_output", c3::lcmt_output(), &C3OutputPublisher::DoCalc) .get_index(); } +// Calculates the LCM output message from the input ports. void C3OutputPublisher::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* C3OutputPublisher::AddLcmPublisherToBuilder( DiagramBuilder& builder, const drake::systems::OutputPort& solution_port, @@ -46,12 +54,14 @@ LcmPublisherSystem* C3OutputPublisher::AddLcmPublisherToBuilder( const std::string& channel, drake::lcm::DrakeLcmInterface* lcm, const drake::systems::TriggerTypeSet& publish_triggers, double publish_period, double publish_offset) { + // Add and connect the C3OutputPublisher 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)); diff --git a/systems/publishers/output_publisher.h b/systems/publishers/output_publisher.h index 9c3c88d..afc78a5 100644 --- a/systems/publishers/output_publisher.h +++ b/systems/publishers/output_publisher.h @@ -13,24 +13,56 @@ namespace c3 { namespace systems { namespace publishers { -/// Converts a OutputVector object to LCM type lcmt_robot_output + +/** + * @class C3OutputPublisher + * @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 C3OutputPublisher : public drake::systems::LeafSystem { public: + /** + * @brief Constructor. Declares input and output ports. + */ C3OutputPublisher(); + /** + * @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, @@ -40,16 +72,22 @@ class C3OutputPublisher : public drake::systems::LeafSystem { 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; - // void OutputNextC3Input(const drake::systems::Context& context, - // drake::systems::BasicVector* u_next) const; - - drake::systems::InputPortIndex c3_solution_port_; - drake::systems::InputPortIndex c3_intermediates_port_; - drake::systems::OutputPortIndex c3_output_port_; + 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 publishers } // namespace systems } // namespace c3 \ No newline at end of file From 48d21334a9ddd38931ee1abfe42df2e385264eac Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 1 Jul 2025 21:18:13 +0000 Subject: [PATCH 04/13] fix: rework force publishing --- bindings/pyc3/c3_multibody_py.cc | 4 +- bindings/pyc3/c3_systems_py.cc | 6 +- lcmtypes/BUILD.bazel | 17 ++- ...cmt_forces.lcm => lcmt_contact_forces.lcm} | 2 +- multibody/geom_geom_collider.cc | 21 +++- multibody/geom_geom_collider.h | 28 ++++- multibody/lcs_factory.cc | 107 +++++++++--------- multibody/lcs_factory.h | 33 +++--- systems/BUILD.bazel | 1 + systems/lcs_factory_system.cc | 16 +-- systems/lcs_factory_system.h | 10 +- systems/publishers/force_publisher.cc | 102 +++++++---------- systems/publishers/force_publisher.h | 4 +- 13 files changed, 181 insertions(+), 170 deletions(-) rename lcmtypes/{lcmt_forces.lcm => lcmt_contact_forces.lcm} (86%) diff --git a/bindings/pyc3/c3_multibody_py.cc b/bindings/pyc3/c3_multibody_py.cc index ca7abea..b9145f5 100644 --- a/bindings/pyc3/c3_multibody_py.cc +++ b/bindings/pyc3/c3_multibody_py.cc @@ -37,8 +37,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("GetWitnessPointsAndForceBasisInWorldFrame", + // &c3::multibody::LCSFactory::GetWitnessPointsAndForceBasisInWorldFrame) .def("UpdateStateAndInput", &c3::multibody::LCSFactory::UpdateStateAndInput, py::arg("state"), py::arg("input")) diff --git a/bindings/pyc3/c3_systems_py.cc b/bindings/pyc3/c3_systems_py.cc index cc12670..67fa389 100644 --- a/bindings/pyc3/c3_systems_py.cc +++ b/bindings/pyc3/c3_systems_py.cc @@ -99,10 +99,10 @@ PYBIND11_MODULE(systems, m) { &LCSFactorySystem::get_input_port_lcs_input, 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, py::return_value_policy::reference); + // .def("get_output_port_lcs_contact_points_and_force_basis", + // &LCSFactorySystem::get_output_port_lcs_contact_points_and_force_basis, + // py::return_value_policy::reference); py::class_(m, "C3Solution") .def(py::init<>()) diff --git a/lcmtypes/BUILD.bazel b/lcmtypes/BUILD.bazel index 75494ac..7e711db 100644 --- a/lcmtypes/BUILD.bazel +++ b/lcmtypes/BUILD.bazel @@ -1,8 +1,8 @@ load( - "@drake//tools/skylark:drake_lcm.bzl", - "drake_lcm_cc_library", - "drake_lcm_java_library", - "drake_lcm_py_library", + "@drake//tools/workspace/lcm:lcm.bzl", + "lcm_cc_library", + "lcm_java_library", + "lcm_py_library", ) load( "@drake//tools/skylark:drake_java.bzl", @@ -13,27 +13,24 @@ load("@drake//tools/lint:lint.bzl", "add_lint_tests") package(default_visibility = ["//visibility:public"]) #Lcm libraries -drake_lcm_cc_library( +lcm_cc_library( name = "lcmt_c3", lcm_package = "c3", lcm_srcs = glob(["*.lcm"]), ) -drake_lcm_java_library( +lcm_java_library( name = "lcmtypes_c3_java", lcm_package = "c3", lcm_srcs = glob(["*.lcm"]), ) -drake_lcm_py_library( +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"]), - deps = [ - "@drake//:module_py", - ], ) drake_java_binary( diff --git a/lcmtypes/lcmt_forces.lcm b/lcmtypes/lcmt_contact_forces.lcm similarity index 86% rename from lcmtypes/lcmt_forces.lcm rename to lcmtypes/lcmt_contact_forces.lcm index c888776..04aedc3 100644 --- a/lcmtypes/lcmt_forces.lcm +++ b/lcmtypes/lcmt_contact_forces.lcm @@ -2,7 +2,7 @@ package c3; /* lcmtype containing information to visualize forces in meshcat */ -struct lcmt_forces +struct lcmt_contact_forces { int64_t utime; diff --git a/multibody/geom_geom_collider.cc b/multibody/geom_geom_collider.cc index cadbfab..40e0e99 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,25 @@ GeomGeomCollider::CalcWitnessPoints(const Context& context) { return std::pair, VectorX>(p_WCa, p_WCb); } +template +Matrix +GeomGeomCollider::CalcForceBasisInWorldFrame( + const Context& context, int num_friction_directions, + const Vector3d& planar_normal) const { + const auto query_result = GetGeometryQueryResult(context); + if (num_friction_directions < 1) { + // Planar contact: basis is constructed from the contact and planar normals. + return ComputePlanarForceBasis(query_result.signed_distance_pair.nhat_BA_W, + planar_normal); + } else { + // 3D contact: build polytope basis and rotate using contact normal. + auto R_WC = drake::math::RotationMatrix::MakeFromOneVector( + query_result.signed_distance_pair.nhat_BA_W, 0); + return ComputePolytopeForceBasis(num_friction_directions) * + R_WC.matrix().transpose(); + } +} + } // namespace multibody } // namespace c3 diff --git a/multibody/geom_geom_collider.h b/multibody/geom_geom_collider.h index 5e6b142..18ccce0 100644 --- a/multibody/geom_geom_collider.h +++ b/multibody/geom_geom_collider.h @@ -99,15 +99,37 @@ class GeomGeomCollider { * closest point on geometry B. */ std::pair, drake::VectorX> CalcWitnessPoints( - const drake::systems::Context& context); + const drake::systems::Context& context); + + /** + * @brief Computes a basis for contact forces in the world frame. + * + * Depending on the number of friction directions, this method constructs + * either a planar (2D) or polytope (3D) basis for the contact forces at the + * collision point, expressed in the world frame. For planar contact + * (num_friction_directions < 1), the basis is constructed from the contact + * normal and the provided planar normal. For 3D contact, a polytope basis is + * generated and rotated to align with the contact normal. + * + * @param context The context for the MultibodyPlant. + * @param num_friction_directions The number of friction directions for the + * polytope approximation. If less than 1, a planar basis is used. + * @param planar_normal The normal vector defining the plane for planar + * contact (default: {0, 1, 0}). + * @return A matrix whose rows form an orthonormal basis for the contact + * forces in the world frame. + */ + Eigen::Matrix CalcForceBasisInWorldFrame( + const drake::systems::Context& context, int num_friction_directions, + const Eigen::Vector3d& planar_normal = {0, 1, 0}) const; private: /** * @brief A struct to hold the results of a geometry query. * * This struct contains the signed distance pair, the frame IDs of the two - * geometries, the frames themselves, and the positions of the closest points - * on each geometry, expressed in their respective frames. + * geometries, the frames themselves, and the positions of the closest + * points on each geometry, expressed in their respective frames. */ struct GeometryQueryResult { /** diff --git a/multibody/lcs_factory.cc b/multibody/lcs_factory.cc index dd915d5..f78d846 100644 --- a/multibody/lcs_factory.cc +++ b/multibody/lcs_factory.cc @@ -137,21 +137,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) { @@ -424,53 +409,63 @@ LCS LCSFactory::LinearizePlantToLCS( return lcs_factory.GenerateLCS(); } -std::pair> -LCSFactory::GetContactJacobianAndPoints() { - VectorXd phi; // Signed distance values for contacts - MatrixXd Jn; // Normal contact Jacobian - MatrixXd Jt; // Tangential contact Jacobian - ComputeContactJacobian(phi, Jn, Jt); - auto [_, contact_points] = FindWitnessPoints(); - - if (frictionless_) { - // if frictionless_, we only need the normal jacobian - return std::make_pair(Jn, contact_points); - } - - if (contact_model_ == ContactModel::kStewartAndTrinkle) { - // if Stewart and Trinkle model, concatenate the normal and tangential - // jacobian - MatrixXd J_c = MatrixXd::Zero(n_contacts_ + Jt_row_sizes_.sum(), n_v_); - J_c << Jn, Jt; - return std::make_pair(J_c, contact_points); - } +std::vector LCSFactory::GetContactDescriptions() { + std::vector normal_contact_descriptions; + std::vector tangential_contact_descriptions; - // Model is Anitescu - int n_lambda_ = Jt_row_sizes_.sum(); - - // Eₜ = blkdiag(e,.., e), e ∈ 1ⁿᵉ - MatrixXd E_t = MatrixXd::Zero(n_contacts_, n_lambda_); for (int i = 0; i < n_contacts_; i++) { - E_t.block(i, Jt_row_sizes_.segment(0, i).sum(), 1, Jt_row_sizes_(i)) = - MatrixXd::Ones(1, Jt_row_sizes_(i)); + multibody::GeomGeomCollider collider(plant_, contact_pairs_[i]); + auto [p_WCa, p_WCb] = collider.CalcWitnessPoints(context_); + auto force_basis = + collider.CalcForceBasisInWorldFrame(context_, n_friction_directions_); + + for (int j = 0; j < force_basis.rows(); j++) { + LCSContactDescription contact_description = { + .witness_point_A = p_WCa, + .witness_point_B = p_WCb, + .force_basis = force_basis.row(j)}; + if (j == 0) + // Normal contact + normal_contact_descriptions.push_back(contact_description); + else + // Tangential contact + tangential_contact_descriptions.push_back(contact_description); + } } - // Apply same friction coefficients to each friction direction - // of the same contact - if (!frictionless_) DRAKE_DEMAND(mu_.size() == (size_t)n_contacts_); - VectorXd muXd = - Eigen::Map(mu_.data(), mu_.size()); - - VectorXd mu_vector = VectorXd::Zero(n_lambda_); - for (int i = 0; i < muXd.rows(); i++) { - mu_vector.segment(Jt_row_sizes_.segment(0, i).sum(), Jt_row_sizes_(i)) = - muXd(i) * VectorXd::Ones(Jt_row_sizes_(i)); + std::vector contact_descriptions; + if (contact_model_ == ContactModel::kFrictionlessSpring) + contact_descriptions.insert(contact_descriptions.end(), + normal_contact_descriptions.begin(), + normal_contact_descriptions.end()); + else if (contact_model_ == ContactModel::kStewartAndTrinkle) { + for (int i = 0; i < n_contacts_; i++) + contact_descriptions.push_back( + LCSContactDescription::CreateSlackVariableDescription()); + contact_descriptions.insert(contact_descriptions.end(), + normal_contact_descriptions.begin(), + normal_contact_descriptions.end()); + contact_descriptions.insert(contact_descriptions.end(), + tangential_contact_descriptions.begin(), + tangential_contact_descriptions.end()); + } else if (contact_model_ == ContactModel::kAnitescu) { + contact_descriptions.insert(contact_descriptions.end(), + tangential_contact_descriptions.begin(), + tangential_contact_descriptions.end()); + DRAKE_ASSERT(n_friction_directions_ > 0); + for (int i = 0; i < tangential_contact_descriptions.size(); i++) { + int normal_index = i / (2 * n_friction_directions_); + DRAKE_ASSERT( + contact_descriptions.at(i).witness_point_A == + normal_contact_descriptions.at(normal_index).witness_point_A); + contact_descriptions.at(i).force_basis = + - contact_descriptions.at(i).force_basis + + mu_[normal_index] * + normal_contact_descriptions.at(normal_index).force_basis; + } } - MatrixXd mu_matrix = mu_vector.asDiagonal(); - // Constructing friction bases Jc = EᵀJₙ + μJₜ - MatrixXd J_c = E_t.transpose() * Jn + mu_matrix * Jt; - return std::make_pair(J_c, contact_points); + return contact_descriptions; } LCS LCSFactory::FixSomeModes(const LCS& other, set active_lambda_inds, diff --git a/multibody/lcs_factory.h b/multibody/lcs_factory.h index e6f802e..762fee6 100644 --- a/multibody/lcs_factory.h +++ b/multibody/lcs_factory.h @@ -47,6 +47,19 @@ inline const std::map& GetContactModelMap() { return kContactModelMap; } +struct LCSContactDescription { + Eigen::Vector3d witness_point_A; ///< Witness point on geometry A. + Eigen::Vector3d witness_point_B; ///< Witness point on geometry B. + Eigen::Vector3d force_basis; ///< Force basis vector + bool is_slack = false; ///< Indicates if the contact variable associate to + ///< the LCS is a slack variable. + + static LCSContactDescription CreateSlackVariableDescription() { + return {Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), + Eigen::Vector3d::Zero(), true}; + } +}; + /** * @class LCSFactory * @brief Factory class for creating Linear Complementarity Systems (LCS) from @@ -91,16 +104,12 @@ class LCSFactory { LCS GenerateLCS(); /** - * @brief Computes the contact Jacobian for a given multibody plant and - * context. - * - * This method calculates the signed distance values and the contact Jacobians - * for normal and tangential forces at the specified contact points. + * @brief Finds the witness points for each contact pair. * - * @return A pair containing the contact Jacobian matrix and a vector of - * contact points. + * @return A pair of vectors containing the witness points on each geometry + * for each contact pair. */ - std::pair> GetContactJacobianAndPoints(); + std::vector GetContactDescriptions(); /** * @brief Updates the state and input vectors in the internal context. @@ -284,14 +293,6 @@ class LCSFactory { */ void ComputeContactJacobian(VectorXd& phi, MatrixXd& Jn, MatrixXd& Jt); - /** - * @brief Finds the witness points for each contact pair. - * - * @return A pair of vectors containing the witness points on each geometry - * for each contact pair. - */ - std::pair, std::vector> FindWitnessPoints(); - // References to the MultibodyPlant and its contexts const drake::multibody::MultibodyPlant& plant_; drake::systems::Context& context_; diff --git a/systems/BUILD.bazel b/systems/BUILD.bazel index 802f1d7..fab00bf 100644 --- a/systems/BUILD.bazel +++ b/systems/BUILD.bazel @@ -101,6 +101,7 @@ cc_library( ], deps = [ ":c3_output", + "//multibody:lcs_factory", "//lcmtypes:lcmt_c3", "@drake//:drake_shared_library", ], diff --git a/systems/lcs_factory_system.cc b/systems/lcs_factory_system.cc index 91bead8..1f80cca 100644 --- a/systems/lcs_factory_system.cc +++ b/systems/lcs_factory_system.cc @@ -9,6 +9,7 @@ using c3::LCS; using c3::multibody::LCSFactory; +using c3::multibody::LCSContactDescription; using c3::systems::TimestampedVector; using drake::multibody::ModelInstanceIndex; using drake::systems::BasicVector; @@ -69,12 +70,11 @@ void LCSFactorySystem::InitializeSystem( &LCSFactorySystem::OutputLCS) .get_index(); - lcs_contact_jacobian_port_ = + lcs_contact_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 +91,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 +102,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..9fec4a8 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_description() const { + return this->get_output_port(lcs_contact_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_output_port_; // Convenience variables for system dimensions int n_q_; ///< Number of positions in the plant. diff --git a/systems/publishers/force_publisher.cc b/systems/publishers/force_publisher.cc index 616585a..3f9412c 100644 --- a/systems/publishers/force_publisher.cc +++ b/systems/publishers/force_publisher.cc @@ -1,5 +1,8 @@ #include "force_publisher.h" +#include + +#include "multibody/lcs_factory.h" #include "systems/framework/c3_output.h" // Drake and Eigen namespace usages for convenience. @@ -12,6 +15,9 @@ using Eigen::Quaterniond; using Eigen::VectorXd; namespace c3 { + +using multibody::LCSContactDescription; + namespace systems { namespace publishers { @@ -24,86 +30,56 @@ ContactForcePublisher::ContactForcePublisher() { // Declare input port for contact Jacobian and contact points. lcs_contact_info_port_ = this->DeclareAbstractInputPort( - "J_lcs, p_lcs", - drake::Value< - std::pair>>()) + "contact_descriptions", + drake::Value>()) .get_index(); this->set_name("c3_contact_force_publisher"); // Declare output port for publishing contact forces. contact_force_output_port_ = - this->DeclareAbstractOutputPort("lcmt_force", lcmt_forces(), + this->DeclareAbstractOutputPort("lcmt_force", lcmt_contact_forces(), &ContactForcePublisher::DoCalc) .get_index(); } // Calculates and outputs the contact forces based on the current context. void ContactForcePublisher::DoCalc(const Context& context, - lcmt_forces* output) const { + 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< - std::pair>>( - context, lcs_contact_info_port_); - // Contact Jacobian - MatrixXd J_c = contact_info->first; - int contact_force_start = solution->lambda_sol_.rows() - J_c.rows(); - bool using_stewart_and_trinkle_model = contact_force_start > 0; - - auto contact_points = contact_info->second; - int forces_per_contact = contact_info->first.rows() / contact_points.size(); - - output->num_forces = forces_per_contact * contact_points.size(); - output->forces.resize(output->num_forces); - - int contact_var_start; - int force_index; + 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 (int contact_index = 0; contact_index < (int)contact_points.size(); - ++contact_index) { - contact_var_start = - contact_force_start + forces_per_contact * contact_index; - force_index = forces_per_contact * contact_index; - for (int i = 0; i < forces_per_contact; ++i) { - int contact_jacobian_row = force_index + i; // index for anitescu model - int contact_var_index = contact_var_start + i; - if (using_stewart_and_trinkle_model) { // index for stweart and trinkle - // model - if (i == 0) { - contact_jacobian_row = contact_index; - contact_var_index = contact_force_start + contact_index; - } else { - contact_jacobian_row = contact_points.size() + - (forces_per_contact - 1) * contact_index + i - - 1; - contact_var_index = contact_force_start + contact_points.size() + - (forces_per_contact - 1) * contact_index + i - 1; - } - } - auto force = lcmt_contact_force(); - // Set contact point position. - force.contact_point[0] = contact_points.at(contact_index)[0]; - force.contact_point[1] = contact_points.at(contact_index)[1]; - force.contact_point[2] = contact_points.at(contact_index)[2]; - // TODO(yangwill): find a cleaner way to figure out the equivalent forces - // VISUALIZING FORCES FOR THE FIRST KNOT POINT - // 6, 7, 8 are the indices for the x,y,z components of the tray - // expressed in the world frame - // Compute force vector in world frame. - std::cout << J_c << std::endl; - exit(0); - force.contact_force[0] = solution->lambda_sol_(contact_var_index, 0) * - J_c.row(contact_jacobian_row)(6); - force.contact_force[1] = solution->lambda_sol_(contact_var_index, 0) * - J_c.row(contact_jacobian_row)(7); - force.contact_force[2] = solution->lambda_sol_(contact_var_index, 0) * - J_c.row(contact_jacobian_row)(8); - output->forces[force_index + i] = force; - } + for (int i = 0; i < contact_info->size()/4; ++i) { + auto force = lcmt_contact_force(); + + if (contact_info->at(i).is_slack) + // If the contact is slack, set the force to zero. + 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; } @@ -123,7 +99,7 @@ LcmPublisherSystem* ContactForcePublisher::AddLcmPublisherToBuilder( // Add and connect the LCM publisher system. auto lcm_force_publisher = - builder.AddSystem(LcmPublisherSystem::Make( + 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()); diff --git a/systems/publishers/force_publisher.h b/systems/publishers/force_publisher.h index f47ac44..8b4463a 100644 --- a/systems/publishers/force_publisher.h +++ b/systems/publishers/force_publisher.h @@ -8,7 +8,7 @@ #include #include -#include "c3/lcmt_forces.hpp" +#include "c3/lcmt_contact_forces.hpp" namespace c3 { namespace systems { @@ -95,7 +95,7 @@ class ContactForcePublisher : public drake::systems::LeafSystem { * @param c3_forces_output Pointer to the output message to populate. */ void DoCalc(const drake::systems::Context& context, - c3::lcmt_forces* c3_forces_output) const; + c3::lcmt_contact_forces* c3_forces_output) const; drake::systems::InputPortIndex c3_solution_port_; ///< Index of the c3 solution input port. From ce82fa5ffaf19df591f0d451c10aec85e544b192 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Thu, 3 Jul 2025 09:56:58 -0400 Subject: [PATCH 05/13] feat: add c3 trajectory generator --- lcmtypes/lcmt_c3_trajectories.lcm | 7 ++ lcmtypes/lcmt_trajectory.lcm | 11 +++ systems/BUILD.bazel | 12 +-- .../c3_output_generator.cc} | 16 ++-- .../c3_output_generator.h} | 10 +-- .../c3_trajectory_generator.cc | 65 ++++++++++++++ .../lcmt_generators/c3_trajectory_generator.h | 89 +++++++++++++++++++ .../c3_trajectory_generator_options.h | 33 +++++++ .../contact_force_generator.cc} | 16 ++-- .../contact_force_generator.h} | 12 +-- 10 files changed, 238 insertions(+), 33 deletions(-) create mode 100644 lcmtypes/lcmt_c3_trajectories.lcm create mode 100644 lcmtypes/lcmt_trajectory.lcm rename systems/{publishers/output_publisher.cc => lcmt_generators/c3_output_generator.cc} (86%) rename systems/{publishers/output_publisher.h => lcmt_generators/c3_output_generator.h} (94%) create mode 100644 systems/lcmt_generators/c3_trajectory_generator.cc create mode 100644 systems/lcmt_generators/c3_trajectory_generator.h create mode 100644 systems/lcmt_generators/c3_trajectory_generator_options.h rename systems/{publishers/force_publisher.cc => lcmt_generators/contact_force_generator.cc} (90%) rename systems/{publishers/force_publisher.h => lcmt_generators/contact_force_generator.h} (94%) diff --git a/lcmtypes/lcmt_c3_trajectories.lcm b/lcmtypes/lcmt_c3_trajectories.lcm new file mode 100644 index 0000000..4152490 --- /dev/null +++ b/lcmtypes/lcmt_c3_trajectories.lcm @@ -0,0 +1,7 @@ +package c3; + +struct lcmt_trajectories +{ + int32_t num_trajectories; + lcmt_trajectory trajectories[num_trajectories]; +} \ No newline at end of file diff --git a/lcmtypes/lcmt_trajectory.lcm b/lcmtypes/lcmt_trajectory.lcm new file mode 100644 index 0000000..a15377b --- /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; + double timestamps[num_timesteps]; + double values[vector_dim][num_timesteps]; +} diff --git a/systems/BUILD.bazel b/systems/BUILD.bazel index fab00bf..a778b9e 100644 --- a/systems/BUILD.bazel +++ b/systems/BUILD.bazel @@ -40,7 +40,7 @@ cc_library( name = "c3_controller", deps = [ ":options", ":framework", - ":publishers", + ":lcmt_generators", "//core:c3", "//core:options", "//multibody:lcs_factory", @@ -90,14 +90,14 @@ cc_library( ) cc_library( - name = "publishers", + name = "lcmt_generators", srcs = [ - "publishers/output_publisher.cc", - "publishers/force_publisher.cc" + "lcmt_generators/output_publisher.cc", + "lcmt_generators/force_publisher.cc" ], hdrs = [ - "publishers/output_publisher.h", - "publishers/force_publisher.h" + "lcmt_generators/output_publisher.h", + "lcmt_generators/force_publisher.h" ], deps = [ ":c3_output", diff --git a/systems/publishers/output_publisher.cc b/systems/lcmt_generators/c3_output_generator.cc similarity index 86% rename from systems/publishers/output_publisher.cc rename to systems/lcmt_generators/c3_output_generator.cc index c18fd96..8bbe5be 100644 --- a/systems/publishers/output_publisher.cc +++ b/systems/lcmt_generators/c3_output_generator.cc @@ -8,10 +8,10 @@ using drake::systems::lcm::LcmPublisherSystem; namespace c3 { namespace systems { -namespace publishers { +namespace lcmt_generators { // Publishes C3Output as an LCM message. -C3OutputPublisher::C3OutputPublisher() { +C3OutputGenerator::C3OutputGenerator() { // Declare input port for the C3 solution. c3_solution_port_ = this->DeclareAbstractInputPort("c3_solution", @@ -27,12 +27,12 @@ C3OutputPublisher::C3OutputPublisher() { // Declare output port for the LCM message. c3_output_port_ = this->DeclareAbstractOutputPort("lcmt_c3_output", c3::lcmt_output(), - &C3OutputPublisher::DoCalc) + &C3OutputGenerator::DoCalc) .get_index(); } // Calculates the LCM output message from the input ports. -void C3OutputPublisher::DoCalc(const drake::systems::Context& context, +void C3OutputGenerator::DoCalc(const drake::systems::Context& context, c3::lcmt_output* output) const { // Retrieve input values. const auto& c3_solution = @@ -47,15 +47,15 @@ void C3OutputPublisher::DoCalc(const drake::systems::Context& context, } // Adds this publisher and an LCM publisher system to the diagram builder. -LcmPublisherSystem* C3OutputPublisher::AddLcmPublisherToBuilder( +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 C3OutputPublisher system. - auto output_publisher = builder.AddSystem(); + // 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, @@ -70,6 +70,6 @@ LcmPublisherSystem* C3OutputPublisher::AddLcmPublisherToBuilder( return lcm_output_publisher; } -} // namespace publishers +} // namespace lcmt_generators } // namespace systems } // namespace c3 \ No newline at end of file diff --git a/systems/publishers/output_publisher.h b/systems/lcmt_generators/c3_output_generator.h similarity index 94% rename from systems/publishers/output_publisher.h rename to systems/lcmt_generators/c3_output_generator.h index afc78a5..0b7d8fa 100644 --- a/systems/publishers/output_publisher.h +++ b/systems/lcmt_generators/c3_output_generator.h @@ -12,22 +12,22 @@ namespace c3 { namespace systems { -namespace publishers { +namespace lcmt_generators { /** - * @class C3OutputPublisher + * @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 C3OutputPublisher : public drake::systems::LeafSystem { +class C3OutputGenerator : public drake::systems::LeafSystem { public: /** * @brief Constructor. Declares input and output ports. */ - C3OutputPublisher(); + C3OutputGenerator(); /** * @brief Returns the input port for the C3 solution vector. @@ -88,6 +88,6 @@ class C3OutputPublisher : public drake::systems::LeafSystem { c3_output_port_; /**< Index for output port. */ }; -} // namespace publishers +} // 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..f858536 --- /dev/null +++ b/systems/lcmt_generators/c3_trajectory_generator.cc @@ -0,0 +1,65 @@ +#include "trajectory_generator.h" + +namespace c3 { +namespace systems { +namespace lcmt_generators { + +TrajectoryGenerator::TrajectoryGenerator(TrajectoryGeneratorOptions options) + : options_(options) { + // Declare input ports for solution and intermediates. + c3_solution_input_port_ = this->DeclareAbstractInputPort( + "c3_solution", drake::Value()).get_index(); + c3_intermediates_input_port_ = this->DeclareAbstractInputPort( + "c3_intermediates", drake::Value()).get_index(); + + // Declare output port for lcmt_trajectory message. + lcmt_trajectory_output_port_ = this->DeclareAbstractOutputPort( + "lcmt_trajectory", &TrajectoryGenerator::GenerateTrajectory).get_index(); + + this->set_name("c3_trajectory_generator"); +} + +void TrajectoryGenerator::GenerateTrajectory( + const drake::systems::Context& context, + lcmt_trajectory* output) const { + // Get input data + const auto* solution = this->EvalInputValue( + context, c3_solution_input_port_); + const auto* intermediates = this->EvalInputValue( + context, c3_intermediates_input_port_); + + // Example: Fill in the output message with dummy data for demonstration. + // In practice, use options_ and input data to populate the message. + output->trajectory_name = "example_trajectory"; + output->num_timesteps = 10; + output->vector_dim = 2; + output->timestamps.resize(10); + output->values.resize(2, std::vector(10, 0.0)); + for (int i = 0; i < 10; ++i) { + output->timestamps[i] = 0.1 * i; + output->values[0][i] = i; + output->values[1][i] = 2 * i; + } +} + +drake::systems::lcm::LcmPublisherSystem* TrajectoryGenerator::AddLcmPublisherToBuilder( + drake::systems::DiagramBuilder& builder, + const drake::systems::OutputPort& c3_solution_port, + const drake::systems::OutputPort& c3_intermediates_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(TrajectoryGeneratorOptions{}); + builder.Connect(c3_solution_port, traj_gen->get_input_port_c3_solution()); + builder.Connect(c3_intermediates_port, traj_gen->get_input_port_c3_intermediates()); + + 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_trajectory(), lcm_pub->get_input_port()); + return lcm_pub; +} + +} // namespace lcmt_generators +} // namespace systems +} // \ 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..32a7a85 --- /dev/null +++ b/systems/lcmt_generators/c3_trajectory_generator.h @@ -0,0 +1,89 @@ +#include +#include +#include +#include + +#include "c3/lcmt_trajectories.hpp" +#include "c3/lcmt_trajectory.hpp" +#include "trajectory_generator_options.h" + +namespace c3 { +namespace systems { +namespace lcmt_generators { + +/** + * @class TrajectoryGenerator + * @brief Drake LeafSystem for generating lcmt_trajectory messages from input + * data. + * + * The TrajectoryGenerator 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 TrajectoryGenerator : public drake::systems::LeafSystem { + public: + /** + * @brief Constructor. Declares input and output ports. + * @param options Configuration options for the trajectory generator. + */ + TrajectoryGenerator(TrajectoryGeneratorOptions options); + + /** + * @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_intermediates_input_port_); + } + + /** + * @brief Returns the input port for the trajectory intermediates data. + * @return Reference to the input port for c3 intermediates data. + */ + const drake::systems::InputPort& get_input_port_c3_intermediates() + 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_trajectory() + const { + return this->get_output_port(lcmt_trajectory_output_port_); + } + + static drake::systems::lcm::LcmPublisherSystem* AddLcmPublisherToBuilder( + drake::systems::DiagramBuilder& builder, + const drake::systems::OutputPort& c3_solution_port, + const drake::systems::OutputPort& c3_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: + // Input port index for the trajectory solution data. + drake::systems::InputPortIndex c3_solution_input_port_; + + // Input port index for the trajectory intermediates data. + drake::systems::InputPortIndex c3_intermediates_input_port_; + + // Output port index for the generated lcmt_trajectory message. + drake::systems::OutputPortIndex lcmt_trajectory_output_port_; + + // Options for configuring the trajectory generator. + TrajectoryGeneratorOptions options_; + + /** + * @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_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_options.h b/systems/lcmt_generators/c3_trajectory_generator_options.h new file mode 100644 index 0000000..ccbc233 --- /dev/null +++ b/systems/lcmt_generators/c3_trajectory_generator_options.h @@ -0,0 +1,33 @@ +#pragma once + +#include +#include + +namespace c3 { +namespace systems { +namespace lcmt_generators { + +struct TrajectoryDescription { + std::string trajectory_name; // Name of the trajectory + std::string variable_type; // Type of C3 variable (e.g., "x", "u", "lambda") + std::vector> indices; + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(trajectory_name)); + a->Visit(DRAKE_NVP(variable_type)); + a->Visit(DRAKE_NVP(indices)); + } +}; + +struct TrajectoryGeneratorOptions { + std::vector + trajectories; // List of trajectories to generate + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(trajectories)); + } +}; + +} // namespace lcmt_generators +} // namespace systems +} // namespace c3 \ No newline at end of file diff --git a/systems/publishers/force_publisher.cc b/systems/lcmt_generators/contact_force_generator.cc similarity index 90% rename from systems/publishers/force_publisher.cc rename to systems/lcmt_generators/contact_force_generator.cc index 3f9412c..a653a50 100644 --- a/systems/publishers/force_publisher.cc +++ b/systems/lcmt_generators/contact_force_generator.cc @@ -19,10 +19,10 @@ namespace c3 { using multibody::LCSContactDescription; namespace systems { -namespace publishers { +namespace lcmt_generators { // Publishes contact force information to LCM for visualization and logging. -ContactForcePublisher::ContactForcePublisher() { +ContactForceGenerator::ContactForceGenerator() { // Declare input port for the C3 solution. c3_solution_port_ = this->DeclareAbstractInputPort( "solution", drake::Value{}) @@ -38,12 +38,12 @@ ContactForcePublisher::ContactForcePublisher() { // Declare output port for publishing contact forces. contact_force_output_port_ = this->DeclareAbstractOutputPort("lcmt_force", lcmt_contact_forces(), - &ContactForcePublisher::DoCalc) + &ContactForceGenerator::DoCalc) .get_index(); } // Calculates and outputs the contact forces based on the current context. -void ContactForcePublisher::DoCalc(const Context& context, +void ContactForceGenerator::DoCalc(const Context& context, lcmt_contact_forces* output) const { // Get Solution from C3 const auto& solution = @@ -84,15 +84,15 @@ void ContactForcePublisher::DoCalc(const Context& context, } // Adds this publisher and an LCM publisher system to the diagram builder. -LcmPublisherSystem* ContactForcePublisher::AddLcmPublisherToBuilder( +LcmPublisherSystem* ContactForceGenerator::AddLcmPublisherToBuilder( DiagramBuilder& builder, const drake::systems::OutputPort& solution_port, const drake::systems::OutputPort& lcs_contact_info_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 ContactForcePublisher system. - auto force_publisher = builder.AddSystem(); + // 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_info_port, force_publisher->get_input_port_lcs_contact_info()); @@ -106,6 +106,6 @@ LcmPublisherSystem* ContactForcePublisher::AddLcmPublisherToBuilder( return lcm_force_publisher; } -} // namespace publishers +} // namespace lcmt_generators } // namespace systems } // namespace c3 \ No newline at end of file diff --git a/systems/publishers/force_publisher.h b/systems/lcmt_generators/contact_force_generator.h similarity index 94% rename from systems/publishers/force_publisher.h rename to systems/lcmt_generators/contact_force_generator.h index 8b4463a..ea50392 100644 --- a/systems/publishers/force_publisher.h +++ b/systems/lcmt_generators/contact_force_generator.h @@ -12,10 +12,10 @@ namespace c3 { namespace systems { -namespace publishers { +namespace lcmt_generators { /** - * @class ContactForcePublisher + * @class ContactForceGenerator * @brief Converts solution vectors and LCS contact information into LCM contact * force messages for publishing. * @details @@ -26,15 +26,15 @@ namespace publishers { * - Includes a static helper to add an LCM publisher system to a * DiagramBuilder for message transmission. */ -class ContactForcePublisher : public drake::systems::LeafSystem { +class ContactForceGenerator : public drake::systems::LeafSystem { public: /** - * @brief Constructs a ContactForcePublisher system. + * @brief Constructs a ContactForceGenerator system. * * Declares input and output ports for the solution vector, LCS contact info, * and contact force output. */ - ContactForcePublisher(); + ContactForceGenerator(); /** * @brief Returns the input port for the c3 solution vector. @@ -105,6 +105,6 @@ class ContactForcePublisher : public drake::systems::LeafSystem { contact_force_output_port_; ///< Index of the contact force output port. }; -} // namespace publishers +} // namespace lcmt_generators } // namespace systems } // namespace c3 \ No newline at end of file From fc01a53e3d1db1f73fafa936ffdb444b547bb47a Mon Sep 17 00:00:00 2001 From: Meow404 Date: Sat, 5 Jul 2025 16:42:44 -0400 Subject: [PATCH 06/13] feat: add trajectory generator to C3 --- ...rajectories.lcm => lcmt_c3_trajectory.lcm} | 3 +- lcmtypes/lcmt_trajectory.lcm | 4 +- systems/BUILD.bazel | 11 +- systems/framework/c3_output.h | 16 +++ .../lcmt_generators/c3_output_generator.cc | 2 +- .../c3_trajectory_generator.cc | 122 +++++++++++++----- .../lcmt_generators/c3_trajectory_generator.h | 38 ++---- ...ons.h => c3_trajectory_generator_config.h} | 19 ++- .../contact_force_generator.cc | 6 +- 9 files changed, 148 insertions(+), 73 deletions(-) rename lcmtypes/{lcmt_c3_trajectories.lcm => lcmt_c3_trajectory.lcm} (68%) rename systems/lcmt_generators/{c3_trajectory_generator_options.h => c3_trajectory_generator_config.h} (55%) diff --git a/lcmtypes/lcmt_c3_trajectories.lcm b/lcmtypes/lcmt_c3_trajectory.lcm similarity index 68% rename from lcmtypes/lcmt_c3_trajectories.lcm rename to lcmtypes/lcmt_c3_trajectory.lcm index 4152490..320e0a4 100644 --- a/lcmtypes/lcmt_c3_trajectories.lcm +++ b/lcmtypes/lcmt_c3_trajectory.lcm @@ -1,7 +1,8 @@ package c3; -struct lcmt_trajectories +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_trajectory.lcm b/lcmtypes/lcmt_trajectory.lcm index a15377b..225b72c 100644 --- a/lcmtypes/lcmt_trajectory.lcm +++ b/lcmtypes/lcmt_trajectory.lcm @@ -6,6 +6,6 @@ struct lcmt_trajectory int32_t num_timesteps; int32_t vector_dim; - double timestamps[num_timesteps]; - double values[vector_dim][num_timesteps]; + float timestamps[num_timesteps]; + float values[vector_dim][num_timesteps]; } diff --git a/systems/BUILD.bazel b/systems/BUILD.bazel index a778b9e..384406d 100644 --- a/systems/BUILD.bazel +++ b/systems/BUILD.bazel @@ -92,12 +92,15 @@ cc_library( cc_library( name = "lcmt_generators", srcs = [ - "lcmt_generators/output_publisher.cc", - "lcmt_generators/force_publisher.cc" + "lcmt_generators/c3_output_generator.cc", + "lcmt_generators/contact_force_generator.cc", + "lcmt_generators/c3_trajectory_generator.cc", ], hdrs = [ - "lcmt_generators/output_publisher.h", - "lcmt_generators/force_publisher.h" + "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 = [ ":c3_output", diff --git a/systems/framework/c3_output.h b/systems/framework/c3_output.h index 9ac77df..08ef4e5 100644 --- a/systems/framework/c3_output.h +++ b/systems/framework/c3_output.h @@ -29,6 +29,22 @@ 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_; diff --git a/systems/lcmt_generators/c3_output_generator.cc b/systems/lcmt_generators/c3_output_generator.cc index 8bbe5be..ebc5e39 100644 --- a/systems/lcmt_generators/c3_output_generator.cc +++ b/systems/lcmt_generators/c3_output_generator.cc @@ -1,4 +1,4 @@ -#include "output_publisher.h" +#include "systems/lcmt_generators/c3_output_generator.h" #include "systems/framework/c3_output.h" diff --git a/systems/lcmt_generators/c3_trajectory_generator.cc b/systems/lcmt_generators/c3_trajectory_generator.cc index f858536..f5fa25b 100644 --- a/systems/lcmt_generators/c3_trajectory_generator.cc +++ b/systems/lcmt_generators/c3_trajectory_generator.cc @@ -1,65 +1,119 @@ -#include "trajectory_generator.h" +#include "systems/lcmt_generators/c3_trajectory_generator.h" + +#include "systems/framework/c3_output.h" namespace c3 { namespace systems { namespace lcmt_generators { -TrajectoryGenerator::TrajectoryGenerator(TrajectoryGeneratorOptions options) - : options_(options) { +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(); - c3_intermediates_input_port_ = this->DeclareAbstractInputPort( - "c3_intermediates", drake::Value()).get_index(); + c3_solution_input_port_ = + this->DeclareAbstractInputPort("c3_solution", + drake::Value()) + .get_index(); // Declare output port for lcmt_trajectory message. - lcmt_trajectory_output_port_ = this->DeclareAbstractOutputPort( - "lcmt_trajectory", &TrajectoryGenerator::GenerateTrajectory).get_index(); + lcmt_c3_trajectory_output_port_ = + this->DeclareAbstractOutputPort( + "lcmt_c3_trajectory", &C3TrajectoryGenerator::GenerateTrajectory) + .get_index(); - this->set_name("c3_trajectory_generator"); + // this->set_name("c3_trajectory_generator"); } -void TrajectoryGenerator::GenerateTrajectory( +void C3TrajectoryGenerator::GenerateTrajectory( const drake::systems::Context& context, - lcmt_trajectory* output) const { - // Get input data + lcmt_c3_trajectory* output) const { + // Get input data from the input port const auto* solution = this->EvalInputValue( context, c3_solution_input_port_); - const auto* intermediates = this->EvalInputValue( - context, c3_intermediates_input_port_); - - // Example: Fill in the output message with dummy data for demonstration. - // In practice, use options_ and input data to populate the message. - output->trajectory_name = "example_trajectory"; - output->num_timesteps = 10; - output->vector_dim = 2; - output->timestamps.resize(10); - output->values.resize(2, std::vector(10, 0.0)); - for (int i = 0; i < 10; ++i) { - output->timestamps[i] = 0.1 * i; - output->values[0][i] = i; - output->values[1][i] = 2 * i; + + 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 = 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* TrajectoryGenerator::AddLcmPublisherToBuilder( +drake::systems::lcm::LcmPublisherSystem* +C3TrajectoryGenerator::AddLcmPublisherToBuilder( drake::systems::DiagramBuilder& builder, + const C3TrajectoryGeneratorConfig config, const drake::systems::OutputPort& c3_solution_port, - const drake::systems::OutputPort& c3_intermediates_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(TrajectoryGeneratorOptions{}); + auto traj_gen = builder.AddSystem(config); builder.Connect(c3_solution_port, traj_gen->get_input_port_c3_solution()); - builder.Connect(c3_intermediates_port, traj_gen->get_input_port_c3_intermediates()); auto lcm_pub = builder.AddSystem( - drake::systems::lcm::LcmPublisherSystem::Make( + drake::systems::lcm::LcmPublisherSystem::Make( channel, lcm, publish_triggers, publish_period, publish_offset)); - builder.Connect(traj_gen->get_output_port_lcmt_trajectory(), lcm_pub->get_input_port()); + builder.Connect(traj_gen->get_output_port_lcmt_c3_trajectory(), + lcm_pub->get_input_port()); return lcm_pub; } } // namespace lcmt_generators } // namespace systems -} // \ No newline at end of file +} // 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 index 32a7a85..c783c4d 100644 --- a/systems/lcmt_generators/c3_trajectory_generator.h +++ b/systems/lcmt_generators/c3_trajectory_generator.h @@ -3,45 +3,36 @@ #include #include -#include "c3/lcmt_trajectories.hpp" +#include "c3/lcmt_c3_trajectory.hpp" #include "c3/lcmt_trajectory.hpp" -#include "trajectory_generator_options.h" +#include "systems/lcmt_generators/c3_trajectory_generator_config.h" namespace c3 { namespace systems { namespace lcmt_generators { /** - * @class TrajectoryGenerator + * @class C3TrajectoryGenerator * @brief Drake LeafSystem for generating lcmt_trajectory messages from input * data. * - * The TrajectoryGenerator system takes trajectory solution and intermediate + * 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 TrajectoryGenerator : public drake::systems::LeafSystem { +class C3TrajectoryGenerator : public drake::systems::LeafSystem { public: /** * @brief Constructor. Declares input and output ports. - * @param options Configuration options for the trajectory generator. + * @param config Configuration options for the trajectory generator. */ - TrajectoryGenerator(TrajectoryGeneratorOptions options); + 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_intermediates_input_port_); - } - - /** - * @brief Returns the input port for the trajectory intermediates data. - * @return Reference to the input port for c3 intermediates data. - */ - const drake::systems::InputPort& get_input_port_c3_intermediates() - const { return this->get_input_port(c3_solution_input_port_); } @@ -49,15 +40,15 @@ class TrajectoryGenerator : public drake::systems::LeafSystem { * @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_trajectory() + const drake::systems::OutputPort& get_output_port_lcmt_c3_trajectory() const { - return this->get_output_port(lcmt_trajectory_output_port_); + 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 drake::systems::OutputPort& c3_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); @@ -66,14 +57,11 @@ class TrajectoryGenerator : public drake::systems::LeafSystem { // Input port index for the trajectory solution data. drake::systems::InputPortIndex c3_solution_input_port_; - // Input port index for the trajectory intermediates data. - drake::systems::InputPortIndex c3_intermediates_input_port_; - // Output port index for the generated lcmt_trajectory message. - drake::systems::OutputPortIndex lcmt_trajectory_output_port_; + drake::systems::OutputPortIndex lcmt_c3_trajectory_output_port_; // Options for configuring the trajectory generator. - TrajectoryGeneratorOptions options_; + C3TrajectoryGeneratorConfig config_; /** * @brief Generates the lcmt_trajectory message from input data. @@ -81,7 +69,7 @@ class TrajectoryGenerator : public drake::systems::LeafSystem { * @param output Pointer to the lcmt_trajectory message to populate. */ void GenerateTrajectory(const drake::systems::Context& context, - lcmt_trajectory* output) const; + lcmt_c3_trajectory* output) const; }; } // namespace lcmt_generators diff --git a/systems/lcmt_generators/c3_trajectory_generator_options.h b/systems/lcmt_generators/c3_trajectory_generator_config.h similarity index 55% rename from systems/lcmt_generators/c3_trajectory_generator_options.h rename to systems/lcmt_generators/c3_trajectory_generator_config.h index ccbc233..a2f2243 100644 --- a/systems/lcmt_generators/c3_trajectory_generator_options.h +++ b/systems/lcmt_generators/c3_trajectory_generator_config.h @@ -8,18 +8,31 @@ namespace systems { namespace lcmt_generators { struct TrajectoryDescription { + struct index_range { + int start; // Start index of the range + int end; // End index of the range + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(start)); + a->Visit(DRAKE_NVP(end)); + } + }; std::string trajectory_name; // Name of the trajectory - std::string variable_type; // Type of C3 variable (e.g., "x", "u", "lambda") - std::vector> indices; + std::string variable_type; // Type of C3 variable ["state", "input", "force"] + std::vector indices; template void Serialize(Archive* a) { a->Visit(DRAKE_NVP(trajectory_name)); a->Visit(DRAKE_NVP(variable_type)); + + DRAKE_ASSERT(variable_type == "state" || variable_type == "input" || + variable_type == "force"); + a->Visit(DRAKE_NVP(indices)); } }; -struct TrajectoryGeneratorOptions { +struct C3TrajectoryGeneratorConfig { std::vector trajectories; // List of trajectories to generate template diff --git a/systems/lcmt_generators/contact_force_generator.cc b/systems/lcmt_generators/contact_force_generator.cc index a653a50..832c885 100644 --- a/systems/lcmt_generators/contact_force_generator.cc +++ b/systems/lcmt_generators/contact_force_generator.cc @@ -1,4 +1,4 @@ -#include "force_publisher.h" +#include "systems/lcmt_generators/contact_force_generator.h" #include @@ -34,7 +34,7 @@ ContactForceGenerator::ContactForceGenerator() { drake::Value>()) .get_index(); - this->set_name("c3_contact_force_publisher"); + this->set_name("c3_contact_force_generator"); // Declare output port for publishing contact forces. contact_force_output_port_ = this->DeclareAbstractOutputPort("lcmt_force", lcmt_contact_forces(), @@ -56,7 +56,7 @@ void ContactForceGenerator::DoCalc(const Context& context, output->forces.clear(); output->num_forces = 0; // Iterate over all contact points and compute forces. - for (int i = 0; i < contact_info->size()/4; ++i) { + for (int i = 0; i < contact_info->size(); ++i) { auto force = lcmt_contact_force(); if (contact_info->at(i).is_slack) From d20a20111091e35110af2131ac6fb6825bb3abc3 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 8 Jul 2025 17:22:24 +0000 Subject: [PATCH 07/13] fix: add shared library --- BUILD.bazel | 2 +- bindings/pyc3/c3_multibody_py.cc | 18 +++- bindings/pyc3/c3_systems_py.cc | 91 ++++++++++++++++++- systems/BUILD.bazel | 52 ++++++++--- .../c3_trajectory_generator.cc | 2 +- .../contact_force_generator.cc | 2 +- 6 files changed, 144 insertions(+), 23 deletions(-) diff --git a/BUILD.bazel b/BUILD.bazel index a791480..2f9bfc6 100644 --- a/BUILD.bazel +++ b/BUILD.bazel @@ -25,4 +25,4 @@ cc_library( ], include_prefix = "c3", visibility = ["//visibility:public"], -) \ No newline at end of file +) diff --git a/bindings/pyc3/c3_multibody_py.cc b/bindings/pyc3/c3_multibody_py.cc index b9145f5..541bfbc 100644 --- a/bindings/pyc3/c3_multibody_py.cc +++ b/bindings/pyc3/c3_multibody_py.cc @@ -26,6 +26,20 @@ PYBIND11_MODULE(multibody, m) { c3::multibody::ContactModel::kFrictionlessSpring) .export_values(); + py::class_(m, "LCSContactDescription") + .def(py::init<>()) + .def_readwrite("witness_point_A", + &c3::multibody::LCSContactDescription::witness_point_A) + .def_readwrite("witness_point_B", + &c3::multibody::LCSContactDescription::witness_point_B) + .def_readwrite("force_basis", + &c3::multibody::LCSContactDescription::force_basis) + .def_readwrite("is_slack", + &c3::multibody::LCSContactDescription::is_slack) + .def_static("CreateSlackVariableDescription", + &c3::multibody::LCSContactDescription:: + CreateSlackVariableDescription); + py::class_(m, "LCSFactory") .def(py::init&, drake::systems::Context&, @@ -37,8 +51,8 @@ PYBIND11_MODULE(multibody, m) { py::arg("plant"), py::arg("context"), py::arg("plant_ad"), py::arg("context_ad"), py::arg("contact_geoms"), py::arg("options")) .def("GenerateLCS", &c3::multibody::LCSFactory::GenerateLCS) - // .def("GetWitnessPointsAndForceBasisInWorldFrame", - // &c3::multibody::LCSFactory::GetWitnessPointsAndForceBasisInWorldFrame) + .def("GetContactDescriptions", + &c3::multibody::LCSFactory::GetContactDescriptions) .def("UpdateStateAndInput", &c3::multibody::LCSFactory::UpdateStateAndInput, py::arg("state"), py::arg("input")) diff --git a/bindings/pyc3/c3_systems_py.cc b/bindings/pyc3/c3_systems_py.cc index 67fa389..5220770 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" @@ -99,10 +102,10 @@ PYBIND11_MODULE(systems, m) { &LCSFactorySystem::get_input_port_lcs_input, 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_description", + &LCSFactorySystem::get_output_port_lcs_contact_description, py::return_value_policy::reference); - // .def("get_output_port_lcs_contact_points_and_force_basis", - // &LCSFactorySystem::get_output_port_lcs_contact_points_and_force_basis, - // py::return_value_policy::reference); py::class_(m, "C3Solution") .def(py::init<>()) @@ -178,6 +181,88 @@ PYBIND11_MODULE(systems, m) { &C3ControllerOptions::state_prediction_joints); m.def("LoadC3ControllerOptions", &LoadC3ControllerOptions); + + // C3OutputGenerator + py::class_>(m, "C3OutputGenerator") + .def(py::init<>()) + .def("get_input_port_c3_solution", + &lcmt_generators::C3OutputGenerator::get_input_port_c3_solution, + py::return_value_policy::reference) + .def("get_input_port_c3_intermediates", + &lcmt_generators::C3OutputGenerator::get_input_port_c3_intermediates, + py::return_value_policy::reference) + .def("get_output_port_c3_output", + &lcmt_generators::C3OutputGenerator::get_output_port_c3_output, + py::return_value_policy::reference) + .def_static("AddLcmPublisherToBuilder", + &lcmt_generators::C3OutputGenerator::AddLcmPublisherToBuilder, + py::arg("builder"), py::arg("solution_port"), + py::arg("intermediates_port"), py::arg("channel"), + py::arg("lcm"), py::arg("publish_triggers"), + py::arg("publish_period"), py::arg("publish_offset")); + + // C3TrajectoryGenerator + py::class_( + m, "TrajectoryDescriptionIndexRange") + .def(py::init<>()) + .def_readwrite( + "start", &lcmt_generators::TrajectoryDescription::index_range::start) + .def_readwrite("end", + &lcmt_generators::TrajectoryDescription::index_range::end); + py::class_(m, "TrajectoryDescription") + .def(py::init<>()) + .def_readwrite("trajectory_name", + &lcmt_generators::TrajectoryDescription::trajectory_name) + .def_readwrite("variable_type", + &lcmt_generators::TrajectoryDescription::variable_type) + .def_readwrite("indices", + &lcmt_generators::TrajectoryDescription::indices); + py::class_( + m, "C3TrajectoryGeneratorConfig") + .def(py::init<>()) + .def_readwrite( + "trajectories", + &lcmt_generators::C3TrajectoryGeneratorConfig::trajectories); + py::class_>(m, "C3TrajectoryGenerator") + .def(py::init()) + .def("get_input_port_c3_solution", + &lcmt_generators::C3TrajectoryGenerator::get_input_port_c3_solution, + py::return_value_policy::reference) + .def("get_output_port_lcmt_c3_trajectory", + &lcmt_generators::C3TrajectoryGenerator:: + get_output_port_lcmt_c3_trajectory, + py::return_value_policy::reference) + .def_static( + "AddLcmPublisherToBuilder", + &lcmt_generators::C3TrajectoryGenerator::AddLcmPublisherToBuilder, + py::arg("builder"), py::arg("config"), py::arg("solution_port"), + py::arg("channel"), py::arg("lcm"), py::arg("publish_triggers"), + py::arg("publish_period"), py::arg("publish_offset")); + + // ContactForceGenerator + py::class_>(m, "ContactForceGenerator") + .def(py::init<>()) + .def("get_input_port_c3_solution", + &lcmt_generators::ContactForceGenerator::get_input_port_c3_solution, + py::return_value_policy::reference) + .def("get_input_port_lcs_contact_info", + &lcmt_generators::ContactForceGenerator:: + get_input_port_lcs_contact_info, + 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/systems/BUILD.bazel b/systems/BUILD.bazel index 384406d..2b36c81 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", ], ) @@ -40,7 +42,6 @@ cc_library( name = "c3_controller", deps = [ ":options", ":framework", - ":lcmt_generators", "//core:c3", "//core:options", "//multibody:lcs_factory", @@ -78,6 +79,36 @@ cc_library( ], ) +cc_library( name = "lcs_simulator", + srcs = [ + "lcs_simulator.cc", + ], + hdrs = [ + "lcs_simulator.h", + ], + deps = [ + "//core:lcs", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "lcs_factory_system", + srcs = [ + "lcs_factory_system.cc", + ], + hdrs = [ + "lcs_factory_system.h", + ], + deps = [ + ":framework", + "//core:lcs", + "//core:options", + "//multibody:lcs_factory", + "@drake//:drake_shared_library", + ], +) + cc_library( name = "options", hdrs = [ @@ -103,26 +134,13 @@ cc_library( "lcmt_generators/c3_trajectory_generator_config.h", ], deps = [ - ":c3_output", + ":framework", "//multibody:lcs_factory", "//lcmtypes:lcmt_c3", "@drake//:drake_shared_library", ], ) -cc_library( - name = "c3_output", - srcs = [ - "framework/c3_output.cc", - ], - hdrs = [ - "framework/c3_output.h", - ], - deps = [ - "//lcmtypes:lcmt_c3", - "@drake//:drake_shared_library", - ] -) @@ -172,5 +190,9 @@ filegroup( "*.h", "**/*.h", ]), +) + +exports_files( + glob(["**/*.h", "**/*.hpp"]), visibility = ["//visibility:public"], ) diff --git a/systems/lcmt_generators/c3_trajectory_generator.cc b/systems/lcmt_generators/c3_trajectory_generator.cc index f5fa25b..af4e4a2 100644 --- a/systems/lcmt_generators/c3_trajectory_generator.cc +++ b/systems/lcmt_generators/c3_trajectory_generator.cc @@ -65,7 +65,7 @@ void C3TrajectoryGenerator::GenerateTrajectory( auto indices = traj_desc.indices; if (indices.empty()) { TrajectoryDescription::index_range default_indices = { - .start = 0, .end = solution_values.rows() - 1}; + .start = 0, .end = (int)solution_values.rows() - 1}; indices.push_back(default_indices); } diff --git a/systems/lcmt_generators/contact_force_generator.cc b/systems/lcmt_generators/contact_force_generator.cc index 832c885..d38c62c 100644 --- a/systems/lcmt_generators/contact_force_generator.cc +++ b/systems/lcmt_generators/contact_force_generator.cc @@ -56,7 +56,7 @@ void ContactForceGenerator::DoCalc(const Context& context, output->forces.clear(); output->num_forces = 0; // Iterate over all contact points and compute forces. - for (int i = 0; i < contact_info->size(); ++i) { + for (size_t i = 0; i < contact_info->size(); ++i) { auto force = lcmt_contact_force(); if (contact_info->at(i).is_slack) From 3c9fafc81aa8e7658f3f33cc72735346c4fb2c2f Mon Sep 17 00:00:00 2001 From: Meow404 Date: Wed, 9 Jul 2025 10:18:08 -0400 Subject: [PATCH 08/13] fix: load default solver options --- core/c3.cc | 1 - core/configs/solve_options_default.hpp | 11 ----------- 2 files changed, 12 deletions(-) delete mode 100644 core/configs/solve_options_default.hpp diff --git a/core/c3.cc b/core/c3.cc index f7848d9..7693c29 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -9,7 +9,6 @@ #include "lcs.h" #include "solver_options_io.h" -#include "configs/solve_options_default.hpp" #include "drake/common/text_logging.h" #include "drake/solvers/mathematical_program.h" diff --git a/core/configs/solve_options_default.hpp b/core/configs/solve_options_default.hpp deleted file mode 100644 index 1890e19..0000000 --- a/core/configs/solve_options_default.hpp +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once -#include -const std::string default_solver_options = - "print_to_console: 0\nlog_file_name: \"\"\nint_options:\n max_iter: " - "1000\n # linsys_solver: 0\n verbose: 0\n warm_start: 1\n polish: 1\n " - "scaled_termination: 1\n check_termination: 25\n polish_refine_iter: 3\n " - " scaling: 10\n adaptive_rho: 1\n adaptive_rho_interval: " - "0\n\ndouble_options:\n rho: 0.0001\n sigma: 1e-6\n eps_abs: 1e-5\n " - "eps_rel: 1e-5\n eps_prim_inf: 1e-5\n eps_dual_inf: 1e-5\n alpha: 1.6\n " - " delta: 1e-6\n adaptive_rho_tolerance: 5\n adaptive_rho_fraction: 0.4\n " - " time_limit: 1.0\n\nstring_options: {}"; From 67217be3a4c50b27bb5d1ae555ba9b55ae88b96a Mon Sep 17 00:00:00 2001 From: Meow404 Date: Sat, 12 Jul 2025 16:22:50 +0000 Subject: [PATCH 09/13] fix: dairlib dependency fix --- BUILD.bazel | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/BUILD.bazel b/BUILD.bazel index 2f9bfc6..eee0451 100644 --- a/BUILD.bazel +++ b/BUILD.bazel @@ -21,8 +21,9 @@ 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", visibility = ["//visibility:public"], -) +) \ No newline at end of file From cf91c2fc17a583f29c877f2fa16f8a957c7b8b94 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 22 Jul 2025 09:59:53 -0400 Subject: [PATCH 10/13] fix: correct force directions --- multibody/geom_geom_collider.cc | 4 ++-- multibody/lcs_factory.cc | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/multibody/geom_geom_collider.cc b/multibody/geom_geom_collider.cc index 40e0e99..b92e098 100644 --- a/multibody/geom_geom_collider.cc +++ b/multibody/geom_geom_collider.cc @@ -280,12 +280,12 @@ GeomGeomCollider::CalcForceBasisInWorldFrame( const auto query_result = GetGeometryQueryResult(context); if (num_friction_directions < 1) { // Planar contact: basis is constructed from the contact and planar normals. - return ComputePlanarForceBasis(query_result.signed_distance_pair.nhat_BA_W, + return ComputePlanarForceBasis(-query_result.signed_distance_pair.nhat_BA_W, planar_normal); } else { // 3D contact: build polytope basis and rotate using contact normal. auto R_WC = drake::math::RotationMatrix::MakeFromOneVector( - query_result.signed_distance_pair.nhat_BA_W, 0); + -query_result.signed_distance_pair.nhat_BA_W, 0); return ComputePolytopeForceBasis(num_friction_directions) * R_WC.matrix().transpose(); } diff --git a/multibody/lcs_factory.cc b/multibody/lcs_factory.cc index f78d846..d3166ed 100644 --- a/multibody/lcs_factory.cc +++ b/multibody/lcs_factory.cc @@ -459,7 +459,7 @@ std::vector LCSFactory::GetContactDescriptions() { contact_descriptions.at(i).witness_point_A == normal_contact_descriptions.at(normal_index).witness_point_A); contact_descriptions.at(i).force_basis = - - contact_descriptions.at(i).force_basis + + contact_descriptions.at(i).force_basis + mu_[normal_index] * normal_contact_descriptions.at(normal_index).force_basis; } From a6422df52df57be54044ae0e27030026e4d991f3 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 29 Jul 2025 15:48:47 +0000 Subject: [PATCH 11/13] fix after rebase --- bindings/pyc3/c3_systems_py.cc | 4 +-- core/BUILD.bazel | 3 -- core/c3.cc | 8 +++-- multibody/test/multibody_test.cc | 23 ++++++++++--- systems/BUILD.bazel | 33 +------------------ systems/framework/c3_output.h | 20 ++++------- .../contact_force_generator.cc | 4 +-- systems/lcs_factory_system.cc | 7 ++-- systems/lcs_factory_system.h | 6 ++-- systems/test/systems_test.cc | 27 ++++++++++----- 10 files changed, 58 insertions(+), 77 deletions(-) diff --git a/bindings/pyc3/c3_systems_py.cc b/bindings/pyc3/c3_systems_py.cc index 5220770..586826c 100644 --- a/bindings/pyc3/c3_systems_py.cc +++ b/bindings/pyc3/c3_systems_py.cc @@ -103,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_description", - &LCSFactorySystem::get_output_port_lcs_contact_description, + .def("get_output_port_lcs_contact_descriptions", + &LCSFactorySystem::get_output_port_lcs_contact_descriptions, py::return_value_policy::reference); py::class_(m, "C3Solution") diff --git a/core/BUILD.bazel b/core/BUILD.bazel index 7fa4685..1c9e34e 100644 --- a/core/BUILD.bazel +++ b/core/BUILD.bazel @@ -49,9 +49,6 @@ cc_library( "c3_qp.h", "c3_plus.h", ], - data = [ - ":default_solver_options", - ], copts = ["-fopenmp"], linkopts = ["-fopenmp"], deps = [ diff --git a/core/c3.cc b/core/c3.cc index 7693c29..93fdfd5 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -438,13 +438,15 @@ 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_); - + try { + MathematicalProgramResult result = osqp_.Solve(prog_); + } catch (const std::exception& e) { + drake::log()->error("C3::SolveQP failed with exception: {}", e.what()); + } if (!result.is_success()) { drake::log()->warn("C3::SolveQP failed to solve the QP with status: {}", result.get_solution_result()); } - StoreQPResults(result, admm_iteration, is_final_solve); return *z_sol_; diff --git a/multibody/test/multibody_test.cc b/multibody/test/multibody_test.cc index d368546..0162f94 100644 --- a/multibody/test/multibody_test.cc +++ b/multibody/test/multibody_test.cc @@ -279,10 +279,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()); @@ -298,8 +297,14 @@ 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); } } @@ -313,6 +318,14 @@ TEST_P(LCSFactoryParameterizedPivotingTest, ComputeContactJacobian) { fixture.options.num_friction_directions_per_contact->begin(), fixture.options.num_friction_directions_per_contact->end(), 0); + EXPECT_EQ(LCSFactory::GetNumContactVariables(options), n_contacts); + for (size_t i = 0; i < contact_descriptions.size(); ++i) { + if (contact_descriptions[i].is_slack) continue; + EXPECT_FALSE(contact_descriptions[i].witness_point_A.isZero()); + EXPECT_FALSE(contact_descriptions[i].witness_point_B.isZero()); + EXPECT_FALSE(contact_descriptions[i].force_basis.isZero()); + } + // Verify Jacobian rows based on contact model switch (fixture.contact_model) { case ContactModel::kStewartAndTrinkle: diff --git a/systems/BUILD.bazel b/systems/BUILD.bazel index 2b36c81..3bba5e8 100644 --- a/systems/BUILD.bazel +++ b/systems/BUILD.bazel @@ -79,36 +79,6 @@ cc_library( ], ) -cc_library( name = "lcs_simulator", - srcs = [ - "lcs_simulator.cc", - ], - hdrs = [ - "lcs_simulator.h", - ], - deps = [ - "//core:lcs", - "@drake//:drake_shared_library", - ], -) - -cc_library( - name = "lcs_factory_system", - srcs = [ - "lcs_factory_system.cc", - ], - hdrs = [ - "lcs_factory_system.h", - ], - deps = [ - ":framework", - "//core:lcs", - "//core:options", - "//multibody:lcs_factory", - "@drake//:drake_shared_library", - ], -) - cc_library( name = "options", hdrs = [ @@ -142,8 +112,6 @@ cc_library( ) - - cc_library( name = "system_utils", hdrs = [ @@ -153,6 +121,7 @@ cc_library( "@drake//:drake_shared_library", ], ) + cc_test( name = "systems_test", srcs = [ diff --git a/systems/framework/c3_output.h b/systems/framework/c3_output.h index 08ef4e5..0ac4e74 100644 --- a/systems/framework/c3_output.h +++ b/systems/framework/c3_output.h @@ -5,10 +5,10 @@ #include #include -#include "c3/lcmt_output.hpp" - #include +#include "c3/lcmt_output.hpp" + using Eigen::MatrixXf; using Eigen::VectorXf; @@ -29,21 +29,13 @@ class C3Output { time_vector_ = VectorXf::Zero(N); }; - Eigen::MatrixXf GetStateSolution() const { - return x_sol_; - } + Eigen::MatrixXf GetStateSolution() const { return x_sol_; } - Eigen::MatrixXf GetForceSolution() const { - return lambda_sol_; - } + Eigen::MatrixXf GetForceSolution() const { return lambda_sol_; } - Eigen::MatrixXf GetInputSolution() const { - return u_sol_; - } + Eigen::MatrixXf GetInputSolution() const { return u_sol_; } - Eigen::VectorXf GetTimeVector() const { - return time_vector_; - } + Eigen::VectorXf GetTimeVector() const { return time_vector_; } // Shape is (variable_vector_size, knot_points) Eigen::VectorXf time_vector_; diff --git a/systems/lcmt_generators/contact_force_generator.cc b/systems/lcmt_generators/contact_force_generator.cc index d38c62c..5eb218c 100644 --- a/systems/lcmt_generators/contact_force_generator.cc +++ b/systems/lcmt_generators/contact_force_generator.cc @@ -52,7 +52,7 @@ void ContactForceGenerator::DoCalc(const Context& context, 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. @@ -60,7 +60,7 @@ void ContactForceGenerator::DoCalc(const Context& context, auto force = lcmt_contact_force(); if (contact_info->at(i).is_slack) - // If the contact is slack, set the force to zero. + // If the contact is slack, ignore it. continue; // Set contact point position. diff --git a/systems/lcs_factory_system.cc b/systems/lcs_factory_system.cc index 1f80cca..e4c3599 100644 --- a/systems/lcs_factory_system.cc +++ b/systems/lcs_factory_system.cc @@ -8,8 +8,8 @@ #include "systems/framework/timestamped_vector.h" using c3::LCS; -using c3::multibody::LCSFactory; using c3::multibody::LCSContactDescription; +using c3::multibody::LCSFactory; using c3::systems::TimestampedVector; using drake::multibody::ModelInstanceIndex; using drake::systems::BasicVector; @@ -70,10 +70,9 @@ void LCSFactorySystem::InitializeSystem( &LCSFactorySystem::OutputLCS) .get_index(); - lcs_contact_output_port_ = + lcs_contact_descriptions_output_port_ = this->DeclareAbstractOutputPort( - "contact_descriptions", - std::vector(), + "contact_descriptions", std::vector(), &LCSFactorySystem::OutputLCSContactDescriptions) .get_index(); } diff --git a/systems/lcs_factory_system.h b/systems/lcs_factory_system.h index 9fec4a8..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_description() const { - return this->get_output_port(lcs_contact_output_port_); + get_output_port_lcs_contact_descriptions() const { + return this->get_output_port(lcs_contact_descriptions_output_port_); } private: @@ -125,7 +125,7 @@ class LCSFactorySystem : public drake::systems::LeafSystem { drake::systems::InputPortIndex lcs_state_input_port_; drake::systems::InputPortIndex lcs_inputs_input_port_; drake::systems::OutputPortIndex lcs_port_; - drake::systems::OutputPortIndex lcs_contact_output_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/systems_test.cc b/systems/test/systems_test.cc index ab81645..26fd00d 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; @@ -340,7 +341,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) { @@ -360,14 +362,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 From 66ebf2faa0e588d89924232e2e622fdae404a2aa Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 29 Jul 2025 19:06:28 +0000 Subject: [PATCH 12/13] test: add unit tests for lcmt generators --- .bazelrc | 4 +- bindings/pyc3/c3_systems_py.cc | 4 +- systems/BUILD.bazel | 12 + .../contact_force_generator.cc | 6 +- .../lcmt_generators/contact_force_generator.h | 6 +- systems/test/generators_test.cc | 371 ++++++++++++++++++ 6 files changed, 394 insertions(+), 9 deletions(-) create mode 100644 systems/test/generators_test.cc diff --git a/.bazelrc b/.bazelrc index 764b3ec..0660780 100644 --- a/.bazelrc +++ b/.bazelrc @@ -51,4 +51,6 @@ 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 + +build --local_resources=cpu=8 \ No newline at end of file diff --git a/bindings/pyc3/c3_systems_py.cc b/bindings/pyc3/c3_systems_py.cc index 586826c..7c397f6 100644 --- a/bindings/pyc3/c3_systems_py.cc +++ b/bindings/pyc3/c3_systems_py.cc @@ -248,9 +248,9 @@ PYBIND11_MODULE(systems, m) { .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_info", + .def("get_input_port_lcs_contact_descriptions", &lcmt_generators::ContactForceGenerator:: - get_input_port_lcs_contact_info, + get_input_port_lcs_contact_descriptions, py::return_value_policy::reference) .def("get_output_port_contact_force", &lcmt_generators::ContactForceGenerator:: diff --git a/systems/BUILD.bazel b/systems/BUILD.bazel index 3bba5e8..3054a8a 100644 --- a/systems/BUILD.bazel +++ b/systems/BUILD.bazel @@ -153,6 +153,18 @@ cc_test( ], ) +cc_test( + name = "generators_test", + srcs = [ + "test/generators_test.cc" + ], + deps = [ + ":lcmt_generators", + "//core:c3_cartpole_problem", + "@gtest//:main", + ], +) + filegroup( name = "headers", srcs = glob([ diff --git a/systems/lcmt_generators/contact_force_generator.cc b/systems/lcmt_generators/contact_force_generator.cc index 5eb218c..4b31ac2 100644 --- a/systems/lcmt_generators/contact_force_generator.cc +++ b/systems/lcmt_generators/contact_force_generator.cc @@ -87,15 +87,15 @@ void ContactForceGenerator::DoCalc(const Context& context, LcmPublisherSystem* ContactForceGenerator::AddLcmPublisherToBuilder( DiagramBuilder& builder, const drake::systems::OutputPort& solution_port, - const drake::systems::OutputPort& lcs_contact_info_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_info_port, - force_publisher->get_input_port_lcs_contact_info()); + 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 = diff --git a/systems/lcmt_generators/contact_force_generator.h b/systems/lcmt_generators/contact_force_generator.h index ea50392..eef266d 100644 --- a/systems/lcmt_generators/contact_force_generator.h +++ b/systems/lcmt_generators/contact_force_generator.h @@ -48,8 +48,8 @@ class ContactForceGenerator : public drake::systems::LeafSystem { * @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_info() - const { + const drake::systems::InputPort& + get_input_port_lcs_contact_descriptions() const { return this->get_input_port(lcs_contact_info_port_); } @@ -82,7 +82,7 @@ class ContactForceGenerator : public drake::systems::LeafSystem { static drake::systems::lcm::LcmPublisherSystem* AddLcmPublisherToBuilder( drake::systems::DiagramBuilder& builder, const drake::systems::OutputPort& solution_port, - const drake::systems::OutputPort& lcs_contact_info_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); 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 From 24d08ee0daf8fa4eebc4a064f12a596b9d671d3e Mon Sep 17 00:00:00 2001 From: Meow404 Date: Fri, 15 Aug 2025 12:52:25 -0400 Subject: [PATCH 13/13] fixes after rebase --- .bazelrc | 2 -- core/BUILD.bazel | 9 ++++++++ core/c3.cc | 26 +++++++++++++++++------ core/c3_options.h | 36 ++++++++++++++------------------ core/c3_plus.cc | 31 +++++++++++++++++++++++---- core/common/logging_utils.hpp | 19 +++++++++++++++++ multibody/geom_geom_collider.cc | 5 ++--- multibody/lcs_factory.cc | 27 +++++++++++++----------- multibody/test/multibody_test.cc | 21 +++++++------------ systems/c3_controller.cc | 3 +-- systems/c3_controller_options.h | 4 ++-- systems/test/systems_test.cc | 4 ++++ 12 files changed, 123 insertions(+), 64 deletions(-) create mode 100644 core/common/logging_utils.hpp diff --git a/.bazelrc b/.bazelrc index 0660780..5197273 100644 --- a/.bazelrc +++ b/.bazelrc @@ -52,5 +52,3 @@ build --action_env=LD_LIBRARY_PATH= build --python_path=python3 build --define=WITH_GUROBI=ON - -build --local_resources=cpu=8 \ No newline at end of file diff --git a/core/BUILD.bazel b/core/BUILD.bazel index 1c9e34e..c047adf 100644 --- a/core/BUILD.bazel +++ b/core/BUILD.bazel @@ -29,6 +29,11 @@ filegroup( ], ) +cc_library( + name = "logging", + hdrs = ["common/logging_utils.hpp"], +) + cc_library( name = "c3", srcs = [ @@ -49,11 +54,15 @@ cc_library( "c3_qp.h", "c3_plus.h", ], + data = [ + ":default_solver_options", + ], copts = ["-fopenmp"], linkopts = ["-fopenmp"], 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 93fdfd5..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" @@ -89,7 +90,6 @@ C3::C3(const LCS& lcs, const CostMatrices& costs, w_sol_ = std::make_unique>(); delta_sol_ = std::make_unique>(); for (int i = 0; i < N_; ++i) { - z_sol_->push_back(Eigen::VectorXd::Zero(n_z_)); x_sol_->push_back(Eigen::VectorXd::Zero(n_x_)); lambda_sol_->push_back(Eigen::VectorXd::Zero(n_lambda_)); u_sol_->push_back(Eigen::VectorXd::Zero(n_u_)); @@ -267,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_) { @@ -317,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); } @@ -332,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; @@ -355,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, @@ -373,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 { @@ -417,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]); @@ -438,16 +451,17 @@ vector C3::SolveQP(const VectorXd& x0, const vector& G, AddAugmentedCost(G, WD, delta, is_final_solve); SetInitialGuessQP(x0, admm_iteration); + drake::log()->trace("C3::SolveQP calling solver."); try { MathematicalProgramResult result = osqp_.Solve(prog_); + if (!result.is_success()) { + drake::log()->warn("C3::SolveQP failed to solve the QP with status: {}", + result.get_solution_result()); + } + StoreQPResults(result, admm_iteration, is_final_solve); } catch (const std::exception& e) { drake::log()->error("C3::SolveQP failed with exception: {}", e.what()); } - if (!result.is_success()) { - drake::log()->warn("C3::SolveQP failed to solve the QP with status: {}", - result.get_solution_result()); - } - StoreQPResults(result, admm_iteration, is_final_solve); return *z_sol_; } diff --git a/core/c3_options.h b/core/c3_options.h index 94d21ab..1f017c2 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 a60a14d..0c2b38c 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]); } @@ -156,6 +173,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/multibody/geom_geom_collider.cc b/multibody/geom_geom_collider.cc index b92e098..7e767ae 100644 --- a/multibody/geom_geom_collider.cc +++ b/multibody/geom_geom_collider.cc @@ -280,12 +280,11 @@ GeomGeomCollider::CalcForceBasisInWorldFrame( const auto query_result = GetGeometryQueryResult(context); if (num_friction_directions < 1) { // Planar contact: basis is constructed from the contact and planar normals. - return ComputePlanarForceBasis(-query_result.signed_distance_pair.nhat_BA_W, - planar_normal); + return ComputePlanarForceBasis(-query_result.nhat_BA_W, planar_normal); } else { // 3D contact: build polytope basis and rotate using contact normal. auto R_WC = drake::math::RotationMatrix::MakeFromOneVector( - -query_result.signed_distance_pair.nhat_BA_W, 0); + -query_result.nhat_BA_W, 0); return ComputePolytopeForceBasis(num_friction_directions) * R_WC.matrix().transpose(); } diff --git a/multibody/lcs_factory.cc b/multibody/lcs_factory.cc index d3166ed..31348b1 100644 --- a/multibody/lcs_factory.cc +++ b/multibody/lcs_factory.cc @@ -416,8 +416,8 @@ std::vector LCSFactory::GetContactDescriptions() { for (int i = 0; i < n_contacts_; i++) { multibody::GeomGeomCollider collider(plant_, contact_pairs_[i]); auto [p_WCa, p_WCb] = collider.CalcWitnessPoints(context_); - auto force_basis = - collider.CalcForceBasisInWorldFrame(context_, n_friction_directions_); + auto force_basis = collider.CalcForceBasisInWorldFrame( + context_, n_friction_directions_per_contact_[i]); for (int j = 0; j < force_basis.rows(); j++) { LCSContactDescription contact_description = { @@ -452,16 +452,19 @@ std::vector LCSFactory::GetContactDescriptions() { contact_descriptions.insert(contact_descriptions.end(), tangential_contact_descriptions.begin(), tangential_contact_descriptions.end()); - DRAKE_ASSERT(n_friction_directions_ > 0); - for (int i = 0; i < tangential_contact_descriptions.size(); i++) { - int normal_index = i / (2 * n_friction_directions_); - DRAKE_ASSERT( - contact_descriptions.at(i).witness_point_A == - normal_contact_descriptions.at(normal_index).witness_point_A); - contact_descriptions.at(i).force_basis = - contact_descriptions.at(i).force_basis + - mu_[normal_index] * - normal_contact_descriptions.at(normal_index).force_basis; + for (int normal_index = 0; normal_index < n_contacts_; normal_index++) { + // Jt_row_sizes_ gives number of friction directions per contact + for (int i = 0; i < Jt_row_sizes_(normal_index); i++) { + int tangential_index = Jt_row_sizes_.segment(0, normal_index).sum() + i; + DRAKE_ASSERT( + contact_descriptions.at(tangential_index).witness_point_A == + normal_contact_descriptions.at(normal_index).witness_point_A); + contact_descriptions.at(tangential_index).force_basis = + mu_[normal_index] * + contact_descriptions.at(tangential_index).force_basis + + normal_contact_descriptions.at(normal_index).force_basis; + contact_descriptions.at(tangential_index).force_basis.normalize(); + } } } diff --git a/multibody/test/multibody_test.cc b/multibody/test/multibody_test.cc index 0162f94..fbd823a 100644 --- a/multibody/test/multibody_test.cc +++ b/multibody/test/multibody_test.cc @@ -265,8 +265,8 @@ TEST_P(LCSFactoryParameterizedPivotingTest, LinearizePlantToLCS) { 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( @@ -296,7 +296,6 @@ TEST_P(LCSFactoryParameterizedPivotingTest, UpdateStateAndInput) { } // Contact Jacobian and points should change - EXPECT_NE(initial_J, updated_J); 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, @@ -309,8 +308,8 @@ TEST_P(LCSFactoryParameterizedPivotingTest, UpdateStateAndInput) { } // 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 = @@ -318,7 +317,6 @@ TEST_P(LCSFactoryParameterizedPivotingTest, ComputeContactJacobian) { fixture.options.num_friction_directions_per_contact->begin(), fixture.options.num_friction_directions_per_contact->end(), 0); - EXPECT_EQ(LCSFactory::GetNumContactVariables(options), n_contacts); for (size_t i = 0; i < contact_descriptions.size(); ++i) { if (contact_descriptions[i].is_slack) continue; EXPECT_FALSE(contact_descriptions[i].witness_point_A.isZero()); @@ -330,23 +328,20 @@ TEST_P(LCSFactoryParameterizedPivotingTest, ComputeContactJacobian) { 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/systems/c3_controller.cc b/systems/c3_controller.cc index 1ec42d4..deb081f 100644 --- a/systems/c3_controller.cc +++ b/systems/c3_controller.cc @@ -111,8 +111,7 @@ C3Controller::C3Controller( .get_index(); c3_intermediates_port_ = this->DeclareAbstractOutputPort( - "intermediates", - C3Output::C3Intermediates(n_x_, n_lambda_, n_u_, N_), + "intermediates", C3Output::C3Intermediates(c3_->GetZSize(), N_), &C3Controller::OutputC3Intermediates) .get_index(); diff --git a/systems/c3_controller_options.h b/systems/c3_controller_options.h index 4bb38b1..ef272f0 100644 --- a/systems/c3_controller_options.h +++ b/systems/c3_controller_options.h @@ -82,9 +82,9 @@ struct C3ControllerOptions { DRAKE_DEMAND(static_cast(c3_options.u_lambda.size()) == expected_lambda_size); if (projection_type == "C3+") { - DRAKE_DEMAND(static_cast(c3_options.g_eta->size()) == + DRAKE_DEMAND(static_cast(c3_options.g_eta_vector.size()) == expected_lambda_size); - DRAKE_DEMAND(static_cast(c3_options.u_eta->size()) == + DRAKE_DEMAND(static_cast(c3_options.u_eta_vector.size()) == expected_lambda_size); } } diff --git a/systems/test/systems_test.cc b/systems/test/systems_test.cc index 26fd00d..ecf862d 100644 --- a/systems/test/systems_test.cc +++ b/systems/test/systems_test.cc @@ -205,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);