diff --git a/MODULE.bazel b/MODULE.bazel index c2ad9d8a1..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 = "e2170374aaec3fc40dc0566e869d980482ed309d" + commit = "5c08cb2e14b1ab10e024cb46e8504970cffcd5ea" ) diff --git a/systems/controllers/BUILD.bazel b/systems/controllers/BUILD.bazel index 7a80aecf1..d1d9109a9 100644 --- a/systems/controllers/BUILD.bazel +++ b/systems/controllers/BUILD.bazel @@ -72,6 +72,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 f51687960..0a0b36ce5 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" @@ -32,6 +33,7 @@ using c3::LCS; using c3::multibody::LCSContactDescription; using c3::multibody::LCSFactory; using c3::systems::C3Output; +using c3::traj_eval::TrajectoryEvaluator; using drake::SortedPair; using drake::geometry::GeometryId; using drake::multibody::ModelInstanceIndex; @@ -81,6 +83,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. @@ -117,9 +122,9 @@ SamplingC3Controller::SamplingC3Controller( } solve_time_filter_constant_ = sampling_c3_options_.solve_time_filter_alpha; - DRAKE_DEMAND(sampling_c3_options_.num_contacts.has_value()); - DRAKE_DEMAND(sampling_c3_options_.num_friction_directions_per_contact.has_value()); + DRAKE_DEMAND( + sampling_c3_options_.num_friction_directions_per_contact.has_value()); n_lambda_ = LCSFactory::GetNumContactVariables( c3::multibody::GetContactModelMap().at( controller_params_.sampling_c3_options.contact_model), @@ -456,6 +461,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. @@ -536,322 +552,193 @@ SamplingC3Controller::SamplingC3Controller( } // This function relies on the previously computed z_fin from Solve. -std::pair> SamplingC3Controller::CalcCost( +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; - + const C3::CostMatrices& cost_mats, const std::shared_ptr& c3_object, + 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(); + 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 + // 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_); + 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_); + 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 + // 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; + 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 = 3 + 7 * num_objects; auto simulate_config = c3::LCSSimulateConfig(); simulate_config.regularized = true; simulate_config.min_exp = -8; - // Simulate the dynamics from the planned inputs. + // Compute the states and controls to use for cost computation, and change the + // cost matrices if necessary. 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); - } - } + // 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); - // 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_ - 1) { - XX[i + 1] = lcs_for_cost.Simulate(XX[i], UU[i], simulate_config); - } - } - } + } else if (cost_type == C3CostComputationType::kUseC3Plan) { + // No need to do anything here. - // 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_; 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); + } 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); + } + + } 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); + } + + } 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; } } } - // 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); + // Compute the cost. + double cost = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + XX, x_desired, Q_cost, UU, R_cost); - // 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); - 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); - } - } - } + 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); - // 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; + 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 += - (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 + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + 3 + 7 * obj_idx, 3 + 7 * obj_idx + 4, XX, x_desired, Q_identity); 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; + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + 3 + 7 * obj_idx + 4, 3 + 7 * obj_idx + 7, XX, x_desired, + Q_identity); 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 + 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 += - (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)); + 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 += - (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_] - x_desired[N_]).transpose() * Q_eff.at(N_) * - (XX[N_] - 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)); - 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) - - x_desired[N_].segment(obj_orientation_index, 4)) - .transpose() * - (XX[N_].segment(obj_orientation_index, 4) - - x_desired[N_].segment(obj_orientation_index, 4)); - error_contrib_obj_pos += (XX[N_].segment(obj_pos_index, 3) - - x_desired[N_].segment(obj_pos_index, 3)) - .transpose() * - (XX[N_].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)); - - 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) - - x_desired[N_].segment(obj_vel_index, 3)) - .transpose() * - (XX[N_].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() * - Q_eff.at(N_).block(0, 0, 3, 3) * - (XX[N_].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) - - 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) - - x_desired[N_].segment(obj_orientation_index, 4)); - cost_contrib_obj_pos += - (XX[N_].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) - - 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)); - 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) - - 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) - - x_desired[N_].segment(obj_ang_vel_index, 3)); - cost_contrib_obj_vel += - (XX[N_].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) - - 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) { + 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; @@ -890,91 +777,10 @@ std::pair> SamplingC3Controller::CalcCost( 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]; - - // 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; + std::pair> ret(cost, XX); return ret; } -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 { @@ -1054,6 +860,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_++; @@ -1081,6 +890,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. @@ -1305,11 +1116,10 @@ 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, 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; diff --git a/systems/controllers/sampling_based_c3_controller.h b/systems/controllers/sampling_based_c3_controller.h index cd6ad34ca..707283055 100644 --- a/systems/controllers/sampling_based_c3_controller.h +++ b/systems/controllers/sampling_based_c3_controller.h @@ -196,14 +196,10 @@ class SamplingC3Controller : public drake::systems::LeafSystem { private: 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; + 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; /// Function for computing one control loop drake::systems::EventStatus ComputePlan( const drake::systems::Context& context, @@ -420,8 +416,10 @@ 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_; + + Eigen::VectorXd Kp_for_cost_; + Eigen::VectorXd Kd_for_cost_; mutable std::vector Q_; mutable std::vector R_;