From 7fd151f88cbc4e1d5af1b53fbe6b6154c562fccd Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Tue, 24 Mar 2026 16:37:31 -0400 Subject: [PATCH 01/11] Switch to version of C3 repo that has cost computation helpers (not currently utilized) --- MODULE.bazel | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/MODULE.bazel b/MODULE.bazel index b28b9b71e..d1c054d03 100644 --- a/MODULE.bazel +++ b/MODULE.bazel @@ -93,8 +93,10 @@ bazel_dep(name = "c3") git_override( module_name = "c3", remote = "https://github.com/DAIRLab/c3.git", - commit = "ee740029598ea0f990cae2beca709d1eeed651f2" + commit = "93362ddefd8e79684948027bef506c997daff3b1" ) +# TODO @bibit: replace the commit hash with the one from merging the C3 cost +# evaluation PR into C3's main branch. INEKF_COMMIT = "297c308e50fa599af92ce3bd5f11d71e2bf8af69" From 8c1a0491cc047a143eb17f9f000b420bc6103f5d Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Mon, 6 Apr 2026 14:13:35 -0400 Subject: [PATCH 02/11] Fix LCS resolution change bug --- systems/controllers/sampling_based_c3_controller.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/systems/controllers/sampling_based_c3_controller.cc b/systems/controllers/sampling_based_c3_controller.cc index 02f660658..9866875e5 100644 --- a/systems/controllers/sampling_based_c3_controller.cc +++ b/systems/controllers/sampling_based_c3_controller.cc @@ -1897,8 +1897,8 @@ SamplingC3Controller::CreateLCSObjectsForSamples( .planar_normal_direction = sampling_c3_options_.planar_normal_direction, .planar_normal_direction_per_contact = std::nullopt, .contact_pair_configs = std::nullopt, - .N = N_ * sampling_c3_options_.lcs_dt_resolution, - .dt = dt_ / sampling_c3_options_.lcs_dt_resolution}; + .N = lcs_factory_options.N * sampling_c3_options_.lcs_dt_resolution, + .dt = lcs_factory_options.dt / sampling_c3_options_.lcs_dt_resolution}; LCS lcs_object_sample_for_cost_simulation = LCSFactory(plant_, *context_, plant_ad_, *context_ad_, resolved_contact_pairs_for_cost_simulation, From 27ce682c2748404a7dd5ee2c1a1acbadf692044b Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Mon, 6 Apr 2026 14:47:16 -0400 Subject: [PATCH 03/11] Fix timestep bug affecting published C3 trajectory timestamps --- systems/controllers/sampling_based_c3_controller.cc | 8 ++++++++ systems/controllers/sampling_based_c3_controller.h | 4 ++-- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/systems/controllers/sampling_based_c3_controller.cc b/systems/controllers/sampling_based_c3_controller.cc index 9866875e5..b16a23d8e 100644 --- a/systems/controllers/sampling_based_c3_controller.cc +++ b/systems/controllers/sampling_based_c3_controller.cc @@ -80,6 +80,9 @@ SamplingC3Controller::SamplingC3Controller( sampling_c3_options_.GetC3Options(crossed_cost_switching_threshold_); DRAKE_DEMAND(sampling_c3_options_.lcs_dt_resolution > 0); + dt_ = + sampling_c3_options_.planning_dt_position; // Initialize dt_ to position + // mode's dt by default. // Initialize Q_ and R_ to proper size. Values don't matter because the // values get rewritten at the beginning of every control loop. @@ -1051,6 +1054,9 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( << std::endl; } crossed_cost_switching_threshold_ = false; + dt_ = + sampling_c3_options_.planning_dt_position; // Always set dt_ according + // to pose or position mode. x_final_target_ = x_lcs_final_des.value(); // is_doing_c3_ = false; detected_goal_changes_++; @@ -1078,6 +1084,8 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( if (pose_diff < progress_params_.cost_switching_threshold_distance * controller_params_.num_objects) { crossed_cost_switching_threshold_ = true; + dt_ = sampling_c3_options_.planning_dt_pose; // Always set dt_ according + // to pose or position mode. std::cout << "Crossed cost switching threshold." << std::endl; // Reset the sample buffers and metrics now that the costs have changed. diff --git a/systems/controllers/sampling_based_c3_controller.h b/systems/controllers/sampling_based_c3_controller.h index 2d07bc026..9b72a015c 100644 --- a/systems/controllers/sampling_based_c3_controller.h +++ b/systems/controllers/sampling_based_c3_controller.h @@ -420,8 +420,8 @@ class SamplingC3Controller : public drake::systems::LeafSystem { /// TODO: There are many mutable class variables, which is not best practice /// in the Drake systems framework. These could be converted to discrete /// state variables. - mutable double dt_ = 0.1; - mutable double dt_cost_ = 0.02; + mutable double dt_; + mutable std::vector Q_; mutable std::vector R_; From 09bf05e42602e83fc9150f53ba8257a7de125600 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Mon, 6 Apr 2026 15:27:10 -0400 Subject: [PATCH 04/11] Fix bug in calculating cost contribution of the last timestep --- .../sampling_based_c3_controller.cc | 75 ++++++++++--------- 1 file changed, 39 insertions(+), 36 deletions(-) diff --git a/systems/controllers/sampling_based_c3_controller.cc b/systems/controllers/sampling_based_c3_controller.cc index b16a23d8e..8e9105112 100644 --- a/systems/controllers/sampling_based_c3_controller.cc +++ b/systems/controllers/sampling_based_c3_controller.cc @@ -757,92 +757,95 @@ std::pair> SamplingC3Controller::CalcCost( DRAKE_DEMAND(!std::isnan(cost_contrib_obj_vel)); // Handle the N_th state. - cost = cost + (XX[N_] - x_desired[N_]).transpose() * Q_eff.at(N_) * - (XX[N_] - x_desired[N_]); + cost = cost + (XX[N_ * resolution] - x_desired[N_]).transpose() * + Q_eff.at(N_) * (XX[N_ * resolution] - x_desired[N_]); error_contrib_ee_pos += - (XX[N_].segment(0, 3) - x_desired[N_].segment(0, 3)).transpose() * - (XX[N_].segment(0, 3) - x_desired[N_].segment(0, 3)); + (XX[N_ * resolution].segment(0, 3) - x_desired[N_].segment(0, 3)) + .transpose() * + (XX[N_ * resolution].segment(0, 3) - x_desired[N_].segment(0, 3)); for (int j = 0; j < num_objects; j++) { obj_orientation_index = 7 * j + 3; obj_pos_index = 7 * j + 7; error_contrib_obj_orientation += - (XX[N_].segment(obj_orientation_index, 4) - + (XX[N_ * resolution].segment(obj_orientation_index, 4) - x_desired[N_].segment(obj_orientation_index, 4)) .transpose() * - (XX[N_].segment(obj_orientation_index, 4) - + (XX[N_ * resolution].segment(obj_orientation_index, 4) - x_desired[N_].segment(obj_orientation_index, 4)); - error_contrib_obj_pos += (XX[N_].segment(obj_pos_index, 3) - + error_contrib_obj_pos += (XX[N_ * resolution].segment(obj_pos_index, 3) - x_desired[N_].segment(obj_pos_index, 3)) .transpose() * - (XX[N_].segment(obj_pos_index, 3) - + (XX[N_ * resolution].segment(obj_pos_index, 3) - x_desired[N_].segment(obj_pos_index, 3)); } - error_contrib_ee_vel += - (XX[N_].segment(ee_vel_index, 3) - x_desired[N_].segment(ee_vel_index, 3)) - .transpose() * - (XX[N_].segment(ee_vel_index, 3) - - x_desired[N_].segment(ee_vel_index, 3)); + error_contrib_ee_vel += (XX[N_ * resolution].segment(ee_vel_index, 3) - + x_desired[N_].segment(ee_vel_index, 3)) + .transpose() * + (XX[N_ * resolution].segment(ee_vel_index, 3) - + x_desired[N_].segment(ee_vel_index, 3)); for (int j = 0; j < num_objects; j++) { obj_ang_vel_index = 6 * j + 6 + 7 * num_objects; obj_vel_index = 6 * j + 9 + 7 * num_objects; - error_contrib_obj_ang_vel += (XX[N_].segment(obj_ang_vel_index, 3) - - x_desired[N_].segment(obj_ang_vel_index, 3)) - .transpose() * - (XX[N_].segment(obj_ang_vel_index, 3) - - x_desired[N_].segment(obj_ang_vel_index, 3)); - error_contrib_obj_vel += (XX[N_].segment(obj_vel_index, 3) - + error_contrib_obj_ang_vel += + (XX[N_ * resolution].segment(obj_ang_vel_index, 3) - + x_desired[N_].segment(obj_ang_vel_index, 3)) + .transpose() * + (XX[N_ * resolution].segment(obj_ang_vel_index, 3) - + x_desired[N_].segment(obj_ang_vel_index, 3)); + error_contrib_obj_vel += (XX[N_ * resolution].segment(obj_vel_index, 3) - x_desired[N_].segment(obj_vel_index, 3)) .transpose() * - (XX[N_].segment(obj_vel_index, 3) - + (XX[N_ * resolution].segment(obj_vel_index, 3) - x_desired[N_].segment(obj_vel_index, 3)); } cost_contrib_ee_pos += - (XX[N_].segment(0, 3) - x_desired[N_].segment(0, 3)).transpose() * + (XX[N_ * resolution].segment(0, 3) - x_desired[N_].segment(0, 3)) + .transpose() * Q_eff.at(N_).block(0, 0, 3, 3) * - (XX[N_].segment(0, 3) - x_desired[N_].segment(0, 3)); + (XX[N_ * resolution].segment(0, 3) - x_desired[N_].segment(0, 3)); for (int j = 0; j < num_objects; j++) { obj_orientation_index = 7 * j + 3; obj_pos_index = 7 * j + 7; cost_contrib_obj_orientation += - (XX[N_].segment(obj_orientation_index, 4) - + (XX[N_ * resolution].segment(obj_orientation_index, 4) - x_desired[N_].segment(obj_orientation_index, 4)) .transpose() * Q_eff.at(N_).block(obj_orientation_index, obj_orientation_index, 4, 4) * - (XX[N_].segment(obj_orientation_index, 4) - + (XX[N_ * resolution].segment(obj_orientation_index, 4) - x_desired[N_].segment(obj_orientation_index, 4)); cost_contrib_obj_pos += - (XX[N_].segment(obj_pos_index, 3) - + (XX[N_ * resolution].segment(obj_pos_index, 3) - x_desired[N_].segment(obj_pos_index, 3)) .transpose() * Q_eff.at(N_).block(obj_pos_index, obj_pos_index, 3, 3) * - (XX[N_].segment(obj_pos_index, 3) - + (XX[N_ * resolution].segment(obj_pos_index, 3) - x_desired[N_].segment(obj_pos_index, 3)); } - cost_contrib_ee_vel += - (XX[N_].segment(ee_vel_index, 3) - x_desired[N_].segment(ee_vel_index, 3)) - .transpose() * - Q_eff.at(N_).block(ee_vel_index, ee_vel_index, 3, 3) * - (XX[N_].segment(ee_vel_index, 3) - - x_desired[N_].segment(ee_vel_index, 3)); + cost_contrib_ee_vel += (XX[N_ * resolution].segment(ee_vel_index, 3) - + x_desired[N_].segment(ee_vel_index, 3)) + .transpose() * + Q_eff.at(N_).block(ee_vel_index, ee_vel_index, 3, 3) * + (XX[N_ * resolution].segment(ee_vel_index, 3) - + x_desired[N_].segment(ee_vel_index, 3)); for (int j = 0; j < num_objects; j++) { obj_ang_vel_index = 6 * j + 6 + 7 * num_objects; obj_vel_index = 6 * j + 9 + 7 * num_objects; cost_contrib_obj_ang_vel += - (XX[N_].segment(obj_ang_vel_index, 3) - + (XX[N_ * resolution].segment(obj_ang_vel_index, 3) - x_desired[N_].segment(obj_ang_vel_index, 3)) .transpose() * Q_eff.at(N_).block(obj_ang_vel_index, obj_ang_vel_index, 3, 3) * - (XX[N_].segment(obj_ang_vel_index, 3) - + (XX[N_ * resolution].segment(obj_ang_vel_index, 3) - x_desired[N_].segment(obj_ang_vel_index, 3)); cost_contrib_obj_vel += - (XX[N_].segment(obj_vel_index, 3) - + (XX[N_ * resolution].segment(obj_vel_index, 3) - x_desired[N_].segment(obj_vel_index, 3)) .transpose() * Q_eff.at(N_).block(obj_vel_index, obj_vel_index, 3, 3) * - (XX[N_].segment(obj_vel_index, 3) - + (XX[N_ * resolution].segment(obj_vel_index, 3) - x_desired[N_].segment(obj_vel_index, 3)); } From 0ef7a857679565f6fa412c82fd4d79e77a3a1b9f Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Tue, 7 Apr 2026 17:08:32 -0400 Subject: [PATCH 05/11] Fix indexing bugs related to different LCS discretizations for cost calculation --- .../sampling_based_c3_controller.cc | 23 +++++++++++-------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/systems/controllers/sampling_based_c3_controller.cc b/systems/controllers/sampling_based_c3_controller.cc index 8e9105112..c0bf512be 100644 --- a/systems/controllers/sampling_based_c3_controller.cc +++ b/systems/controllers/sampling_based_c3_controller.cc @@ -566,7 +566,7 @@ std::pair> SamplingC3Controller::CalcCost( for (int i = 0; i < N_ * resolution; i++) { UU[i] = z_fin[i / resolution].segment(n_x_ + n_lambda_, n_u_); XX[i] = z_fin[i / resolution].segment(0, n_x_); - if (i == N_ - 1) { + if (i == N_ * resolution - 1) { XX[i + 1] = lcs_for_cost.Simulate(XX[i], UU[i], simulate_config); } } @@ -582,11 +582,14 @@ std::pair> SamplingC3Controller::CalcCost( XX[i + 1] = lcs_for_cost.Simulate(XX[i], UU[i], simulate_config); } // Replace ee traj with those from z_fin. - for (int i = 0; i < N_; i++) { + for (int i = 0; i < N_ * resolution; i++) { XX[i].segment(0, 3) = z_fin[i / resolution].segment(0, 3); - if (i == N_ - 1) { - XX[i + 1].segment(0, 3) = - lcs_for_cost.Simulate(XX[i], UU[i], simulate_config).segment(0, 3); + XX[i].segment(ee_vel_index, 3) = + z_fin[i / resolution].segment(ee_vel_index, 3); + if (i == N_ * resolution - 1) { + VectorXd last_x = lcs_for_cost.Simulate(XX[i], UU[i], simulate_config); + XX[i + 1].segment(0, 3) = last_x.segment(0, 3); + XX[i + 1].segment(ee_vel_index, 3) = last_x.segment(ee_vel_index, 3); } } } @@ -608,11 +611,13 @@ std::pair> SamplingC3Controller::CalcCost( // Replace the end effector position and velocity plans with the ones from // the C3 plan. for (int i = 0; i < N_ * resolution; i++) { - XX[i].segment(0, 3) = z_fin[i].segment(0, 3); - XX[i].segment(ee_vel_index, 3) = z_fin[i].segment(ee_vel_index, 3); + XX[i].segment(0, 3) = z_fin[i / resolution].segment(0, 3); + XX[i].segment(ee_vel_index, 3) = + z_fin[i / resolution].segment(ee_vel_index, 3); if (i == N_ * resolution - 1) { - XX[i + 1].segment(0, 3) = z_fin[i].segment(0, 3); - XX[i + 1].segment(ee_vel_index, 3) = z_fin[i].segment(ee_vel_index, 3); + VectorXd last_x = lcs_for_cost.Simulate(XX[i], UU[i], simulate_config); + XX[i + 1].segment(0, 3) = last_x.segment(0, 3); + XX[i + 1].segment(ee_vel_index, 3) = last_x.segment(ee_vel_index, 3); } } } From 453423b358494f30ce55394e9a0b17696b15b7eb Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Wed, 22 Apr 2026 15:06:23 -0400 Subject: [PATCH 06/11] Introduce new cost calculation relying on C3 repo functionality; all cost types match closely; includes test printouts; to be cleaned for final use --- MODULE.bazel | 7 +- systems/controllers/BUILD.bazel | 1 + .../sampling_based_c3_controller.cc | 187 ++++++++++++++++-- .../sampling_based_c3_controller.h | 12 ++ 4 files changed, 190 insertions(+), 17 deletions(-) diff --git a/MODULE.bazel b/MODULE.bazel index d1c054d03..9b3318da6 100644 --- a/MODULE.bazel +++ b/MODULE.bazel @@ -93,10 +93,11 @@ bazel_dep(name = "c3") git_override( module_name = "c3", remote = "https://github.com/DAIRLab/c3.git", - commit = "93362ddefd8e79684948027bef506c997daff3b1" + #commit = "ef873a41eb73f73760fd7820f3d5efa2ede3df52" # Drake mismatch + #commit = "ddd70cb3309fc8542e57a54d2a1f25ecd4786be0" # Drake mismatch + commit = "9cacea5c2c8ee10f0efabd1b022bb39737139798" # works + #commit = "412a14c5ef4883a72f655df2f8689c313615b43f" # no matching function call ) -# TODO @bibit: replace the commit hash with the one from merging the C3 cost -# evaluation PR into C3's main branch. INEKF_COMMIT = "297c308e50fa599af92ce3bd5f11d71e2bf8af69" diff --git a/systems/controllers/BUILD.bazel b/systems/controllers/BUILD.bazel index b76ad0be5..94e345379 100644 --- a/systems/controllers/BUILD.bazel +++ b/systems/controllers/BUILD.bazel @@ -111,6 +111,7 @@ cc_library( "//examples/sampling_c3/parameter_headers:progress_params", "//common:update_context", "@c3//:libc3", + "@c3//core:traj_eval", "@drake//:drake_shared_library", ], copts = [ diff --git a/systems/controllers/sampling_based_c3_controller.cc b/systems/controllers/sampling_based_c3_controller.cc index c0bf512be..8762f03f7 100644 --- a/systems/controllers/sampling_based_c3_controller.cc +++ b/systems/controllers/sampling_based_c3_controller.cc @@ -12,6 +12,7 @@ #include "c3/core/c3_plus.h" #include "c3/core/c3_qp.h" #include "c3/core/lcs.h" +#include "c3/core/traj_eval.h" #include "c3/multibody/lcs_factory.h" #include "common/quaternion_error_hessian.h" #include "dairlib/lcmt_radio_out.hpp" @@ -30,6 +31,7 @@ using c3::C3Plus; using c3::C3QP; using c3::LCS; using c3::multibody::LCSFactory; +using c3::traj_eval::TrajectoryEvaluator; using dairlib::C3Output; using drake::SortedPair; using drake::geometry::GeometryId; @@ -456,6 +458,17 @@ SamplingC3Controller::SamplingC3Controller( DeclareForcedDiscreteUpdateEvent(&SamplingC3Controller::ComputePlan); + // Set Kp and Kd vectors for cost computation. These are only used for + // certain cost computation types. + Kp_for_cost_ = VectorXd::Zero(n_x_); + Kp_for_cost_(0) = sampling_c3_options_.Kp_for_ee_pd_rollout[0]; + Kp_for_cost_(1) = sampling_c3_options_.Kp_for_ee_pd_rollout[1]; + Kp_for_cost_(2) = sampling_c3_options_.Kp_for_ee_pd_rollout[2]; + Kd_for_cost_ = VectorXd::Zero(n_x_); + Kd_for_cost_(n_q_ + 0) = sampling_c3_options_.Kd_for_ee_pd_rollout[0]; + Kd_for_cost_(n_q_ + 1) = sampling_c3_options_.Kd_for_ee_pd_rollout[1]; + Kd_for_cost_(n_q_ + 2) = sampling_c3_options_.Kd_for_ee_pd_rollout[2]; + // Set parallelization settings. omp_set_dynamic(0); // Explicitly disable dynamic teams. omp_set_nested(1); // Enable nested threading. @@ -535,7 +548,144 @@ SamplingC3Controller::SamplingC3Controller( } } +std::pair> SamplingC3Controller::NewCalcCost( + C3CostComputationType cost_type, const LCS& lcs_for_cost, + const C3::CostMatrices& cost_mats, const std::shared_ptr& c3_object, + const bool& force_tracking_disabled, int num_objects) const { + // Extract needed information from the C3 object. + const LCS lcs_for_plan = c3_object->GetLCS(); + vector x_desired = c3_object->GetDesiredState(); + vector x_plan = c3_object->GetStateSolution(); + vector u_plan = c3_object->GetInputSolution(); + + // TODO @bibit: The original CalcCost extracts the x and u trajectories from + // the C3 object's z_sol_ vector, which can differ from the C3 object's + // x_sol_ and u_sol_ vectors if end_on_qp_step is false. This may be + // considered a bug in C3, but for now, extract the same trajectories to + // maintain functionality. If C3 is fixed so that x_sol_ and u_sol_ (and + // lambda_sol_) always reflect the trajectories corresponding to z_sol_, then + // the following 5 lines can be removed since x_plan and u_plan will already + // be correct from the above getters. + vector z_plan = c3_object->GetFullSolution(); + for (int i = 0; i < N_; i++) { + x_plan[i] = z_plan[i].segment(0, n_x_); + u_plan[i] = z_plan[i].segment(n_x_ + n_lambda_, n_u_); + } + DRAKE_THROW_UNLESS(z_plan.size() == N_); + DRAKE_THROW_UNLESS(x_plan.size() == N_); + DRAKE_THROW_UNLESS(u_plan.size() == N_); + + // The x_plan from the C3 object does not include the x_N state, so add it in + // by simulating forward one step from x_{N-1} and u_{N-1}. + auto simulate_config = c3::LCSSimulateConfig(); + simulate_config.regularized = true; + simulate_config.min_exp = -8; + // TODO @bibit: figure out which LCS makes the most sense to simulate here + // (probably lcs_for_plan), or do x[N] = A x[N-1] + B u[N-1] + D lambda[N-1] + + // d instead of LCS simulating. + x_plan.push_back( + lcs_for_cost.Simulate(x_plan.back(), u_plan.back(), simulate_config)); + + // Initialize the cost-driving trajectories to match the C3 plan. + vector XX = x_plan; + vector UU = u_plan; + + // Declare the matrices to use for cost computation. + vector Q_cost = cost_mats.Q; + vector R_cost = cost_mats.R; + + // Set a few more variables necessary for some of the cost types. + const int ee_vel_index = 7 * num_objects + 3; + + // Compute the states and controls to use for cost computation, and change the + // cost matrices if necessary. + if (cost_type == C3CostComputationType::kSimLCS) { + // Simulate the LCS from initial condition using the C3 plan's controls. + XX = TrajectoryEvaluator::SimulateLCSOverTrajectory( + x_plan[0], u_plan, lcs_for_plan, lcs_for_cost, simulate_config); + + } else if (cost_type == C3CostComputationType::kUseC3Plan) { + // No need to do anything here. + + } else if (cost_type == C3CostComputationType::kSimLCSReplaceC3EEPlan) { + // Simulate the LCS from initial condition using the C3 plan's controls. + vector XX_sim = TrajectoryEvaluator::SimulateLCSOverTrajectory( + x_plan[0], u_plan, lcs_for_plan, lcs_for_cost, simulate_config); + + // Use the simulated object trajectories but the planned robot trajectory. + for (int i = 0; i < N_ + 1; i++) { + XX[i].segment(3, 7 * num_objects) = XX_sim[i].segment(3, 7 * num_objects); + XX[i].segment(ee_vel_index + 3, 6 * num_objects) = + XX_sim[i].segment(ee_vel_index + 3, 6 * num_objects); + } + // TODO @bibit: don't do the other weird sim step. + // TEMP BELOW // + VectorXd last_x = + lcs_for_cost.Simulate(XX[N_ - 1], UU[N_ - 1], simulate_config); + XX[N_].segment(0, 3) = last_x.segment(0, 3); + XX[N_].segment(ee_vel_index, 3) = last_x.segment(ee_vel_index, 3); + // TEMP ABOVE // + + } else if (cost_type == C3CostComputationType::kSimImpedance) { + // Simulate PD with feedforward control using the C3 plan's states and + // controls from the initial condition. + auto [XX_sim, UU_sim] = TrajectoryEvaluator::SimulatePDControlWithLCS( + x_plan, UU, Kp_for_cost_, Kd_for_cost_, lcs_for_plan, lcs_for_cost, + !force_tracking_disabled, simulate_config); + XX = XX_sim; + UU = UU_sim; + + } else if (cost_type == C3CostComputationType::kSimImpedanceReplaceC3EEPlan) { + // Simulate PD with feedforward control using the C3 plan's states and + // controls from the initial condition. + auto [XX_sim, UU_sim] = TrajectoryEvaluator::SimulatePDControlWithLCS( + x_plan, UU, Kp_for_cost_, Kd_for_cost_, lcs_for_plan, lcs_for_cost, + !force_tracking_disabled, simulate_config); + UU = UU_sim; + + // Use the simulated object trajectories but the planned robot trajectory. + for (int i = 0; i < N_ + 1; i++) { + XX[i].segment(3, 7 * num_objects) = XX_sim[i].segment(3, 7 * num_objects); + XX[i].segment(ee_vel_index + 3, 6 * num_objects) = + XX_sim[i].segment(ee_vel_index + 3, 6 * num_objects); + } + // TODO @bibit: don't do the other weird sim step. + // TEMP BELOW // + VectorXd last_x = + lcs_for_cost.Simulate(XX[N_ - 1], UU[N_ - 1], simulate_config); + XX[N_].segment(0, 3) = last_x.segment(0, 3); + XX[N_].segment(ee_vel_index, 3) = last_x.segment(ee_vel_index, 3); + // TEMP ABOVE // + + } else if (cost_type == C3CostComputationType::kSimImpedanceObjectCostOnly) { + // Simulate PD with feedforward control using the C3 plan's states and + // controls from the initial condition. + auto [XX_sim, UU_sim] = TrajectoryEvaluator::SimulatePDControlWithLCS( + x_plan, UU, Kp_for_cost_, Kd_for_cost_, lcs_for_plan, lcs_for_cost, + !force_tracking_disabled, simulate_config); + XX = XX_sim; + UU = UU_sim; + + // Set R and the robot portion of the Q matrix to zero so that only the + // object state errors contribute to cost. + for (int i = 0; i < N_ + 1; i++) { + Q_cost[i].block(0, 0, 3, 3) *= 0.0; + Q_cost[i].block(3 + 7 * num_objects, 3 + 7 * num_objects, 3, 3) *= 0.0; + if (i < N_) { + R_cost[i] *= 0.0; + } + } + } + + double cost = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + XX, x_desired, Q_cost, UU, R_cost); + + std::pair> ret(cost, XX); + return ret; +} + // This function relies on the previously computed z_fin from Solve. +// TODO @bibit: remove this and replace with NewCalcCost std::pair> SamplingC3Controller::CalcCost( C3CostComputationType cost_type, const LCS& lcs_for_cost, const C3::CostMatrices& c3_cost, const std::vector x_desired, @@ -905,19 +1055,11 @@ std::pair> SamplingC3Controller::CalcCost( } XX_downsampled[N_] = XX[N_ * resolution]; - // for (int i = 0; i < XX_downsampled.size(); i++) { - // std::cout << XX_downsampled[i].transpose() << std::endl; - // } - // std::cout << "\n\n" << std::endl; - std::pair> ret(cost, XX_downsampled); - // for (int j =0; j <= N_; j++) { - // std::cout << XX[j].transpose() << std::endl; - // } - // std::cout << "\n\n" << std::endl; return ret; } +// TODO @bibit: remove std::pair, std::vector> SamplingC3Controller::SimulatePDControl( const LCS& lcs_for_cost, const std::vector z_fin, @@ -1229,6 +1371,7 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( // Parallelize over computing C3 costs for each sample. auto c3_start = std::chrono::high_resolution_clock::now(); + std::cout << "LOOP START" << std::endl; // TODO @bibit: remove after testing #pragma omp parallel for num_threads(num_threads_to_use_) for (int i = 0; i < num_total_samples; i++) { bool print_cost_breakdown = @@ -1318,17 +1461,33 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( test_c3_object->Solve(test_state); auto cc_start = std::chrono::high_resolution_clock::now(); - std::pair> cost_trajectory_pair = - CalcCost(cost_type, lcs_candidates_for_cost.at(i), c3_costmat, - x_desired, test_c3_object->GetFullSolution(), - force_tracking_disabled, controller_params_.num_objects, - print_cost_breakdown, verbose_); + std::pair> cost_trajectory_pair = CalcCost( + cost_type, lcs_candidates_for_cost.at(i), c3_costmat, x_desired, + test_c3_object->GetFullSolution(), force_tracking_disabled, + controller_params_.num_objects, print_cost_breakdown, verbose_); + // NewCalcCost(cost_type, lcs_candidates_for_cost.at(i), c3_costmat, + // test_c3_object, force_tracking_disabled, + // controller_params_.num_objects); auto cc_end = std::chrono::high_resolution_clock::now(); std::chrono::duration duration_ms = cc_end - cc_start; double c3_cost = cost_trajectory_pair.first; all_sample_dynamically_feasible_plans_.at(i) = cost_trajectory_pair.second; + /* TEMPORARY TEST: Check that the new and old cost computation methods + produce the same results. */ + auto [cost_new, __] = NewCalcCost( + cost_type, lcs_candidates_for_cost.at(i), c3_costmat, test_c3_object, + force_tracking_disabled, controller_params_.num_objects); + std::cout << "Sample " << i << " cost difference: " + << std::abs(c3_cost - cost_new) / c3_cost * 100.0 << "% change" + << " (old cost: " << c3_cost << ", new cost: " << cost_new << ")" + << std::endl; + + // Quit the program if a huge cost error is detected. + DRAKE_THROW_UNLESS(cost_new < 1e10); + /* END OF TEMPORARY TEST */ + #pragma omp critical { c3_objects.at(i) = test_c3_object; diff --git a/systems/controllers/sampling_based_c3_controller.h b/systems/controllers/sampling_based_c3_controller.h index 9b72a015c..e1b367082 100644 --- a/systems/controllers/sampling_based_c3_controller.h +++ b/systems/controllers/sampling_based_c3_controller.h @@ -194,6 +194,16 @@ class SamplingC3Controller : public drake::systems::LeafSystem { } private: + // TODO @bibit: Functionality to add into NewCalcCost: + // [x] enable/disable force tracking + // [ ] print cost breakdown + // [ ] verbose printouts + std::pair> NewCalcCost( + C3CostComputationType cost_type, const c3::LCS& lcs_for_cost, + const c3::C3::CostMatrices& cost_mats, + const std::shared_ptr& c3_object, + const bool& force_tracking_disabled, int num_objects) const; + // TODO @bibit: remove and replace with NewCalcCost std::pair> CalcCost( C3CostComputationType cost_type, const c3::LCS& lcs_for_cost, const c3::C3::CostMatrices& c3_cost, @@ -422,6 +432,8 @@ class SamplingC3Controller : public drake::systems::LeafSystem { /// state variables. mutable double dt_; + Eigen::VectorXd Kp_for_cost_; + Eigen::VectorXd Kd_for_cost_; mutable std::vector Q_; mutable std::vector R_; From 364d46f6f0781ec4b496ba632186d8a782e0b89f Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Wed, 22 Apr 2026 18:30:52 -0400 Subject: [PATCH 07/11] Use x_N implied by the C3 plan's last x, u, and lambda along with the LCS matrices --- MODULE.bazel | 7 +--- .../sampling_based_c3_controller.cc | 35 +++++++------------ 2 files changed, 13 insertions(+), 29 deletions(-) diff --git a/MODULE.bazel b/MODULE.bazel index bb298fbc7..49ecc4bf8 100644 --- a/MODULE.bazel +++ b/MODULE.bazel @@ -91,12 +91,7 @@ bazel_dep(name = "c3") git_override( module_name = "c3", remote = "https://github.com/DAIRLab/c3.git", - commit = "ef873a41eb73f73760fd7820f3d5efa2ede3df52" # after: might work? - #commit = "ef873a41eb73f73760fd7820f3d5efa2ede3df52" # before: Drake mismatch - #commit = "ddd70cb3309fc8542e57a54d2a1f25ecd4786be0" # before: Drake mismatch - #commit = "9cacea5c2c8ee10f0efabd1b022bb39737139798" # before: works - #commit = "412a14c5ef4883a72f655df2f8689c313615b43f" # no matching function call - #commit = "e2170374aaec3fc40dc0566e869d980482ed309d" # from push_anything_dev + commit = "ef873a41eb73f73760fd7820f3d5efa2ede3df52" ) diff --git a/systems/controllers/sampling_based_c3_controller.cc b/systems/controllers/sampling_based_c3_controller.cc index c91d52e96..83b062a91 100644 --- a/systems/controllers/sampling_based_c3_controller.cc +++ b/systems/controllers/sampling_based_c3_controller.cc @@ -560,6 +560,7 @@ std::pair> SamplingC3Controller::NewCalcCost( vector x_desired = c3_object->GetDesiredState(); vector x_plan = c3_object->GetStateSolution(); vector u_plan = c3_object->GetInputSolution(); + vector lambda_plan = c3_object->GetForceSolution(); // TODO @bibit: The original CalcCost extracts the x and u trajectories from // the C3 object's z_sol_ vector, which can differ from the C3 object's @@ -572,6 +573,7 @@ std::pair> SamplingC3Controller::NewCalcCost( vector z_plan = c3_object->GetFullSolution(); for (int i = 0; i < N_; i++) { x_plan[i] = z_plan[i].segment(0, n_x_); + lambda_plan[i] = z_plan[i].segment(n_x_, n_lambda_); u_plan[i] = z_plan[i].segment(n_x_ + n_lambda_, n_u_); } DRAKE_THROW_UNLESS(z_plan.size() == N_); @@ -579,15 +581,13 @@ std::pair> SamplingC3Controller::NewCalcCost( DRAKE_THROW_UNLESS(u_plan.size() == N_); // The x_plan from the C3 object does not include the x_N state, so add it in - // by simulating forward one step from x_{N-1} and u_{N-1}. - auto simulate_config = c3::LCSSimulateConfig(); - simulate_config.regularized = true; - simulate_config.min_exp = -8; - // TODO @bibit: figure out which LCS makes the most sense to simulate here - // (probably lcs_for_plan), or do x[N] = A x[N-1] + B u[N-1] + D lambda[N-1] + - // d instead of LCS simulating. - x_plan.push_back( - lcs_for_cost.Simulate(x_plan.back(), u_plan.back(), simulate_config)); + // using the LCS rollout from the last x, u, and lambda. + Eigen::MatrixXd A_N = lcs_for_plan.A().back(); + Eigen::MatrixXd B_N = lcs_for_plan.B().back(); + Eigen::MatrixXd D_N = lcs_for_plan.D().back(); + Eigen::VectorXd d_N = lcs_for_plan.d().back(); + x_plan.push_back(A_N * x_plan.back() + B_N * u_plan.back() + + D_N * lambda_plan.back() + d_N); // Initialize the cost-driving trajectories to match the C3 plan. vector XX = x_plan; @@ -599,6 +599,9 @@ std::pair> SamplingC3Controller::NewCalcCost( // Set a few more variables necessary for some of the cost types. const int ee_vel_index = 7 * num_objects + 3; + auto simulate_config = c3::LCSSimulateConfig(); + simulate_config.regularized = true; + simulate_config.min_exp = -8; // Compute the states and controls to use for cost computation, and change the // cost matrices if necessary. @@ -621,13 +624,6 @@ std::pair> SamplingC3Controller::NewCalcCost( XX[i].segment(ee_vel_index + 3, 6 * num_objects) = XX_sim[i].segment(ee_vel_index + 3, 6 * num_objects); } - // TODO @bibit: don't do the other weird sim step. - // TEMP BELOW // - VectorXd last_x = - lcs_for_cost.Simulate(XX[N_ - 1], UU[N_ - 1], simulate_config); - XX[N_].segment(0, 3) = last_x.segment(0, 3); - XX[N_].segment(ee_vel_index, 3) = last_x.segment(ee_vel_index, 3); - // TEMP ABOVE // } else if (cost_type == C3CostComputationType::kSimImpedance) { // Simulate PD with feedforward control using the C3 plan's states and @@ -652,13 +648,6 @@ std::pair> SamplingC3Controller::NewCalcCost( XX[i].segment(ee_vel_index + 3, 6 * num_objects) = XX_sim[i].segment(ee_vel_index + 3, 6 * num_objects); } - // TODO @bibit: don't do the other weird sim step. - // TEMP BELOW // - VectorXd last_x = - lcs_for_cost.Simulate(XX[N_ - 1], UU[N_ - 1], simulate_config); - XX[N_].segment(0, 3) = last_x.segment(0, 3); - XX[N_].segment(ee_vel_index, 3) = last_x.segment(ee_vel_index, 3); - // TEMP ABOVE // } else if (cost_type == C3CostComputationType::kSimImpedanceObjectCostOnly) { // Simulate PD with feedforward control using the C3 plan's states and From 41ff53a03a0db5afaec4057227aaff9942f66ada Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Thu, 23 Apr 2026 13:56:02 -0400 Subject: [PATCH 08/11] Add and test cost breakdown in new cost calculation; test with printouts to be removed --- MODULE.bazel | 2 +- .../sampling_based_c3_controller.cc | 117 +++++++++++++++++- .../sampling_based_c3_controller.h | 7 +- 3 files changed, 118 insertions(+), 8 deletions(-) diff --git a/MODULE.bazel b/MODULE.bazel index 49ecc4bf8..3d823904c 100644 --- a/MODULE.bazel +++ b/MODULE.bazel @@ -91,7 +91,7 @@ bazel_dep(name = "c3") git_override( module_name = "c3", remote = "https://github.com/DAIRLab/c3.git", - commit = "ef873a41eb73f73760fd7820f3d5efa2ede3df52" + commit = "f96553a7c0f7d43cc53e44a6515995945a6559b5" ) diff --git a/systems/controllers/sampling_based_c3_controller.cc b/systems/controllers/sampling_based_c3_controller.cc index 83b062a91..c2cb3081e 100644 --- a/systems/controllers/sampling_based_c3_controller.cc +++ b/systems/controllers/sampling_based_c3_controller.cc @@ -554,7 +554,8 @@ SamplingC3Controller::SamplingC3Controller( std::pair> SamplingC3Controller::NewCalcCost( C3CostComputationType cost_type, const LCS& lcs_for_cost, const C3::CostMatrices& cost_mats, const std::shared_ptr& c3_object, - const bool& force_tracking_disabled, int num_objects) const { + const bool& force_tracking_disabled, int num_objects, + const bool& print_cost_breakdown) const { // Extract needed information from the C3 object. const LCS lcs_for_plan = c3_object->GetLCS(); vector x_desired = c3_object->GetDesiredState(); @@ -598,7 +599,7 @@ std::pair> SamplingC3Controller::NewCalcCost( vector R_cost = cost_mats.R; // Set a few more variables necessary for some of the cost types. - const int ee_vel_index = 7 * num_objects + 3; + const int ee_vel_index = 3 + 7 * num_objects; auto simulate_config = c3::LCSSimulateConfig(); simulate_config.regularized = true; simulate_config.min_exp = -8; @@ -669,9 +670,112 @@ std::pair> SamplingC3Controller::NewCalcCost( } } + // Compute the cost. double cost = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( XX, x_desired, Q_cost, UU, R_cost); + if (print_cost_breakdown) { + std::cout << "===== NEW COST BREAKDOWN =====" << std::endl; + // Errors + MatrixXd Q_identity = MatrixXd::Identity(n_x_, n_x_); + double error_contrib_ee_pos = + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(0, 3, XX, x_desired, + Q_identity); + double error_contrib_ee_vel = + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + ee_vel_index, ee_vel_index + 3, XX, x_desired, Q_identity); + + double error_contrib_obj_orientation = 0.0; + double error_contrib_obj_pos = 0.0; + double error_contrib_obj_ang_vel = 0.0; + double error_contrib_obj_vel = 0.0; + for (int obj_idx = 0; obj_idx < num_objects; obj_idx++) { + error_contrib_obj_orientation += + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + 3 + 7 * obj_idx, 3 + 7 * obj_idx + 4, XX, x_desired, Q_identity); + error_contrib_obj_pos += + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + 3 + 7 * obj_idx + 4, 3 + 7 * obj_idx + 7, XX, x_desired, + Q_identity); + error_contrib_obj_ang_vel += + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + ee_vel_index + 3 + 6 * obj_idx, + ee_vel_index + 3 + 6 * obj_idx + 3, XX, x_desired, Q_identity); + error_contrib_obj_vel += + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + ee_vel_index + 3 + 6 * obj_idx + 3, + ee_vel_index + 3 + 6 * obj_idx + 6, XX, x_desired, Q_identity); + } + + // Costs + double cost_contrib_ee_pos = + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(0, 3, XX, x_desired, + Q_cost); + double cost_contrib_ee_vel = + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + ee_vel_index, ee_vel_index + 3, XX, x_desired, Q_cost); + double cost_contrib_u = + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(UU, R_cost); + + double cost_contrib_obj_orientation = 0.0; + double cost_contrib_obj_pos = 0.0; + double cost_contrib_obj_ang_vel = 0.0; + double cost_contrib_obj_vel = 0.0; + for (int obj_idx = 0; obj_idx < num_objects; obj_idx++) { + cost_contrib_obj_orientation += + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + 3 + 7 * obj_idx, 3 + 7 * obj_idx + 4, XX, x_desired, Q_cost); + cost_contrib_obj_pos += + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + 3 + 7 * obj_idx + 4, 3 + 7 * obj_idx + 7, XX, x_desired, Q_cost); + cost_contrib_obj_ang_vel += + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + ee_vel_index + 3 + 6 * obj_idx, + ee_vel_index + 3 + 6 * obj_idx + 3, XX, x_desired, Q_cost); + cost_contrib_obj_vel += + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + ee_vel_index + 3 + 6 * obj_idx + 3, + ee_vel_index + 3 + 6 * obj_idx + 6, XX, x_desired, Q_cost); + } + + std::cout << "Error breakdown" << std::endl; + std::cout << "\t total error contribution from x_ee: " + << error_contrib_ee_pos << std::endl; + std::cout << "\t total error contribution from q_obj: " + << error_contrib_obj_orientation << std::endl; + std::cout << "\t total error contribution from x_obj: " + << error_contrib_obj_pos << std::endl; + std::cout << "\t total error contribution from v_ee: " + << error_contrib_ee_vel << std::endl; + std::cout << "\t total error contribution from w_obj: " + << error_contrib_obj_ang_vel << std::endl; + std::cout << "\t total error contribution from v_obj: " + << error_contrib_obj_vel << std::endl; + + std::cout << "\nCOST BREAKDOWN" << std::endl; + std::cout << "\t total cost contribution from x_ee: " << cost_contrib_ee_pos + << std::endl; + std::cout << "\t total cost contribution from q_obj: " + << cost_contrib_obj_orientation << std::endl; + std::cout << "\t total cost contribution from x_obj: " + << cost_contrib_obj_pos << std::endl; + std::cout << "\t total cost contribution from v_ee: " << cost_contrib_ee_vel + << std::endl; + std::cout << "\t total cost contribution from w_obj: " + << cost_contrib_obj_ang_vel << std::endl; + std::cout << "\t total cost contribution from v_obj: " + << cost_contrib_obj_vel << std::endl; + std::cout << "\t total cost contribution from u: " << cost_contrib_u + << std::endl; + + std::cout << "\t total cost is: " << cost << std::endl; + std::cout << "\t total cost object terms only is : " + << cost_contrib_obj_pos + cost_contrib_obj_orientation + + cost_contrib_obj_vel + cost_contrib_obj_ang_vel + << std::endl; + std::cout << "\n\n"; + } + std::pair> ret(cost, XX); return ret; } @@ -1002,6 +1106,7 @@ std::pair> SamplingC3Controller::CalcCost( } if (verbose || print_cost_breakdown) { + std::cout << "===== OLD COST BREAKDOWN =====" << std::endl; std::cout << "Error breakdown" << std::endl; std::cout << "\t total error contribution from x_ee: " << error_contrib_ee_pos << std::endl; @@ -1456,7 +1561,8 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( std::pair> cost_trajectory_pair = CalcCost( cost_type, lcs_candidates_for_cost.at(i), c3_costmat, x_desired, test_c3_object->GetFullSolution(), force_tracking_disabled, - controller_params_.num_objects, print_cost_breakdown, verbose_); + controller_params_.num_objects, print_cost_breakdown, + verbose_ || (i == SampleIndex::kCurrentLocation)); // NewCalcCost(cost_type, lcs_candidates_for_cost.at(i), c3_costmat, // test_c3_object, force_tracking_disabled, // controller_params_.num_objects); @@ -1470,7 +1576,9 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( produce the same results. */ auto [cost_new, __] = NewCalcCost( cost_type, lcs_candidates_for_cost.at(i), c3_costmat, test_c3_object, - force_tracking_disabled, controller_params_.num_objects); + force_tracking_disabled, controller_params_.num_objects, + print_cost_breakdown || verbose_ || + (i == SampleIndex::kCurrentLocation)); std::cout << "Sample " << i << " cost difference: " << std::abs(c3_cost - cost_new) / c3_cost * 100.0 << "% change" << " (old cost: " << c3_cost << ", new cost: " << cost_new << ")" @@ -1478,6 +1586,7 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( // Quit the program if a huge cost error is detected. DRAKE_THROW_UNLESS(cost_new < 1e10); + DRAKE_THROW_UNLESS(i != SampleIndex::kCurrentLocation); /* END OF TEMPORARY TEST */ #pragma omp critical diff --git a/systems/controllers/sampling_based_c3_controller.h b/systems/controllers/sampling_based_c3_controller.h index 5ff50182e..afbddb4d5 100644 --- a/systems/controllers/sampling_based_c3_controller.h +++ b/systems/controllers/sampling_based_c3_controller.h @@ -196,13 +196,14 @@ class SamplingC3Controller : public drake::systems::LeafSystem { private: // TODO @bibit: Functionality to add into NewCalcCost: // [x] enable/disable force tracking - // [ ] print cost breakdown - // [ ] verbose printouts + // [x] print cost breakdown + // [x] verbose printouts std::pair> NewCalcCost( C3CostComputationType cost_type, const c3::LCS& lcs_for_cost, const c3::C3::CostMatrices& cost_mats, const std::shared_ptr& c3_object, - const bool& force_tracking_disabled, int num_objects) const; + const bool& force_tracking_disabled, int num_objects, + const bool& print_cost_breakdown) const; // TODO @bibit: remove and replace with NewCalcCost std::pair> CalcCost( C3CostComputationType cost_type, const c3::LCS& lcs_for_cost, From 29c343a7ce6a715f11b77e6e8ce42baed2ac7b85 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Thu, 23 Apr 2026 14:15:50 -0400 Subject: [PATCH 09/11] Finish migrating sample cost calculations to C3-repo-based implementations --- .../sampling_based_c3_controller.cc | 473 +----------------- .../sampling_based_c3_controller.h | 17 +- 2 files changed, 6 insertions(+), 484 deletions(-) diff --git a/systems/controllers/sampling_based_c3_controller.cc b/systems/controllers/sampling_based_c3_controller.cc index c2cb3081e..0a0b36ce5 100644 --- a/systems/controllers/sampling_based_c3_controller.cc +++ b/systems/controllers/sampling_based_c3_controller.cc @@ -551,7 +551,8 @@ SamplingC3Controller::SamplingC3Controller( } } -std::pair> SamplingC3Controller::NewCalcCost( +// This function relies on the previously computed z_fin from Solve. +std::pair> SamplingC3Controller::CalcCost( C3CostComputationType cost_type, const LCS& lcs_for_cost, const C3::CostMatrices& cost_mats, const std::shared_ptr& c3_object, const bool& force_tracking_disabled, int num_objects, @@ -780,448 +781,6 @@ std::pair> SamplingC3Controller::NewCalcCost( return ret; } -// This function relies on the previously computed z_fin from Solve. -// TODO @bibit: remove this and replace with NewCalcCost -std::pair> SamplingC3Controller::CalcCost( - C3CostComputationType cost_type, const LCS& lcs_for_cost, - const C3::CostMatrices& c3_cost, const std::vector x_desired, - const std::vector z_fin, bool force_tracking_disabled, - int num_objects, bool print_cost_breakdown, bool verbose) const { - int resolution = sampling_c3_options_.lcs_dt_resolution; - - vector UU(N_ * resolution, VectorXd::Zero(n_u_)); - std::vector XX(N_ * resolution + 1, VectorXd::Zero(n_x_)); - - const int ee_vel_index = 7 * num_objects + 3; - - auto simulate_config = c3::LCSSimulateConfig(); - simulate_config.regularized = true; - simulate_config.min_exp = -8; - - // Simulate the dynamics from the planned inputs. - if (cost_type == C3CostComputationType::kSimLCS) { - XX[0] = z_fin[0].segment(0, n_x_); - for (int i = 0; i < N_ * resolution; i++) { - UU[i] = z_fin[i / resolution].segment(n_x_ + n_lambda_, n_u_); - XX[i + 1] = lcs_for_cost.Simulate(XX[i], UU[i], simulate_config); - } - } - - // Use the C3 plan. - else if (cost_type == C3CostComputationType::kUseC3Plan) { - for (int i = 0; i < N_ * resolution; i++) { - UU[i] = z_fin[i / resolution].segment(n_x_ + n_lambda_, n_u_); - XX[i] = z_fin[i / resolution].segment(0, n_x_); - if (i == N_ * resolution - 1) { - XX[i + 1] = lcs_for_cost.Simulate(XX[i], UU[i], simulate_config); - } - } - } - - // Simulate the dynamics from the planned inputs only for the object; use - // the planned EE trajectory. - else if (cost_type == C3CostComputationType::kSimLCSReplaceC3EEPlan) { - // Simulate the object trajectory. - XX[0] = z_fin[0].segment(0, n_x_); - for (int i = 0; i < N_ * resolution; i++) { - UU[i] = z_fin[i / resolution].segment(n_x_ + n_lambda_, n_u_); - XX[i + 1] = lcs_for_cost.Simulate(XX[i], UU[i], simulate_config); - } - // Replace ee traj with those from z_fin. - for (int i = 0; i < N_ * resolution; i++) { - XX[i].segment(0, 3) = z_fin[i / resolution].segment(0, 3); - XX[i].segment(ee_vel_index, 3) = - z_fin[i / resolution].segment(ee_vel_index, 3); - if (i == N_ * resolution - 1) { - VectorXd last_x = lcs_for_cost.Simulate(XX[i], UU[i], simulate_config); - XX[i + 1].segment(0, 3) = last_x.segment(0, 3); - XX[i + 1].segment(ee_vel_index, 3) = last_x.segment(ee_vel_index, 3); - } - } - } - - // Try to emulate the real cost of the system associated not only applying - // the planned u but also the u associated with tracking the position plan - // over time. - else if (cost_type == C3CostComputationType::kSimImpedance) { - std::tie(XX, UU) = SimulatePDControl(lcs_for_cost, z_fin, num_objects, - force_tracking_disabled, verbose); - } - - // The same as the previous cost type except the EE states are replaced with - // the plan from C3 at the end. - else if (cost_type == C3CostComputationType::kSimImpedanceReplaceC3EEPlan) { - std::tie(XX, UU) = SimulatePDControl(lcs_for_cost, z_fin, num_objects, - force_tracking_disabled, verbose); - - // Replace the end effector position and velocity plans with the ones from - // the C3 plan. - for (int i = 0; i < N_ * resolution; i++) { - XX[i].segment(0, 3) = z_fin[i / resolution].segment(0, 3); - XX[i].segment(ee_vel_index, 3) = - z_fin[i / resolution].segment(ee_vel_index, 3); - if (i == N_ * resolution - 1) { - VectorXd last_x = lcs_for_cost.Simulate(XX[i], UU[i], simulate_config); - XX[i + 1].segment(0, 3) = last_x.segment(0, 3); - XX[i + 1].segment(ee_vel_index, 3) = last_x.segment(ee_vel_index, 3); - } - } - } - - // The same as the previous cost type except only object terms contribute to - // the final cost. - if (cost_type == C3CostComputationType::kSimImpedanceObjectCostOnly) { - std::tie(XX, UU) = SimulatePDControl(lcs_for_cost, z_fin, num_objects, - force_tracking_disabled, verbose); - } - - // Declare Q_eff and R_eff as the Q and R to use for cost computation. - std::vector Q_eff = c3_cost.Q; - std::vector R_eff = c3_cost.R; - - // Calculate the cost over the N+1 time steps. - double cost = 0; - - // used only for verbose mode printouts - double error_contrib_ee_pos = 0; - double error_contrib_obj_orientation = 0; - double error_contrib_obj_pos = 0; - double error_contrib_ee_vel = 0; - double error_contrib_obj_ang_vel = 0; - double error_contrib_obj_vel = 0; - - double cost_contrib_ee_pos = 0; - double cost_contrib_obj_orientation = 0; - double cost_contrib_obj_pos = 0; - double cost_contrib_ee_vel = 0; - double cost_contrib_obj_ang_vel = 0; - double cost_contrib_obj_vel = 0; - - int obj_orientation_index = 0; - int obj_pos_index = 0; - int obj_ang_vel_index = 0; - int obj_vel_index = 0; - - double cost_contrib_u = 0; - - // Calculate the error and cost contributions for each state. - for (int i = 0; i < N_ * resolution; i += resolution) { - // ee_pos - error_contrib_ee_pos += - (XX[i].segment(0, 3) - x_desired[i / resolution].segment(0, 3)) - .transpose() * - (XX[i].segment(0, 3) - x_desired[i / resolution].segment(0, 3)); - cost_contrib_ee_pos += - (XX[i].segment(0, 3) - x_desired[i / resolution].segment(0, 3)) - .transpose() * - Q_eff.at(i / resolution).block(0, 0, 3, 3) * - (XX[i].segment(0, 3) - x_desired[i / resolution].segment(0, 3)); - // obj_orientation - for (int j = 0; j < num_objects; j++) { - obj_orientation_index = 7 * j + 3; - obj_pos_index = 7 * j + 7; - error_contrib_obj_orientation += - (XX[i].segment(obj_orientation_index, 4) - - x_desired[i / resolution].segment(obj_orientation_index, 4)) - .transpose() * - (XX[i].segment(obj_orientation_index, 4) - - x_desired[i / resolution].segment(obj_orientation_index, 4)); - cost_contrib_obj_orientation += - (XX[i].segment(obj_orientation_index, 4) - - x_desired[i / resolution].segment(obj_orientation_index, 4)) - .transpose() * - Q_eff.at(i / resolution) - .block(obj_orientation_index, obj_orientation_index, 4, 4) * - (XX[i].segment(obj_orientation_index, 4) - - x_desired[i / resolution].segment(obj_orientation_index, 4)); - // obj_pos - error_contrib_obj_pos += - (XX[i].segment(obj_pos_index, 3) - - x_desired[i / resolution].segment(obj_pos_index, 3)) - .transpose() * - (XX[i].segment(obj_pos_index, 3) - - x_desired[i / resolution].segment(obj_pos_index, 3)); - cost_contrib_obj_pos += - (XX[i].segment(obj_pos_index, 3) - - x_desired[i / resolution].segment(obj_pos_index, 3)) - .transpose() * - Q_eff.at(i / resolution).block(obj_pos_index, obj_pos_index, 3, 3) * - (XX[i].segment(obj_pos_index, 3) - - x_desired[i / resolution].segment(obj_pos_index, 3)); - } - // ee_vel - error_contrib_ee_vel += - (XX[i].segment(ee_vel_index, 3) - - x_desired[i / resolution].segment(ee_vel_index, 3)) - .transpose() * - (XX[i].segment(ee_vel_index, 3) - - x_desired[i / resolution].segment(ee_vel_index, 3)); - cost_contrib_ee_vel += - (XX[i].segment(ee_vel_index, 3) - - x_desired[i / resolution].segment(ee_vel_index, 3)) - .transpose() * - Q_eff.at(i / resolution).block(ee_vel_index, ee_vel_index, 3, 3) * - (XX[i].segment(ee_vel_index, 3) - - x_desired[i / resolution].segment(ee_vel_index, 3)); - // obj_ang_vel - for (int j = 0; j < num_objects; j++) { - obj_ang_vel_index = 6 * j + 6 + 7 * num_objects; - obj_vel_index = 6 * j + 9 + 7 * num_objects; - error_contrib_obj_ang_vel += - (XX[i].segment(obj_ang_vel_index, 3) - - x_desired[i / resolution].segment(obj_ang_vel_index, 3)) - .transpose() * - (XX[i].segment(obj_ang_vel_index, 3) - - x_desired[i / resolution].segment(obj_ang_vel_index, 3)); - cost_contrib_obj_ang_vel += - (XX[i].segment(obj_ang_vel_index, 3) - - x_desired[i / resolution].segment(obj_ang_vel_index, 3)) - .transpose() * - Q_eff.at(i / resolution) - .block(obj_ang_vel_index, obj_ang_vel_index, 3, 3) * - (XX[i].segment(obj_ang_vel_index, 3) - - x_desired[i / resolution].segment(obj_ang_vel_index, 3)); - // obj_vel - error_contrib_obj_vel += - (XX[i].segment(obj_vel_index, 3) - - x_desired[i / resolution].segment(obj_vel_index, 3)) - .transpose() * - (XX[i].segment(obj_vel_index, 3) - - x_desired[i / resolution].segment(obj_vel_index, 3)); - cost_contrib_obj_vel += - (XX[i].segment(obj_vel_index, 3) - - x_desired[i / resolution].segment(obj_vel_index, 3)) - .transpose() * - Q_eff.at(i / resolution).block(obj_vel_index, obj_vel_index, 3, 3) * - (XX[i].segment(obj_vel_index, 3) - - x_desired[i / resolution].segment(obj_vel_index, 3)); - } - - cost = cost + - (XX[i] - x_desired[i / resolution]).transpose() * - Q_eff.at(i / resolution) * (XX[i] - x_desired[i / resolution]) + - UU[i].transpose() * R_eff.at(i / resolution) * UU[i]; - - cost_contrib_u += UU[i].transpose() * R_eff.at(i / resolution) * UU[i]; - } - - DRAKE_DEMAND(!std::isnan(cost_contrib_obj_vel)); - - // Handle the N_th state. - cost = cost + (XX[N_ * resolution] - x_desired[N_]).transpose() * - Q_eff.at(N_) * (XX[N_ * resolution] - x_desired[N_]); - - error_contrib_ee_pos += - (XX[N_ * resolution].segment(0, 3) - x_desired[N_].segment(0, 3)) - .transpose() * - (XX[N_ * resolution].segment(0, 3) - x_desired[N_].segment(0, 3)); - for (int j = 0; j < num_objects; j++) { - obj_orientation_index = 7 * j + 3; - obj_pos_index = 7 * j + 7; - error_contrib_obj_orientation += - (XX[N_ * resolution].segment(obj_orientation_index, 4) - - x_desired[N_].segment(obj_orientation_index, 4)) - .transpose() * - (XX[N_ * resolution].segment(obj_orientation_index, 4) - - x_desired[N_].segment(obj_orientation_index, 4)); - error_contrib_obj_pos += (XX[N_ * resolution].segment(obj_pos_index, 3) - - x_desired[N_].segment(obj_pos_index, 3)) - .transpose() * - (XX[N_ * resolution].segment(obj_pos_index, 3) - - x_desired[N_].segment(obj_pos_index, 3)); - } - error_contrib_ee_vel += (XX[N_ * resolution].segment(ee_vel_index, 3) - - x_desired[N_].segment(ee_vel_index, 3)) - .transpose() * - (XX[N_ * resolution].segment(ee_vel_index, 3) - - x_desired[N_].segment(ee_vel_index, 3)); - - for (int j = 0; j < num_objects; j++) { - obj_ang_vel_index = 6 * j + 6 + 7 * num_objects; - obj_vel_index = 6 * j + 9 + 7 * num_objects; - error_contrib_obj_ang_vel += - (XX[N_ * resolution].segment(obj_ang_vel_index, 3) - - x_desired[N_].segment(obj_ang_vel_index, 3)) - .transpose() * - (XX[N_ * resolution].segment(obj_ang_vel_index, 3) - - x_desired[N_].segment(obj_ang_vel_index, 3)); - error_contrib_obj_vel += (XX[N_ * resolution].segment(obj_vel_index, 3) - - x_desired[N_].segment(obj_vel_index, 3)) - .transpose() * - (XX[N_ * resolution].segment(obj_vel_index, 3) - - x_desired[N_].segment(obj_vel_index, 3)); - } - - cost_contrib_ee_pos += - (XX[N_ * resolution].segment(0, 3) - x_desired[N_].segment(0, 3)) - .transpose() * - Q_eff.at(N_).block(0, 0, 3, 3) * - (XX[N_ * resolution].segment(0, 3) - x_desired[N_].segment(0, 3)); - for (int j = 0; j < num_objects; j++) { - obj_orientation_index = 7 * j + 3; - obj_pos_index = 7 * j + 7; - cost_contrib_obj_orientation += - (XX[N_ * resolution].segment(obj_orientation_index, 4) - - x_desired[N_].segment(obj_orientation_index, 4)) - .transpose() * - Q_eff.at(N_).block(obj_orientation_index, obj_orientation_index, 4, 4) * - (XX[N_ * resolution].segment(obj_orientation_index, 4) - - x_desired[N_].segment(obj_orientation_index, 4)); - cost_contrib_obj_pos += - (XX[N_ * resolution].segment(obj_pos_index, 3) - - x_desired[N_].segment(obj_pos_index, 3)) - .transpose() * - Q_eff.at(N_).block(obj_pos_index, obj_pos_index, 3, 3) * - (XX[N_ * resolution].segment(obj_pos_index, 3) - - x_desired[N_].segment(obj_pos_index, 3)); - } - cost_contrib_ee_vel += (XX[N_ * resolution].segment(ee_vel_index, 3) - - x_desired[N_].segment(ee_vel_index, 3)) - .transpose() * - Q_eff.at(N_).block(ee_vel_index, ee_vel_index, 3, 3) * - (XX[N_ * resolution].segment(ee_vel_index, 3) - - x_desired[N_].segment(ee_vel_index, 3)); - for (int j = 0; j < num_objects; j++) { - obj_ang_vel_index = 6 * j + 6 + 7 * num_objects; - obj_vel_index = 6 * j + 9 + 7 * num_objects; - cost_contrib_obj_ang_vel += - (XX[N_ * resolution].segment(obj_ang_vel_index, 3) - - x_desired[N_].segment(obj_ang_vel_index, 3)) - .transpose() * - Q_eff.at(N_).block(obj_ang_vel_index, obj_ang_vel_index, 3, 3) * - (XX[N_ * resolution].segment(obj_ang_vel_index, 3) - - x_desired[N_].segment(obj_ang_vel_index, 3)); - cost_contrib_obj_vel += - (XX[N_ * resolution].segment(obj_vel_index, 3) - - x_desired[N_].segment(obj_vel_index, 3)) - .transpose() * - Q_eff.at(N_).block(obj_vel_index, obj_vel_index, 3, 3) * - (XX[N_ * resolution].segment(obj_vel_index, 3) - - x_desired[N_].segment(obj_vel_index, 3)); - } - - if (cost_type == C3CostComputationType::kSimImpedanceObjectCostOnly) { - cost = cost_contrib_obj_pos + cost_contrib_obj_orientation + - cost_contrib_obj_vel + cost_contrib_obj_ang_vel; - } - - if (verbose || print_cost_breakdown) { - std::cout << "===== OLD COST BREAKDOWN =====" << std::endl; - std::cout << "Error breakdown" << std::endl; - std::cout << "\t total error contribution from x_ee: " - << error_contrib_ee_pos << std::endl; - std::cout << "\t total error contribution from q_obj: " - << error_contrib_obj_orientation << std::endl; - std::cout << "\t total error contribution from x_obj: " - << error_contrib_obj_pos << std::endl; - std::cout << "\t total error contribution from v_ee: " - << error_contrib_ee_vel << std::endl; - std::cout << "\t total error contribution from w_obj: " - << error_contrib_obj_ang_vel << std::endl; - std::cout << "\t total error contribution from v_obj: " - << error_contrib_obj_vel << std::endl; - - std::cout << "\nCOST BREAKDOWN" << std::endl; - std::cout << "\t total cost contribution from x_ee: " << cost_contrib_ee_pos - << std::endl; - std::cout << "\t total cost contribution from q_obj: " - << cost_contrib_obj_orientation << std::endl; - std::cout << "\t total cost contribution from x_obj: " - << cost_contrib_obj_pos << std::endl; - std::cout << "\t total cost contribution from v_ee: " << cost_contrib_ee_vel - << std::endl; - std::cout << "\t total cost contribution from w_obj: " - << cost_contrib_obj_ang_vel << std::endl; - std::cout << "\t total cost contribution from v_obj: " - << cost_contrib_obj_vel << std::endl; - std::cout << "\t total cost contribution from u: " << cost_contrib_u - << std::endl; - - std::cout << "\t total cost is: " << cost << std::endl; - std::cout << "\t total cost object terms only is : " - << cost_contrib_obj_pos + cost_contrib_obj_orientation + - cost_contrib_obj_vel + cost_contrib_obj_ang_vel - << std::endl; - std::cout << "\n\n"; - } - - std::vector XX_downsampled(N_ + 1, VectorXd::Zero(n_x_)); - - for (int i = 0; i < N_ * resolution; i += resolution) { - XX_downsampled[i / resolution] = XX[i]; - } - XX_downsampled[N_] = XX[N_ * resolution]; - - std::pair> ret(cost, XX_downsampled); - return ret; -} - -// TODO @bibit: remove -std::pair, std::vector> -SamplingC3Controller::SimulatePDControl( - const LCS& lcs_for_cost, const std::vector z_fin, - int num_objects, bool force_tracking_disabled, bool verbose) const { - if (verbose) { - std::cout << "\nSIMULATING PD CONTROL" << std::endl; - } - - const int ee_vel_index = 7 * num_objects + 3; - int resolution = sampling_c3_options_.lcs_dt_resolution; - - auto simulate_config = c3::LCSSimulateConfig(); - simulate_config.regularized = true; - simulate_config.min_exp = -8; - - // Obtain the solutions from C3. - vector UU(N_ * resolution, VectorXd::Zero(n_u_)); - std::vector XX(N_ * resolution + 1, VectorXd::Zero(n_x_)); - for (int i = 0; i < N_ * resolution; i++) { - UU[i] = z_fin[i / resolution].segment(n_x_ + n_lambda_, n_u_); - XX[i] = z_fin[i / resolution].segment(0, n_x_); - if (i == N_ * resolution - 1) { - XX[i + 1] = lcs_for_cost.Simulate(XX[i], UU[i], simulate_config); - } - } - - // Set the PD gains for the emulated tracking controller. - Eigen::VectorXd Kp_vector = Eigen::Map( - sampling_c3_options_.Kp_for_ee_pd_rollout.data(), - sampling_c3_options_.Kp_for_ee_pd_rollout.size()); - Eigen::VectorXd Kd_vector = Eigen::Map( - sampling_c3_options_.Kd_for_ee_pd_rollout.data(), - sampling_c3_options_.Kd_for_ee_pd_rollout.size()); - - Eigen::MatrixXd Kp = Kp_vector.asDiagonal(); - Eigen::MatrixXd Kd = Kd_vector.asDiagonal(); - - // Obtain modified solutions for the PD controller. - std::vector UU_new(N_ * resolution, VectorXd::Zero(n_u_)); - std::vector XX_new(N_ * resolution + 1, - VectorXd::Zero(n_x_)); - - XX_new[0] = z_fin[0].segment(0, n_x_); - // This will just be the original u from z_fin[0] for the first time step. - // if the radio input is true, then the u will only emulate position - // tracking using the PD controller. - for (int i = 0; i < N_ * resolution; i++) { - if (force_tracking_disabled) { - UU_new[i] = Kp * (XX[i].segment(0, 3) - XX_new[i].segment(0, 3)) + - Kd * (XX[i].segment(ee_vel_index, 3) - - XX_new[i].segment(ee_vel_index, 3)); - } else { - UU_new[i] = UU[i] + Kp * (XX[i].segment(0, 3) - XX_new[i].segment(0, 3)) + - Kd * (XX[i].segment(ee_vel_index, 3) - - XX_new[i].segment(ee_vel_index, 3)); - } - if (verbose) { - std::cout << "simulated step " << i + 1 << std::endl; - } - XX_new[i + 1] = - lcs_for_cost.Simulate(XX_new[i], UU_new[i], simulate_config); - } - return {XX_new, UU_new}; -} - drake::systems::EventStatus SamplingC3Controller::ComputePlan( const Context& context, DiscreteValues* discrete_state) const { @@ -1468,7 +1027,6 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( // Parallelize over computing C3 costs for each sample. auto c3_start = std::chrono::high_resolution_clock::now(); - std::cout << "LOOP START" << std::endl; // TODO @bibit: remove after testing #pragma omp parallel for num_threads(num_threads_to_use_) for (int i = 0; i < num_total_samples; i++) { bool print_cost_breakdown = @@ -1559,36 +1117,15 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( auto cc_start = std::chrono::high_resolution_clock::now(); std::pair> cost_trajectory_pair = CalcCost( - cost_type, lcs_candidates_for_cost.at(i), c3_costmat, x_desired, - test_c3_object->GetFullSolution(), force_tracking_disabled, - controller_params_.num_objects, print_cost_breakdown, - verbose_ || (i == SampleIndex::kCurrentLocation)); - // NewCalcCost(cost_type, lcs_candidates_for_cost.at(i), c3_costmat, - // test_c3_object, force_tracking_disabled, - // controller_params_.num_objects); + cost_type, lcs_candidates_for_cost.at(i), c3_costmat, test_c3_object, + force_tracking_disabled, controller_params_.num_objects, + print_cost_breakdown || verbose_); auto cc_end = std::chrono::high_resolution_clock::now(); std::chrono::duration duration_ms = cc_end - cc_start; double c3_cost = cost_trajectory_pair.first; all_sample_dynamically_feasible_plans_.at(i) = cost_trajectory_pair.second; - /* TEMPORARY TEST: Check that the new and old cost computation methods - produce the same results. */ - auto [cost_new, __] = NewCalcCost( - cost_type, lcs_candidates_for_cost.at(i), c3_costmat, test_c3_object, - force_tracking_disabled, controller_params_.num_objects, - print_cost_breakdown || verbose_ || - (i == SampleIndex::kCurrentLocation)); - std::cout << "Sample " << i << " cost difference: " - << std::abs(c3_cost - cost_new) / c3_cost * 100.0 << "% change" - << " (old cost: " << c3_cost << ", new cost: " << cost_new << ")" - << std::endl; - - // Quit the program if a huge cost error is detected. - DRAKE_THROW_UNLESS(cost_new < 1e10); - DRAKE_THROW_UNLESS(i != SampleIndex::kCurrentLocation); - /* END OF TEMPORARY TEST */ - #pragma omp critical { c3_objects.at(i) = test_c3_object; diff --git a/systems/controllers/sampling_based_c3_controller.h b/systems/controllers/sampling_based_c3_controller.h index afbddb4d5..707283055 100644 --- a/systems/controllers/sampling_based_c3_controller.h +++ b/systems/controllers/sampling_based_c3_controller.h @@ -194,27 +194,12 @@ class SamplingC3Controller : public drake::systems::LeafSystem { } private: - // TODO @bibit: Functionality to add into NewCalcCost: - // [x] enable/disable force tracking - // [x] print cost breakdown - // [x] verbose printouts - std::pair> NewCalcCost( + std::pair> CalcCost( C3CostComputationType cost_type, const c3::LCS& lcs_for_cost, const c3::C3::CostMatrices& cost_mats, const std::shared_ptr& c3_object, const bool& force_tracking_disabled, int num_objects, const bool& print_cost_breakdown) const; - // TODO @bibit: remove and replace with NewCalcCost - std::pair> CalcCost( - C3CostComputationType cost_type, const c3::LCS& lcs_for_cost, - const c3::C3::CostMatrices& c3_cost, - const std::vector x_desired, - const std::vector z_fin, bool force_tracking_disabled, - int num_objects, bool print_cost_breakdown, bool verbose) const; - std::pair, std::vector> - SimulatePDControl(const c3::LCS& lcs_for_cost, - const std::vector z_fin, int num_objects, - bool force_tracking_disabled, bool verbose) const; /// Function for computing one control loop drake::systems::EventStatus ComputePlan( const drake::systems::Context& context, From 3b033ecdbe4ee3c3107e65d251d31aae8135a2d9 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Thu, 23 Apr 2026 14:22:22 -0400 Subject: [PATCH 10/11] Use updated C3 commit --- MODULE.bazel | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MODULE.bazel b/MODULE.bazel index 3d823904c..643c5d7d6 100644 --- a/MODULE.bazel +++ b/MODULE.bazel @@ -91,7 +91,7 @@ bazel_dep(name = "c3") git_override( module_name = "c3", remote = "https://github.com/DAIRLab/c3.git", - commit = "f96553a7c0f7d43cc53e44a6515995945a6559b5" + commit = "d95bdf834be3b031ac3d7772dfdacda97723dd5a" ) From 90e3d40710ee4a3d2e4428ec96c7e880fa2c6dd4 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Thu, 23 Apr 2026 16:26:51 -0400 Subject: [PATCH 11/11] Update MODULE.bazel to point to C3 main branch after C3 merge --- MODULE.bazel | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MODULE.bazel b/MODULE.bazel index 643c5d7d6..0205eb3e8 100644 --- a/MODULE.bazel +++ b/MODULE.bazel @@ -91,7 +91,7 @@ bazel_dep(name = "c3") git_override( module_name = "c3", remote = "https://github.com/DAIRLab/c3.git", - commit = "d95bdf834be3b031ac3d7772dfdacda97723dd5a" + commit = "5c08cb2e14b1ab10e024cb46e8504970cffcd5ea" )