From eda17fb8281f2111a61ec213789bf9685f51c984 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Mon, 23 Jun 2025 12:39:30 -0400 Subject: [PATCH 01/17] 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 0602dac..da6c609 100644 --- a/systems/c3_controller.h +++ b/systems/c3_controller.h @@ -113,6 +113,10 @@ class C3Controller : public drake::systems::LeafSystem { c3_->AddLinearConstraint(A, lower_bound, upper_bound, constraint); } + void SetSolverOptions(const drake::solvers::SolverOptions& options) { + c3_->SetSolverOptions(options); + } + private: /** * @struct JointDescription From cb679de88c9b1b8505ad54edb65c86782291ef57 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 24 Jun 2025 10:22:47 -0400 Subject: [PATCH 02/17] 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 f46c20a..40d2f01 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 54fe405..b6052a4 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", @@ -86,6 +87,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 1e5d8b1abb276fadbfc77f0827955ed455b1e7f4 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Mon, 30 Jun 2025 09:30:48 -0400 Subject: [PATCH 03/17] 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 d1a44be7325c41ab3024d9c68b8cd8a8ac6f1f4c Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 1 Jul 2025 21:18:13 +0000 Subject: [PATCH 04/17] 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 0560d07..4f96f99 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 ebeb288..07212f7 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 ffce8c0..58000c4 100644 --- a/multibody/geom_geom_collider.h +++ b/multibody/geom_geom_collider.h @@ -99,14 +99,36 @@ class GeomGeomCollider { * closest point on geometry B. */ std::pair, drake::VectorX> CalcWitnessPoints( - const drake::systems::Context& context); + const drake::systems::Context& context); + + /** + * @brief Computes a basis for contact forces in the world frame. + * + * Depending on the number of friction directions, this method constructs + * either a planar (2D) or polytope (3D) basis for the contact forces at the + * collision point, expressed in the world frame. For planar contact + * (num_friction_directions < 1), the basis is constructed from the contact + * normal and the provided planar normal. For 3D contact, a polytope basis is + * generated and rotated to align with the contact normal. + * + * @param context The context for the MultibodyPlant. + * @param num_friction_directions The number of friction directions for the + * polytope approximation. If less than 1, a planar basis is used. + * @param planar_normal The normal vector defining the plane for planar + * contact (default: {0, 1, 0}). + * @return A matrix whose rows form an orthonormal basis for the contact + * forces in the world frame. + */ + Eigen::Matrix CalcForceBasisInWorldFrame( + const drake::systems::Context& context, int num_friction_directions, + const Eigen::Vector3d& planar_normal = {0, 1, 0}) const; /** * @brief A struct to hold the results of a geometry query. * * This struct contains the signed distance pair, the frame IDs of the two - * geometries, the frames themselves, and the positions of the closest points - * on each geometry, expressed in their respective frames. + * geometries, the frames themselves, and the positions of the closest + * points on each geometry, expressed in their respective frames. */ struct GeometryQueryResult { /** diff --git a/multibody/lcs_factory.cc b/multibody/lcs_factory.cc index 23537d9..e1bce88 100644 --- a/multibody/lcs_factory.cc +++ b/multibody/lcs_factory.cc @@ -236,21 +236,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) { @@ -523,53 +508,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 f8e13e0..f9ea0be 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 @@ -112,16 +125,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. @@ -316,14 +325,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 b6052a4..3e01629 100644 --- a/systems/BUILD.bazel +++ b/systems/BUILD.bazel @@ -102,6 +102,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 91d35a0d1ed1642ac53a18813e9ed7d7e0b2faa9 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Thu, 3 Jul 2025 09:56:58 -0400 Subject: [PATCH 05/17] 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 3e01629..37d8e60 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", @@ -91,14 +91,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 46972b92f752b1f8a6f180534f979aa59d36009b Mon Sep 17 00:00:00 2001 From: Meow404 Date: Sat, 5 Jul 2025 16:42:44 -0400 Subject: [PATCH 06/17] 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 37d8e60..00d0cb3 100644 --- a/systems/BUILD.bazel +++ b/systems/BUILD.bazel @@ -93,12 +93,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 7055d2af9d6e255b644a9ee261e8ce4ef1b573c8 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 8 Jul 2025 17:22:24 +0000 Subject: [PATCH 07/17] 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 4f96f99..4d366c6 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 07212f7..c814d1d 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<>()) @@ -182,6 +185,88 @@ PYBIND11_MODULE(systems, m) { &C3ControllerOptions::quaternion_regularizer_fraction); m.def("LoadC3ControllerOptions", &LoadC3ControllerOptions); + + // C3OutputGenerator + py::class_>(m, "C3OutputGenerator") + .def(py::init<>()) + .def("get_input_port_c3_solution", + &lcmt_generators::C3OutputGenerator::get_input_port_c3_solution, + py::return_value_policy::reference) + .def("get_input_port_c3_intermediates", + &lcmt_generators::C3OutputGenerator::get_input_port_c3_intermediates, + py::return_value_policy::reference) + .def("get_output_port_c3_output", + &lcmt_generators::C3OutputGenerator::get_output_port_c3_output, + py::return_value_policy::reference) + .def_static("AddLcmPublisherToBuilder", + &lcmt_generators::C3OutputGenerator::AddLcmPublisherToBuilder, + py::arg("builder"), py::arg("solution_port"), + py::arg("intermediates_port"), py::arg("channel"), + py::arg("lcm"), py::arg("publish_triggers"), + py::arg("publish_period"), py::arg("publish_offset")); + + // C3TrajectoryGenerator + py::class_( + m, "TrajectoryDescriptionIndexRange") + .def(py::init<>()) + .def_readwrite( + "start", &lcmt_generators::TrajectoryDescription::index_range::start) + .def_readwrite("end", + &lcmt_generators::TrajectoryDescription::index_range::end); + py::class_(m, "TrajectoryDescription") + .def(py::init<>()) + .def_readwrite("trajectory_name", + &lcmt_generators::TrajectoryDescription::trajectory_name) + .def_readwrite("variable_type", + &lcmt_generators::TrajectoryDescription::variable_type) + .def_readwrite("indices", + &lcmt_generators::TrajectoryDescription::indices); + py::class_( + m, "C3TrajectoryGeneratorConfig") + .def(py::init<>()) + .def_readwrite( + "trajectories", + &lcmt_generators::C3TrajectoryGeneratorConfig::trajectories); + py::class_>(m, "C3TrajectoryGenerator") + .def(py::init()) + .def("get_input_port_c3_solution", + &lcmt_generators::C3TrajectoryGenerator::get_input_port_c3_solution, + py::return_value_policy::reference) + .def("get_output_port_lcmt_c3_trajectory", + &lcmt_generators::C3TrajectoryGenerator:: + get_output_port_lcmt_c3_trajectory, + py::return_value_policy::reference) + .def_static( + "AddLcmPublisherToBuilder", + &lcmt_generators::C3TrajectoryGenerator::AddLcmPublisherToBuilder, + py::arg("builder"), py::arg("config"), py::arg("solution_port"), + py::arg("channel"), py::arg("lcm"), py::arg("publish_triggers"), + py::arg("publish_period"), py::arg("publish_offset")); + + // ContactForceGenerator + py::class_>(m, "ContactForceGenerator") + .def(py::init<>()) + .def("get_input_port_c3_solution", + &lcmt_generators::ContactForceGenerator::get_input_port_c3_solution, + py::return_value_policy::reference) + .def("get_input_port_lcs_contact_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 00d0cb3..eb635ec 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", @@ -79,6 +80,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 = [ @@ -104,26 +135,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", - ] -) cc_library( @@ -205,5 +223,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 8cdbdd513b17e0a2a52236ea6253b18704dc6b1d Mon Sep 17 00:00:00 2001 From: Meow404 Date: Wed, 9 Jul 2025 10:18:08 -0400 Subject: [PATCH 08/17] 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 40d2f01..f46c20a 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 98ffa97daf5a0d9b746e72cd3e6716932646971e Mon Sep 17 00:00:00 2001 From: Meow404 Date: Sat, 12 Jul 2025 16:22:50 +0000 Subject: [PATCH 09/17] 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 201a762b5cf40e742abff1245b5b78dbd412c87a Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 22 Jul 2025 09:59:53 -0400 Subject: [PATCH 10/17] 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 e1bce88..0933d71 100644 --- a/multibody/lcs_factory.cc +++ b/multibody/lcs_factory.cc @@ -558,7 +558,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 90ed5db7810c2ee57647d863e9a5bbc6ec192e70 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 29 Jul 2025 15:48:47 +0000 Subject: [PATCH 11/17] 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 c814d1d..0ad2b65 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 ec5c92e..1e1bf03 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 f46c20a..8361ccb 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -437,13 +437,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 dd9d9cc..40fe0b0 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 eb635ec..f6deb83 100644 --- a/systems/BUILD.bazel +++ b/systems/BUILD.bazel @@ -80,36 +80,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 = "quaternion_error_hessian", srcs = [ @@ -166,6 +134,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 a4cd6a5fadc851c253aa33c98488f9921e281966 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 29 Jul 2025 19:06:28 +0000 Subject: [PATCH 12/17] 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 0ad2b65..b49f075 100644 --- a/bindings/pyc3/c3_systems_py.cc +++ b/bindings/pyc3/c3_systems_py.cc @@ -252,9 +252,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 f6deb83..59fd10b 100644 --- a/systems/BUILD.bazel +++ b/systems/BUILD.bazel @@ -186,6 +186,18 @@ cc_test( size = "small", ) +cc_test( + name = "generators_test", + srcs = [ + "test/generators_test.cc" + ], + deps = [ + ":lcmt_generators", + "//core:c3_cartpole_problem", + "@gtest//:main", + ], +) + filegroup( name = "headers", srcs = glob([ 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 6d7e0d5b7649a2cc0a032ec4fd56c0e31cf83a17 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Fri, 15 Aug 2025 12:52:25 -0400 Subject: [PATCH 13/17] fixes after rebase --- .bazelrc | 2 -- core/BUILD.bazel | 9 ++++++++ core/c3.cc | 25 +++++++++++++++++----- 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(+), 63 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 1e1bf03..7e1bbb0 100644 --- a/core/BUILD.bazel +++ b/core/BUILD.bazel @@ -29,6 +29,11 @@ filegroup( ], ) +cc_library( + name = "logging", + hdrs = ["common/logging_utils.hpp"], +) + cc_library( name = "c3", srcs = [ @@ -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 8361ccb..bd4fb50 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -7,6 +7,7 @@ #include #include +#include "common/logging_utils.hpp" #include "lcs.h" #include "solver_options_io.h" @@ -266,6 +267,7 @@ const std::vector& C3::GetTargetCost() { } void C3::Solve(const VectorXd& x0) { + drake::log()->debug("C3::Solve called"); auto start = std::chrono::high_resolution_clock::now(); // Set the initial state constraint if (initial_state_constraint_) { @@ -316,6 +318,8 @@ void C3::Solve(const VectorXd& x0) { std::vector w(N_, VectorXd::Zero(n_z_)); vector G = cost_matrices_.G; + drake::log()->debug("C3::Solve starting ADMM iterations."); + for (int iter = 0; iter < options_.admm_iter; iter++) { ADMMStep(x0, &delta, &w, &G, iter); } @@ -331,6 +335,7 @@ void C3::Solve(const VectorXd& x0) { *delta_sol_ = delta; if (!options_.end_on_qp_step) { + drake::log()->debug("C3::Solve compute a half step."); *z_sol_ = delta; z_sol_->at(0).segment(0, n_x_) = x0; x_sol_->at(0) = x0; @@ -354,6 +359,7 @@ void C3::Solve(const VectorXd& x0) { solve_time_ = std::chrono::duration_cast(elapsed).count() / 1e6; + drake::log()->debug("C3::Solve completed in {} seconds.", solve_time_); } void C3::ADMMStep(const VectorXd& x0, vector* delta, @@ -372,6 +378,7 @@ void C3::ADMMStep(const VectorXd& x0, vector* delta, ZW[i] = w->at(i) + z[i]; } + drake::log()->debug("C3::ADMMStep SolveProjection step."); if (cost_matrices_.U[0].isZero(0)) { *delta = SolveProjection(*G, ZW, admm_iteration); } else { @@ -416,10 +423,17 @@ void C3::StoreQPResults(const MathematicalProgramResult& result, z_sol_->at(i).segment(0, n_x_) = result.GetSolution(x_[i]); z_sol_->at(i).segment(n_x_, n_lambda_) = result.GetSolution(lambda_[i]); z_sol_->at(i).segment(n_x_ + n_lambda_, n_u_) = result.GetSolution(u_[i]); + + drake::log()->trace( + "C3::StoreQPResults storing solution for time step {}: " + "lambda = {}", + i, EigenToString(lambda_sol_->at(i).transpose())); } if (!warm_start_) return; // No warm start, so no need to update warm start parameters + + drake::log()->trace("C3::StoreQPResults storing warm start values."); for (int i = 0; i < N_ + 1; ++i) { if (i < N_) { warm_start_x_[admm_iteration][i] = result.GetSolution(x_[i]); @@ -437,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 2734960..af0d543 100644 --- a/core/c3_options.h +++ b/core/c3_options.h @@ -76,6 +76,7 @@ struct C3Options { std::vector g_lambda_t; std::vector g_lambda; std::vector g_u; + std::vector g_eta_vector; std::optional> g_eta_slack; std::optional> g_eta_n; std::optional> g_eta_t; @@ -88,6 +89,7 @@ struct C3Options { std::vector u_lambda_t; std::vector u_lambda; std::vector u_u; + std::vector u_eta_vector; std::optional> u_eta_slack; std::optional> u_eta_n; std::optional> u_eta_t; @@ -150,16 +152,14 @@ struct C3Options { } g_vector.insert(g_vector.end(), g_lambda.begin(), g_lambda.end()); g_vector.insert(g_vector.end(), g_u.begin(), g_u.end()); - if (g_eta != std::nullopt || g_eta_slack != std::nullopt) { - if (g_eta == std::nullopt || g_eta->empty()) { - g_vector.insert(g_vector.end(), g_eta_slack->begin(), - g_eta_slack->end()); - g_vector.insert(g_vector.end(), g_eta_n->begin(), g_eta_n->end()); - g_vector.insert(g_vector.end(), g_eta_t->begin(), g_eta_t->end()); - } else { - g_vector.insert(g_vector.end(), g_eta->begin(), g_eta->end()); - } + g_eta_vector = g_eta.value_or(std::vector()); + if (g_eta_vector.empty() && g_eta_slack.has_value()) { + g_eta_vector.insert(g_eta_vector.end(), g_eta_slack->begin(), + g_eta_slack->end()); + g_eta_vector.insert(g_eta_vector.end(), g_eta_n->begin(), g_eta_n->end()); + g_eta_vector.insert(g_eta_vector.end(), g_eta_t->begin(), g_eta_t->end()); } + g_vector.insert(g_vector.end(), g_eta_vector.begin(), g_eta_vector.end()); u_vector = std::vector(); u_vector.insert(u_vector.end(), u_x.begin(), u_x.end()); @@ -170,16 +170,14 @@ struct C3Options { } u_vector.insert(u_vector.end(), u_lambda.begin(), u_lambda.end()); u_vector.insert(u_vector.end(), u_u.begin(), u_u.end()); - if (u_eta != std::nullopt || u_eta_slack != std::nullopt) { - if (u_eta == std::nullopt || u_eta->empty()) { - u_vector.insert(u_vector.end(), u_eta_slack->begin(), - u_eta_slack->end()); - u_vector.insert(u_vector.end(), u_eta_n->begin(), u_eta_n->end()); - u_vector.insert(u_vector.end(), u_eta_t->begin(), u_eta_t->end()); - } else { - u_vector.insert(u_vector.end(), u_eta->begin(), u_eta->end()); - } + u_eta_vector = u_eta.value_or(std::vector()); + if (u_eta_vector.empty() && u_eta_slack.has_value()) { + u_eta_vector.insert(u_eta_vector.end(), u_eta_slack->begin(), + u_eta_slack->end()); + u_eta_vector.insert(u_eta_vector.end(), u_eta_n->begin(), u_eta_n->end()); + u_eta_vector.insert(u_eta_vector.end(), u_eta_t->begin(), u_eta_t->end()); } + u_vector.insert(u_vector.end(), u_eta_vector.begin(), u_eta_vector.end()); Eigen::VectorXd q = Eigen::Map( this->q_vector.data(), this->q_vector.size()); @@ -190,8 +188,6 @@ struct C3Options { Eigen::VectorXd u = Eigen::Map( this->u_vector.data(), this->u_vector.size()); - DRAKE_DEMAND(g.size() == u.size()); - Q = w_Q * q.asDiagonal(); R = w_R * r.asDiagonal(); G = w_G * g.asDiagonal(); diff --git a/core/c3_plus.cc b/core/c3_plus.cc index 2554b2d..4b36d99 100644 --- a/core/c3_plus.cc +++ b/core/c3_plus.cc @@ -5,8 +5,11 @@ #include #include "c3_options.h" +#include "common/logging_utils.hpp" #include "lcs.h" +#include "drake/common/text_logging.h" + namespace c3 { using Eigen::MatrixXd; @@ -19,6 +22,16 @@ C3Plus::C3Plus(const LCS& lcs, const CostMatrices& costs, const vector& xdesired, const C3Options& options) : C3(lcs, costs, xdesired, options, lcs.num_states() + 2 * lcs.num_lambdas() + lcs.num_inputs()) { + if (warm_start_) { + warm_start_eta_.resize(options_.admm_iter + 1); + for (int iter = 0; iter < options_.admm_iter + 1; ++iter) { + warm_start_eta_[iter].resize(N_); + for (int i = 0; i < N_; ++i) { + warm_start_eta_[iter][i] = VectorXd::Zero(n_lambda_); + } + } + } + // Initialize eta as optimization variables eta_ = vector(); eta_sol_ = std::make_unique>(); @@ -40,10 +53,7 @@ C3Plus::C3Plus(const LCS& lcs, const CostMatrices& costs, EtaLinEq.block(0, n_x_ + n_lambda_, n_lambda_, n_u_) = lcs_.H().at(i); eta_constraints_[i] = - prog_ - .AddLinearEqualityConstraint( - EtaLinEq, -lcs_.c().at(i), - {x_.at(i), lambda_.at(i), u_.at(i), eta_.at(i)}) + prog_.AddLinearEqualityConstraint(EtaLinEq, -lcs_.c().at(i), z_.at(i)) .evaluator() .get(); } @@ -81,16 +91,23 @@ void C3Plus::SetInitialGuessQP(const Eigen::VectorXd& x0, int admm_iteration) { void C3Plus::StoreQPResults(const MathematicalProgramResult& result, int admm_iteration, bool is_final_solve) { C3::StoreQPResults(result, admm_iteration, is_final_solve); + drake::log()->trace("C3Plus::StoreQPResults storing eta results."); for (int i = 0; i < N_; i++) { if (is_final_solve) { eta_sol_->at(i) = result.GetSolution(eta_[i]); } z_sol_->at(i).segment(n_x_ + n_lambda_ + n_u_, n_lambda_) = result.GetSolution(eta_[i]); + drake::log()->trace( + "C3Plus::StoreQPResults storing solution for time step {}: " + "eta = {}", + i, EigenToString(eta_sol_->at(i).transpose())); } if (!warm_start_) return; // No warm start, so no need to update warm start parameters + + drake::log()->trace("C3Plus::StoreQPResults storing warm start eta."); for (int i = 0; i < N_; ++i) { warm_start_eta_[admm_iteration][i] = result.GetSolution(eta_[i]); } @@ -178,6 +195,12 @@ VectorXd C3Plus::SolveSingleProjection(const MatrixXd& U, VectorXd lambda_c = delta_c.segment(n_x_, n_lambda_); VectorXd eta_c = delta_c.segment(n_x_ + n_lambda_ + n_u_, n_lambda_); + drake::log()->trace( + "C3Plus::SolveSingleProjection ADMM iteration {}: pre-projection " + "lambda = {}, eta = {}", + admm_iteration, EigenToString(lambda_c.transpose()), + EigenToString(eta_c.transpose())); + // Set the smaller of lambda and eta to zero Eigen::Array eta_larger = eta_c.array() * w_eta_vec.array().sqrt() > diff --git a/core/common/logging_utils.hpp b/core/common/logging_utils.hpp new file mode 100644 index 0000000..b5d6c6d --- /dev/null +++ b/core/common/logging_utils.hpp @@ -0,0 +1,19 @@ +#include +#include + +#include + +/** + * Converts an Eigen matrix to a string representation. + * This function is useful for logging or debugging purposes. + * + * @tparam Derived The type of the Eigen matrix. + * @param mat The Eigen matrix to convert. + * @return A string representation of the matrix. + */ +template +std::string EigenToString(const Eigen::MatrixBase& mat) { + std::stringstream ss; + ss << mat; + return ss.str(); +} \ No newline at end of file diff --git a/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 0933d71..1c7d94e 100644 --- a/multibody/lcs_factory.cc +++ b/multibody/lcs_factory.cc @@ -515,8 +515,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 = { @@ -551,16 +551,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 40fe0b0..3aeb479 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 338bf32..8e4fc15 100644 --- a/systems/c3_controller.cc +++ b/systems/c3_controller.cc @@ -119,8 +119,7 @@ C3Controller::C3Controller( .get_index(); c3_intermediates_port_ = this->DeclareAbstractOutputPort( - "intermediates", - C3Output::C3Intermediates(n_x_, n_lambda_, n_u_, N_), + "intermediates", C3Output::C3Intermediates(c3_->GetZSize(), N_), &C3Controller::OutputC3Intermediates) .get_index(); diff --git a/systems/c3_controller_options.h b/systems/c3_controller_options.h index 94e20e8..6493606 100644 --- a/systems/c3_controller_options.h +++ b/systems/c3_controller_options.h @@ -89,9 +89,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); From eb60fcf1d098f1bb40d5dc9bb3b5c10b14cac991 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Sun, 22 Feb 2026 13:53:20 -0500 Subject: [PATCH 14/17] fix: remove constant naming for systems --- systems/lcmt_generators/c3_output_generator.cc | 2 -- systems/lcmt_generators/c3_trajectory_generator.cc | 1 - systems/lcmt_generators/contact_force_generator.cc | 1 - 3 files changed, 4 deletions(-) diff --git a/systems/lcmt_generators/c3_output_generator.cc b/systems/lcmt_generators/c3_output_generator.cc index ebc5e39..1db767c 100644 --- a/systems/lcmt_generators/c3_output_generator.cc +++ b/systems/lcmt_generators/c3_output_generator.cc @@ -22,8 +22,6 @@ C3OutputGenerator::C3OutputGenerator() { 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(), diff --git a/systems/lcmt_generators/c3_trajectory_generator.cc b/systems/lcmt_generators/c3_trajectory_generator.cc index af4e4a2..a14ecc1 100644 --- a/systems/lcmt_generators/c3_trajectory_generator.cc +++ b/systems/lcmt_generators/c3_trajectory_generator.cc @@ -20,7 +20,6 @@ C3TrajectoryGenerator::C3TrajectoryGenerator(C3TrajectoryGeneratorConfig config) "lcmt_c3_trajectory", &C3TrajectoryGenerator::GenerateTrajectory) .get_index(); - // this->set_name("c3_trajectory_generator"); } void C3TrajectoryGenerator::GenerateTrajectory( diff --git a/systems/lcmt_generators/contact_force_generator.cc b/systems/lcmt_generators/contact_force_generator.cc index 4b31ac2..d92f9b3 100644 --- a/systems/lcmt_generators/contact_force_generator.cc +++ b/systems/lcmt_generators/contact_force_generator.cc @@ -34,7 +34,6 @@ ContactForceGenerator::ContactForceGenerator() { drake::Value>()) .get_index(); - this->set_name("c3_contact_force_generator"); // Declare output port for publishing contact forces. contact_force_output_port_ = this->DeclareAbstractOutputPort("lcmt_force", lcmt_contact_forces(), From 523067adeace29c762d5b7977a0d9e2fc92339ea Mon Sep 17 00:00:00 2001 From: Meow404 Date: Sun, 22 Feb 2026 15:04:46 -0500 Subject: [PATCH 15/17] fix: generate planar contact description based on planar normal --- multibody/lcs_factory.cc | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/multibody/lcs_factory.cc b/multibody/lcs_factory.cc index 1c7d94e..03bd9b3 100644 --- a/multibody/lcs_factory.cc +++ b/multibody/lcs_factory.cc @@ -515,8 +515,14 @@ 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_); + Eigen::Vector3d planar_normal; + // For frictionless or single friction direction contacts, the force basis + // is just the contact normal + if (n_friction_directions_per_contact_[i] == 1) + planar_normal = Eigen::Map( + planar_normal_direction_per_contact_[i].data()); auto force_basis = collider.CalcForceBasisInWorldFrame( - context_, n_friction_directions_per_contact_[i]); + context_, n_friction_directions_per_contact_[i], planar_normal); for (int j = 0; j < force_basis.rows(); j++) { LCSContactDescription contact_description = { From cee6a818b9dfd267a622787c4133fd1a5fcdea81 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Sun, 22 Feb 2026 16:11:03 -0500 Subject: [PATCH 16/17] reviewer requests --- multibody/geom_geom_collider.cc | 23 ++++++ multibody/lcs_factory.cc | 76 +++++++++++++++++-- multibody/lcs_factory.h | 2 + multibody/test/multibody_test.cc | 2 +- .../c3_trajectory_generator.cc | 1 - .../c3_trajectory_generator_config.h | 62 +++++++++++++-- 6 files changed, 150 insertions(+), 16 deletions(-) diff --git a/multibody/geom_geom_collider.cc b/multibody/geom_geom_collider.cc index 7e767ae..e8e5747 100644 --- a/multibody/geom_geom_collider.cc +++ b/multibody/geom_geom_collider.cc @@ -278,11 +278,34 @@ GeomGeomCollider::CalcForceBasisInWorldFrame( const Context& context, int num_friction_directions, const Vector3d& planar_normal) const { const auto query_result = GetGeometryQueryResult(context); + + // Compute the force basis in the world frame for contact forces. + // + // IMPORTANT: Force Direction Convention + // ====================================== + // The forces computed here represent forces that object A exerts on object B. + // This is why we use -nhat_BA_W (negative of the normal from B to A). + // + // Recall that nhat_BA_W points from body B towards body A in the world frame. + // For contact forces, we want the force that A applies to B, which acts in + // the direction opposite to nhat_BA_W (i.e., from A towards B). + // + // For friction cone discretization: + // - The first basis vector is the normal force direction: -nhat_BA_W + // - The remaining basis vectors span the tangent plane (friction directions) + if (num_friction_directions < 1) { // Planar contact: basis is constructed from the contact and planar normals. + // The planar_normal defines the plane of admissible motion. + // This is typically used for 2D contact scenarios or constrained motion. return ComputePlanarForceBasis(-query_result.nhat_BA_W, planar_normal); } else { // 3D contact: build polytope basis and rotate using contact normal. + // The polytope basis consists of 2*num_friction_directions + 1 vectors: + // - 1 normal force vector + // - 2*num_friction_directions tangential vectors (forming a friction cone) + // We rotate this basis from the contact frame to the world frame using + // R_WC. auto R_WC = drake::math::RotationMatrix::MakeFromOneVector( -query_result.nhat_BA_W, 0); return ComputePolytopeForceBasis(num_friction_directions) * diff --git a/multibody/lcs_factory.cc b/multibody/lcs_factory.cc index 03bd9b3..63684af 100644 --- a/multibody/lcs_factory.cc +++ b/multibody/lcs_factory.cc @@ -2,7 +2,6 @@ #include -#include "multibody/geom_geom_collider.h" #include "multibody/multibody_utils.h" #include "drake/common/text_logging.h" @@ -196,6 +195,7 @@ LCSFactory::LCSFactory( } } // Non-planar contacts keep the default empty value {} + geom_geom_colliders_per_contact_.emplace_back(plant_, contact_pairs_[i]); } Jt_row_sizes_ = 2 * Eigen::Map( n_friction_directions_per_contact_.data(), @@ -212,14 +212,14 @@ void LCSFactory::ComputeContactJacobian(VectorXd& phi, MatrixXd& Jn, double phi_i; MatrixX J_i; for (int i = 0; i < n_contacts_; i++) { - multibody::GeomGeomCollider collider(plant_, contact_pairs_[i]); if (frictionless_ || n_friction_directions_per_contact_[i] == 1) { Eigen::Vector3d planar_normal = Eigen::Map( planar_normal_direction_per_contact_[i].data()); - std::tie(phi_i, J_i) = collider.EvalPlanar(context_, planar_normal); + std::tie(phi_i, J_i) = geom_geom_colliders_per_contact_[i].EvalPlanar( + context_, planar_normal); } else - std::tie(phi_i, J_i) = collider.EvalPolytope( + std::tie(phi_i, J_i) = geom_geom_colliders_per_contact_[i].EvalPolytope( context_, n_friction_directions_per_contact_[i]); // Signed distance value for contact i @@ -513,16 +513,17 @@ std::vector LCSFactory::GetContactDescriptions() { std::vector tangential_contact_descriptions; for (int i = 0; i < n_contacts_; i++) { - multibody::GeomGeomCollider collider(plant_, contact_pairs_[i]); - auto [p_WCa, p_WCb] = collider.CalcWitnessPoints(context_); + auto [p_WCa, p_WCb] = + geom_geom_colliders_per_contact_[i].CalcWitnessPoints(context_); Eigen::Vector3d planar_normal; // For frictionless or single friction direction contacts, the force basis // is just the contact normal if (n_friction_directions_per_contact_[i] == 1) planar_normal = Eigen::Map( planar_normal_direction_per_contact_[i].data()); - auto force_basis = collider.CalcForceBasisInWorldFrame( - context_, n_friction_directions_per_contact_[i], planar_normal); + auto force_basis = + geom_geom_colliders_per_contact_[i].CalcForceBasisInWorldFrame( + context_, n_friction_directions_per_contact_[i], planar_normal); for (int j = 0; j < force_basis.rows(); j++) { LCSContactDescription contact_description = { @@ -538,12 +539,63 @@ std::vector LCSFactory::GetContactDescriptions() { } } + // =========================================================================== + // Contact Force Variable Stacking Convention + // =========================================================================== + // + // The order in which contact force variables (λ) are stacked depends on the + // contact model. This ordering is crucial for understanding the structure of + // the LCS matrices (D, E, F, H, c). + // + // Stewart-Trinkle Model (n_λ = 2*n_contacts + n_tangential): + // ----------------------------------------------------------- + // λ = [γ₁, γ₂, ..., γₙ, <- Slack variables (n_contacts) + // λₙ₁, λₙ₂, ..., λₙₙ, <- Normal forces (n_contacts) + // λₜ₁₁, λₜ₁₂, ..., λₜ₁ₘ₁, <- Tangential forces for contact 1 + // λₜ₂₁, λₜ₂₂, ..., λₜ₂ₘ₂, <- Tangential forces for contact 2 + // ... + // λₜₙ₁, λₜₙ₂, ..., λₜₙₘₙ] <- Tangential forces for contact n + // + // where: + // - n = n_contacts (number of contact pairs) + // - mᵢ = 2 * num_friction_directions[i] (tangential force components per + // contact) + // - γᵢ are slack variables for the friction cone constraints + // + // Example with 2 contacts, 2 friction directions each: + // λ = [γ₁, γ₂, λₙ₁, λₙ₂, λₜ₁₁, λₜ₁₂, λₜ₁₃, λₜ₁₄, λₜ₂₁, λₜ₂₂, λₜ₂₃, λₜ₂₄] + // ↑─────↑ ↑─────↑ ↑─────────────────────↑ ↑─────────────────────↑ + // slacks normals tangential (contact 1) tangential (contact 2) + // + // Anitescu Model (n_λ = n_tangential): + // ------------------------------------- + // λ = [λc₁₁, λc₁₂, ..., λc₁ₘ₁, <- Combined forces for contact 1 + // λc₂₁, λc₂₂, ..., λc₂ₘ₂, <- Combined forces for contact 2 + // ... + // λcₙ₁, λcₙ₂, ..., λcₙₘₙ] <- Combined forces for contact n + // + // where each λcᵢⱼ represents a combined normal-tangential force: + // λcᵢⱼ acts along the direction: μᵢ * tangent_directionᵢⱼ + + // normal_directionᵢ + // + // Example with 2 contacts, 2 friction directions each: + // λ = [λc₁₁, λc₁₂, λc₁₃, λc₁₄, λc₂₁, λc₂₂, λc₂₃, λc₂₄] + // ↑─────────────────────↑ ↑─────────────────────↑ + // combined (contact 1) combined (contact 2) + // + // Frictionless Spring Model (n_λ = n_contacts): + // ---------------------------------------------- + // λ = [λₙ₁, λₙ₂, ..., λₙₙ] <- Normal forces only + // + // =========================================================================== + std::vector contact_descriptions; if (contact_model_ == ContactModel::kFrictionlessSpring) contact_descriptions.insert(contact_descriptions.end(), normal_contact_descriptions.begin(), normal_contact_descriptions.end()); else if (contact_model_ == ContactModel::kStewartAndTrinkle) { + // Stack as: [slack variables, normal forces, tangential forces] for (int i = 0; i < n_contacts_; i++) contact_descriptions.push_back( LCSContactDescription::CreateSlackVariableDescription()); @@ -554,9 +606,13 @@ std::vector LCSFactory::GetContactDescriptions() { tangential_contact_descriptions.begin(), tangential_contact_descriptions.end()); } else if (contact_model_ == ContactModel::kAnitescu) { + // Start with tangential basis vectors contact_descriptions.insert(contact_descriptions.end(), tangential_contact_descriptions.begin(), tangential_contact_descriptions.end()); + + // For each contact, modify tangential force bases to include normal + // component resulting in combined friction cone approximation vectors for (int normal_index = 0; normal_index < n_contacts_; normal_index++) { // Jt_row_sizes_ gives number of friction directions per contact for (int i = 0; i < Jt_row_sizes_(normal_index); i++) { @@ -564,6 +620,10 @@ std::vector LCSFactory::GetContactDescriptions() { DRAKE_ASSERT( contact_descriptions.at(tangential_index).witness_point_A == normal_contact_descriptions.at(normal_index).witness_point_A); + + // Combine tangential and normal force directions: + // force_basis = μ * tangent_direction + normal_direction + // This creates the edges of the linearized friction cone contact_descriptions.at(tangential_index).force_basis = mu_[normal_index] * contact_descriptions.at(tangential_index).force_basis + diff --git a/multibody/lcs_factory.h b/multibody/lcs_factory.h index f9ea0be..44552fc 100644 --- a/multibody/lcs_factory.h +++ b/multibody/lcs_factory.h @@ -7,6 +7,7 @@ #include #include "core/lcs.h" +#include "multibody/geom_geom_collider.h" #include "multibody/lcs_factory_options.h" #include "drake/common/sorted_pair.h" @@ -336,6 +337,7 @@ class LCSFactory { LCSFactoryOptions options_; int n_contacts_; ///< Number of contact points. + std::vector> geom_geom_colliders_per_contact_; std::vector n_friction_directions_per_contact_; ///< Number of friction directions. std::vector> diff --git a/multibody/test/multibody_test.cc b/multibody/test/multibody_test.cc index 3aeb479..02fb7cd 100644 --- a/multibody/test/multibody_test.cc +++ b/multibody/test/multibody_test.cc @@ -329,7 +329,7 @@ TEST_P(LCSFactoryParameterizedPivotingTest, CheckContactDescriptionSizes) { case ContactModel::kStewartAndTrinkle: // Normal + tangential directions for all contacts EXPECT_EQ(contact_descriptions.size(), - 2*n_contacts + n_tangential_directions); + 2 * n_contacts + n_tangential_directions); break; case ContactModel::kFrictionlessSpring: // Normal directions only diff --git a/systems/lcmt_generators/c3_trajectory_generator.cc b/systems/lcmt_generators/c3_trajectory_generator.cc index a14ecc1..eedd972 100644 --- a/systems/lcmt_generators/c3_trajectory_generator.cc +++ b/systems/lcmt_generators/c3_trajectory_generator.cc @@ -19,7 +19,6 @@ C3TrajectoryGenerator::C3TrajectoryGenerator(C3TrajectoryGeneratorConfig config) this->DeclareAbstractOutputPort( "lcmt_c3_trajectory", &C3TrajectoryGenerator::GenerateTrajectory) .get_index(); - } void C3TrajectoryGenerator::GenerateTrajectory( diff --git a/systems/lcmt_generators/c3_trajectory_generator_config.h b/systems/lcmt_generators/c3_trajectory_generator_config.h index a2f2243..8127e4d 100644 --- a/systems/lcmt_generators/c3_trajectory_generator_config.h +++ b/systems/lcmt_generators/c3_trajectory_generator_config.h @@ -8,18 +8,67 @@ namespace systems { namespace lcmt_generators { struct TrajectoryDescription { + // =========================================================================== + // Index Range for Extracting Vector Slices + // =========================================================================== + // + // The index_range struct defines a contiguous slice of a vector (state, + // input, or force) to extract and publish as a separate trajectory. + // + // Purpose: + // -------- + // C3 systems typically have large state/input/force vectors. Often, we only + // want to visualize or log a subset of these variables. For example: + // - Extract positions [0:6] from a 14-dimensional state vector + // - Extract a single actuator force [3] from a 10-dimensional input vector + // - Extract contact forces [0:3] from a 20-dimensional force vector + // + // Usage: + // ------ + // Given a vector x of size n, index_range {start, end} extracts: + // x[start], x[start+1], ..., x[end-1] + // + // This follows standard C++ iterator conventions where: + // - start: inclusive (first element to include) + // - end: exclusive (one past the last element to include) + // + // Examples: + // --------- + // 1. Extract full 7-DOF robot joint positions: + // {start: 0, end: 7} -> extracts indices [0, 1, 2, 3, 4, 5, 6] + // + // 2. Extract x, y, z position from state: + // {start: 0, end: 3} -> extracts indices [0, 1, 2] + // + // 3. Extract single element: + // {start: 5, end: 6} -> extracts index [5] + // + // Multiple Ranges: + // ---------------- + // The 'indices' vector allows specifying multiple non-contiguous ranges + // that will be concatenated into a single trajectory. For example: + // indices: [{start: 0, end: 3}, {start: 7, end: 10}] + // -> extracts [0, 1, 2, 7, 8, 9] and concatenates them + // + // This is useful when you want to publish related but non-contiguous + // variables together, such as positions from multiple joints that aren't + // adjacent in the state vector. + // =========================================================================== struct index_range { - int start; // Start index of the range - int end; // End index of the range + int start; // Start index (inclusive) - first element to extract + int end; // End index (exclusive) - one past the last element to extract + template void Serialize(Archive* a) { a->Visit(DRAKE_NVP(start)); a->Visit(DRAKE_NVP(end)); } }; - std::string trajectory_name; // Name of the trajectory - std::string variable_type; // Type of C3 variable ["state", "input", "force"] - std::vector indices; + + std::string trajectory_name; // Name of the trajectory for LCM publishing + std::string variable_type; // Type of C3 variable: "state", "input", or "force" + std::vector indices; // List of index ranges to extract and concatenate + template void Serialize(Archive* a) { a->Visit(DRAKE_NVP(trajectory_name)); @@ -34,7 +83,8 @@ struct TrajectoryDescription { struct C3TrajectoryGeneratorConfig { std::vector - trajectories; // List of trajectories to generate + trajectories; // List of trajectories to generate and publish via LCM + template void Serialize(Archive* a) { a->Visit(DRAKE_NVP(trajectories)); From 1ab4ccd95d845d9b2bc039829a2530bb776975ef Mon Sep 17 00:00:00 2001 From: Meow404 Date: Sun, 22 Feb 2026 21:30:42 -0500 Subject: [PATCH 17/17] feat: add contact evaluators for lcs factory --- bindings/pyc3/c3_multibody_py.cc | 10 +- .../c3Plus_controller_cartpole_options.yaml | 2 - .../c3_controller_cartpole_options.yaml | 2 - multibody/BUILD.bazel | 35 +- multibody/contact_evaluator.cc | 12 + multibody/contact_evaluator.h | 157 ++++++++ multibody/lcs_factory.cc | 359 ++++++++++-------- multibody/lcs_factory.h | 42 +- multibody/lcs_factory_options.h | 204 ++++++++-- multibody/test/multibody_test.cc | 92 ++--- .../lcs_factory_pivoting_options.yaml | 2 +- systems/c3_controller.cc | 2 +- systems/c3_controller_options.h | 13 - .../c3_trajectory_generator_config.h | 6 +- systems/lcs_factory_system.cc | 2 +- .../test/resources/quaternion_cost_test.yaml | 3 +- systems/test/systems_test.cc | 5 +- 17 files changed, 654 insertions(+), 294 deletions(-) create mode 100644 multibody/contact_evaluator.cc create mode 100644 multibody/contact_evaluator.h diff --git a/bindings/pyc3/c3_multibody_py.cc b/bindings/pyc3/c3_multibody_py.cc index 4d366c6..c4c5ac9 100644 --- a/bindings/pyc3/c3_multibody_py.cc +++ b/bindings/pyc3/c3_multibody_py.cc @@ -74,10 +74,12 @@ PYBIND11_MODULE(multibody, m) { &c3::multibody::LCSFactory::GetNumContactVariables), py::arg("contact_model"), py::arg("num_contacts"), py::arg("num_friction_directions")) - .def_static("GetNumContactVariables", - py::overload_cast( - &c3::multibody::LCSFactory::GetNumContactVariables), - py::arg("options")); + .def_static( + "GetNumContactVariables", + py::overload_cast&, + const c3::LCSFactoryOptions&>( + &c3::multibody::LCSFactory::GetNumContactVariables), + py::arg("plant"), py::arg("options")); py::class_(m, "ContactPairConfig") .def(py::init<>()) diff --git a/examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml b/examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml index e25e758..19832e8 100644 --- a/examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml +++ b/examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml @@ -11,10 +11,8 @@ lcs_factory_options: #options are 'stewart_and_trinkle' or 'anitescu' # contact_model : 'stewart_and_trinkle' contact_model: "frictionless_spring" - num_friction_directions: 0 num_contacts: 2 spring_stiffness: 100 - mu: [0, 0] N: 5 dt: 0.01 planar_normal_direction: [0, 1, 0] diff --git a/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml b/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml index 7954fd4..185a02f 100644 --- a/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml +++ b/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml @@ -15,10 +15,8 @@ lcs_factory_options: #options are 'stewart_and_trinkle' or 'anitescu' # contact_model : 'stewart_and_trinkle' contact_model: "frictionless_spring" - num_friction_directions: 0 num_contacts: 2 spring_stiffness: 100 - mu: [ 0, 0 ] N: 5 dt: 0.01 planar_normal_direction: [0, 1, 0] diff --git a/multibody/BUILD.bazel b/multibody/BUILD.bazel index f446985..1eb1472 100644 --- a/multibody/BUILD.bazel +++ b/multibody/BUILD.bazel @@ -12,22 +12,45 @@ cc_library( ], ) +cc_library( + name = "geom_geom_collider", + srcs = ["geom_geom_collider.cc"], + hdrs = ["geom_geom_collider.h"], + deps = [ + "@drake//:drake_shared_library", + ], + visibility = ["//visibility:public"], +) + +cc_library( + name = "multibody_utils", + srcs = ["multibody_utils.cc"], + hdrs = ["multibody_utils.h"], + deps = [ + "@drake//:drake_shared_library", + ], + visibility = ["//visibility:public"], +) + cc_library( name = "lcs_factory", - srcs = ["lcs_factory.cc", - "geom_geom_collider.cc", - "multibody_utils.cc"], + srcs = [ + "lcs_factory.cc", + "contact_evaluator.cc" + ], hdrs = [ - "lcs_factory.h", - "geom_geom_collider.h", - "multibody_utils.h", + "lcs_factory.h", + "contact_evaluator.h" ], deps = [ + ":geom_geom_collider", + ":multibody_utils", ":options", '//core:lcs', '//core:options', "@drake//:drake_shared_library", ], + visibility = ["//visibility:public"], ) cc_library( diff --git a/multibody/contact_evaluator.cc b/multibody/contact_evaluator.cc new file mode 100644 index 0000000..4649a7a --- /dev/null +++ b/multibody/contact_evaluator.cc @@ -0,0 +1,12 @@ +#include "multibody/contact_evaluator.h" + +namespace c3 { +namespace multibody { + +// Explicit template instantiations for double precision +template class ContactEvaluator; +template class PolytopeContactEvaluator; +template class PlanarContactEvaluator; + +} // namespace multibody +} // namespace c3 \ No newline at end of file diff --git a/multibody/contact_evaluator.h b/multibody/contact_evaluator.h new file mode 100644 index 0000000..602513d --- /dev/null +++ b/multibody/contact_evaluator.h @@ -0,0 +1,157 @@ +#pragma once + +#include + +#include + +#include "geom_geom_collider.h" + +#include "drake/common/sorted_pair.h" +#include "drake/geometry/geometry_ids.h" +#include "drake/multibody/plant/multibody_plant.h" + +namespace c3 { +namespace multibody { + +/** + * @brief Abstract base class for evaluating contact constraints. + * + * ContactEvaluator provides an interface for computing signed distance and + * contact Jacobians for different friction models (planar vs polytope). + */ +template +class ContactEvaluator { + public: + ContactEvaluator( + const drake::multibody::MultibodyPlant& plant, + const drake::SortedPair& contact_pair) + : collider_(plant, contact_pair) {} + + virtual ~ContactEvaluator() = default; + + /** + * @brief Evaluates the signed distance and contact Jacobian. + * + * @param context The context for the MultibodyPlant. + * @param wrt Whether to compute Jacobian w.r.t. q or v. + * @return Pair of (signed_distance, contact_jacobian) + */ + virtual std::pair> Eval( + const drake::systems::Context& context, + drake::multibody::JacobianWrtVariable wrt = + drake::multibody::JacobianWrtVariable::kV) const = 0; + + /** + * @brief Returns the number of friction directions for this contact model. + */ + virtual int GetNumFrictionDirections() const = 0; + + /** + * @brief Computes the force basis for the contact constraint. + * + * For polytope contacts, returns the friction cone approximation basis. + * For planar contacts, returns the planar friction basis. + * + * @param context The context for the MultibodyPlant. + * @return Matrix where each row is a force direction in world frame. + */ + virtual Eigen::Matrix CalcForceBasis( + const drake::systems::Context& context) const = 0; + + /** + * @brief Computes witness points (closest points on each geometry). + * + * @param context The context for the MultibodyPlant. + * @return Pair of witness points in world frame. + */ + std::pair CalcWitnessPoints( + const drake::systems::Context& context) const { + return collider_.CalcWitnessPoints(context); + } + + protected: + mutable GeomGeomCollider collider_; +}; + +/** + * @brief Contact evaluator for polytope friction cone approximation. + */ +template +class PolytopeContactEvaluator : public ContactEvaluator { + public: + PolytopeContactEvaluator( + const drake::multibody::MultibodyPlant& plant, + const drake::SortedPair& contact_pair, + int num_friction_directions) + : ContactEvaluator(plant, contact_pair), + num_friction_directions_(num_friction_directions) { + DRAKE_DEMAND(num_friction_directions_ > 1); + } + + std::pair> Eval( + const drake::systems::Context& context, + drake::multibody::JacobianWrtVariable wrt = + drake::multibody::JacobianWrtVariable::kV) const override { + return this->collider_.EvalPolytope(context, num_friction_directions_, wrt); + } + + int GetNumFrictionDirections() const override { + return num_friction_directions_; + } + + Eigen::Matrix CalcForceBasis( + const drake::systems::Context& context) const override { + return this->collider_.CalcForceBasisInWorldFrame(context, + num_friction_directions_); + } + + private: + int num_friction_directions_; +}; + +/** + * @brief Contact evaluator for planar (2D) friction. + */ +template +class PlanarContactEvaluator : public ContactEvaluator { + public: + PlanarContactEvaluator( + const drake::multibody::MultibodyPlant& plant, + const drake::SortedPair& contact_pair, + const Eigen::Vector3d& planar_normal) + : ContactEvaluator(plant, contact_pair), + planar_normal_(planar_normal) { + // Validate unit vector + DRAKE_DEMAND(std::abs(planar_normal_.norm() - 1.0) < 1e-6); + } + + std::pair> Eval( + const drake::systems::Context& context, + drake::multibody::JacobianWrtVariable wrt = + drake::multibody::JacobianWrtVariable::kV) const override { + return this->collider_.EvalPlanar(context, planar_normal_, wrt); + } + + int GetNumFrictionDirections() const override { + return 1; // Planar contact has 1 friction direction + } + + Eigen::Matrix CalcForceBasis( + const drake::systems::Context& context) const override { + // For planar contact, pass num_friction_directions = 0 to trigger planar + // mode + return this->collider_.CalcForceBasisInWorldFrame(context, 0, + planar_normal_); + } + + private: + Eigen::Vector3d planar_normal_; +}; + +} // namespace multibody +} // namespace c3 + +// Explicitly instantiate for double +extern template class c3::multibody::ContactEvaluator; +extern template class c3::multibody::PolytopeContactEvaluator; +extern template class c3::multibody::PlanarContactEvaluator; \ No newline at end of file diff --git a/multibody/lcs_factory.cc b/multibody/lcs_factory.cc index 63684af..c597ab8 100644 --- a/multibody/lcs_factory.cc +++ b/multibody/lcs_factory.cc @@ -28,6 +28,94 @@ using Eigen::VectorXd; namespace c3 { namespace multibody { +// Helper function to expand contact_pair_configs into per-contact arrays +struct ExpandedContactConfig { + std::vector> contact_geoms; + std::vector num_friction_directions_per_contact; + std::vector mu; + std::vector>> + planar_normal_direction_per_contact; + int num_contacts() const { return contact_geoms.size(); } +}; + +ExpandedContactConfig ExpandContactPairConfigs( + const drake::multibody::MultibodyPlant& plant, + const drake::systems::Context& context, + const std::vector& configs) { + ExpandedContactConfig result; + + for (const auto& pair : configs) { + std::vector body_A_collision_geoms = + plant.GetCollisionGeometriesForBody(plant.GetBodyByName(pair.body_A)); + std::vector body_B_collision_geoms = + plant.GetCollisionGeometriesForBody(plant.GetBodyByName(pair.body_B)); + + // Resolve indices: use all geometries if not provided or if empty + std::vector body_A_indices = + pair.body_A_collision_geom_indices.value_or(std::vector{}); + std::vector body_B_indices = + pair.body_B_collision_geom_indices.value_or(std::vector{}); + + if (body_A_indices.empty()) { + // Use all geometries for body A + body_A_indices.resize(body_A_collision_geoms.size()); + std::iota(body_A_indices.begin(), body_A_indices.end(), 0); + } else { + // Validate provided indices for body A + for (int idx : body_A_indices) { + DRAKE_DEMAND(idx >= 0 && + idx < static_cast(body_A_collision_geoms.size())); + } + } + + if (body_B_indices.empty()) { + // Use all geometries for body B + body_B_indices.resize(body_B_collision_geoms.size()); + std::iota(body_B_indices.begin(), body_B_indices.end(), 0); + } else { + // Validate provided indices for body B + for (int idx : body_B_indices) { + DRAKE_DEMAND(idx >= 0 && + idx < static_cast(body_B_collision_geoms.size())); + } + } + + // Create contact pairs for all combinations and replicate properties + std::vector> + AB_geometric_pairs; + for (int i : body_A_indices) { + for (int j : body_B_indices) { + AB_geometric_pairs.emplace_back(body_A_collision_geoms[i], + body_B_collision_geoms[j]); + } + } + + // If no limit on the number of contact pairs is specified, or if the limit + // is equal to the total number of pairs, return all pairs. + if (pair.num_active_contact_pairs.has_value() && + pair.num_active_contact_pairs.value() != AB_geometric_pairs.size()) { + AB_geometric_pairs = LCSFactory::GetNClosestContactPairs( + plant, context, AB_geometric_pairs, + pair.num_active_contact_pairs.value()); + } + + result.contact_geoms.insert(result.contact_geoms.end(), + AB_geometric_pairs.begin(), + AB_geometric_pairs.end()); + int n_active_contacts = AB_geometric_pairs.size(); + result.mu.insert(result.mu.end(), n_active_contacts, pair.mu); + result.num_friction_directions_per_contact.insert( + result.num_friction_directions_per_contact.end(), n_active_contacts, + pair.num_friction_directions); + result.planar_normal_direction_per_contact.insert( + result.planar_normal_direction_per_contact.end(), n_active_contacts, + pair.planar_normal_direction.value_or(std::array{})); + } + + return result; +} + +// Constructor 1: Takes only LCSFactoryOptions with contact_pair_configs LCSFactory::LCSFactory( const drake::multibody::MultibodyPlant& plant, drake::systems::Context& context, @@ -48,69 +136,94 @@ LCSFactory::LCSFactory( dt_(options.dt) { DRAKE_DEMAND(options.contact_pair_configs.has_value()); - n_contacts_ = 0; + // Expand contact_pair_configs into per-contact arrays using plant + auto expanded = ExpandContactPairConfigs( + plant, context, options.contact_pair_configs.value()); - mu_.clear(); - n_friction_directions_per_contact_.clear(); - contact_pairs_.clear(); - planar_normal_direction_per_contact_.clear(); + contact_pairs_ = expanded.contact_geoms; + n_contacts_ = contact_pairs_.size(); + mu_ = expanded.mu; - // Process each contact pair configuration to populate contact pairs, friction - // coefficients, and friction directions. This allows for different friction - // properties for different pairs of bodies. - for (auto& pair : options.contact_pair_configs.value()) { - std::vector> collision_geom_pairs; - std::vector body_A_collision_geoms = - plant_.GetCollisionGeometriesForBody(plant_.GetBodyByName(pair.body_A)); - std::vector body_B_collision_geoms = - plant_.GetCollisionGeometriesForBody(plant_.GetBodyByName(pair.body_B)); - - // If no specific indices are provided, use all collision geometries for - // each body. - if (pair.body_A_collision_geom_indices.empty()) { - pair.body_A_collision_geom_indices.resize(body_A_collision_geoms.size()); - std::iota(pair.body_A_collision_geom_indices.begin(), - pair.body_A_collision_geom_indices.end(), 0); - } - if (pair.body_B_collision_geom_indices.empty()) { - pair.body_B_collision_geom_indices.resize(body_B_collision_geoms.size()); - std::iota(pair.body_B_collision_geom_indices.begin(), - pair.body_B_collision_geom_indices.end(), 0); - } + // Create contact evaluators + InitializeContactEvaluators(expanded.num_friction_directions_per_contact, + expanded.planar_normal_direction_per_contact); +} - for (int i : pair.body_A_collision_geom_indices) { - for (int j : pair.body_B_collision_geom_indices) { - collision_geom_pairs.emplace_back(SortedPair( - body_A_collision_geoms[i], body_B_collision_geoms[j])); - } - } +// Constructor 2: Takes explicit contact_geoms with per-contact arrays +LCSFactory::LCSFactory( + const MultibodyPlant& plant, Context& context, + const MultibodyPlant& plant_ad, Context& context_ad, + const std::vector>& + contact_geoms, + const LCSFactoryOptions& options) + : plant_(plant), + context_(context), + plant_ad_(plant_ad), + context_ad_(context_ad), + contact_pairs_(contact_geoms), + options_(options), + n_contacts_(contact_geoms.size()), + contact_model_(GetContactModelMap().at(options_.contact_model)), + n_q_(plant_.num_positions()), + n_v_(plant_.num_velocities()), + n_x_(n_q_ + n_v_), + n_u_(plant_.num_actuators()), + frictionless_(contact_model_ == ContactModel::kFrictionlessSpring), + dt_(options.dt) { + // Resolve configuration using options resolver methods + std::vector friction_dirs = options_.ResolveNumFrictionDirections(); + mu_ = options_.ResolveMu(); + std::vector>> planar_normals = + options_.ResolvePlanarNormals(); + + DRAKE_DEMAND(friction_dirs.size() == (size_t)n_contacts_); + DRAKE_DEMAND(mu_.size() == (size_t)n_contacts_); + DRAKE_DEMAND(planar_normals.size() == (size_t)n_contacts_); + + // Create contact evaluators + InitializeContactEvaluators(friction_dirs, planar_normals); +} - // If no limit on the number of contact pairs is specified, or if the limit - // is equal to the total number of pairs, return all pairs. - if (pair.num_active_contact_pairs.has_value() && - pair.num_active_contact_pairs.value() != collision_geom_pairs.size()) - collision_geom_pairs = - GetNClosestContactPairs(plant_, context_, collision_geom_pairs, - pair.num_active_contact_pairs.value()); - - contact_pairs_.insert(contact_pairs_.end(), collision_geom_pairs.begin(), - collision_geom_pairs.end()); - mu_.insert(mu_.end(), collision_geom_pairs.size(), pair.mu); - n_friction_directions_per_contact_.insert( - n_friction_directions_per_contact_.end(), collision_geom_pairs.size(), - pair.num_friction_directions); - planar_normal_direction_per_contact_.insert( - planar_normal_direction_per_contact_.end(), collision_geom_pairs.size(), - pair.planar_normal_direction.value_or(std::array{})); +// Common initialization logic for contact evaluators +void LCSFactory::InitializeContactEvaluators( + const std::vector& friction_dirs, + const std::vector>>& planar_normals) { + DRAKE_DEMAND(friction_dirs.size() == (size_t)n_contacts_); + DRAKE_DEMAND(planar_normals.size() == (size_t)n_contacts_); + + contact_evaluators_.clear(); + contact_evaluators_.reserve(n_contacts_); - n_contacts_ += collision_geom_pairs.size(); + for (int i = 0; i < n_contacts_; ++i) { + if (frictionless_ || friction_dirs[i] == 1) { + // Planar contact + DRAKE_DEMAND(planar_normals[i].has_value()); + Eigen::Vector3d planar_normal = + Eigen::Map(planar_normals[i].value().data()); + contact_evaluators_.push_back( + std::make_unique>( + plant_, contact_pairs_[i], planar_normal)); + } else { + // Polytope contact + contact_evaluators_.push_back( + std::make_unique>( + plant_, contact_pairs_[i], friction_dirs[i])); + } + } + + // Calculate n_lambda_ and Jt_row_sizes_ from evaluators + std::vector n_friction_directions_from_evaluators; + for (const auto& evaluator : contact_evaluators_) { + n_friction_directions_from_evaluators.push_back( + evaluator->GetNumFrictionDirections()); } n_lambda_ = multibody::LCSFactory::GetNumContactVariables( - contact_model_, n_contacts_, n_friction_directions_per_contact_); + contact_model_, n_contacts_, n_friction_directions_from_evaluators); + Jt_row_sizes_ = 2 * Eigen::Map( - n_friction_directions_per_contact_.data(), - n_friction_directions_per_contact_.size()); + n_friction_directions_from_evaluators.data(), + n_friction_directions_from_evaluators.size()); } std::vector> LCSFactory::GetNClosestContactPairs( @@ -147,61 +260,6 @@ std::vector> LCSFactory::GetNClosestContactPairs( return active_contact_pairs; } -LCSFactory::LCSFactory( - const MultibodyPlant& plant, Context& context, - const MultibodyPlant& plant_ad, Context& context_ad, - const std::vector>& - contact_geoms, - const LCSFactoryOptions& options) - : plant_(plant), - context_(context), - plant_ad_(plant_ad), - context_ad_(context_ad), - contact_pairs_(contact_geoms), - options_(options), - n_contacts_(contact_geoms.size()), - n_friction_directions_per_contact_( - options_.num_friction_directions_per_contact.value()), - contact_model_(GetContactModelMap().at(options_.contact_model)), - n_q_(plant_.num_positions()), - n_v_(plant_.num_velocities()), - n_x_(n_q_ + n_v_), - n_lambda_(multibody::LCSFactory::GetNumContactVariables( - contact_model_, n_contacts_, n_friction_directions_per_contact_)), - n_u_(plant_.num_actuators()), - mu_(options.mu.value()), - frictionless_(contact_model_ == ContactModel::kFrictionlessSpring), - dt_(options.dt) { - // Handle planar normal directions for contacts with 1 friction direction - // Initialize all with empty default values - planar_normal_direction_per_contact_.resize(n_contacts_, {}); - - for (int i = 0; i < n_contacts_; ++i) { - if (n_friction_directions_per_contact_[i] == 1) { - // Planar contact - assign the appropriate normal - if (options_.planar_normal_direction.has_value()) { - // Single planar normal provided - use for all planar contacts - DRAKE_DEMAND(options_.planar_normal_direction.value().size() == 3); - planar_normal_direction_per_contact_[i] = - options_.planar_normal_direction.value(); - } else { - // Per-contact planar normals provided - DRAKE_DEMAND(options_.planar_normal_direction_per_contact.has_value()); - DRAKE_DEMAND( - options_.planar_normal_direction_per_contact.value().size() == - (size_t)n_contacts_); - planar_normal_direction_per_contact_[i] = - options_.planar_normal_direction_per_contact.value()[i]; - } - } - // Non-planar contacts keep the default empty value {} - geom_geom_colliders_per_contact_.emplace_back(plant_, contact_pairs_[i]); - } - Jt_row_sizes_ = 2 * Eigen::Map( - n_friction_directions_per_contact_.data(), - n_friction_directions_per_contact_.size()); -} - void LCSFactory::ComputeContactJacobian(VectorXd& phi, MatrixXd& Jn, MatrixXd& Jt) { phi.resize(n_contacts_); // Signed distance values for contacts @@ -212,15 +270,8 @@ void LCSFactory::ComputeContactJacobian(VectorXd& phi, MatrixXd& Jn, double phi_i; MatrixX J_i; for (int i = 0; i < n_contacts_; i++) { - if (frictionless_ || n_friction_directions_per_contact_[i] == 1) { - Eigen::Vector3d planar_normal = - Eigen::Map( - planar_normal_direction_per_contact_[i].data()); - std::tie(phi_i, J_i) = geom_geom_colliders_per_contact_[i].EvalPlanar( - context_, planar_normal); - } else - std::tie(phi_i, J_i) = geom_geom_colliders_per_contact_[i].EvalPolytope( - context_, n_friction_directions_per_contact_[i]); + // Use polymorphic Eval method + auto [phi_i, J_i] = contact_evaluators_[i]->Eval(context_); // Signed distance value for contact i phi(i) = phi_i; @@ -360,7 +411,8 @@ LCS LCSFactory::GenerateLCS() { } else if (contact_model_ == ContactModel::kFrictionlessSpring) { // Frictionless spring FormulateFrictionlessSpringContactDynamics( - phi, Jn, qdotNv, options_.spring_stiffness, M, D, E, F, H, c); + phi, Jn, qdotNv, options_.spring_stiffness.value_or(1.0), M, D, E, F, H, + c); } else { throw std::out_of_range("Unsupported contact model."); } @@ -513,29 +565,20 @@ std::vector LCSFactory::GetContactDescriptions() { std::vector tangential_contact_descriptions; for (int i = 0; i < n_contacts_; i++) { - auto [p_WCa, p_WCb] = - geom_geom_colliders_per_contact_[i].CalcWitnessPoints(context_); - Eigen::Vector3d planar_normal; - // For frictionless or single friction direction contacts, the force basis - // is just the contact normal - if (n_friction_directions_per_contact_[i] == 1) - planar_normal = Eigen::Map( - planar_normal_direction_per_contact_[i].data()); - auto force_basis = - geom_geom_colliders_per_contact_[i].CalcForceBasisInWorldFrame( - context_, n_friction_directions_per_contact_[i], planar_normal); + auto [p_WCa, p_WCb] = contact_evaluators_[i]->CalcWitnessPoints(context_); + auto force_basis = contact_evaluators_[i]->CalcForceBasis(context_); 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 + + if (j == 0) { normal_contact_descriptions.push_back(contact_description); - else - // Tangential contact + } else { tangential_contact_descriptions.push_back(contact_description); + } } } @@ -772,35 +815,33 @@ int LCSFactory::GetNumContactVariables(ContactModel contact_model, num_friction_directions_per_contact); } -int LCSFactory::GetNumContactVariables(const LCSFactoryOptions options) { +int LCSFactory::GetNumContactVariables( + const drake::multibody::MultibodyPlant& plant, + const LCSFactoryOptions& options) { multibody::ContactModel contact_model = GetContactModelMap().at(options.contact_model); - std::vector n_friction_directions_per_contact; - if (options.num_friction_directions_per_contact.has_value()) { + + int n_contacts = options.ResolveNumContacts(); + std::vector n_friction_directions_per_contact = + options.ResolveNumFrictionDirections(); + if (options.contact_pair_configs.has_value()) { + // If contact pair configs are provided, they take precedence over the + // options for number of contacts and friction directions. We can expand the + // contact pair configs to get the actual number of contacts and friction + // directions per contact. + + // Use default context since we only need the geometry query results to + // expand the contact pair configs, and the geometry query results do not + // depend on the state of the plant. + auto context = plant.CreateDefaultContext(); + auto expanded = ExpandContactPairConfigs( + plant, *context, options.contact_pair_configs.value()); + n_contacts = expanded.num_contacts(); n_friction_directions_per_contact = - options.num_friction_directions_per_contact.value(); - } else if (options.contact_pair_configs.has_value()) { - for (auto& pair_config : options.contact_pair_configs.value()) { - std::vector n_friction_directions_for_contact( - pair_config.body_A_collision_geom_indices.size() * - pair_config.body_B_collision_geom_indices.size(), - pair_config.num_friction_directions); - n_friction_directions_per_contact.insert( - n_friction_directions_per_contact.end(), - n_friction_directions_for_contact.begin(), - n_friction_directions_for_contact.end()); - } - } else if (options.num_friction_directions.has_value()) { - n_friction_directions_per_contact = std::vector( - options.num_contacts, options.num_friction_directions.value()); - } else { - throw std::runtime_error( - "LCSFactoryOptions must specify num_friction_directions_per_contact, " - "num_friction_directions, or contact_pair_configs."); + expanded.num_friction_directions_per_contact; } - DRAKE_DEMAND(n_friction_directions_per_contact.size() == - (size_t)options.num_contacts); - return GetNumContactVariables(contact_model, options.num_contacts, + + return GetNumContactVariables(contact_model, n_contacts, n_friction_directions_per_contact); } diff --git a/multibody/lcs_factory.h b/multibody/lcs_factory.h index 44552fc..00b4064 100644 --- a/multibody/lcs_factory.h +++ b/multibody/lcs_factory.h @@ -7,6 +7,7 @@ #include #include "core/lcs.h" +#include "multibody/contact_evaluator.h" #include "multibody/geom_geom_collider.h" #include "multibody/lcs_factory_options.h" @@ -223,13 +224,32 @@ class LCSFactory { * This is the preferred overload as it encapsulates all contact model and * friction configuration in a single options object. * + * @param plant The MultibodyPlant to analyze for contact information. * @param options The LCS options specifying contact model and friction * properties. * @return int The number of contact variables. */ - static int GetNumContactVariables(const LCSFactoryOptions options); + static int GetNumContactVariables( + const drake::multibody::MultibodyPlant& plant, + const LCSFactoryOptions& options); private: + /** + * @brief Initializes contact evaluators for all contact pairs. + * + * This method creates and configures ContactEvaluator objects for each + * contact pair, setting up the friction directions and planar normal + * constraints as specified. + * + * @param friction_dirs Vector specifying the number of friction directions + * for each contact. + * @param planar_normals Vector of optional planar normal vectors for each + * contact. If specified, constrains the contact normal to lie in a plane. + */ + void InitializeContactEvaluators( + const std::vector& friction_dirs, + const std::vector>>& planar_normals); + /** * @brief Formulates the contact dynamics for the frictionless spring contact * model. @@ -337,19 +357,13 @@ class LCSFactory { LCSFactoryOptions options_; int n_contacts_; ///< Number of contact points. - std::vector> geom_geom_colliders_per_contact_; - std::vector - n_friction_directions_per_contact_; ///< Number of friction directions. - std::vector> - planar_normal_direction_per_contact_; ///< Optional normal vector for - ///< planar contact for each - ///< contact. - ContactModel contact_model_; ///< The contact model being used. - int n_q_; ///< Number of configuration variables. - int n_v_; ///< Number of velocity variables. - int n_x_; ///< Number of state variables. - int n_lambda_; ///< Number of contact force variables. - int n_u_; ///< Number of input variables. + std::vector>> contact_evaluators_; + ContactModel contact_model_; ///< The contact model being used. + int n_q_; ///< Number of configuration variables. + int n_v_; ///< Number of velocity variables. + int n_x_; ///< Number of state variables. + int n_lambda_; ///< Number of contact force variables. + int n_u_; ///< Number of input variables. std::vector mu_; ///< Vector of friction coefficients. bool frictionless_; ///< Flag indicating frictionless contacts. diff --git a/multibody/lcs_factory_options.h b/multibody/lcs_factory_options.h index b234e02..3cb8114 100644 --- a/multibody/lcs_factory_options.h +++ b/multibody/lcs_factory_options.h @@ -10,12 +10,12 @@ namespace c3 { struct ContactPairConfig { std::string body_A; std::string body_B; - std::vector body_A_collision_geom_indices; - std::vector body_B_collision_geom_indices; + std::optional> body_A_collision_geom_indices; + std::optional> body_B_collision_geom_indices; int num_friction_directions; + double mu; // friction coefficient std::optional> planar_normal_direction; // optional normal vector for planar contact - double mu; // friction coefficient std::optional num_active_contact_pairs; // Number of geometry-geometry contact pairs to // consider (e.g., if body_A has 10 geoms and @@ -34,65 +34,187 @@ struct ContactPairConfig { a->Visit(DRAKE_NVP(planar_normal_direction)); a->Visit(DRAKE_NVP(mu)); a->Visit(DRAKE_NVP(num_active_contact_pairs)); + + ValidateContactPairConfig(); + } + + private: + void ValidateContactPairConfig() const { + DRAKE_DEMAND(!body_A.empty()); + DRAKE_DEMAND(!body_B.empty()); + if (body_A_collision_geom_indices.has_value()) { + for (int idx : body_A_collision_geom_indices.value()) { + DRAKE_DEMAND(idx >= 0); + } + } + if (body_B_collision_geom_indices.has_value()) { + for (int idx : body_B_collision_geom_indices.value()) { + DRAKE_DEMAND(idx >= 0); + } + } + DRAKE_DEMAND(num_friction_directions >= 0); + DRAKE_DEMAND(mu >= 0); // Friction coefficient must be non-negative + if (num_friction_directions == 1) { + DRAKE_DEMAND( + planar_normal_direction.has_value()); // For planar contact, the + // normal direction must be + // specified + const auto& normal = planar_normal_direction.value(); + double norm = std::sqrt(normal[0] * normal[0] + normal[1] * normal[1] + + normal[2] * normal[2]); + DRAKE_DEMAND(std::abs(norm - 1) < 1e-6); // Ensure it's a unit vector + } + + if (num_active_contact_pairs.has_value()) { + DRAKE_DEMAND(num_active_contact_pairs.value() > 0); + } } }; struct LCSFactoryOptions { - std::string contact_model; // "stewart_and_trinkle" or "anitescu" or - // "frictionless_spring" - int num_contacts; // Number of contacts in the system - double spring_stiffness; // Spring stiffness for frictionless spring model - std::optional - num_friction_directions; // Number of friction directions per contact - std::optional> - num_friction_directions_per_contact; // Number of friction directions per - std::optional> - mu; // Friction coefficient for each contact - std::optional> - planar_normal_direction; // Optional normal vector for planar contact - std::optional>> - planar_normal_direction_per_contact; // Optional normal vector for planar - // contact for each contact + // Contact model type: "stewart_and_trinkle", "anitescu", or + // "frictionless_spring" + std::string contact_model; + int N; // Number of time steps in the prediction horizon + double dt; // Time step size (seconds) + + // Total number of contact points in the system + std::optional num_contacts; + + // Spring stiffness for frictionless_spring contact model (must be positive) + std::optional spring_stiffness; - std::optional> - contact_pair_configs; // Optional detailed contact pair configurations + // Detailed per-contact-pair configurations (alternative to global settings) + std::optional> contact_pair_configs; - int N; // number of time steps in the prediction horizon - double dt; // time step size + // Per-contact friction parameters (must match num_contacts if provided) + std::optional> num_friction_directions_per_contact; + std::optional> mu_per_contact; + std::optional>> + planar_normal_direction_per_contact; + + // Global friction parameters (applied to all contacts if per-contact not + // specified) + std::optional num_friction_directions; + std::optional mu; + std::optional> planar_normal_direction; template void Serialize(Archive* a) { a->Visit(DRAKE_NVP(contact_model)); a->Visit(DRAKE_NVP(num_contacts)); a->Visit(DRAKE_NVP(spring_stiffness)); - a->Visit(DRAKE_NVP(num_friction_directions)); + a->Visit(DRAKE_NVP(contact_pair_configs)); a->Visit(DRAKE_NVP(num_friction_directions_per_contact)); + a->Visit(DRAKE_NVP(mu_per_contact)); + a->Visit(DRAKE_NVP(planar_normal_direction_per_contact)); + a->Visit(DRAKE_NVP(num_friction_directions)); a->Visit(DRAKE_NVP(mu)); a->Visit(DRAKE_NVP(planar_normal_direction)); - a->Visit(DRAKE_NVP(planar_normal_direction_per_contact)); - a->Visit(DRAKE_NVP(contact_pair_configs)); - a->Visit(DRAKE_NVP(N)); a->Visit(DRAKE_NVP(dt)); - // Contact Pair Information contained in contact_pair_configs - if (contact_pair_configs.has_value()) { - DRAKE_DEMAND(contact_pair_configs.value().size() == (size_t)num_contacts); - } else { - // ensure mu and num_friction_directions are properly specified - DRAKE_DEMAND(mu.has_value()); - DRAKE_DEMAND(mu.value().size() == (size_t)num_contacts); - // if a single num_friction_directions is provided, use it for all - // contacts - if (num_friction_directions.has_value()) { - num_friction_directions_per_contact = - std::vector(num_contacts, num_friction_directions.value()); + if (contact_model == "frictionless_spring") { + DRAKE_DEMAND(spring_stiffness.has_value()); + DRAKE_DEMAND(spring_stiffness.value() > 0); + } + } + + // Returns the total number of contact points. + // Requires: num_contacts must be set and non-negative. + int ResolveNumContacts() const { + DRAKE_DEMAND(num_contacts.has_value()); + DRAKE_DEMAND(num_contacts.value() >= 0); + return num_contacts.value(); + } + + // Resolves the number of friction directions for each contact point. + // Returns a vector of length num_contacts where each element specifies + // the friction direction count for that contact. + // Uses per-contact values if available, otherwise broadcasts the global + // value to all contacts. + // Requires: num_contacts and either num_friction_directions_per_contact + // or num_friction_directions must be set. + std::vector ResolveNumFrictionDirections() const { + if (contact_model == "frictionless_spring") + return std::vector(ResolveNumContacts(), 0); + + DRAKE_DEMAND(num_friction_directions_per_contact.has_value() || + num_friction_directions.has_value()); + + if (num_friction_directions_per_contact.has_value()) { + DRAKE_DEMAND(num_friction_directions_per_contact.value().size() == + (size_t)num_contacts.value()); + for (int n : num_friction_directions_per_contact.value()) { + DRAKE_DEMAND(n >= 0); + } + return num_friction_directions_per_contact.value(); + } + + return std::vector(ResolveNumContacts(), + num_friction_directions.value()); + } + + // Resolves the friction coefficient (mu) for each contact point. + // Returns a vector of length num_contacts where each element specifies + // the friction coefficient for that contact. + // Uses per-contact values if available, otherwise broadcasts the global + // value to all contacts. + // Requires: num_contacts and either mu_per_contact or mu must be set. + std::vector ResolveMu() const { + if (contact_model == "frictionless_spring") + return std::vector(ResolveNumContacts(), 0.0); + + DRAKE_DEMAND(mu_per_contact.has_value() || mu.has_value()); + + if (mu_per_contact.has_value()) { + DRAKE_DEMAND(mu_per_contact.value().size() == + (size_t)num_contacts.value()); + for (double coeff : mu_per_contact.value()) { + DRAKE_DEMAND(coeff >= 0); + } + return mu_per_contact.value(); + } + + return std::vector(ResolveNumContacts(), mu.value()); + } + + // Resolves the planar normal direction for each contact point. + // Returns a vector of length num_contacts where each element is either: + // - A unit normal vector for planar contacts (num_friction_directions==1) + // - std::nullopt for non-planar contacts (num_friction_directions!=1) + // Uses per-contact normals if available, otherwise broadcasts the global + // normal to all planar contacts. + // Requires: num_contacts must be set, and planar contacts must have + // either planar_normal_direction_per_contact or + // planar_normal_direction specified. + std::vector>> ResolvePlanarNormals() + const { + DRAKE_DEMAND(num_contacts.has_value()); + DRAKE_DEMAND(num_contacts.value() >= 0); + + auto n_friction_direction_per_contact = ResolveNumFrictionDirections(); + DRAKE_DEMAND(n_friction_direction_per_contact.size() == + (size_t)num_contacts.value()); + + std::vector>> resolved_normals; + for (size_t i = 0; i < n_friction_direction_per_contact.size(); ++i) { + if (contact_model == "frictionless_spring" || + n_friction_direction_per_contact[i] == 1) { + if (planar_normal_direction_per_contact.has_value()) { + DRAKE_DEMAND(planar_normal_direction_per_contact.value().size() == + (size_t)num_contacts.value()); + resolved_normals.push_back( + planar_normal_direction_per_contact.value()[i]); + } else { + DRAKE_DEMAND(planar_normal_direction.has_value()); + resolved_normals.push_back(planar_normal_direction); + } } else { - DRAKE_DEMAND(num_friction_directions_per_contact.has_value()); - DRAKE_DEMAND(num_friction_directions_per_contact.value().size() == - (size_t)num_contacts); + resolved_normals.push_back(std::nullopt); } } + return resolved_normals; } }; diff --git a/multibody/test/multibody_test.cc b/multibody/test/multibody_test.cc index 02fb7cd..08697d1 100644 --- a/multibody/test/multibody_test.cc +++ b/multibody/test/multibody_test.cc @@ -45,42 +45,6 @@ using drake::systems::System; using Eigen::MatrixXd; using Eigen::VectorXd; -// Test the static method for computing number of contact variables -GTEST_TEST(LCSFactoryTest, GetNumContactVariables) { - // Test with different contact models and friction properties - EXPECT_EQ(LCSFactory::GetNumContactVariables(ContactModel::kStewartAndTrinkle, - 2, 4), - 20); - EXPECT_EQ(LCSFactory::GetNumContactVariables(ContactModel::kAnitescu, 3, 2), - 12); - EXPECT_EQ(LCSFactory::GetNumContactVariables( - ContactModel::kFrictionlessSpring, 3, 0), - 3); - EXPECT_THROW(LCSFactory::GetNumContactVariables(ContactModel::kUnknown, 3, 0), - std::out_of_range); - - // Test with LCSFactoryOptions struct - LCSFactoryOptions options; - options.contact_model = "stewart_and_trinkle"; - options.num_friction_directions = 4; - options.num_contacts = 2; - EXPECT_EQ(LCSFactory::GetNumContactVariables(options), 20); - - options.contact_model = "anitescu"; - options.num_friction_directions = 2; - options.num_contacts = 3; - EXPECT_EQ(LCSFactory::GetNumContactVariables(options), 12); - - options.contact_model = "frictionless_spring"; - options.num_friction_directions = 0; - options.num_contacts = 3; - EXPECT_EQ(LCSFactory::GetNumContactVariables(options), 3); - - // Test error handling for invalid contact model - options.contact_model = "some_random_contact_model"; - EXPECT_THROW(LCSFactory::GetNumContactVariables(options), std::out_of_range); -} - // Helper struct to hold common test fixture data for cube pivoting scenarios struct PivotingTestFixture { DiagramBuilder plant_builder; @@ -110,8 +74,9 @@ struct PivotingTestFixture { // Initialize friction directions per contact if not set in config if (!options.num_friction_directions_per_contact) { - options.num_friction_directions_per_contact = std::vector( - options.num_contacts, options.num_friction_directions.value()); + options.num_friction_directions_per_contact = + std::vector(options.ResolveNumContacts(), + options.num_friction_directions.value()); } // Build diagram and create contexts @@ -161,6 +126,43 @@ class LCSFactoryPivotingTest : public testing::Test { PivotingTestFixture fixture; }; +// Test the static method for computing number of contact variables +TEST_F(LCSFactoryPivotingTest, GetNumContactVariables) { + // Test with different contact models and friction properties + EXPECT_EQ(LCSFactory::GetNumContactVariables(ContactModel::kStewartAndTrinkle, + 2, 4), + 20); + EXPECT_EQ(LCSFactory::GetNumContactVariables(ContactModel::kAnitescu, 3, 2), + 12); + EXPECT_EQ(LCSFactory::GetNumContactVariables( + ContactModel::kFrictionlessSpring, 3, 0), + 3); + EXPECT_THROW(LCSFactory::GetNumContactVariables(ContactModel::kUnknown, 3, 0), + std::out_of_range); + + // Test with LCSFactoryOptions struct + LCSFactoryOptions options; + options.contact_model = "stewart_and_trinkle"; + options.num_friction_directions = 4; + options.num_contacts = 2; + EXPECT_EQ(LCSFactory::GetNumContactVariables(*fixture.plant, options), 20); + + options.contact_model = "anitescu"; + options.num_friction_directions = 2; + options.num_contacts = 3; + EXPECT_EQ(LCSFactory::GetNumContactVariables(*fixture.plant, options), 12); + + options.contact_model = "frictionless_spring"; + options.num_friction_directions = 0; + options.num_contacts = 3; + EXPECT_EQ(LCSFactory::GetNumContactVariables(*fixture.plant, options), 3); + + // Test error handling for invalid contact model + options.contact_model = "some_random_contact_model"; + EXPECT_THROW(LCSFactory::GetNumContactVariables(*fixture.plant, options), + std::out_of_range); +} + // Test that contact pairs can be parsed from options instead of explicit list TEST_F(LCSFactoryPivotingTest, ContactPairParsing) { std::unique_ptr contact_pair_parsed_factory; @@ -188,8 +190,8 @@ TEST_F(LCSFactoryPivotingTest, ContactPairParsing) { EXPECT_EQ(lcs.num_states(), fixture.plant->num_positions() + fixture.plant->num_velocities()); EXPECT_EQ(lcs.num_inputs(), fixture.plant->num_actuators()); - EXPECT_EQ(lcs.num_lambdas(), - LCSFactory::GetNumContactVariables(fixture.options)); + EXPECT_EQ(lcs.num_lambdas(), LCSFactory::GetNumContactVariables( + *fixture.plant, fixture.options)); } // Parameterized test fixture for testing different contact models and friction @@ -207,11 +209,13 @@ class LCSFactoryParameterizedPivotingTest VectorXd::Zero(fixture.plant->num_actuators()); // Set contact model and friction directions from test parameters + fixture.options.contact_pair_configs = std::nullopt; fixture.options.contact_model = std::get<0>(GetParam()); fixture.contact_model = GetContactModelMap().at(fixture.options.contact_model); fixture.options.num_friction_directions_per_contact = std::vector(fixture.contact_pairs.size(), std::get<1>(GetParam())); + fixture.options.planar_normal_direction = std::array({0, 0, 1}); // Create factory with parameterized options fixture.lcs_factory = std::make_unique( @@ -234,8 +238,8 @@ TEST_P(LCSFactoryParameterizedPivotingTest, GenerateLCS) { EXPECT_EQ(lcs.num_states(), fixture.plant->num_positions() + fixture.plant->num_velocities()); EXPECT_EQ(lcs.num_inputs(), fixture.plant->num_actuators()); - EXPECT_EQ(lcs.num_lambdas(), - LCSFactory::GetNumContactVariables(fixture.options)); + EXPECT_EQ(lcs.num_lambdas(), LCSFactory::GetNumContactVariables( + *fixture.plant, fixture.options)); } // Test static linearization method for different contact models @@ -257,8 +261,8 @@ TEST_P(LCSFactoryParameterizedPivotingTest, LinearizePlantToLCS) { EXPECT_EQ(lcs.num_states(), fixture.plant->num_positions() + fixture.plant->num_velocities()); EXPECT_EQ(lcs.num_inputs(), fixture.plant->num_actuators()); - EXPECT_EQ(lcs.num_lambdas(), - LCSFactory::GetNumContactVariables(fixture.options)); + EXPECT_EQ(lcs.num_lambdas(), LCSFactory::GetNumContactVariables( + *fixture.plant, fixture.options)); } // Test that updating state and input changes contact-dependent LCS matrices diff --git a/multibody/test/resources/lcs_factory_pivoting_options.yaml b/multibody/test/resources/lcs_factory_pivoting_options.yaml index 1b7b7f6..6f82dd9 100644 --- a/multibody/test/resources/lcs_factory_pivoting_options.yaml +++ b/multibody/test/resources/lcs_factory_pivoting_options.yaml @@ -3,7 +3,7 @@ num_friction_directions: 1 planar_normal_direction: [0, 1, 0] num_contacts: 3 spring_stiffness: 0 -mu: [0.1, 0.1, 0.1] +mu: 0.1 N: 10 dt: 0.01 contact_pair_configs: # Not used but kept to test parsing diff --git a/systems/c3_controller.cc b/systems/c3_controller.cc index 8e4fc15..1b99a58 100644 --- a/systems/c3_controller.cc +++ b/systems/c3_controller.cc @@ -71,7 +71,7 @@ C3Controller::C3Controller( // Determine the size of lambda based on the contact model n_lambda_ = multibody::LCSFactory::GetNumContactVariables( - controller_options_.lcs_factory_options); + plant_, controller_options_.lcs_factory_options); // Placeholder vector for initialization VectorXd zeros = VectorXd::Zero(n_x_ + n_lambda_ + n_u_); diff --git a/systems/c3_controller_options.h b/systems/c3_controller_options.h index 6493606..54457c4 100644 --- a/systems/c3_controller_options.h +++ b/systems/c3_controller_options.h @@ -81,19 +81,6 @@ struct C3ControllerOptions { if (projection_type == "QP") { DRAKE_DEMAND(lcs_factory_options.contact_model == "anitescu"); } - - int expected_lambda_size = - LCSFactory::GetNumContactVariables(lcs_factory_options); - DRAKE_DEMAND(static_cast(c3_options.g_lambda.size()) == - expected_lambda_size); - DRAKE_DEMAND(static_cast(c3_options.u_lambda.size()) == - expected_lambda_size); - if (projection_type == "C3+") { - DRAKE_DEMAND(static_cast(c3_options.g_eta_vector.size()) == - expected_lambda_size); - DRAKE_DEMAND(static_cast(c3_options.u_eta_vector.size()) == - expected_lambda_size); - } } }; diff --git a/systems/lcmt_generators/c3_trajectory_generator_config.h b/systems/lcmt_generators/c3_trajectory_generator_config.h index 8127e4d..b9e93c5 100644 --- a/systems/lcmt_generators/c3_trajectory_generator_config.h +++ b/systems/lcmt_generators/c3_trajectory_generator_config.h @@ -66,8 +66,10 @@ struct TrajectoryDescription { }; std::string trajectory_name; // Name of the trajectory for LCM publishing - std::string variable_type; // Type of C3 variable: "state", "input", or "force" - std::vector indices; // List of index ranges to extract and concatenate + std::string + variable_type; // Type of C3 variable: "state", "input", or "force" + std::vector + indices; // List of index ranges to extract and concatenate template void Serialize(Archive* a) { diff --git a/systems/lcs_factory_system.cc b/systems/lcs_factory_system.cc index e4c3599..85c28ec 100644 --- a/systems/lcs_factory_system.cc +++ b/systems/lcs_factory_system.cc @@ -53,7 +53,7 @@ void LCSFactorySystem::InitializeSystem( this->set_name("lcs_factory_system"); n_x_ = plant.num_positions() + plant.num_velocities(); - n_lambda_ = multibody::LCSFactory::GetNumContactVariables(options); + n_lambda_ = multibody::LCSFactory::GetNumContactVariables(plant, options); n_u_ = plant.num_actuators(); lcs_state_input_port_ = diff --git a/systems/test/resources/quaternion_cost_test.yaml b/systems/test/resources/quaternion_cost_test.yaml index dfaeeb0..caa08aa 100644 --- a/systems/test/resources/quaternion_cost_test.yaml +++ b/systems/test/resources/quaternion_cost_test.yaml @@ -15,8 +15,7 @@ lcs_factory_options: contact_model: "anitescu" num_friction_directions: 2 num_contacts: 8 - spring_stiffness: 0 - mu: [0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4] + mu: 0.4 N: 10 dt: 0.02 diff --git a/systems/test/systems_test.cc b/systems/test/systems_test.cc index ecf862d..6fd56f0 100644 --- a/systems/test/systems_test.cc +++ b/systems/test/systems_test.cc @@ -356,8 +356,9 @@ TEST_F(LCSFactorySystemTest, OutputLCSIsValid) { const auto& lcs = lcs_output->get_data(0)->get_value(); EXPECT_EQ(lcs.num_states(), plant->num_positions() + plant->num_velocities()); EXPECT_EQ(lcs.num_inputs(), plant->num_actuators()); - EXPECT_EQ(lcs.num_lambdas(), LCSFactory::GetNumContactVariables( - controller_options.lcs_factory_options)); + EXPECT_EQ(lcs.num_lambdas(), + LCSFactory::GetNumContactVariables( + *plant, controller_options.lcs_factory_options)); EXPECT_EQ(lcs.dt(), controller_options.lcs_factory_options.dt); EXPECT_EQ(lcs.N(), controller_options.lcs_factory_options.N); }