diff --git a/MODULE.bazel b/MODULE.bazel index 7b559c289..05d731d40 100644 --- a/MODULE.bazel +++ b/MODULE.bazel @@ -89,6 +89,17 @@ archive_override( strip_prefix = "drake-{}".format(DRAKE_VERSION.lstrip("v")), ) +bazel_dep(name = "c3") +# git_override( +# module_name = "c3", +# remote = "https://github.com/DAIRLab/c3.git", +# commit = "2bd13495c3306cf7d53992e2495774d1ad454cca" +# ) +local_path_override( + module_name = "c3", + path = "/home/stephen/Workspace/DAIR/c3" +) + INEKF_COMMIT = "297c308e50fa599af92ce3bd5f11d71e2bf8af69" INEKF_CHECKSUM = "c5a056ce00e1625e52f5a71b1d5c202acd53c1a8c7bca33da458db1e4f3f2edf" diff --git a/examples/sampling_c3/anything/parameters/sampling_c3plus_options.yaml b/examples/sampling_c3/anything/parameters/sampling_c3plus_options.yaml index 84dd4054c..06978efbd 100644 --- a/examples/sampling_c3/anything/parameters/sampling_c3plus_options.yaml +++ b/examples/sampling_c3/anything/parameters/sampling_c3plus_options.yaml @@ -1,6 +1,6 @@ ### C3 options admm_iter: 3 -rho: 0 #This isn't used anywhere! +# rho: 0 #This isn't used anywhere! rho_scale: 3 num_threads: 5 num_outer_threads: 5 @@ -8,11 +8,14 @@ delta_option: 1 projection_type: 'C3+' # 'MIQP' or 'QP' or 'C3+' contact_model: 'anitescu' # 'stewart_and_trinkle' or 'anitescu'. warm_start: false +scale_lcs: true end_on_qp_step: false solve_time_filter_alpha: 0.95 publish_frequency: 0 -penalize_changes_in_u_across_solves: true # Penalize (u-u_prev) instead of u. +penalize_input_change: false # Penalize (u-u_prev) instead of u. num_friction_directions: 2 +spring_stiffness: 0.0 # Not used in C3+. +final_augmented_cost_scaling: 1000.0 N: 7 gamma: 1.0 # discount factor on MPC costs @@ -148,12 +151,12 @@ u_eta_position_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, ### NO NEED TO CHANGE THE BELOW. Parameters needed for the C3Options struct but ### are overwritten by other sampling C3 parameters. -use_predicted_x0: false # instead: use_predicted_x0_c3, +# use_predicted_x0: false # instead: use_predicted_x0_c3, # use_predicted_x0_repos, # use_predicted_x0_reset_mechanism dt: 0 # instead: planning_dt_pose, planning_dt_position -dt_cost: 0 -solve_dt: 0 # unused +# solve_dt: 0 # unused +dt_cost: 0 mu: [] # instead based on indexing into mu_per_pair_type num_contacts: 0 # instead based on summing index of resolve_contacts_to_lists # Instead for the below, index into their _list versions. diff --git a/examples/sampling_c3/generate_samples.cc b/examples/sampling_c3/generate_samples.cc index ac9b94ec8..7559f9983 100644 --- a/examples/sampling_c3/generate_samples.cc +++ b/examples/sampling_c3/generate_samples.cc @@ -827,7 +827,7 @@ bool IsSampleWithinDistanceOfSurface( multibody::GeomGeomCollider collider(plant, pair); auto [phi_i, J_i] = collider.EvalPolytope( - *context, sampling_c3_options.num_friction_directions); + *context, sampling_c3_options.num_friction_directions.value()); distances.push_back(phi_i); } diff --git a/examples/sampling_c3/generate_samples.h b/examples/sampling_c3/generate_samples.h index 6ab10d12e..d980e50ce 100644 --- a/examples/sampling_c3/generate_samples.h +++ b/examples/sampling_c3/generate_samples.h @@ -10,7 +10,6 @@ #include "examples/sampling_c3/parameter_headers/sampling_params.h" #include "multibody/geom_geom_collider.h" #include "multibody/multibody_utils.h" -#include "solvers/c3_options.h" #include #include "systems/controllers/face.h" diff --git a/examples/sampling_c3/jacktoy/parameters/sampling_c3plus_options.yaml b/examples/sampling_c3/jacktoy/parameters/sampling_c3plus_options.yaml index 80a7cdd05..70388580d 100644 --- a/examples/sampling_c3/jacktoy/parameters/sampling_c3plus_options.yaml +++ b/examples/sampling_c3/jacktoy/parameters/sampling_c3plus_options.yaml @@ -1,6 +1,6 @@ ### C3 options admm_iter: 3 -rho: 0 #This isn't used anywhere! +# rho: 0 #This isn't used anywhere! rho_scale: 3 num_threads: 5 num_outer_threads: 4 @@ -8,11 +8,14 @@ delta_option: 1 projection_type: 'C3+' # 'MIQP' or 'QP' or 'C3+' contact_model: 'anitescu' # 'stewart_and_trinkle' or 'anitescu'. warm_start: false +scale_lcs: true end_on_qp_step: false +# use_robust_formulation: false solve_time_filter_alpha: 0.95 publish_frequency: 0 -penalize_changes_in_u_across_solves: false # Penalize (u-u_prev) instead of u. +penalize_input_change: false # Penalize (u-u_prev) instead of u. num_friction_directions: 2 +spring_stiffness: 0.0 # Not used in C3+. N: 5 gamma: 1.0 # discount factor on MPC costs @@ -346,11 +349,11 @@ u_eta_position_list: [ ### NO NEED TO CHANGE THE BELOW. Parameters needed for the C3Options struct but ### are overwritten by other sampling C3 parameters. -use_predicted_x0: false # instead: use_predicted_x0_c3, +# use_predicted_x0: false # instead: use_predicted_x0_c3, # use_predicted_x0_repos, # use_predicted_x0_reset_mechanism dt: 0 # instead: planning_dt_pose, planning_dt_position -solve_dt: 0 # unused +# solve_dt: 0 # unused mu: [] # instead based on indexing into mu_per_pair_type num_contacts: 0 # instead based on summing index of resolve_contacts_to_lists # Instead for the below, index into their _list versions. diff --git a/examples/sampling_c3/parameter_headers/BUILD.bazel b/examples/sampling_c3/parameter_headers/BUILD.bazel index 8377beeb8..6e7d43ce8 100644 --- a/examples/sampling_c3/parameter_headers/BUILD.bazel +++ b/examples/sampling_c3/parameter_headers/BUILD.bazel @@ -2,7 +2,8 @@ cc_library( name = "sampling_c3_options", hdrs = ["sampling_c3_options.h"], visibility = ["//visibility:public"], # Allow all subpackages to use it - deps = ["//solvers:c3"] + deps = ["@c3//:libc3", + ], ) cc_library( diff --git a/examples/sampling_c3/parameter_headers/progress_params.h b/examples/sampling_c3/parameter_headers/progress_params.h index ed4cca9e2..58b942f79 100644 --- a/examples/sampling_c3/parameter_headers/progress_params.h +++ b/examples/sampling_c3/parameter_headers/progress_params.h @@ -3,7 +3,6 @@ #include "drake/common/yaml/yaml_read_archive.h" #include "common/file_utils.h" -#include "solvers/c3_options.h" /* C3 progress metric options, all phrased as improvement requirements over a @@ -22,6 +21,31 @@ enum ProgressMetric { kConfigCostDrop }; +/* Ways of computing C3 costs after solving the MPC problem: + 0. kSimLCS: Simulate the LCS dynamics from the planned + inputs. + 1. kUseC3Plan: Use the C3 planned trajectory and inputs. + 2. kSimLCSReplaceC3EEPlan: Simulate the LCS dynamics from the planned + inputs only for the object; use the planned + EE trajectory. + 3. kSimImpedance: Try to emulate the real cost of the system + associated not only applying the planned + inputs, but also tracking the planned EE + trajectory with an impedance controller. + 4. kSimImpedanceReplaceC3EEPlan: The same as kSimImpedance except the EE + states are replaced with the plan from C3 at + the end. + 5. kSimImpedanceObjectCostOnly: The same as kSimImpedance except only the + object terms contribute to the final cost. +*/ +enum C3CostComputationType { + kSimLCS, + kUseC3Plan, + kSimLCSReplaceC3EEPlan, + kSimImpedance, + kSimImpedanceReplaceC3EEPlan, + kSimImpedanceObjectCostOnly, +}; struct SamplingC3ProgressParams { C3CostComputationType cost_type; diff --git a/examples/sampling_c3/parameter_headers/sampling_c3_options.h b/examples/sampling_c3/parameter_headers/sampling_c3_options.h index 07e43dd42..ba325e850 100644 --- a/examples/sampling_c3/parameter_headers/sampling_c3_options.h +++ b/examples/sampling_c3/parameter_headers/sampling_c3_options.h @@ -1,13 +1,21 @@ #pragma once -#include #include +#include #include -#include "solvers/c3_options.h" +#include "c3/core/c3_options.h" +#include "c3/multibody/lcs_factory_options.h" #include "drake/common/yaml/yaml_read_archive.h" -struct SamplingC3Options : C3Options { +using c3::C3Options; +using c3::LCSFactoryOptions; + +struct SamplingC3Options : C3Options, LCSFactoryOptions { + std::string projection_type; // "QP" or "MIQP" or "C3+" + double solve_time_filter_alpha = 0.0; + double publish_frequency = 100.0; // Hz + int num_outer_threads; // for outer sampling loop. /// Additional radius-based workspace limit. @@ -36,7 +44,7 @@ struct SamplingC3Options : C3Options { int num_planar_contacts_cost; int n_lambda_with_tangential_cost; - std::vector num_friction_directions_per_contact_cost; + std::vector num_friction_directions_per_contact_for_cost; std::vector starting_index_per_contact_in_lambda_t_vector_cost; double planning_dt_pose; @@ -92,7 +100,6 @@ struct SamplingC3Options : C3Options { std::vector> g_lambda_position_list; std::vector g_u_position; - std::vector u_x_position; std::vector> u_gamma_position_list; std::vector> u_lambda_n_position_list; @@ -110,12 +117,26 @@ struct SamplingC3Options : C3Options { std::vector> u_eta_t_position_list; std::vector> u_eta_position_list; + std::vector + u_horizontal_limits; ///< Limits for horizontal actuator inputs. + std::vector + u_vertical_limits; ///< Limits for vertical actuator inputs. + std::vector + workspace_limits; ///< Workspace boundaries as vectors. + double workspace_margins; ///< Margins to be maintained within the workspace. + C3Options c3_options_pose; + LCSFactoryOptions lcs_factory_options_pose; C3Options c3_options_position; + LCSFactoryOptions lcs_factory_options_position; template void Serialize(Archive* a) { C3Options::Serialize(a); + LCSFactoryOptions::Serialize(a); + a->Visit(DRAKE_NVP(projection_type)); + a->Visit(DRAKE_NVP(solve_time_filter_alpha)); + a->Visit(DRAKE_NVP(publish_frequency)); a->Visit(DRAKE_NVP(num_outer_threads)); a->Visit(DRAKE_NVP(robot_radius_limits)); a->Visit(DRAKE_NVP(use_predicted_x0_c3)); @@ -193,30 +214,39 @@ struct SamplingC3Options : C3Options { a->Visit(DRAKE_NVP(u_eta_t_position_list)); a->Visit(DRAKE_NVP(u_eta_position_list)); + a->Visit(DRAKE_NVP(u_horizontal_limits)); + a->Visit(DRAKE_NVP(u_vertical_limits)); + a->Visit(DRAKE_NVP(workspace_limits)); + a->Visit(DRAKE_NVP(workspace_margins)); + + DRAKE_ASSERT(num_friction_directions.has_value()); + // Set a few parameters based on num_contacts_index, differentiating between // for C3 solve and for C3 cost computation. resolve_contacts_to = resolve_contacts_to_lists[num_contacts_index]; resolve_contacts_to_for_cost = - resolve_contacts_to_lists[num_contacts_index_for_cost]; + resolve_contacts_to_lists[num_contacts_index_for_cost]; - if (!include_walls){ - if (resolve_contacts_to.back() != 0 || resolve_contacts_to_for_cost.back() != 0){ + if (!include_walls) { + if (resolve_contacts_to.back() != 0 || + resolve_contacts_to_for_cost.back() != 0) { throw std::runtime_error( - "Walls are not included, the number of contacts between object and wall must be 0."); + "Walls are not included, the number of contacts between object and " + "wall must be 0."); } } - num_contacts = std::accumulate( - resolve_contacts_to.begin(), resolve_contacts_to.end(), 0); - num_contacts_for_cost = std::accumulate( - resolve_contacts_to_for_cost.begin(), resolve_contacts_to_for_cost.end(), - 0); - mu.clear(); + num_contacts = std::accumulate(resolve_contacts_to.begin(), + resolve_contacts_to.end(), 0); + num_contacts_for_cost = + std::accumulate(resolve_contacts_to_for_cost.begin(), + resolve_contacts_to_for_cost.end(), 0); + mu = std::vector(); for (size_t i = 0; i < mu_per_pair_type.size(); ++i) { int repeat = resolve_contacts_to_lists[num_contacts_index][i]; - mu.insert(mu.end(), repeat, mu_per_pair_type[i]); + mu.value().insert(mu.value().end(), repeat, mu_per_pair_type[i]); } - mu_for_cost.clear(); + mu_for_cost = std::vector(); for (size_t i = 0; i < mu_per_pair_type.size(); ++i) { int repeat = resolve_contacts_to_lists[num_contacts_index_for_cost][i]; mu_for_cost.insert(mu_for_cost.end(), repeat, mu_per_pair_type[i]); @@ -224,295 +254,321 @@ struct SamplingC3Options : C3Options { // Process planar contact info std::tie(num_planar_contacts, num_friction_directions_per_contact) = - ProcessPlanarContactInformation(resolve_as_planar_contacts_list, - resolve_contacts_to, num_friction_directions); - std::tie(num_planar_contacts_cost, num_friction_directions_per_contact_cost) = - ProcessPlanarContactInformation(resolve_as_planar_contacts_list, - resolve_contacts_to_for_cost, num_friction_directions); + ProcessPlanarContactInformation(resolve_as_planar_contacts_list, + resolve_contacts_to, + num_friction_directions.value()); + std::tie(num_planar_contacts_cost, + num_friction_directions_per_contact_for_cost) = + ProcessPlanarContactInformation(resolve_as_planar_contacts_list, + resolve_contacts_to_for_cost, + num_friction_directions.value()); for (size_t i = 0; i < num_contacts; ++i) { starting_index_per_contact_in_lambda_t_vector.push_back( - 2 * std::accumulate(num_friction_directions_per_contact.begin(), - num_friction_directions_per_contact.begin()+i,0)); + 2 * std::accumulate(num_friction_directions_per_contact.begin(), + num_friction_directions_per_contact.begin() + i, + 0)); } for (size_t i = 0; i < num_contacts_for_cost; ++i) { starting_index_per_contact_in_lambda_t_vector_cost.push_back( - 2 * std::accumulate(num_friction_directions_per_contact_cost.begin(), - num_friction_directions_per_contact_cost.begin()+i,0)); + 2 * std::accumulate( + num_friction_directions_per_contact_for_cost.begin(), + num_friction_directions_per_contact_for_cost.begin() + i, 0)); } - n_lambda_with_tangential = - 2 * num_friction_directions * (num_contacts - num_planar_contacts) + - 2 * num_planar_contacts; + n_lambda_with_tangential = 2 * num_friction_directions.value() * + (num_contacts - num_planar_contacts) + + 2 * num_planar_contacts; n_lambda_with_tangential_cost = - 2 * num_friction_directions * (num_contacts_for_cost - num_planar_contacts_cost) + + 2 * num_friction_directions.value() * + (num_contacts_for_cost - num_planar_contacts_cost) + 2 * num_planar_contacts_cost; // Create C3 options for both pose and position tracking. - SetCommonC3Options(&c3_options_pose); - SetPoseTrackingOptions(&c3_options_pose); - SetCommonC3Options(&c3_options_position); - SetPositionTrackingOptions(&c3_options_position); + SetCommonOptions(&c3_options_pose, &lcs_factory_options_pose); + SetPoseTrackingOptions(&c3_options_pose, &lcs_factory_options_pose); + SetCommonOptions(&c3_options_position, &lcs_factory_options_position); + SetPositionTrackingOptions(&c3_options_position, + &lcs_factory_options_position); } C3Options GetC3Options(const bool& is_pose_tracking) const { - if (is_pose_tracking) { return c3_options_pose; } + if (is_pose_tracking) { + return c3_options_pose; + } return c3_options_position; } - private: - void PopulateCostMatricesFromVectors(C3Options* options) const { - std::vector g_vector = std::vector(); - g_vector.insert(g_vector.end(), options->g_x.begin(), options->g_x.end()); - if (options->contact_model == "stewart_and_trinkle") { - g_vector.insert(g_vector.end(), options->g_gamma.begin(), - options->g_gamma.end()); - g_vector.insert(g_vector.end(), options->g_lambda_n.begin(), - options->g_lambda_n.end()); - g_vector.insert(g_vector.end(), options->g_lambda_t.begin(), - options->g_lambda_t.end()); + LCSFactoryOptions GetLCSFactoryOptions(const bool& is_pose_tracking) const { + if (is_pose_tracking) { + return lcs_factory_options_pose; + } + return lcs_factory_options_position; + } + + private: + void PopulateCostMatricesFromVectors(C3Options* options) const { + std::vector g_vector = std::vector(); + g_vector.insert(g_vector.end(), options->g_x.begin(), options->g_x.end()); + if (contact_model == "stewart_and_trinkle") { + g_vector.insert(g_vector.end(), options->g_gamma.begin(), + options->g_gamma.end()); + g_vector.insert(g_vector.end(), options->g_lambda_n.begin(), + options->g_lambda_n.end()); + g_vector.insert(g_vector.end(), options->g_lambda_t.begin(), + options->g_lambda_t.end()); + } else { + g_vector.insert(g_vector.end(), options->g_lambda.begin(), + options->g_lambda.end()); + } + g_vector.insert(g_vector.end(), options->g_u.begin(), options->g_u.end()); + + if (projection_type == "C3+") { + if (contact_model == "stewart_and_trinkle") { + g_vector.insert(g_vector.end(), options->g_eta_slack->begin(), + options->g_eta_slack->end()); + g_vector.insert(g_vector.end(), options->g_eta_n->begin(), + options->g_eta_n->end()); + g_vector.insert(g_vector.end(), options->g_eta_t->begin(), + options->g_eta_t->end()); } else { - g_vector.insert(g_vector.end(), options->g_lambda.begin(), - options->g_lambda.end()); - } - g_vector.insert(g_vector.end(), options->g_u.begin(), options->g_u.end()); - - if (options->projection_type == "C3+") { - if (options->contact_model == "stewart_and_trinkle") { - g_vector.insert(g_vector.end(), options->g_eta_slack.begin(), - options->g_eta_slack.end()); - g_vector.insert(g_vector.end(), options->g_eta_n.begin(), - options->g_eta_n.end()); - g_vector.insert(g_vector.end(), options->g_eta_t.begin(), - options->g_eta_t.end()); - } else { - g_vector.insert(g_vector.end(), options->g_eta.begin(), - options->g_eta.end()); - } + g_vector.insert(g_vector.end(), options->g_eta->begin(), + options->g_eta->end()); } + } - std::vector u_vector = std::vector(); - u_vector.insert(u_vector.end(), options->u_x.begin(), options->u_x.end()); - if (options->contact_model == "stewart_and_trinkle") { - u_vector.insert(u_vector.end(), options->u_gamma.begin(), - options->u_gamma.end()); - u_vector.insert(u_vector.end(), options->u_lambda_n.begin(), - options->u_lambda_n.end()); - u_vector.insert(u_vector.end(), options->u_lambda_t.begin(), - options->u_lambda_t.end()); + std::vector u_vector = std::vector(); + u_vector.insert(u_vector.end(), options->u_x.begin(), options->u_x.end()); + if (contact_model == "stewart_and_trinkle") { + u_vector.insert(u_vector.end(), options->u_gamma.begin(), + options->u_gamma.end()); + u_vector.insert(u_vector.end(), options->u_lambda_n.begin(), + options->u_lambda_n.end()); + u_vector.insert(u_vector.end(), options->u_lambda_t.begin(), + options->u_lambda_t.end()); + } else { + u_vector.insert(u_vector.end(), options->u_lambda.begin(), + options->u_lambda.end()); + } + u_vector.insert(u_vector.end(), options->u_u.begin(), options->u_u.end()); + + if (projection_type == "C3+") { + if (contact_model == "stewart_and_trinkle") { + u_vector.insert(u_vector.end(), options->u_eta_slack->begin(), + options->u_eta_slack->end()); + u_vector.insert(u_vector.end(), options->u_eta_n->begin(), + options->u_eta_n->end()); + u_vector.insert(u_vector.end(), options->u_eta_t->begin(), + options->u_eta_t->end()); } else { - u_vector.insert(u_vector.end(), options->u_lambda.begin(), - options->u_lambda.end()); - } - u_vector.insert(u_vector.end(), options->u_u.begin(), options->u_u.end()); - - if (options->projection_type == "C3+") { - if (options->contact_model == "stewart_and_trinkle") { - u_vector.insert(u_vector.end(), options->u_eta_slack.begin(), - options->u_eta_slack.end()); - u_vector.insert(u_vector.end(), options->u_eta_n.begin(), - options->u_eta_n.end()); - u_vector.insert(u_vector.end(), options->u_eta_t.begin(), - options->u_eta_t.end()); - } else { - u_vector.insert(u_vector.end(), options->u_eta.begin(), - options->u_eta.end()); - } + u_vector.insert(u_vector.end(), options->u_eta->begin(), + options->u_eta->end()); } - - options->g_vector = g_vector; - options->u_vector = u_vector; - - Eigen::VectorXd q = Eigen::Map( - options->q_vector.data(), options->q_vector.size()); - Eigen::VectorXd r = Eigen::Map( - options->r_vector.data(), options->r_vector.size()); - Eigen::VectorXd g = Eigen::Map( - options->g_vector.data(), options->g_vector.size()); - Eigen::VectorXd u = Eigen::Map( - options->u_vector.data(), options->u_vector.size()); - - options->Q = options->w_Q * q.asDiagonal(); - options->R = options->w_R * r.asDiagonal(); - options->G = options->w_G * g.asDiagonal(); - options->U = options->w_U * u.asDiagonal(); } - void SetCommonC3Options(C3Options* options) const { - options->admm_iter = admm_iter; - options->rho = rho; - options->rho_scale = rho_scale; - options->num_threads = num_threads; - options->delta_option = delta_option; - options->contact_model = contact_model; - options->projection_type = projection_type; - options->warm_start = warm_start; - options->use_predicted_x0 = false; // unused by sampling C3 - options->end_on_qp_step = end_on_qp_step; - options->solve_time_filter_alpha = solve_time_filter_alpha; - options->publish_frequency = publish_frequency; - - options->workspace_limits = workspace_limits; - options->workspace_margins = workspace_margins; - options->u_horizontal_limits = u_horizontal_limits; - options->u_vertical_limits = u_vertical_limits; - - options->N = N; - options->gamma = gamma; - - options->solve_dt = 0; // unused in all of C3 - options-> lcs_dt_resolution = lcs_dt_resolution; - options->num_friction_directions = num_friction_directions; - - options->qp_projection_alpha = qp_projection_alpha; - options->qp_projection_scaling = qp_projection_scaling; - options->penalize_changes_in_u_across_solves = - penalize_changes_in_u_across_solves; - - options->mu = mu; - options->num_contacts = num_contacts; - } + options->g_vector = g_vector; + options->u_vector = u_vector; + + Eigen::VectorXd q = Eigen::Map( + options->q_vector.data(), options->q_vector.size()); + Eigen::VectorXd r = Eigen::Map( + options->r_vector.data(), options->r_vector.size()); + Eigen::VectorXd g = Eigen::Map( + options->g_vector.data(), options->g_vector.size()); + Eigen::VectorXd u = Eigen::Map( + options->u_vector.data(), options->u_vector.size()); + + options->Q = options->w_Q * q.asDiagonal(); + options->R = options->w_R * r.asDiagonal(); + options->G = options->w_G * g.asDiagonal(); + options->U = options->w_U * u.asDiagonal(); + } - void SetPositionTrackingOptions(C3Options* options) const { - options->dt = planning_dt_position; - options->dt_cost = planning_dt_position / lcs_dt_resolution; - options->w_Q = w_Q_position; - options->w_R = w_R_position; - options->w_G = w_G_position; - options->w_U = w_U_position; - options->q_vector = q_vector_position; - options->r_vector = r_vector_position; - - options->g_x = g_x_position; - options->g_gamma = g_gamma_position_list[num_contacts_index]; - options->g_lambda_n = g_lambda_n_position_list[num_contacts_index]; - options->g_lambda_t = g_lambda_t_position_list[num_contacts_index]; - options->g_lambda = g_lambda_position_list[num_contacts_index]; - options->g_u = g_u_position; - - - options->u_x = u_x_position; - options->u_gamma = u_gamma_position_list[num_contacts_index]; - options->u_lambda_n = u_lambda_n_position_list[num_contacts_index]; - options->u_lambda_t = u_lambda_t_position_list[num_contacts_index]; - options->u_lambda = u_lambda_position_list[num_contacts_index]; - options->u_u = u_u_position; - - // Only applicable for C3+ - if (options->projection_type == "C3+") { - options->g_eta_slack = g_eta_slack_position_list[num_contacts_index]; - options->g_eta_n = g_eta_n_position_list[num_contacts_index]; - options->g_eta_t = g_eta_t_position_list[num_contacts_index]; - options->g_eta = g_eta_position_list[num_contacts_index]; - options->u_eta_slack = u_eta_slack_position_list[num_contacts_index]; - options->u_eta_n = u_eta_n_position_list[num_contacts_index]; - options->u_eta_t = u_eta_t_position_list[num_contacts_index]; - options->u_eta = u_eta_position_list[num_contacts_index]; - } - MakePlanarLambdaCost(options); - PopulateCostMatricesFromVectors(options); + void SetCommonOptions(C3Options* c3_options, + LCSFactoryOptions* lcs_factory_options) const { + c3_options->warm_start = warm_start; + c3_options->penalize_input_change = penalize_input_change; + c3_options->end_on_qp_step = end_on_qp_step; + c3_options->scale_lcs = scale_lcs; + + c3_options->num_threads = num_threads; + c3_options->delta_option = delta_option; + + c3_options->admm_iter = admm_iter; + + c3_options->gamma = gamma; + c3_options->rho_scale = rho_scale; + + c3_options->M = M; + c3_options->qp_projection_alpha = qp_projection_alpha; + c3_options->qp_projection_scaling = qp_projection_scaling; + c3_options->final_augmented_cost_scaling = final_augmented_cost_scaling; + + lcs_factory_options->contact_model = contact_model; + lcs_factory_options->num_contacts = num_contacts; + lcs_factory_options->num_friction_directions = num_friction_directions; + lcs_factory_options->spring_stiffness = 0; // not used in sampling C3 + lcs_factory_options->mu = mu; + lcs_factory_options->N = N; + lcs_factory_options->num_friction_directions_per_contact = + num_friction_directions_per_contact; + } + + void SetPositionTrackingOptions( + C3Options* c3_options, LCSFactoryOptions* lcs_factory_options) const { + lcs_factory_options->dt = planning_dt_position; + c3_options->w_Q = w_Q_position; + c3_options->w_R = w_R_position; + c3_options->w_G = w_G_position; + c3_options->w_U = w_U_position; + c3_options->q_vector = q_vector_position; + c3_options->r_vector = r_vector_position; + + c3_options->g_x = g_x_position; + c3_options->g_gamma = g_gamma_position_list[num_contacts_index]; + c3_options->g_lambda_n = g_lambda_n_position_list[num_contacts_index]; + c3_options->g_lambda_t = g_lambda_t_position_list[num_contacts_index]; + c3_options->g_lambda = g_lambda_position_list[num_contacts_index]; + c3_options->g_u = g_u_position; + + c3_options->u_x = u_x_position; + c3_options->u_gamma = u_gamma_position_list[num_contacts_index]; + c3_options->u_lambda_n = u_lambda_n_position_list[num_contacts_index]; + c3_options->u_lambda_t = u_lambda_t_position_list[num_contacts_index]; + c3_options->u_lambda = u_lambda_position_list[num_contacts_index]; + c3_options->u_u = u_u_position; + + // Only applicable for C3+ + if (projection_type == "C3+") { + c3_options->g_eta_slack = g_eta_slack_position_list[num_contacts_index]; + c3_options->g_eta_n = g_eta_n_position_list[num_contacts_index]; + c3_options->g_eta_t = g_eta_t_position_list[num_contacts_index]; + c3_options->g_eta = g_eta_position_list[num_contacts_index]; + c3_options->u_eta_slack = u_eta_slack_position_list[num_contacts_index]; + c3_options->u_eta_n = u_eta_n_position_list[num_contacts_index]; + c3_options->u_eta_t = u_eta_t_position_list[num_contacts_index]; + c3_options->u_eta = u_eta_position_list[num_contacts_index]; } - void SetPoseTrackingOptions(C3Options* options) const { - options->dt = planning_dt_pose; - options->dt_cost = planning_dt_pose / lcs_dt_resolution; - options->w_Q = w_Q; - options->w_R = w_R; - options->w_G = w_G; - options->w_U = w_U; - options->q_vector = q_vector; - options->r_vector = r_vector; - - options->g_x = g_x; - options->g_gamma = g_gamma_list[num_contacts_index]; - options->g_lambda_n = g_lambda_n_list[num_contacts_index]; - options->g_lambda_t = g_lambda_t_list[num_contacts_index]; - options->g_lambda = g_lambda_list[num_contacts_index]; - options->g_u = g_u; - - options->u_x = u_x; - options->u_gamma = u_gamma_list[num_contacts_index]; - options->u_lambda_n = u_lambda_n_list[num_contacts_index]; - options->u_lambda_t = u_lambda_t_list[num_contacts_index]; - options->u_lambda = u_lambda_list[num_contacts_index]; - options->u_u = u_u; - - if (options->projection_type == "C3+") { - options->g_eta_slack = g_eta_slack_list[num_contacts_index]; - options->g_eta_n = g_eta_n_list[num_contacts_index]; - options->g_eta_t = g_eta_t_list[num_contacts_index]; - options->g_eta = g_eta_list[num_contacts_index]; - options->u_eta_slack = u_eta_slack_list[num_contacts_index]; - options->u_eta_n = u_eta_n_list[num_contacts_index]; - options->u_eta_t = u_eta_t_list[num_contacts_index]; - options->u_eta = u_eta_list[num_contacts_index]; - } - MakePlanarLambdaCost(options); - PopulateCostMatricesFromVectors(options); + MakePlanarLambdaCost(c3_options, lcs_factory_options); + PopulateCostMatricesFromVectors(c3_options); + } + + void SetPoseTrackingOptions(C3Options* c3_options, + LCSFactoryOptions* lcs_factory_options) const { + lcs_factory_options->dt = planning_dt_pose; + c3_options->w_Q = w_Q; + c3_options->w_R = w_R; + c3_options->w_G = w_G; + c3_options->w_U = w_U; + c3_options->q_vector = q_vector; + c3_options->r_vector = r_vector; + + c3_options->g_x = g_x; + c3_options->g_gamma = g_gamma_list[num_contacts_index]; + c3_options->g_lambda_n = g_lambda_n_list[num_contacts_index]; + c3_options->g_lambda_t = g_lambda_t_list[num_contacts_index]; + c3_options->g_lambda = g_lambda_list[num_contacts_index]; + c3_options->g_u = g_u; + + c3_options->u_x = u_x; + c3_options->u_gamma = u_gamma_list[num_contacts_index]; + c3_options->u_lambda_n = u_lambda_n_list[num_contacts_index]; + c3_options->u_lambda_t = u_lambda_t_list[num_contacts_index]; + c3_options->u_lambda = u_lambda_list[num_contacts_index]; + c3_options->u_u = u_u; + + // Only applicable for C3+ + if (projection_type == "C3+") { + c3_options->g_eta_slack = g_eta_slack_list[num_contacts_index]; + c3_options->g_eta_n = g_eta_n_list[num_contacts_index]; + c3_options->g_eta_t = g_eta_t_list[num_contacts_index]; + c3_options->g_eta = g_eta_list[num_contacts_index]; + c3_options->u_eta_slack = u_eta_slack_list[num_contacts_index]; + c3_options->u_eta_n = u_eta_n_list[num_contacts_index]; + c3_options->u_eta_t = u_eta_t_list[num_contacts_index]; + c3_options->u_eta = u_eta_list[num_contacts_index]; } + MakePlanarLambdaCost(c3_options, lcs_factory_options); + PopulateCostMatricesFromVectors(c3_options); + } + // Convert lambda weights to the planar form: - void MakePlanarLambdaCost(C3Options* options) const{ - int offset = 0; - for (size_t i = 0; i < resolve_contacts_to_lists[num_contacts_index].size();++i) { - if (resolve_as_planar_contacts_list[i]) { - int erase_start_index_for_lambda = - 2 * num_friction_directions * std::accumulate( - resolve_contacts_to_lists[num_contacts_index].begin(), - resolve_contacts_to_lists[num_contacts_index].begin() + i, 0) - offset; - int erase_end_index_for_lambda = - erase_start_index_for_lambda + 2 * (num_friction_directions - 1) * - resolve_contacts_to_lists[num_contacts_index][i]; - - options->g_lambda.erase( - options->g_lambda.begin()+ erase_start_index_for_lambda, - options->g_lambda.begin()+ erase_end_index_for_lambda); - options->u_lambda.erase( - options->u_lambda.begin() + erase_start_index_for_lambda, - options->u_lambda.begin() + erase_end_index_for_lambda); - options->g_lambda_t.erase( - options->g_lambda_t.begin() + erase_start_index_for_lambda, - options->g_lambda_t.begin() + erase_end_index_for_lambda); - options->u_lambda_t.erase( - options->u_lambda_t.begin() + erase_start_index_for_lambda, - options->u_lambda_t.begin() + erase_end_index_for_lambda); - - if (options->projection_type == "C3+") { - options->g_eta.erase( - options->g_eta.begin() + erase_start_index_for_lambda, - options->g_eta.begin() + erase_end_index_for_lambda); - options->u_eta.erase( - options->u_eta.begin() + erase_start_index_for_lambda, - options->u_eta.begin() + erase_end_index_for_lambda); - options->g_eta_t.erase( - options->g_eta_t.begin() + erase_start_index_for_lambda, - options->g_eta_t.begin() + erase_end_index_for_lambda); - options->u_eta_t.erase( - options->u_eta_t.begin() + erase_start_index_for_lambda, - options->u_eta_t.begin() + erase_end_index_for_lambda); - } - offset += 2 * (num_friction_directions - 1) * - resolve_contacts_to_lists[num_contacts_index][i]; + void MakePlanarLambdaCost(C3Options* c3_options, + LCSFactoryOptions* lcs_factory_options) const { + int offset = 0; + for (size_t i = 0; i < resolve_contacts_to_lists[num_contacts_index].size(); + ++i) { + if (resolve_as_planar_contacts_list[i]) { + int erase_start_index_for_lambda = + 2 * num_friction_directions.value() * + std::accumulate( + resolve_contacts_to_lists[num_contacts_index].begin(), + resolve_contacts_to_lists[num_contacts_index].begin() + i, + 0) - + offset; + int erase_end_index_for_lambda = + erase_start_index_for_lambda + + 2 * (num_friction_directions.value() - 1) * + resolve_contacts_to_lists[num_contacts_index][i]; + + c3_options->g_lambda.erase( + c3_options->g_lambda.begin() + erase_start_index_for_lambda, + c3_options->g_lambda.begin() + erase_end_index_for_lambda); + c3_options->u_lambda.erase( + c3_options->u_lambda.begin() + erase_start_index_for_lambda, + c3_options->u_lambda.begin() + erase_end_index_for_lambda); + c3_options->g_lambda_t.erase( + c3_options->g_lambda_t.begin() + erase_start_index_for_lambda, + c3_options->g_lambda_t.begin() + erase_end_index_for_lambda); + c3_options->u_lambda_t.erase( + c3_options->u_lambda_t.begin() + erase_start_index_for_lambda, + c3_options->u_lambda_t.begin() + erase_end_index_for_lambda); + + if (projection_type == "C3+") { + DRAKE_ASSERT(c3_options->g_eta.has_value()); + auto& g_eta = c3_options->g_eta.value(); + DRAKE_ASSERT(c3_options->u_eta.has_value()); + auto& u_eta = c3_options->u_eta.value(); + DRAKE_ASSERT(c3_options->g_eta_t.has_value()); + auto& g_eta_t = c3_options->g_eta_t.value(); + DRAKE_ASSERT(c3_options->u_eta_t.has_value()); + auto& u_eta_t = c3_options->u_eta_t.value(); + g_eta.erase(g_eta.begin() + erase_start_index_for_lambda, + g_eta.begin() + erase_end_index_for_lambda); + u_eta.erase(u_eta.begin() + erase_start_index_for_lambda, + u_eta.begin() + erase_end_index_for_lambda); + g_eta_t.erase(g_eta_t.begin() + erase_start_index_for_lambda, + g_eta_t.begin() + erase_end_index_for_lambda); + u_eta_t.erase(u_eta_t.begin() + erase_start_index_for_lambda, + u_eta_t.begin() + erase_end_index_for_lambda); } + offset += 2 * (num_friction_directions.value() - 1) * + resolve_contacts_to_lists[num_contacts_index][i]; } } + } - //Compute total number of planar friction directions - //and create a vector that contains the number of friction directions for each contact point + // Compute total number of planar friction directions + // and create a vector that contains the number of friction directions for + // each contact point std::pair> ProcessPlanarContactInformation( - const std::vector& resolve_as_planar_contacts_list, - const std::vector& resolve_contacts_to_list,int num_friction_directions) { - int num_planar_contacts = 0; - int planar_contact = 1; - std::vector num_friction_directions_per_contact; - for (int i = 0; i < resolve_contacts_to_list.size(); ++i) { - for (int j = 0; j < resolve_contacts_to_list[i]; ++j) { - num_planar_contacts += (resolve_as_planar_contacts_list[i] ? 1 : 0); - num_friction_directions_per_contact.push_back(resolve_as_planar_contacts_list[i] ? - planar_contact : num_friction_directions); - } + const std::vector& resolve_as_planar_contacts_list, + const std::vector& resolve_contacts_to_list, + int num_friction_directions) { + int num_planar_contacts = 0; + int planar_contact = 1; + std::vector num_friction_directions_per_contact; + for (int i = 0; i < resolve_contacts_to_list.size(); ++i) { + for (int j = 0; j < resolve_contacts_to_list[i]; ++j) { + num_planar_contacts += (resolve_as_planar_contacts_list[i] ? 1 : 0); + num_friction_directions_per_contact.push_back( + resolve_as_planar_contacts_list[i] ? planar_contact + : num_friction_directions); } - return std::pair>(num_planar_contacts, num_friction_directions_per_contact); } - + return std::pair>( + num_planar_contacts, num_friction_directions_per_contact); + } }; diff --git a/examples/sampling_c3/push_t/parameters/sampling_c3plus_options.yaml b/examples/sampling_c3/push_t/parameters/sampling_c3plus_options.yaml index d40798d52..77889dd76 100644 --- a/examples/sampling_c3/push_t/parameters/sampling_c3plus_options.yaml +++ b/examples/sampling_c3/push_t/parameters/sampling_c3plus_options.yaml @@ -1,6 +1,6 @@ ### C3 options admm_iter: 3 -rho: 0 #This isn't used anywhere! +# rho: 0 #This isn't used anywhere! rho_scale: 3 num_threads: 5 num_outer_threads: 4 @@ -8,11 +8,14 @@ delta_option: 1 projection_type: 'C3+' # 'MIQP' or 'QP' or 'C3+' contact_model: 'anitescu' # 'stewart_and_trinkle' or 'anitescu'. warm_start: false +scale_lcs: true end_on_qp_step: false +# use_robust_formulation: false solve_time_filter_alpha: 0.95 publish_frequency: 0 -penalize_changes_in_u_across_solves: false # Penalize (u-u_prev) instead of u. +penalize_input_change: false # Penalize (u-u_prev) instead of u. num_friction_directions: 2 +spring_stiffness: 0.0 # Not used in C3+. N: 5 gamma: 1.0 # discount factor on MPC costs @@ -184,11 +187,11 @@ u_eta_position_list: [ ### NO NEED TO CHANGE THE BELOW. Parameters needed for the C3Options struct but ### are overwritten by other sampling C3 parameters. -use_predicted_x0: false # instead: use_predicted_x0_c3, +# use_predicted_x0: false # instead: use_predicted_x0_c3, # use_predicted_x0_repos, # use_predicted_x0_reset_mechanism dt: 0 # instead: planning_dt_pose, planning_dt_position -solve_dt: 0 # unused +# solve_dt: 0 # unused mu: [] # instead based on indexing into mu_per_pair_type num_contacts: 0 # instead based on summing index of resolve_contacts_to_lists # Instead for the below, index into their _list versions. diff --git a/examples/sampling_c3/test/lcm_log_loader.cc b/examples/sampling_c3/test/lcm_log_loader.cc index 269840559..32171d547 100644 --- a/examples/sampling_c3/test/lcm_log_loader.cc +++ b/examples/sampling_c3/test/lcm_log_loader.cc @@ -153,6 +153,7 @@ int DoMain(int argc, char* argv[]) { SamplingC3Options sampling_c3_options = drake::yaml::LoadYamlFile(sampling_c3_options_path + ".yaml"); + int N = sampling_c3_options.N; // NOTE: can temporarily hard code many more ADMM iterations or other // changes here, e.g.: // c3_options.admm_iter = 8; @@ -244,23 +245,23 @@ int DoMain(int argc, char* argv[]) { Eigen::VectorXd x_lcs_desired = Eigen::VectorXd::Zero(19); Eigen::VectorXd x_lcs_final_desired = Eigen::VectorXd::Zero(19); Eigen::MatrixXd dyn_feas_curr_plan_obj_pos = - Eigen::MatrixXd::Zero(3, c3_options.N + 1); + Eigen::MatrixXd::Zero(3, N + 1); Eigen::MatrixXd dyn_feas_curr_plan_ee_pos = - Eigen::MatrixXd::Zero(3, c3_options.N + 1); + Eigen::MatrixXd::Zero(3, N + 1); Eigen::MatrixXd dyn_feas_curr_plan_obj_orientation = - Eigen::MatrixXd::Zero(4, c3_options.N + 1); + Eigen::MatrixXd::Zero(4, N + 1); Eigen::MatrixXd dyn_feas_best_plan_obj_pos = - Eigen::MatrixXd::Zero(3, c3_options.N + 1); + Eigen::MatrixXd::Zero(3, N + 1); Eigen::MatrixXd dyn_feas_best_plan_ee_pos = - Eigen::MatrixXd::Zero(3, c3_options.N + 1); + Eigen::MatrixXd::Zero(3, N + 1); Eigen::MatrixXd dyn_feas_best_plan_obj_orientation = - Eigen::MatrixXd::Zero(4, c3_options.N + 1); + Eigen::MatrixXd::Zero(4, N + 1); - Eigen::MatrixXd u_sol = Eigen::MatrixXd::Zero(3, c3_options.N); - Eigen::MatrixXd x_sol = Eigen::MatrixXd::Zero(19, c3_options.N); - Eigen::MatrixXd lambda_sol = Eigen::MatrixXd::Zero(16, c3_options.N); - Eigen::MatrixXd w_sol = Eigen::MatrixXd::Zero(38, c3_options.N); - Eigen::MatrixXd delta_sol = Eigen::MatrixXd::Zero(38, c3_options.N); + Eigen::MatrixXd u_sol = Eigen::MatrixXd::Zero(3, N); + Eigen::MatrixXd x_sol = Eigen::MatrixXd::Zero(19, N); + Eigen::MatrixXd lambda_sol = Eigen::MatrixXd::Zero(16, N); + Eigen::MatrixXd w_sol = Eigen::MatrixXd::Zero(38, N); + Eigen::MatrixXd delta_sol = Eigen::MatrixXd::Zero(38, N); // Collect the sample locations std::vector sample_locations_in_log; @@ -324,13 +325,13 @@ int DoMain(int argc, char* argv[]) { << " and event " << "timestamp " << adjusted_utimestamp / 1e6 << std::endl; for (int i = 0; i < 4; i++) { - for (int j = 0; j < c3_options.N + 1; j++) { + for (int j = 0; j < N + 1; j++) { dyn_feas_curr_plan_obj_orientation(i, j) = message.saved_traj.trajectories[0].datapoints[i][j]; } } for (int i = 0; i < 3; i++) { - for (int j = 0; j < c3_options.N + 1; j++) { + for (int j = 0; j < N + 1; j++) { dyn_feas_curr_plan_obj_pos(i, j) = message.saved_traj.trajectories[1].datapoints[i][j]; } @@ -349,7 +350,7 @@ int DoMain(int argc, char* argv[]) { << " and event timestamp " << adjusted_utimestamp / 1e6 << std::endl; for (int i = 0; i < 3; i++) { - for (int j = 0; j < c3_options.N + 1; j++) { + for (int j = 0; j < N + 1; j++) { dyn_feas_curr_plan_ee_pos(i, j) = message.saved_traj.trajectories[0].datapoints[i][j]; } @@ -368,13 +369,13 @@ int DoMain(int argc, char* argv[]) { << " and event timestamp " << adjusted_utimestamp / 1e6 << std::endl; for (int i = 0; i < 4; i++) { - for (int j = 0; j < c3_options.N + 1; j++) { + for (int j = 0; j < N + 1; j++) { dyn_feas_best_plan_obj_orientation(i, j) = message.saved_traj.trajectories[0].datapoints[i][j]; } } for (int i = 0; i < 3; i++) { - for (int j = 0; j < c3_options.N + 1; j++) { + for (int j = 0; j < N + 1; j++) { dyn_feas_best_plan_obj_pos(i, j) = message.saved_traj.trajectories[1].datapoints[i][j]; } @@ -393,7 +394,7 @@ int DoMain(int argc, char* argv[]) { << " and event timestamp " << adjusted_utimestamp / 1e6 << std::endl; for (int i = 0; i < 3; i++) { - for (int j = 0; j < c3_options.N + 1; j++) { + for (int j = 0; j < N + 1; j++) { dyn_feas_best_plan_ee_pos(i, j) = message.saved_traj.trajectories[0].datapoints[i][j]; } @@ -411,31 +412,31 @@ int DoMain(int argc, char* argv[]) { << (message.utime) / 1e6 << " and event timestamp " << adjusted_utimestamp / 1e6 << std::endl; for (int i = 0; i < 3; i++) { - for (int j = 0; j < c3_options.N; j++) { + for (int j = 0; j < N; j++) { u_sol(i, j) = static_cast(message.c3_solution.u_sol[i][j]); } } for (int i = 0; i < 19; i++) { - for (int j = 0; j < c3_options.N; j++) { + for (int j = 0; j < N; j++) { x_sol(i, j) = static_cast(message.c3_solution.x_sol[i][j]); } } for (int i = 0; i < 16; i++) { - for (int j = 0; j < c3_options.N; j++) { + for (int j = 0; j < N; j++) { lambda_sol(i, j) = static_cast(message.c3_solution.lambda_sol[i][j]); } } for (int i = 0; i < 38; i++) { - for (int j = 0; j < c3_options.N; j++) { + for (int j = 0; j < N; j++) { w_sol(i, j) = static_cast(message.c3_intermediates.w_sol[i][j]); } } for (int i = 0; i < 38; i++) { - for (int j = 0; j < c3_options.N; j++) { + for (int j = 0; j < N; j++) { delta_sol(i, j) = static_cast(message.c3_intermediates.delta_sol[i][j]); } @@ -506,12 +507,12 @@ int DoMain(int argc, char* argv[]) { (x_lcs_desired != Eigen::VectorXd::Zero(19)) && (x_lcs_final_desired != Eigen::VectorXd::Zero(19)) && (dyn_feas_curr_plan_ee_pos != - Eigen::MatrixXd::Zero(3, c3_options.N + 1)) && + Eigen::MatrixXd::Zero(3, N + 1)) && (dyn_feas_curr_plan_obj_pos != - Eigen::MatrixXd::Zero(3, c3_options.N + 1)) && + Eigen::MatrixXd::Zero(3, N + 1)) && (dyn_feas_curr_plan_obj_orientation != - Eigen::MatrixXd::Zero(4, c3_options.N + 1)) && - (u_sol != Eigen::MatrixXd::Zero(3, c3_options.N)) && + Eigen::MatrixXd::Zero(4, N + 1)) && + (u_sol != Eigen::MatrixXd::Zero(3, N)) && (is_c3_mode_set) && (sample_locations_in_log.size() > 0) && (sample_costs_in_log.size() > 0)) { break; diff --git a/solvers/base_c3.cc b/solvers/base_c3.cc index f6ad49a5c..f5e0d81db 100644 --- a/solvers/base_c3.cc +++ b/solvers/base_c3.cc @@ -359,437 +359,437 @@ void C3Base::Solve(const VectorXd& x0, bool verbose) { 1e6; } -// This function relies on the previously computed zfin_ from Solve. -std::pair> C3Base::CalcCost( - C3CostComputationType cost_type, - std::vector Kp_for_ee_pd_rollout, - std::vector Kd_for_ee_pd_rollout, - bool force_tracking_disabled, - int num_objects, - bool print_cost_breakdown, - bool verbose) const { - - int resolution = options_.lcs_dt_resolution; +// // This function relies on the previously computed zfin_ from Solve. +// std::pair> C3Base::CalcCost( +// C3CostComputationType cost_type, +// std::vector Kp_for_ee_pd_rollout, +// std::vector Kd_for_ee_pd_rollout, +// bool force_tracking_disabled, +// int num_objects, +// bool print_cost_breakdown, +// bool verbose) const { + +// int resolution = options_.lcs_dt_resolution; - std::vector UU(N_*resolution, VectorXd::Zero(k_)); - std::vector XX(N_*resolution + 1, VectorXd::Zero(n_)); - - const int ee_vel_index = 7 * num_objects + 3; - // Simulate the dynamics from the planned inputs. - if (cost_type == C3CostComputationType::kSimLCS) { - XX[0] = zfin_[0].segment(0, n_); - for (int i = 0; i < N_ * resolution; i++) { - UU[i] = zfin_[i / resolution].segment(n_ + m_, k_); - if (lcs_for_cost_) { - XX[i+1] = lcs_for_cost_->Simulate(XX[i], UU[i]); - } - else { - XX[i+1] = lcs_.Simulate(XX[i], UU[i]); - } - } - } - - // Use the C3 plan. - else if (cost_type == C3CostComputationType::kUseC3Plan) { - for (int i = 0; i < N_ * resolution; i++) { - UU[i] = zfin_[i / resolution].segment(n_ + m_, k_); - XX[i] = zfin_[i / resolution].segment(0, n_); - if (i == N_-1) { - if (lcs_for_cost_) { - XX[i+1] = lcs_for_cost_->Simulate(XX[i], UU[i]); - } - else { - XX[i+1] = lcs_.Simulate(XX[i], UU[i]); - } - } - } - } - - // 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] = zfin_[0].segment(0, n_); - for (int i = 0; i < N_ * resolution; i++) { - UU[i] = zfin_[i / resolution].segment(n_ + m_, k_); - if (lcs_for_cost_) { - XX[i+1] = lcs_for_cost_->Simulate(XX[i], UU[i]); - } - else { - XX[i+1] = lcs_.Simulate(XX[i], UU[i]); - } - } - // Replace ee traj with those from zfin_. - for (int i = 0; i < N_; i++) { - XX[i].segment(0,3) = zfin_[i / resolution].segment(0,3); - if (i == N_-1) { - if (lcs_for_cost_) { - XX[i+1].segment(0,3) = - lcs_for_cost_->Simulate(XX[i], UU[i]).segment(0,3); - } - else { - XX[i+1].segment(0,3) = lcs_.Simulate(XX[i], UU[i]).segment(0,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(Kp_for_ee_pd_rollout, - Kd_for_ee_pd_rollout, - 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(Kp_for_ee_pd_rollout, - Kd_for_ee_pd_rollout, - 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) = zfin_[i].segment(0,3); - XX[i].segment(ee_vel_index,3) = zfin_[i].segment(ee_vel_index,3); - if (i == N_*resolution-1) { - XX[i+1].segment(0,3) = zfin_[i].segment(0,3); - XX[i+1].segment(ee_vel_index,3) = zfin_[i].segment(ee_vel_index,3); - } - } - } - - // The same as the previous cost type except only object terms contribute to - // the final cost. - else if (cost_type == C3CostComputationType::kSimImpedanceObjectCostOnly) { - std::tie(XX, UU) = SimulatePDControl(Kp_for_ee_pd_rollout, - Kd_for_ee_pd_rollout, - 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 = Q_; - std::vector R_eff = 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_] - 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) { - std::cout<<"Error breakdown"< XX_downsampled(N_ + 1, VectorXd::Zero(n_)); - - for (int i = 0; i < N_*resolution; i += resolution) { - XX_downsampled[i/resolution] = XX[i]; - } - XX_downsampled[N_] = XX[N_*resolution]; +// std::vector UU(N_*resolution, VectorXd::Zero(k_)); +// std::vector XX(N_*resolution + 1, VectorXd::Zero(n_)); + +// const int ee_vel_index = 7 * num_objects + 3; +// // Simulate the dynamics from the planned inputs. +// if (cost_type == C3CostComputationType::kSimLCS) { +// XX[0] = zfin_[0].segment(0, n_); +// for (int i = 0; i < N_ * resolution; i++) { +// UU[i] = zfin_[i / resolution].segment(n_ + m_, k_); +// if (lcs_for_cost_) { +// XX[i+1] = lcs_for_cost_->Simulate(XX[i], UU[i]); +// } +// else { +// XX[i+1] = lcs_.Simulate(XX[i], UU[i]); +// } +// } +// } + +// // Use the C3 plan. +// else if (cost_type == C3CostComputationType::kUseC3Plan) { +// for (int i = 0; i < N_ * resolution; i++) { +// UU[i] = zfin_[i / resolution].segment(n_ + m_, k_); +// XX[i] = zfin_[i / resolution].segment(0, n_); +// if (i == N_-1) { +// if (lcs_for_cost_) { +// XX[i+1] = lcs_for_cost_->Simulate(XX[i], UU[i]); +// } +// else { +// XX[i+1] = lcs_.Simulate(XX[i], UU[i]); +// } +// } +// } +// } + +// // 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] = zfin_[0].segment(0, n_); +// for (int i = 0; i < N_ * resolution; i++) { +// UU[i] = zfin_[i / resolution].segment(n_ + m_, k_); +// if (lcs_for_cost_) { +// XX[i+1] = lcs_for_cost_->Simulate(XX[i], UU[i]); +// } +// else { +// XX[i+1] = lcs_.Simulate(XX[i], UU[i]); +// } +// } +// // Replace ee traj with those from zfin_. +// for (int i = 0; i < N_; i++) { +// XX[i].segment(0,3) = zfin_[i / resolution].segment(0,3); +// if (i == N_-1) { +// if (lcs_for_cost_) { +// XX[i+1].segment(0,3) = +// lcs_for_cost_->Simulate(XX[i], UU[i]).segment(0,3); +// } +// else { +// XX[i+1].segment(0,3) = lcs_.Simulate(XX[i], UU[i]).segment(0,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(Kp_for_ee_pd_rollout, +// Kd_for_ee_pd_rollout, +// 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(Kp_for_ee_pd_rollout, +// Kd_for_ee_pd_rollout, +// 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) = zfin_[i].segment(0,3); +// XX[i].segment(ee_vel_index,3) = zfin_[i].segment(ee_vel_index,3); +// if (i == N_*resolution-1) { +// XX[i+1].segment(0,3) = zfin_[i].segment(0,3); +// XX[i+1].segment(ee_vel_index,3) = zfin_[i].segment(ee_vel_index,3); +// } +// } +// } + +// // The same as the previous cost type except only object terms contribute to +// // the final cost. +// else if (cost_type == C3CostComputationType::kSimImpedanceObjectCostOnly) { +// std::tie(XX, UU) = SimulatePDControl(Kp_for_ee_pd_rollout, +// Kd_for_ee_pd_rollout, +// 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 = Q_; +// std::vector R_eff = 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_] - 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) { +// std::cout<<"Error breakdown"< XX_downsampled(N_ + 1, VectorXd::Zero(n_)); + +// 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; - return ret; -} - -std::pair, std::vector> -C3Base::SimulatePDControl( - std::vector Kp_for_ee_pd_rollout, std::vector Kd_for_ee_pd_rollout, int num_objects, - bool force_tracking_disabled, bool verbose) const -{ - if (verbose) { - std::cout<<"\nSIMULATING PD CONTROL"< UU(N_*resolution, VectorXd::Zero(k_)); - std::vector XX(N_*resolution +1, VectorXd::Zero(n_)); - for (int i = 0; i < N_*resolution; i++) { - UU[i] = zfin_[i / resolution].segment(n_ + m_, k_); - XX[i] = zfin_[i / resolution].segment(0, n_); - if (i == N_*resolution-1) { - if (lcs_for_cost_) { - XX[i+1] = lcs_for_cost_->Simulate(XX[i], UU[i]); - } - else{ - XX[i+1] = lcs_.Simulate(XX[i], UU[i]); - } - } - } - - // Smooth XX and UU - // for (int i = 0; i < N_-1; i++) { - // Eigen::VectorXd UU_start = UU[i * resolution]; - // Eigen::VectorXd UU_end = UU[(i+1) * resolution]; - // Eigen::VectorXd XX_start = XX[i * resolution]; - // Eigen::VectorXd XX_end = XX[(i+1) * resolution]; - - // Eigen::VectorXd UU_diff = UU_end - UU_start; - // Eigen::VectorXd XX_diff = XX_end - XX_start; - - // for (int j = 0; j < resolution; j++) { - // UU[i * resolution + j] = UU[i*resolution] + (j / resolution) * UU_diff; - // XX[i * resolution + j] = XX[i*resolution] + (j / resolution) * XX_diff; - // } - // } - - // Set the PD gains for the emulated tracking controller. - Eigen::VectorXd Kp_vector = Eigen::Map(Kp_for_ee_pd_rollout.data(), Kp_for_ee_pd_rollout.size()); - Eigen::VectorXd Kd_vector = Eigen::Map(Kd_for_ee_pd_rollout.data(), 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(k_)); - std::vector XX_new(N_*resolution+1, VectorXd::Zero(n_)); - - XX_new[0] = zfin_[0].segment(0, n_); - // This will just be the original u from zfin_[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 (lcs_for_cost_) { - - if (verbose) { - std::cout<<"simulated step "<Simulate(XX_new[i], UU_new[i], verbose); - } - else { - XX_new[i+1] = lcs_.Simulate(XX_new[i], UU_new[i]); - } - } - return {XX_new, UU_new}; -} +// // 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; +// } + +// std::pair, std::vector> +// C3Base::SimulatePDControl( +// std::vector Kp_for_ee_pd_rollout, std::vector Kd_for_ee_pd_rollout, int num_objects, +// bool force_tracking_disabled, bool verbose) const +// { +// if (verbose) { +// std::cout<<"\nSIMULATING PD CONTROL"< UU(N_*resolution, VectorXd::Zero(k_)); +// std::vector XX(N_*resolution +1, VectorXd::Zero(n_)); +// for (int i = 0; i < N_*resolution; i++) { +// UU[i] = zfin_[i / resolution].segment(n_ + m_, k_); +// XX[i] = zfin_[i / resolution].segment(0, n_); +// if (i == N_*resolution-1) { +// if (lcs_for_cost_) { +// XX[i+1] = lcs_for_cost_->Simulate(XX[i], UU[i]); +// } +// else{ +// XX[i+1] = lcs_.Simulate(XX[i], UU[i]); +// } +// } +// } + +// // Smooth XX and UU +// // for (int i = 0; i < N_-1; i++) { +// // Eigen::VectorXd UU_start = UU[i * resolution]; +// // Eigen::VectorXd UU_end = UU[(i+1) * resolution]; +// // Eigen::VectorXd XX_start = XX[i * resolution]; +// // Eigen::VectorXd XX_end = XX[(i+1) * resolution]; + +// // Eigen::VectorXd UU_diff = UU_end - UU_start; +// // Eigen::VectorXd XX_diff = XX_end - XX_start; + +// // for (int j = 0; j < resolution; j++) { +// // UU[i * resolution + j] = UU[i*resolution] + (j / resolution) * UU_diff; +// // XX[i * resolution + j] = XX[i*resolution] + (j / resolution) * XX_diff; +// // } +// // } + +// // Set the PD gains for the emulated tracking controller. +// Eigen::VectorXd Kp_vector = Eigen::Map(Kp_for_ee_pd_rollout.data(), Kp_for_ee_pd_rollout.size()); +// Eigen::VectorXd Kd_vector = Eigen::Map(Kd_for_ee_pd_rollout.data(), 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(k_)); +// std::vector XX_new(N_*resolution+1, VectorXd::Zero(n_)); + +// XX_new[0] = zfin_[0].segment(0, n_); +// // This will just be the original u from zfin_[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 (lcs_for_cost_) { + +// if (verbose) { +// std::cout<<"simulated step "<Simulate(XX_new[i], UU_new[i], verbose); +// } +// else { +// XX_new[i+1] = lcs_.Simulate(XX_new[i], UU_new[i]); +// } +// } +// return {XX_new, UU_new}; +// } void C3Base::ADMMStep(const VectorXd& x0, vector* delta, diff --git a/solvers/base_c3.h b/solvers/base_c3.h index 49d3545a5..c643526c4 100644 --- a/solvers/base_c3.h +++ b/solvers/base_c3.h @@ -47,38 +47,38 @@ class C3Base { /// @return void void Solve(const Eigen::VectorXd& x0, bool verbose = false); - /// Compute the MPC cost, using previously solved MPC solution. - /// @param cost_type The method of computing the cost - /// @param Kp_for_ee_pd_rollout Proportional gain for simulated EE PD control - /// used for some of the cost types - /// @param Kd_for_ee_pd_rollout Derivative gain for simulated EE PD control - /// used for some of the cost types - /// @param force_tracking_disabled Whether to simulate EE PD control with - /// feedforward u from the MPC solution - /// @param print_cost_breakdown Whether to print the cost breakdown - /// @param verbose Whether to print additional information - /// @return The cost and its associated state trajectory - std::pair> CalcCost( - C3CostComputationType cost_type = kSimLCSReplaceC3EEPlan, - std::vector Kp_for_ee_pd_rollout = {0.0, 0.0, 0.0}, - std::vector Kd_for_ee_pd_rollout = {0.0, 0.0, 0.0}, - bool force_tracking_disabled = false, int num_objects = 1, - bool print_cost_breakdown = false, bool verbose = false) const; - - /// Helper function to simulate the dynamics with PD control on the EE - /// location and velocity plans, and the control input plans. Used for cost - /// types that simulate the impedance control. - /// @param Kp_for_ee_pd_rollout Proportional gain for simulated EE PD control - /// @param Kd_for_ee_pd_rollout Derivative gain for simulated EE PD control - /// @param force_tracking_disabled Whether to simulate EE PD control with - /// feedforward u from the MPC solution - /// @param verbose Whether to print additional information - /// @return the simulated state and input trajectories - std::pair, std::vector> - SimulatePDControl(std::vector Kp_for_ee_pd_rollout = {0.0, 0.0, 0.0}, - std::vector Kd_for_ee_pd_rollout = {0.0, 0.0, 0.0}, int num_objects = 1, - bool force_tracking_disabled = false, - bool verbose = false) const; +// /// Compute the MPC cost, using previously solved MPC solution. +// /// @param cost_type The method of computing the cost +// /// @param Kp_for_ee_pd_rollout Proportional gain for simulated EE PD control +// /// used for some of the cost types +// /// @param Kd_for_ee_pd_rollout Derivative gain for simulated EE PD control +// /// used for some of the cost types +// /// @param force_tracking_disabled Whether to simulate EE PD control with +// /// feedforward u from the MPC solution +// /// @param print_cost_breakdown Whether to print the cost breakdown +// /// @param verbose Whether to print additional information +// /// @return The cost and its associated state trajectory +// std::pair> CalcCost( +// C3CostComputationType cost_type = kSimLCSReplaceC3EEPlan, +// std::vector Kp_for_ee_pd_rollout = {0.0, 0.0, 0.0}, +// std::vector Kd_for_ee_pd_rollout = {0.0, 0.0, 0.0}, +// bool force_tracking_disabled = false, int num_objects = 1, +// bool print_cost_breakdown = false, bool verbose = false) const; + +// /// Helper function to simulate the dynamics with PD control on the EE +// /// location and velocity plans, and the control input plans. Used for cost +// /// types that simulate the impedance control. +// /// @param Kp_for_ee_pd_rollout Proportional gain for simulated EE PD control +// /// @param Kd_for_ee_pd_rollout Derivative gain for simulated EE PD control +// /// @param force_tracking_disabled Whether to simulate EE PD control with +// /// feedforward u from the MPC solution +// /// @param verbose Whether to print additional information +// /// @return the simulated state and input trajectories +// std::pair, std::vector> +// SimulatePDControl(std::vector Kp_for_ee_pd_rollout = {0.0, 0.0, 0.0}, +// std::vector Kd_for_ee_pd_rollout = {0.0, 0.0, 0.0}, int num_objects = 1, +// bool force_tracking_disabled = false, +// bool verbose = false) const; /// Solve a single ADMM step. /// @param x0 The initial state of the system diff --git a/solvers/c3_options.h b/solvers/c3_options.h index 49b05e193..64a750a43 100644 --- a/solvers/c3_options.h +++ b/solvers/c3_options.h @@ -3,33 +3,6 @@ #include "drake/common/yaml/yaml_read_archive.h" - -/* Ways of computing C3 costs after solving the MPC problem: - 0. kSimLCS: Simulate the LCS dynamics from the planned - inputs. - 1. kUseC3Plan: Use the C3 planned trajectory and inputs. - 2. kSimLCSReplaceC3EEPlan: Simulate the LCS dynamics from the planned - inputs only for the object; use the planned - EE trajectory. - 3. kSimImpedance: Try to emulate the real cost of the system - associated not only applying the planned - inputs, but also tracking the planned EE - trajectory with an impedance controller. - 4. kSimImpedanceReplaceC3EEPlan: The same as kSimImpedance except the EE - states are replaced with the plan from C3 at - the end. - 5. kSimImpedanceObjectCostOnly: The same as kSimImpedance except only the - object terms contribute to the final cost. -*/ -enum C3CostComputationType { - kSimLCS, - kUseC3Plan, - kSimLCSReplaceC3EEPlan, - kSimImpedance, - kSimImpedanceReplaceC3EEPlan, - kSimImpedanceObjectCostOnly, -}; - struct C3Options { // Hyperparameters int admm_iter; // total number of ADMM iterations diff --git a/systems/controllers/BUILD.bazel b/systems/controllers/BUILD.bazel index c765d51d4..b76ad0be5 100644 --- a/systems/controllers/BUILD.bazel +++ b/systems/controllers/BUILD.bazel @@ -100,7 +100,6 @@ cc_library( "//solvers:solver_options_io", ":face", "//systems/framework:vector", - "@drake//:drake_shared_library", "//examples/sampling_c3:generate_samples", "//examples/sampling_c3:reposition", "//common:quaternion_error_hessian", @@ -111,6 +110,8 @@ cc_library( "//examples/sampling_c3/parameter_headers:reposition_params", "//examples/sampling_c3/parameter_headers:progress_params", "//common:update_context", + "@c3//:libc3", + "@drake//:drake_shared_library", ], copts = [ "-fopenmp", diff --git a/systems/controllers/sampling_based_c3_controller.cc b/systems/controllers/sampling_based_c3_controller.cc index 6478d887e..da6843f36 100644 --- a/systems/controllers/sampling_based_c3_controller.cc +++ b/systems/controllers/sampling_based_c3_controller.cc @@ -8,22 +8,28 @@ #include #include +#include "c3/core/c3_miqp.h" +#include "c3/core/c3_plus.h" +#include "c3/core/c3_qp.h" +#include "c3/multibody/lcs_factory.h" #include "common/quaternion_error_hessian.h" #include "dairlib/lcmt_radio_out.hpp" -#include "drake/multibody/plant/multibody_plant.h" #include "examples/sampling_c3/generate_samples.h" #include "examples/sampling_c3/reposition.h" #include "multibody/multibody_utils.h" -#include "solvers/c3_miqp.h" -#include "solvers/c3_options.h" -#include "solvers/c3_qp.h" -#include "solvers/c3_plus.h" -#include "solvers/lcs.h" #include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/multibody/plant/multibody_plant.h" namespace dairlib { +using c3::C3; +using c3::C3MIQP; +using c3::C3Plus; +using c3::C3QP; +using c3::LCS; +using c3::multibody::LCSFactory; +using dairlib::C3Output; using drake::SortedPair; using drake::geometry::GeometryId; using drake::multibody::ModelInstanceIndex; @@ -37,12 +43,6 @@ using Eigen::MatrixXf; using Eigen::Vector3d; using Eigen::VectorXd; using Eigen::VectorXf; -using solvers::C3Base; -using solvers::C3MIQP; -using solvers::C3QP; -using solvers::C3Plus; -using solvers::LCS; -using solvers::LCSFactory; using std::vector; using systems::TimestampedVector; @@ -56,8 +56,7 @@ SamplingC3Controller::SamplingC3Controller( const std::vector< std::vector>>& contact_geoms, - SamplingC3ControllerParams controller_params, - bool verbose) + SamplingC3ControllerParams controller_params, bool verbose) : plant_(plant), context_(context), plant_ad_(plant_ad), @@ -76,10 +75,10 @@ SamplingC3Controller::SamplingC3Controller( this->set_name("sampling_c3_controller"); // Build C3Options from SamplingC3Options. - C3Options c3_options = sampling_c3_options_.GetC3Options( - crossed_cost_switching_threshold_); + C3Options c3_options = + sampling_c3_options_.GetC3Options(crossed_cost_switching_threshold_); - DRAKE_DEMAND(c3_options.lcs_dt_resolution > 0); + DRAKE_DEMAND(sampling_c3_options_.lcs_dt_resolution > 0); // Initialize Q_ and R_ to proper size. Values don't matter because the // values get rewritten at the beginning of every control loop. @@ -99,8 +98,8 @@ SamplingC3Controller::SamplingC3Controller( n_x_ = n_q_ + n_v_; if (verbose_) { - std::cout << "resolution: " << c3_options.lcs_dt_resolution << std::endl; - std::cout << "dt_cost: " << c3_options.dt_cost << std::endl; + std::cout << "resolution: " << sampling_c3_options_.lcs_dt_resolution + << std::endl; std::cout << "n_q_" << n_q_ << std::endl; std::cout << "n_v_" << n_v_ << std::endl; std::cout << "n_u_" << n_u_ << std::endl; @@ -116,88 +115,83 @@ SamplingC3Controller::SamplingC3Controller( } solve_time_filter_constant_ = sampling_c3_options_.solve_time_filter_alpha; - if (sampling_c3_options_.contact_model == "stewart_and_trinkle") { - contact_model_ = solvers::ContactModel::kStewartAndTrinkle; - n_lambda_ = - 2 * sampling_c3_options_.num_contacts + sampling_c3_options_.n_lambda_with_tangential; - } else if (sampling_c3_options_.contact_model == "anitescu") { - contact_model_ = solvers::ContactModel::kAnitescu; - n_lambda_ = sampling_c3_options_.n_lambda_with_tangential; - } else { - std::cerr << ("Unknown or unsupported contact model: " + - sampling_c3_options_.contact_model) << std::endl; - DRAKE_THROW_UNLESS(false); - } + n_lambda_ = LCSFactory::GetNumContactVariables( + c3::multibody::GetContactModelMap().at( + controller_params_.sampling_c3_options.contact_model), + sampling_c3_options_.num_contacts, + sampling_c3_options_.num_friction_directions_per_contact); // Placeholder LCS will have correct size as it's already determined by the // contact model. - auto lcs_placeholder = CreatePlaceholderLCS(); + auto lcs_placeholder = + LCS::CreatePlaceholderLCS(n_x_, n_u_, n_lambda_, sampling_c3_options_.N, + sampling_c3_options_.planning_dt_position); + auto x_desired_placeholder = std::vector(N_ + 1, VectorXd::Zero(n_x_)); if (sampling_c3_options_.projection_type == "MIQP") { - c3_curr_plan_ = std::make_unique( - lcs_placeholder, C3Base::CostMatrices(Q_, R_, G_, U_), x_desired_placeholder, - c3_options); - - c3_best_plan_ = std::make_unique( - lcs_placeholder, C3Base::CostMatrices(Q_, R_, G_, U_), x_desired_placeholder, - c3_options); - + c3_curr_plan_ = std::make_unique(lcs_placeholder, + C3::CostMatrices(Q_, R_, G_, U_), + x_desired_placeholder, c3_options); + c3_best_plan_ = std::make_unique(lcs_placeholder, + C3::CostMatrices(Q_, R_, G_, U_), + x_desired_placeholder, c3_options); c3_buffer_plan_ = std::make_unique( - lcs_placeholder, C3Base::CostMatrices(Q_, R_, G_, U_), - x_desired_placeholder, c3_options); - - } else if (sampling_c3_options_.projection_type == "QP") { - c3_curr_plan_ = std::make_unique( - lcs_placeholder, C3Base::CostMatrices(Q_, R_, G_, U_), + lcs_placeholder, C3::CostMatrices(Q_, R_, G_, U_), x_desired_placeholder, c3_options); - c3_best_plan_ = std::make_unique( - lcs_placeholder, C3Base::CostMatrices(Q_, R_, G_, U_), x_desired_placeholder, - c3_options); - c3_buffer_plan_ = std::make_unique( - lcs_placeholder, C3Base::CostMatrices(Q_, R_, G_, U_), x_desired_placeholder, - c3_options); + } else if (sampling_c3_options_.projection_type == "QP") { + c3_curr_plan_ = std::make_unique(lcs_placeholder, + C3::CostMatrices(Q_, R_, G_, U_), + x_desired_placeholder, c3_options); + c3_best_plan_ = std::make_unique(lcs_placeholder, + C3::CostMatrices(Q_, R_, G_, U_), + x_desired_placeholder, c3_options); + c3_buffer_plan_ = std::make_unique(lcs_placeholder, + C3::CostMatrices(Q_, R_, G_, U_), + x_desired_placeholder, c3_options); } else if (sampling_c3_options_.projection_type == "C3+") { - c3_curr_plan_ = std::make_unique( - lcs_placeholder, C3Base::CostMatrices(Q_, R_, G_, U_), x_desired_placeholder, - c3_options); - c3_best_plan_ = std::make_unique( - lcs_placeholder, C3Base::CostMatrices(Q_, R_, G_, U_), x_desired_placeholder, - c3_options); + c3_curr_plan_ = std::make_unique(lcs_placeholder, + C3::CostMatrices(Q_, R_, G_, U_), + x_desired_placeholder, c3_options); + c3_best_plan_ = std::make_unique(lcs_placeholder, + C3::CostMatrices(Q_, R_, G_, U_), + x_desired_placeholder, c3_options); c3_buffer_plan_ = std::make_unique( - lcs_placeholder, C3Base::CostMatrices(Q_, R_, G_, U_), x_desired_placeholder, - c3_options); + lcs_placeholder, C3::CostMatrices(Q_, R_, G_, U_), + x_desired_placeholder, c3_options); } else { std::cerr << ("Unknown projection type") << std::endl; DRAKE_THROW_UNLESS(false); } n_z_ = c3_curr_plan_->GetZSize(); - c3_curr_plan_->SetOsqpSolverOptions(solver_options_); - c3_best_plan_->SetOsqpSolverOptions(solver_options_); - c3_buffer_plan_->SetOsqpSolverOptions(solver_options_); - std::cout << "Set solver options" << std::endl; + c3_curr_plan_->SetSolverOptions(solver_options_); + c3_best_plan_->SetSolverOptions(solver_options_); + c3_buffer_plan_->SetSolverOptions(solver_options_); + // Set actor bounds. if (!sampling_c3_options_.include_walls) { - // Set actor bounds. for (int i = 0; i < sampling_c3_options_.workspace_limits.size(); ++i) { Eigen::RowVectorXd A = VectorXd::Zero(n_x_); A.segment(0, 3) = sampling_c3_options_.workspace_limits[i].segment(0, 3); // TODO @bibit: For the T example, the z constraint is an equality - // constraint. This will be reflected in the params but need to make sure to - // put a comment here when the T example is added. - // The fourth parameter decides which optimization variable the constraint - // is applied to. 1 = x, 2 = u, 3 = lambda. + // constraint. This will be reflected in the params but need to make sure + // to put a comment here when the T example is added. The fourth parameter + // decides which optimization variable the constraint is applied to. 1 = + // x, 2 = u, 3 = lambda. c3_curr_plan_->AddLinearConstraint( - A, c3_options.workspace_limits[i][3], c3_options.workspace_limits[i][4], - 1); + A, sampling_c3_options_.workspace_limits[i][3], + sampling_c3_options_.workspace_limits[i][4], + c3::ConstraintVariable::STATE); c3_best_plan_->AddLinearConstraint( - A, c3_options.workspace_limits[i][3], c3_options.workspace_limits[i][4], - 1); + A, sampling_c3_options_.workspace_limits[i][3], + sampling_c3_options_.workspace_limits[i][4], + c3::ConstraintVariable::STATE); c3_buffer_plan_->AddLinearConstraint( - A, c3_options.workspace_limits[i][3], c3_options.workspace_limits[i][4], - 1); + A, sampling_c3_options_.workspace_limits[i][3], + sampling_c3_options_.workspace_limits[i][4], + c3::ConstraintVariable::STATE); } } @@ -205,24 +199,33 @@ SamplingC3Controller::SamplingC3Controller( Eigen::RowVectorXd A = VectorXd::Zero(n_u_); A(i) = 1.0; c3_curr_plan_->AddLinearConstraint( - A, c3_options.u_horizontal_limits[0], c3_options.u_horizontal_limits[1], - 2); + A, sampling_c3_options_.u_horizontal_limits[0], + sampling_c3_options_.u_horizontal_limits[1], + c3::ConstraintVariable::INPUT); c3_best_plan_->AddLinearConstraint( - A, c3_options.u_horizontal_limits[0], c3_options.u_horizontal_limits[1], - 2); + A, sampling_c3_options_.u_horizontal_limits[0], + sampling_c3_options_.u_horizontal_limits[1], + c3::ConstraintVariable::INPUT); c3_buffer_plan_->AddLinearConstraint( - A, c3_options.u_horizontal_limits[0], c3_options.u_horizontal_limits[1], - 2); + A, sampling_c3_options_.u_horizontal_limits[0], + sampling_c3_options_.u_horizontal_limits[1], + c3::ConstraintVariable::INPUT); } for (int i : vector({2})) { Eigen::RowVectorXd A = VectorXd::Zero(n_u_); A(i) = 1.0; c3_curr_plan_->AddLinearConstraint( - A, c3_options.u_vertical_limits[0], c3_options.u_vertical_limits[1], 2); + A, sampling_c3_options_.u_vertical_limits[0], + sampling_c3_options_.u_vertical_limits[1], + c3::ConstraintVariable::INPUT); c3_best_plan_->AddLinearConstraint( - A, c3_options.u_vertical_limits[0], c3_options.u_vertical_limits[1], 2); + A, sampling_c3_options_.u_vertical_limits[0], + sampling_c3_options_.u_vertical_limits[1], + c3::ConstraintVariable::INPUT); c3_buffer_plan_->AddLinearConstraint( - A, c3_options.u_vertical_limits[0], c3_options.u_vertical_limits[1], 2); + A, sampling_c3_options_.u_vertical_limits[0], + sampling_c3_options_.u_vertical_limits[1], + c3::ConstraintVariable::INPUT); } // Input ports. @@ -347,9 +350,9 @@ SamplingC3Controller::SamplingC3Controller( &SamplingC3Controller::OutputTrajExecuteActor) .get_index(); is_c3_mode_port_ = - this->DeclareAbstractOutputPort( - "is_c3_mode", dairlib::lcmt_timestamped_saved_traj(), - &SamplingC3Controller::OutputIsC3Mode) + this->DeclareAbstractOutputPort("is_c3_mode", + dairlib::lcmt_timestamped_saved_traj(), + &SamplingC3Controller::OutputIsC3Mode) .get_index(); // Output ports for dynamically feasible plans used for cost computation and @@ -409,34 +412,35 @@ SamplingC3Controller::SamplingC3Controller( sample_costs_buffer_ = -1 * VectorXd::Ones(sampling_params_.N_sample_buffer); sample_buffer_configurations_port_ = this->DeclareAbstractOutputPort( - "sample_buffer_configurations", sample_buffer_, - &SamplingC3Controller::OutputSampleBufferConfigurations) - .get_index(); + "sample_buffer_configurations", sample_buffer_, + &SamplingC3Controller::OutputSampleBufferConfigurations) + .get_index(); sample_buffer_costs_port_ = this->DeclareAbstractOutputPort( - "sample_buffer_costs", sample_costs_buffer_, - &SamplingC3Controller::OutputSampleBufferCosts) - .get_index(); + "sample_buffer_costs", sample_costs_buffer_, + &SamplingC3Controller::OutputSampleBufferCosts) + .get_index(); // Unsuccessful sample buffer related output ports. - unsuccessful_sample_buffer_ = MatrixXd::Zero( - sampling_params_.N_unsuccessful_sample_buffer, n_q_); - unsuccessful_sample_costs_buffer_ = -1 * VectorXd::Ones( - sampling_params_.N_unsuccessful_sample_buffer); + unsuccessful_sample_buffer_ = + MatrixXd::Zero(sampling_params_.N_unsuccessful_sample_buffer, n_q_); + unsuccessful_sample_costs_buffer_ = + -1 * VectorXd::Ones(sampling_params_.N_unsuccessful_sample_buffer); unsuccessful_sample_buffer_configurations_port_ = this->DeclareAbstractOutputPort( - "unsuccessful_sample_buffer_configurations", - unsuccessful_sample_buffer_, - &SamplingC3Controller::OutputUnsuccessfulSampleBufferConfigurations) - .get_index(); + "unsuccessful_sample_buffer_configurations", + unsuccessful_sample_buffer_, + &SamplingC3Controller:: + OutputUnsuccessfulSampleBufferConfigurations) + .get_index(); unsuccessful_sample_buffer_costs_port_ = this->DeclareAbstractOutputPort( - "unsuccessful_sample_buffer_costs", - unsuccessful_sample_costs_buffer_, - &SamplingC3Controller::OutputUnsuccessfulSampleBufferCosts) - .get_index(); + "unsuccessful_sample_buffer_costs", + unsuccessful_sample_costs_buffer_, + &SamplingC3Controller::OutputUnsuccessfulSampleBufferCosts) + .get_index(); plan_start_time_index_ = DeclareDiscreteState(1); x_pred_curr_plan_ = VectorXd::Zero(n_x_); @@ -466,83 +470,502 @@ SamplingC3Controller::SamplingC3Controller( // Below code loads in the mesh and enumerates triangular faces. if (sampling_params_.sampling_strategy == SamplingStrategy::kMeshNormal || - sampling_params_.sampling_strategy == SamplingStrategy::kMeshNormalMultiObject) { + sampling_params_.sampling_strategy == + SamplingStrategy::kMeshNormalMultiObject) { std::vector mesh_paths; for (std::string base_name : controller_params_.base_names) { - std::string path = "examples/sampling_c3/urdf/" + base_name + "/" + base_name + ".obj"; + std::string path = + "examples/sampling_c3/urdf/" + base_name + "/" + base_name + ".obj"; mesh_paths.push_back(path); } if (mesh_paths.empty()) { - throw std::runtime_error("SamplingC3Controller: no mesh files found in SceneGraph"); + throw std::runtime_error( + "SamplingC3Controller: no mesh files found in SceneGraph"); } // N OBJECTS // Store faces and bins for each object for (const std::string& mesh_path : mesh_paths) { - drake::geometry::TriangleSurfaceMesh* mesh = - new drake::geometry::TriangleSurfaceMesh( - drake::geometry::ReadObjToTriangleSurfaceMesh(mesh_path, 1.0)); - - const auto& vertices = mesh->vertices(); - int num_tri = mesh->num_triangles(); - - std::vector object_faces; - std::vector object_bins; - object_bins.push_back(0.0); - - double cumulative_area = 0.0; - - for (int i = 0; i < num_tri; ++i) { - auto tri = mesh->triangles()[i]; - Eigen::Vector3d v0 = vertices[tri.vertex(0)]; - Eigen::Vector3d v1 = vertices[tri.vertex(1)]; - Eigen::Vector3d v2 = vertices[tri.vertex(2)]; - Eigen::Vector3d normal = (v1 - v0).cross(v2 - v0).normalized(); - - // reject faces with normal vectors that are too vertical, with some buffer to account for inaccurate object tracking - double z_accept = std::pow(sampling_params_.buffer_distance, 2) - std::pow(sampling_params_.sample_projection_clearance, 2); - if (std::pow(std::abs(normal[2]), 2) < z_accept + 0.04) { - double area = 0.5 * (v1 - v0).cross(v2 - v0).norm(); - object_faces.push_back({area, normal, {v0, v1, v2}}); - cumulative_area += area; - object_bins.push_back(cumulative_area); - } + drake::geometry::TriangleSurfaceMesh* mesh = + new drake::geometry::TriangleSurfaceMesh( + drake::geometry::ReadObjToTriangleSurfaceMesh(mesh_path, 1.0)); + + const auto& vertices = mesh->vertices(); + int num_tri = mesh->num_triangles(); + + std::vector object_faces; + std::vector object_bins; + object_bins.push_back(0.0); + + double cumulative_area = 0.0; + + for (int i = 0; i < num_tri; ++i) { + auto tri = mesh->triangles()[i]; + Eigen::Vector3d v0 = vertices[tri.vertex(0)]; + Eigen::Vector3d v1 = vertices[tri.vertex(1)]; + Eigen::Vector3d v2 = vertices[tri.vertex(2)]; + Eigen::Vector3d normal = (v1 - v0).cross(v2 - v0).normalized(); + + // reject faces with normal vectors that are too vertical, with some + // buffer to account for inaccurate object tracking + double z_accept = + std::pow(sampling_params_.buffer_distance, 2) - + std::pow(sampling_params_.sample_projection_clearance, 2); + if (std::pow(std::abs(normal[2]), 2) < z_accept + 0.04) { + double area = 0.5 * (v1 - v0).cross(v2 - v0).norm(); + object_faces.push_back({area, normal, {v0, v1, v2}}); + cumulative_area += area; + object_bins.push_back(cumulative_area); } + } - if (object_faces.empty()) { - throw std::runtime_error("No valid faces found in " + mesh_path); - } + if (object_faces.empty()) { + throw std::runtime_error("No valid faces found in " + mesh_path); + } - faces_per_object_.push_back(std::move(object_faces)); - face_bins_per_object_.push_back(std::move(object_bins)); - total_area_per_object_.push_back(cumulative_area); + faces_per_object_.push_back(std::move(object_faces)); + face_bins_per_object_.push_back(std::move(object_bins)); + total_area_per_object_.push_back(cumulative_area); } - } } +// 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& 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; + + // 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]); + } + } + + // 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 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]); + } + // 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]).segment(0, 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].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); + } + } + } + + // 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_] - 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) { + 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_)); -LCS SamplingC3Controller::CreatePlaceholderLCS() const { - MatrixXd A = MatrixXd::Ones(n_x_, n_x_); - MatrixXd B = MatrixXd::Zero(n_x_, n_u_); - VectorXd d = VectorXd::Zero(n_x_); - MatrixXd D = MatrixXd::Ones(n_x_, n_lambda_); - MatrixXd E = MatrixXd::Zero(n_lambda_, n_x_); - MatrixXd F = MatrixXd::Zero(n_lambda_, n_lambda_); - MatrixXd H = MatrixXd::Zero(n_lambda_, n_u_); - VectorXd c = VectorXd::Zero(n_lambda_); - return LCS(A, B, D, d, E, F, H, c, sampling_c3_options_.N, - sampling_c3_options_.planning_dt_position); + 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; + 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; + + // 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]); + } + } + + // 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]); + } + return {XX_new, UU_new}; +} drake::systems::EventStatus SamplingC3Controller::ComputePlan( const Context& context, DiscreteValues* discrete_state) const { auto start = std::chrono::high_resolution_clock::now(); - // Evaluate input ports. const auto& radio_out = this->EvalInputValue(context, radio_port_); @@ -551,9 +974,11 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( const BasicVector& x_lcs_des = *this->template EvalVectorInput(context, target_input_port_); const BasicVector& x_lcs_final_des = - *this->template EvalVectorInput(context, final_target_input_port_); + *this->template EvalVectorInput(context, + final_target_input_port_); const TimestampedVector* lcs_x_curr = - (TimestampedVector*)this->EvalVectorInput(context, lcs_state_input_port_); + (TimestampedVector*)this->EvalVectorInput(context, + lcs_state_input_port_); // Store the current LCS state. drake::VectorX x_lcs_curr = lcs_x_curr->get_data(); ee_position_curr_ = x_lcs_curr.segment(0, 3); @@ -582,16 +1007,17 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( current_orientation_error_ = 0; for (int i = 0; i < controller_params_.num_objects; i++) { - double error = (x_lcs_curr.segment(7 + 7*i, 3) - - x_lcs_final_des.get_value().segment(7 + 7*i, 3)).norm(); + double error = (x_lcs_curr.segment(7 + 7 * i, 3) - + x_lcs_final_des.get_value().segment(7 + 7 * i, 3)) + .norm(); current_position_error_ += error; - Eigen::Quaterniond curr_quat( - x_lcs_curr[3 + 7*i], x_lcs_curr[4 + 7*i], - x_lcs_curr[5 + 7*i], x_lcs_curr[6 + 7*i]); - Eigen::Quaterniond des_quat( - x_lcs_final_des.get_value()[3 + 7*i], x_lcs_final_des.get_value()[4 + 7*i], - x_lcs_final_des.get_value()[5 + 7*i], x_lcs_final_des.get_value()[6 + 7*i]); + Eigen::Quaterniond curr_quat(x_lcs_curr[3 + 7 * i], x_lcs_curr[4 + 7 * i], + x_lcs_curr[5 + 7 * i], x_lcs_curr[6 + 7 * i]); + Eigen::Quaterniond des_quat(x_lcs_final_des.get_value()[3 + 7 * i], + x_lcs_final_des.get_value()[4 + 7 * i], + x_lcs_final_des.get_value()[5 + 7 * i], + x_lcs_final_des.get_value()[6 + 7 * i]); Eigen::AngleAxis angle_axis_diff(des_quat * curr_quat.inverse()); current_orientation_error_ += angle_axis_diff.angle(); } @@ -600,8 +1026,8 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( // about position until the switching threshold has been crossed again. // Exclude the EE goal from the comparison, since that always changes to be // above the current object location. - if (!x_final_target_.segment(3, n_x_-3).isApprox( - x_lcs_final_des.value().segment(3, n_x_-3), 1e-5)) { + if (!x_final_target_.segment(3, n_x_ - 3) + .isApprox(x_lcs_final_des.value().segment(3, n_x_ - 3), 1e-5)) { std::cout << "Detected goal change!" << std::endl; if (verbose_) { std::cout << " Last goal: " << x_final_target_.transpose() << std::endl; @@ -615,17 +1041,18 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( } crossed_cost_switching_threshold_ = false; x_final_target_ = x_lcs_final_des.value(); - //is_doing_c3_ = false; + // is_doing_c3_ = false; detected_goal_changes_++; // Reset the sample buffers now that the costs have changed. sample_buffer_ = MatrixXd::Zero(sampling_params_.N_sample_buffer, n_q_); - sample_costs_buffer_ = -1*VectorXd::Ones(sampling_params_.N_sample_buffer); + sample_costs_buffer_ = + -1 * VectorXd::Ones(sampling_params_.N_sample_buffer); num_in_buffer_ = 0; - unsuccessful_sample_buffer_ = MatrixXd::Zero( - sampling_params_.N_unsuccessful_sample_buffer, n_q_); - unsuccessful_sample_costs_buffer_ = -1 * VectorXd::Ones( - sampling_params_.N_unsuccessful_sample_buffer); + unsuccessful_sample_buffer_ = + MatrixXd::Zero(sampling_params_.N_unsuccessful_sample_buffer, n_q_); + unsuccessful_sample_costs_buffer_ = + -1 * VectorXd::Ones(sampling_params_.N_unsuccessful_sample_buffer); num_in_unsuccessful_buffer_ = 0; } @@ -633,32 +1060,38 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( if (!crossed_cost_switching_threshold_) { double pose_diff = 0; for (int i = 0; i < controller_params_.num_objects; i++) { - pose_diff += (x_lcs_curr.segment(7 + 7*i ,2) - - x_lcs_final_des.value().segment(7 + 7*i, 2)).norm(); + pose_diff += (x_lcs_curr.segment(7 + 7 * i, 2) - + x_lcs_final_des.value().segment(7 + 7 * i, 2)) + .norm(); } if (pose_diff < progress_params_.cost_switching_threshold_distance * - controller_params_.num_objects) { + controller_params_.num_objects) { crossed_cost_switching_threshold_ = true; std::cout << "Crossed cost switching threshold." << std::endl; // Reset the sample buffers and metrics now that the costs have changed. sample_buffer_ = MatrixXd::Zero(sampling_params_.N_sample_buffer, n_q_); sample_costs_buffer_ = - -1 * VectorXd::Ones(sampling_params_.N_sample_buffer); + -1 * VectorXd::Ones(sampling_params_.N_sample_buffer); num_in_buffer_ = 0; - unsuccessful_sample_buffer_ = MatrixXd::Zero( - sampling_params_.N_unsuccessful_sample_buffer, n_q_); - unsuccessful_sample_costs_buffer_ = -1 * VectorXd::Ones( - sampling_params_.N_unsuccessful_sample_buffer); + unsuccessful_sample_buffer_ = + MatrixXd::Zero(sampling_params_.N_unsuccessful_sample_buffer, n_q_); + unsuccessful_sample_costs_buffer_ = + -1 * VectorXd::Ones(sampling_params_.N_unsuccessful_sample_buffer); num_in_unsuccessful_buffer_ = 0; - if (is_doing_c3_) { ResetProgressMetrics(); } + if (is_doing_c3_) { + ResetProgressMetrics(); + } } } // Build C3Options from SamplingC3Options based on the // crossed_cost_switching_threshold_ flag. - C3Options c3_options = sampling_c3_options_.GetC3Options( - crossed_cost_switching_threshold_); + C3Options c3_options = + sampling_c3_options_.GetC3Options(crossed_cost_switching_threshold_); + LCSFactoryOptions lcs_factory_options = + sampling_c3_options_.GetLCSFactoryOptions( + crossed_cost_switching_threshold_); // Update the cost matrices: Q_, R_, G_, and U_. UpdateCostMatrices(x_lcs_curr, x_lcs_des, c3_options); @@ -668,21 +1101,21 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( std::vector object_on_target; bool all_reached = true; for (int i = 0; i < controller_params_.num_objects; i++) { - double object_position_error = ( - x_lcs_curr.segment(7 + 7*i, 2) - x_lcs_des.get_value().segment(7 + 7*i, 2) - ).norm(); - Eigen::Quaterniond q_des(x_lcs_des.get_value().segment<4>(3 + 7*i)); - Eigen::Quaterniond q_curr(x_lcs_curr.segment<4>(3 + 7*i)); + double object_position_error = (x_lcs_curr.segment(7 + 7 * i, 2) - + x_lcs_des.get_value().segment(7 + 7 * i, 2)) + .norm(); + Eigen::Quaterniond q_des(x_lcs_des.get_value().segment<4>(3 + 7 * i)); + Eigen::Quaterniond q_curr(x_lcs_curr.segment<4>(3 + 7 * i)); Eigen::AngleAxisd angle_axis_diff(q_des * q_curr.inverse()); double object_angular_error = angle_axis_diff.angle(); object_on_target.push_back( - ((crossed_cost_switching_threshold_) && - (object_position_error < goal_params_.position_success_threshold) && - (object_angular_error < goal_params_.orientation_success_threshold)) || - ((!crossed_cost_switching_threshold_) && - (object_position_error < goal_params_.position_success_threshold))); + ((crossed_cost_switching_threshold_) && + (object_position_error < goal_params_.position_success_threshold) && + (object_angular_error < goal_params_.orientation_success_threshold)) || + ((!crossed_cost_switching_threshold_) && + (object_position_error < goal_params_.position_success_threshold))); all_reached = all_reached && object_on_target[i]; } @@ -691,8 +1124,9 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( if (achieved_fixed_goal_ || (all_reached && goal_params_.goal_mode == GoalMode::kFixedGoal)) { achieved_fixed_goal_ = true; - int num_samples = is_doing_c3_? sampling_params_.num_additional_samples_c3 : - sampling_params_.num_additional_samples_repos; + int num_samples = is_doing_c3_ + ? sampling_params_.num_additional_samples_c3 + : sampling_params_.num_additional_samples_repos; for (int i = 0; i < num_samples; i++) { candidate_states.push_back(x_lcs_curr); candidate_states[i].head(3) << 0.3, 0.4, 0.1; @@ -701,23 +1135,24 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( // Generate new samples according to sampling strategy. else { candidate_states = GenerateSampleStates( - n_q_, n_v_, n_u_, x_lcs_curr, is_doing_c3_, sampling_params_, - sampling_c3_options_, plant_, context_, plant_ad_, context_ad_, - contact_pairs_, faces_, face_bins_, faces_per_object_, - face_bins_per_object_, total_area_per_object_, object_on_target, - unsuccessful_sample_buffer_ - ); + n_q_, n_v_, n_u_, x_lcs_curr, is_doing_c3_, sampling_params_, + sampling_c3_options_, plant_, context_, plant_ad_, context_ad_, + contact_pairs_, faces_, face_bins_, faces_per_object_, + face_bins_per_object_, total_area_per_object_, object_on_target, + unsuccessful_sample_buffer_); } // Ensure prev_repositing_target_ is not inside the object const auto& query_port = plant_.get_geometry_query_input_port(); const auto& query_object = - query_port.template Eval>(*context_); + query_port.template Eval>(*context_); - const auto& results = query_object.ComputeSignedDistanceToPoint(prev_repositioning_target_.segment(0, 3)); + const auto& results = query_object.ComputeSignedDistanceToPoint( + prev_repositioning_target_.segment(0, 3)); bool in_collision = false; for (int i = 0; i < controller_params_.num_objects; i++) { - for (int j = 2 + 13*i; j < 12 + 13*i; j++) { // Check each convex piece, assumes 10 pieces/object + for (int j = 2 + 13 * i; j < 12 + 13 * i; + j++) { // Check each convex piece, assumes 10 pieces/object if (results[j].distance <= sampling_params_.sample_projection_clearance) { in_collision = true; break; @@ -746,22 +1181,19 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( for (int i = 0; i < num_total_samples; i++) { all_sample_locations_.push_back(candidate_states[i].head(3)); } - // Make LCS objects for each sample. auto lcs_pair = SamplingC3Controller::CreateLCSObjectsForSamples( - candidate_states, x_lcs_curr, c3_options, c3_options); - std::vector lcs_candidates = lcs_pair.first; - std::vector lcs_candidates_for_cost = lcs_pair.second; - + candidate_states, x_lcs_curr, lcs_factory_options); + std::vector lcs_candidates = lcs_pair.first; + std::vector lcs_candidates_for_cost = lcs_pair.second; // Prepare variables that will get used or filled in by parallelization. all_sample_costs_ = std::vector(num_total_samples, -1); all_sample_dynamically_feasible_plans_ = - std::vector>( - num_total_samples, - std::vector(N_ + 1, VectorXd::Zero(n_x_))); - std::vector> c3_objects( - num_total_samples, nullptr); + std::vector>( + num_total_samples, + std::vector(N_ + 1, VectorXd::Zero(n_x_))); + std::vector> c3_objects(num_total_samples, nullptr); bool force_tracking_disabled = radio_out->channel[11]; C3CostComputationType cost_type = progress_params_.cost_type; if (!crossed_cost_switching_threshold_) { @@ -769,49 +1201,59 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( } // Parallelize over computing C3 costs for each sample. -auto c3_start = std::chrono::high_resolution_clock::now(); + auto c3_start = std::chrono::high_resolution_clock::now(); #pragma omp parallel for num_threads(num_threads_to_use_) for (int i = 0; i < num_total_samples; i++) { - bool print_cost_breakdown = radio_out->channel[7] && - (i == SampleIndex::kCurrentLocation); + bool print_cost_breakdown = + radio_out->channel[7] && (i == SampleIndex::kCurrentLocation); // Get the candidate state and its LCS representation. Eigen::VectorXd test_state = candidate_states.at(i); - solvers::LCS test_system = lcs_candidates.at(i); + LCS test_system = lcs_candidates.at(i); // Set up C3 with proper projection type and post-solve cost matrices. - std::shared_ptr test_c3_object; - std::vector x_desired = - std::vector(N_ + 1, x_lcs_des.value()); - if (c3_options.projection_type == "MIQP") { - test_c3_object = std::make_shared( - test_system, C3Base::CostMatrices(Q_, R_, G_, U_), x_desired, c3_options); - } else if (c3_options.projection_type == "QP") { - test_c3_object = std::make_shared( - test_system, C3Base::CostMatrices(Q_, R_, G_, U_), x_desired, c3_options); - } else if (c3_options.projection_type == "C3+") { - test_c3_object = std::make_shared( - test_system, C3Base::CostMatrices(Q_, R_, G_, U_), x_desired, c3_options); - } // Unknown projection types are rejected in the initialization. - + std::shared_ptr test_c3_object; + std::vector x_desired(N_ + 1, x_lcs_des.value()); + + C3::CostMatrices c3_costmat(Q_, R_, G_, U_); + if (sampling_c3_options_.projection_type == "MIQP") { + test_c3_object = std::make_shared(test_system, c3_costmat, + x_desired, c3_options); + } else if (sampling_c3_options_.projection_type == "QP") { + test_c3_object = std::make_shared(test_system, c3_costmat, + x_desired, c3_options); + } else if (sampling_c3_options_.projection_type == "C3+") { + test_c3_object = std::make_shared(test_system, c3_costmat, + x_desired, c3_options); + } // Unknown projection types are rejected in the initialization. if (!sampling_c3_options_.include_walls) { // Set actor bounds. for (int i = 0; i < sampling_c3_options_.workspace_limits.size(); ++i) { Eigen::RowVectorXd A = VectorXd::Zero(n_x_); - A.segment(0, 3) = sampling_c3_options_.workspace_limits[i].segment(0, 3); + A.segment(0, 3) = + sampling_c3_options_.workspace_limits[i].segment(0, 3); test_c3_object->AddLinearConstraint( - A, c3_options.workspace_limits[i][3] - sampling_c3_options_.workspace_margins, - c3_options.workspace_limits[i][4] + sampling_c3_options_.workspace_margins, 1); + A, + sampling_c3_options_.workspace_limits[i][3] - + sampling_c3_options_.workspace_margins, + sampling_c3_options_.workspace_limits[i][4] + + sampling_c3_options_.workspace_margins, + c3::ConstraintVariable::STATE); } // Set object bounds for (int i = 0; i < sampling_c3_options_.workspace_limits.size(); ++i) { for (int j = 0; j < controller_params_.num_objects; j++) { Eigen::RowVectorXd A = VectorXd::Zero(n_x_); - A.segment(7 + 7*j, 3) = sampling_c3_options_.workspace_limits[i].segment(0, 3); + A.segment(7 + 7 * j, 3) = + sampling_c3_options_.workspace_limits[i].segment(0, 3); test_c3_object->AddLinearConstraint( - A, c3_options.workspace_limits[i][3] - sampling_c3_options_.workspace_margins, - c3_options.workspace_limits[i][4] + sampling_c3_options_.workspace_margins, 1); + A, + sampling_c3_options_.workspace_limits[i][3] - + sampling_c3_options_.workspace_margins, + sampling_c3_options_.workspace_limits[i][4] + + sampling_c3_options_.workspace_margins, + c3::ConstraintVariable::STATE); } } } @@ -820,8 +1262,8 @@ auto c3_start = std::chrono::high_resolution_clock::now(); for (int i : vector({0, 1, 2})) { Eigen::RowVectorXd A = VectorXd::Zero(n_x_); A(n_q_ + i) = 1.0; - test_c3_object->AddLinearConstraint( - A, -0.14, 0.14, 1); + test_c3_object->AddLinearConstraint(A, -0.14, 0.14, + c3::ConstraintVariable::STATE); } // Add force constraints @@ -829,42 +1271,44 @@ auto c3_start = std::chrono::high_resolution_clock::now(); Eigen::RowVectorXd A = VectorXd::Zero(n_u_); A(i) = 1.0; test_c3_object->AddLinearConstraint( - A, c3_options.u_horizontal_limits[0], c3_options.u_horizontal_limits[1], 2); + A, sampling_c3_options_.u_horizontal_limits[0], + sampling_c3_options_.u_horizontal_limits[1], + c3::ConstraintVariable::INPUT); } for (int i : vector({2})) { Eigen::RowVectorXd A = VectorXd::Zero(n_u_); A(i) = 1.0; test_c3_object->AddLinearConstraint( - A, c3_options.u_vertical_limits[0], c3_options.u_vertical_limits[1], 2); + A, sampling_c3_options_.u_vertical_limits[0], + sampling_c3_options_.u_vertical_limits[1], + c3::ConstraintVariable::INPUT); } - - test_c3_object->UpdateCostLCS(lcs_candidates_for_cost.at(i)); - // Solve C3, store resulting object and cost. - test_c3_object->SetOsqpSolverOptions(solver_options_); - test_c3_object->Solve(test_state, verbose_); + test_c3_object->SetSolverOptions(solver_options_); + test_c3_object->Solve(test_state); auto cc_start = std::chrono::high_resolution_clock::now(); std::pair> cost_trajectory_pair = - test_c3_object->CalcCost( - cost_type, sampling_c3_options_.Kp_for_ee_pd_rollout, - sampling_c3_options_.Kd_for_ee_pd_rollout, force_tracking_disabled, - controller_params_.num_objects, print_cost_breakdown, verbose_); + 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_); 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; - #pragma omp critical +#pragma omp critical { c3_objects.at(i) = test_c3_object; } // Add travel cost (just looking at xy displacement, not also z). - double xy_travel_distance = (test_state.head(2)-x_lcs_curr.head(2)).norm(); + double xy_travel_distance = + (test_state.head(2) - x_lcs_curr.head(2)).norm(); all_sample_costs_[i] = - c3_cost + progress_params_.travel_cost_per_meter * xy_travel_distance; + c3_cost + progress_params_.travel_cost_per_meter * xy_travel_distance; // Add additional costs based on repositioning progress. if ((i == SampleIndex::kCurrentReposTarget) && finished_reposition_flag_) { @@ -872,8 +1316,8 @@ auto c3_start = std::chrono::high_resolution_clock::now(); finished_reposition_flag_ = false; } } - auto c3_end = std::chrono::high_resolution_clock::now(); - std::chrono::duration duration_ms = c3_end - c3_start; + auto c3_end = std::chrono::high_resolution_clock::now(); + std::chrono::duration duration_ms = c3_end - c3_start; // End of parallelization // Update the sample buffer. Do this before switching modes since 1) if in @@ -900,7 +1344,7 @@ auto c3_start = std::chrono::high_resolution_clock::now(); hyst_c3_to_repos_frac = progress_params_.hyst_c3_to_repos_frac_position; hyst_repos_to_c3_frac = progress_params_.hyst_repos_to_c3_frac_position; hyst_repos_to_repos_frac = - progress_params_.hyst_repos_to_repos_frac_position; + progress_params_.hyst_repos_to_repos_frac_position; } // Review the cost results to determine the best sample. @@ -908,14 +1352,16 @@ auto c3_start = std::chrono::high_resolution_clock::now(); double best_other_cost; if (num_total_samples > 1) { std::vector additional_sample_cost_vector = std::vector( - all_sample_costs_.begin() + 1, all_sample_costs_.end()); + all_sample_costs_.begin() + 1, all_sample_costs_.end()); best_other_cost = *std::min_element(additional_sample_cost_vector.begin(), additional_sample_cost_vector.end()); std::vector::iterator it = - std::min_element(std::begin(additional_sample_cost_vector), - std::end(additional_sample_cost_vector)); - best_sample_index_ = (SampleIndex)( - std::distance(std::begin(additional_sample_cost_vector), it) + 1); + std::min_element(std::begin(additional_sample_cost_vector), + std::end(additional_sample_cost_vector)); + best_sample_index_ = + (SampleIndex)(std::distance(std::begin(additional_sample_cost_vector), + it) + + 1); } else { force_c3_mode = true; } @@ -932,25 +1378,26 @@ auto c3_start = std::chrono::high_resolution_clock::now(); // Determine whether to do C3 or reposition. mode_switch_reason_ = ModeSwitchReason::kNoSwitch; double curr_cost = all_sample_costs_[SampleIndex::kCurrentLocation]; - double repos_target_cost =all_sample_costs_[SampleIndex::kCurrentReposTarget]; + double repos_target_cost = + all_sample_costs_[SampleIndex::kCurrentReposTarget]; if (is_doing_c3_ == true) { // Currently doing C3. pursued_target_source_ = PursuedTargetSource::kNoTarget; // Keep track of progress while in C3 mode. bool met_minimum_progress = true; // Reset by below function. bool print_current_pos_and_rot_cost = radio_out->channel[6]; - KeepTrackOfC3ModeProgress( - x_lcs_curr, x_lcs_final_des, met_minimum_progress, - print_current_pos_and_rot_cost); + KeepTrackOfC3ModeProgress(x_lcs_curr, x_lcs_final_des, met_minimum_progress, + print_current_pos_and_rot_cost); // Switch to repositioning if fixed goals have all been met. if (achieved_fixed_goal_) { is_doing_c3_ = false; - std::cout<<"All objects on target, switching to repositioning mode"< 0)) { + (sampling_params_.num_additional_samples_c3 > 0)) { is_doing_c3_ = false; mode_switch_reason_ = ModeSwitchReason::kToReposUnproductive; std::cout << "Repositioning after not making progress in C3" << std::endl; @@ -958,13 +1405,12 @@ auto c3_start = std::chrono::high_resolution_clock::now(); // Switch to repositioning if one of the other samples is better, with // hysteresis. - else if ( - ((!progress_params_.use_relative_hysteresis && - curr_cost > best_other_cost + hyst_c3_to_repos) || - (progress_params_.use_relative_hysteresis && - curr_cost > best_other_cost + hyst_c3_to_repos_frac*curr_cost)) && - !force_c3_mode) - { + else if (((!progress_params_.use_relative_hysteresis && + curr_cost > best_other_cost + hyst_c3_to_repos) || + (progress_params_.use_relative_hysteresis && + curr_cost > + best_other_cost + hyst_c3_to_repos_frac * curr_cost)) && + !force_c3_mode) { is_doing_c3_ = false; mode_switch_reason_ = ModeSwitchReason::kToReposCost; std::cout << "Repositioning because found good sample" << std::endl; @@ -995,13 +1441,11 @@ auto c3_start = std::chrono::high_resolution_clock::now(); // repositioning target. If the lowest cost sample is not at least the // hysteresis amount better than the current repositioning target, then // continue pursuing the previous repositioning target. - if ( - (repos_target_cost < best_other_cost + hyst_repos_to_repos - && !progress_params_.use_relative_hysteresis) || - (repos_target_cost < best_other_cost + hyst_repos_to_repos_frac* - repos_target_cost - && progress_params_.use_relative_hysteresis)) - { + if ((repos_target_cost < best_other_cost + hyst_repos_to_repos && + !progress_params_.use_relative_hysteresis) || + (repos_target_cost < + best_other_cost + hyst_repos_to_repos_frac * repos_target_cost && + progress_params_.use_relative_hysteresis)) { best_sample_index_ = SampleIndex::kCurrentReposTarget; best_other_cost = repos_target_cost; finished_reposition_flag_ = false; @@ -1018,7 +1462,7 @@ auto c3_start = std::chrono::high_resolution_clock::now(); if (!progress_params_.use_relative_hysteresis) { best_other_cost += hyst_repos_to_repos; } else { - best_other_cost += hyst_repos_to_repos_frac*repos_target_cost; + best_other_cost += hyst_repos_to_repos_frac * repos_target_cost; } } } @@ -1032,11 +1476,15 @@ auto c3_start = std::chrono::high_resolution_clock::now(); double y_max = sampling_c3_options_.workspace_limits[1][4]; // if ee is close to wall, raise z_height to avoid hitting - if ((x_lcs_curr[0] <= x_min + 0.05 && x_lcs_curr[0] >= x_min - sampling_c3_options_.workspace_margins) || - (x_lcs_curr[0] >= x_max - 0.05 && x_lcs_curr[0] <= x_max + sampling_c3_options_.workspace_margins) || - (x_lcs_curr[1] <= y_min + 0.05 && x_lcs_curr[1] >= y_min - sampling_c3_options_.workspace_margins) || - (x_lcs_curr[1] >= y_max - 0.05 && x_lcs_curr[1] <= y_max + sampling_c3_options_.workspace_margins)) { - wall_offset = 0.01; + if ((x_lcs_curr[0] <= x_min + 0.05 && + x_lcs_curr[0] >= x_min - sampling_c3_options_.workspace_margins) || + (x_lcs_curr[0] >= x_max - 0.05 && + x_lcs_curr[0] <= x_max + sampling_c3_options_.workspace_margins) || + (x_lcs_curr[1] <= y_min + 0.05 && + x_lcs_curr[1] >= y_min - sampling_c3_options_.workspace_margins) || + (x_lcs_curr[1] >= y_max - 0.05 && + x_lcs_curr[1] <= y_max + sampling_c3_options_.workspace_margins)) { + wall_offset = 0.01; } } @@ -1054,17 +1502,19 @@ auto c3_start = std::chrono::high_resolution_clock::now(); // Stay in repositioning if fixed goal is met. else if (achieved_fixed_goal_) { finished_reposition_flag_ = false; - std::cout << "All objects at fixed goals; stay out of the way." << std::endl; + std::cout << "All objects at fixed goals; stay out of the way." + << std::endl; } // Switch to C3 if the current sample is better, with hysteresis. - else if ( - (!progress_params_.use_relative_hysteresis && - best_other_cost > curr_cost + hyst_repos_to_c3) || - (progress_params_.use_relative_hysteresis && - best_other_cost > curr_cost + hyst_repos_to_c3_frac*best_other_cost) && - (x_lcs_curr[2] < sampling_params_.z_height + sampling_params_.c3_min_clearance + wall_offset || - !sampling_params_.ee_z_close)) - { + else if ((!progress_params_.use_relative_hysteresis && + best_other_cost > curr_cost + hyst_repos_to_c3) || + (progress_params_.use_relative_hysteresis && + best_other_cost > + curr_cost + hyst_repos_to_c3_frac * best_other_cost) && + (x_lcs_curr[2] < sampling_params_.z_height + + sampling_params_.c3_min_clearance + + wall_offset || + !sampling_params_.ee_z_close)) { is_doing_c3_ = true; finished_reposition_flag_ = false; if (repos_target_cost > progress_params_.finished_reposition_cost) { @@ -1083,7 +1533,6 @@ auto c3_start = std::chrono::high_resolution_clock::now(); } } - if (verbose_) { std::cout << "All sample costs before hystereses: " << std::endl; for (int i = 0; i < num_total_samples; i++) { @@ -1104,7 +1553,6 @@ auto c3_start = std::chrono::high_resolution_clock::now(); UpdateC3ExecutionTrajectory(x_lcs_curr, t); UpdateRepositioningExecutionTrajectory(x_lcs_curr, t); - if (verbose_) { std::cout << "x_pred_curr_plan_ after updating: " << x_pred_curr_plan_.transpose() << std::endl; @@ -1112,11 +1560,11 @@ auto c3_start = std::chrono::high_resolution_clock::now(); for (int i = 0; i < N_; i++) { std::cout << "z[" << i << "]: " << zs[i].transpose() << std::endl; } - solvers::LCS verbose_lcs = lcs_candidates.at(SampleIndex::kCurrentLocation); - Eigen::MatrixXd E = verbose_lcs.E_[0]; - Eigen::MatrixXd F = verbose_lcs.F_[0]; - Eigen::MatrixXd H = verbose_lcs.H_[0]; - Eigen::VectorXd c = verbose_lcs.c_[0]; + LCS verbose_lcs = lcs_candidates.at(SampleIndex::kCurrentLocation); + Eigen::MatrixXd E = verbose_lcs.E()[0]; + Eigen::MatrixXd F = verbose_lcs.F()[0]; + Eigen::MatrixXd H = verbose_lcs.H()[0]; + Eigen::VectorXd c = verbose_lcs.c()[0]; std::cout << "\nRight side of complementarity: " << std::endl; for (int i = 0; i < N_; i++) { Eigen::VectorXd x = zs[i].head(n_x_); @@ -1147,7 +1595,7 @@ auto c3_start = std::chrono::high_resolution_clock::now(); for (int i = 0; i < N_ + 1; i++) { std::cout << all_sample_dynamically_feasible_plans_ .at(SampleIndex::kCurrentLocation)[i] - .segment(n_q_-7, 7) + .segment(n_q_ - 7, 7) .transpose() << std::endl; } @@ -1155,13 +1603,14 @@ auto c3_start = std::chrono::high_resolution_clock::now(); // Add delay. std::this_thread::sleep_for( - std::chrono::milliseconds(controller_params_.control_loop_delay_ms)); + std::chrono::milliseconds(controller_params_.control_loop_delay_ms)); // End of control loop cleanup. auto finish = std::chrono::high_resolution_clock::now(); auto elapsed = finish - start; double solve_time = - std::chrono::duration_cast(elapsed).count()/1e6; + std::chrono::duration_cast(elapsed).count() / + 1e6; filtered_solve_time_ = (1 - solve_time_filter_constant_) * solve_time + (solve_time_filter_constant_)*filtered_solve_time_; @@ -1186,9 +1635,9 @@ void SamplingC3Controller::ResolvePredictedEEState( // Detect if prediction is requested in current mode. bool in_c3_mode_and_predict = - sampling_c3_options_.use_predicted_x0_c3 && is_doing_c3_; + sampling_c3_options_.use_predicted_x0_c3 && is_doing_c3_; bool in_repos_mode_and_predict = - sampling_c3_options_.use_predicted_x0_repos && !is_doing_c3_; + sampling_c3_options_.use_predicted_x0_repos && !is_doing_c3_; if (!x_pred_curr_plan_.isZero() && !is_teleop && (in_c3_mode_and_predict || in_repos_mode_and_predict)) { @@ -1236,13 +1685,13 @@ void SamplingC3Controller::ClampEndEffectorAcceleration( float nominal_accel = sampling_c3_options_.nominal_ee_accel; for (int i = 0; i < 3; i++) { x_lcs_curr[i] = std::clamp( - x_pred_curr_plan_[i], - x_lcs_curr[i] - nominal_accel * approx_loop_dt * approx_loop_dt, - x_lcs_curr[i] + nominal_accel * approx_loop_dt * approx_loop_dt); - x_lcs_curr[n_q_ + i] = std::clamp( - x_pred_curr_plan_[n_q_ + i], - x_lcs_curr[n_q_ + i] - nominal_accel * approx_loop_dt, - x_lcs_curr[n_q_ + i] + nominal_accel * approx_loop_dt); + x_pred_curr_plan_[i], + x_lcs_curr[i] - nominal_accel * approx_loop_dt * approx_loop_dt, + x_lcs_curr[i] + nominal_accel * approx_loop_dt * approx_loop_dt); + x_lcs_curr[n_q_ + i] = + std::clamp(x_pred_curr_plan_[n_q_ + i], + x_lcs_curr[n_q_ + i] - nominal_accel * approx_loop_dt, + x_lcs_curr[n_q_ + i] + nominal_accel * approx_loop_dt); } } @@ -1253,18 +1702,18 @@ void SamplingC3Controller::CheckForWorkspaceLimitViolations( // xyz checks for (int i = 0; i < sampling_c3_options_.workspace_limits.size(); ++i) { DRAKE_DEMAND(lcs_x_curr->get_data().segment(0, 3).transpose() * - sampling_c3_options_.workspace_limits[i].segment(0, 3) > + sampling_c3_options_.workspace_limits[i].segment(0, 3) > sampling_c3_options_.workspace_limits[i][3]); DRAKE_DEMAND(lcs_x_curr->get_data().segment(0, 3).transpose() * - sampling_c3_options_.workspace_limits[i].segment(0, 3) < + sampling_c3_options_.workspace_limits[i].segment(0, 3) < sampling_c3_options_.workspace_limits[i][4]); } // radius checks DRAKE_DEMAND(std::pow(lcs_x_curr->get_data()[0], 2) + - std::pow(lcs_x_curr->get_data()[1], 2) > + std::pow(lcs_x_curr->get_data()[1], 2) > std::pow(sampling_c3_options_.robot_radius_limits[0], 2)); DRAKE_DEMAND(std::pow(lcs_x_curr->get_data()[0], 2) + - std::pow(lcs_x_curr->get_data()[1], 2) < + std::pow(lcs_x_curr->get_data()[1], 2) < std::pow(sampling_c3_options_.robot_radius_limits[1], 2)); } @@ -1279,8 +1728,6 @@ void SamplingC3Controller::UpdateCostMatrices( U_.clear(); double discount_factor = 1; - dt_ = c3_options.dt; - dt_cost_ = c3_options.dt_cost; for (int i = 0; i < N_ + 1; ++i) { Q_.push_back(discount_factor * c3_options.Q); discount_factor *= c3_options.gamma; @@ -1295,46 +1742,52 @@ void SamplingC3Controller::UpdateCostMatrices( crossed_cost_switching_threshold_) { std::vector quats; for (int i = 0; i < controller_params_.num_objects; i++) { - quats.push_back(x_lcs_curr.segment(3 + 7*i, 4)); + quats.push_back(x_lcs_curr.segment(3 + 7 * i, 4)); } std::vector quats_desired; for (int i = 0; i < controller_params_.num_objects; i++) { - quats_desired.push_back(x_lcs_des.get_value().segment(3 + 7*i, 4)); + quats_desired.push_back(x_lcs_des.get_value().segment(3 + 7 * i, 4)); } std::vector Q_quaternion_dependent_costs; for (int i = 0; i < controller_params_.num_objects; i++) { - Q_quaternion_dependent_costs.push_back(hessian_of_squared_quaternion_angle_difference(quats.at(i), quats_desired.at(i))); + Q_quaternion_dependent_costs.push_back( + hessian_of_squared_quaternion_angle_difference(quats.at(i), + quats_desired.at(i))); } // Get the eigenvalues of the hessian to regularize so the Q matrix is // always PSD. std::vector min_eigvals; for (int i = 0; i < controller_params_.num_objects; i++) { - min_eigvals.push_back(Q_quaternion_dependent_costs.at(i).eigenvalues().real().minCoeff()); + min_eigvals.push_back( + Q_quaternion_dependent_costs.at(i).eigenvalues().real().minCoeff()); } std::vector Q_quaternion_dependent_regularizers_part_1; for (int i = 0; i < controller_params_.num_objects; i++) { - Q_quaternion_dependent_regularizers_part_1.push_back(std::max(0.0, -min_eigvals.at(i)) * Eigen::MatrixXd::Identity(4, 4)); + Q_quaternion_dependent_regularizers_part_1.push_back( + std::max(0.0, -min_eigvals.at(i)) * Eigen::MatrixXd::Identity(4, 4)); } std::vector Q_quaternion_dependent_regularizers_part_2; for (int i = 0; i < controller_params_.num_objects; i++) { - Q_quaternion_dependent_regularizers_part_2.push_back(quats_desired.at(i) * quats_desired.at(i).transpose()); + Q_quaternion_dependent_regularizers_part_2.push_back( + quats_desired.at(i) * quats_desired.at(i).transpose()); } for (int i = 0; i < controller_params_.num_objects; i++) { DRAKE_ASSERT( - Q_quaternion_dependent_costs.at(i).rows() == 4 && - Q_quaternion_dependent_costs.at(i).cols() == 4 && - Q_quaternion_dependent_regularizers_part_2.at(i).rows() == 4 && - Q_quaternion_dependent_regularizers_part_2.at(i).cols() == 4); + Q_quaternion_dependent_costs.at(i).rows() == 4 && + Q_quaternion_dependent_costs.at(i).cols() == 4 && + Q_quaternion_dependent_regularizers_part_2.at(i).rows() == 4 && + Q_quaternion_dependent_regularizers_part_2.at(i).cols() == 4); } double discount_factor = 1; for (int i = 0; i < N_ + 1; ++i) { for (int j = 0; j < controller_params_.num_objects; j++) { - Q_[i].block(3 + 7*j, 3 + 7*j, 4, 4) = - discount_factor * sampling_c3_options_.q_quaternion_dependent_weight * - (Q_quaternion_dependent_costs.at(j) + - Q_quaternion_dependent_regularizers_part_1.at(j) + - sampling_c3_options_.q_quaternion_dependent_regularizer_fraction * - Q_quaternion_dependent_regularizers_part_2.at(j)); + Q_[i].block(3 + 7 * j, 3 + 7 * j, 4, 4) = + discount_factor * + sampling_c3_options_.q_quaternion_dependent_weight * + (Q_quaternion_dependent_costs.at(j) + + Q_quaternion_dependent_regularizers_part_1.at(j) + + sampling_c3_options_.q_quaternion_dependent_regularizer_fraction * + Q_quaternion_dependent_regularizers_part_2.at(j)); discount_factor *= sampling_c3_options_.gamma; } } @@ -1350,15 +1803,63 @@ void SamplingC3Controller::UpdateCostMatrices( } } +vector> SamplingC3Controller::GetResolvedContactPairs( + const MultibodyPlant& plant, const Context& context, + const vector>>& contact_geoms, + const vector& resolve_contacts_to_list, + std::vector num_friction_directions, bool verbose) const { + int n_contacts = std::accumulate(resolve_contacts_to_list.begin(), + resolve_contacts_to_list.end(), 0); + std::vector> resolved_contacts; + resolved_contacts.reserve(n_contacts); + + for (int i = 0; i < contact_geoms.size(); i++) { + DRAKE_DEMAND(contact_geoms[i].size() >= resolve_contacts_to_list[i]); + + const auto& candidates = contact_geoms[i]; + const int num_to_select = resolve_contacts_to_list[i]; + + if (verbose && candidates.size() > 1) { + std::cout << "Contact pair " << i << " : choosing between:" << std::endl; + } + + std::vector distances; + distances.reserve(candidates.size()); + + for (const auto& pair : candidates) { + multibody::GeomGeomCollider collider(plant, pair); + auto [phi_i, J_i] = + collider.EvalPolytope(context, num_friction_directions[i]); + distances.push_back(phi_i); + // if (verbose) { + // PrintVerboseContactInfo(plant, context, pair, phi_i); + // } + } + + for (int j = 0; j < num_to_select; ++j) { + auto min_it = std::min_element(distances.begin(), distances.end()); + int min_index = std::distance(distances.begin(), min_it); + resolved_contacts.push_back(candidates[min_index]); + distances[min_index] = std::numeric_limits::infinity(); + + if (verbose && candidates.size() > 1) { + std::cout << " --> Chose option " << min_index << std::endl; + } + } + } + DRAKE_DEMAND(resolved_contacts.size() == n_contacts); + return resolved_contacts; +} + // Create LCS objects (for the C3 solve and also for the C3 cost calculation) // for each sample. -std::pair, std::vector> +std::pair, std::vector> SamplingC3Controller::CreateLCSObjectsForSamples( const std::vector& candidate_states, - const drake::VectorX& x_lcs_curr, const C3Options& c3_options, - const C3Options& c3_options_curr_location) const { - std::vector lcs_candidates; - std::vector lcs_candidates_for_cost; + const drake::VectorX& x_lcs_curr, + const LCSFactoryOptions& lcs_factory_options) const { + std::vector lcs_candidates; + std::vector lcs_candidates_for_cost; int num_total_samples = candidate_states.size(); for (int i = 0; i < num_total_samples; i++) { @@ -1368,36 +1869,39 @@ SamplingC3Controller::CreateLCSObjectsForSamples( // Resolve the contact pairs and create the LCS. vector> resolved_contact_pairs = - LCSFactory::PreProcessor( - plant_, *context_, contact_pairs_, - sampling_c3_options_.resolve_contacts_to, - c3_options.num_friction_directions, verbose_); - - solvers::LCS lcs_object_sample = solvers::LCSFactory::LinearizePlantToLCS( - plant_, *context_, plant_ad_, *context_ad_, resolved_contact_pairs, - c3_options.mu, dt_, N_, sampling_c3_options_.n_lambda_with_tangential, - sampling_c3_options_.num_friction_directions_per_contact, - sampling_c3_options_.starting_index_per_contact_in_lambda_t_vector, - contact_model_); - + GetResolvedContactPairs( + plant_, *context_, contact_pairs_, + sampling_c3_options_.resolve_contacts_to, + sampling_c3_options_.num_friction_directions_per_contact, verbose_); + LCS lcs_object_sample = + LCSFactory(plant_, *context_, plant_ad_, *context_ad_, + resolved_contact_pairs, lcs_factory_options) + .GenerateLCS(); lcs_candidates.push_back(lcs_object_sample); // Create different LCS objects for cost calculation. vector> resolved_contact_pairs_for_cost_simulation; - resolved_contact_pairs_for_cost_simulation = LCSFactory::PreProcessor( - plant_, *context_, contact_pairs_, - sampling_c3_options_.resolve_contacts_to_for_cost, - sampling_c3_options_.num_friction_directions, verbose_); - solvers::LCS lcs_object_sample_for_cost_simulation = - solvers::LCSFactory::LinearizePlantToLCS( - plant_, *context_, plant_ad_, *context_ad_, - resolved_contact_pairs_for_cost_simulation, - sampling_c3_options_.mu_for_cost, dt_cost_, - N_ * sampling_c3_options_.lcs_dt_resolution, - sampling_c3_options_.n_lambda_with_tangential_cost, - sampling_c3_options_.num_friction_directions_per_contact_cost, - sampling_c3_options_.starting_index_per_contact_in_lambda_t_vector_cost, - contact_model_); + resolved_contact_pairs_for_cost_simulation = GetResolvedContactPairs( + plant_, *context_, contact_pairs_, + sampling_c3_options_.resolve_contacts_to_for_cost, + sampling_c3_options_.num_friction_directions_per_contact_for_cost, + verbose_); + LCSFactoryOptions lcs_factory_options_for_cost = { + .contact_model = controller_params_.sampling_c3_options.contact_model, + .num_contacts = resolved_contact_pairs_for_cost_simulation.size(), + .spring_stiffness = 0.0, + .num_friction_directions = std::nullopt, + .num_friction_directions_per_contact = + sampling_c3_options_.num_friction_directions_per_contact_for_cost, + .mu = sampling_c3_options_.mu_for_cost, + .contact_pair_configs = std::nullopt, + .N = N_ * sampling_c3_options_.lcs_dt_resolution, + .dt = 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, + lcs_factory_options_for_cost) + .GenerateLCS(); lcs_candidates_for_cost.push_back(lcs_object_sample_for_cost_simulation); } @@ -1407,24 +1911,23 @@ SamplingC3Controller::CreateLCSObjectsForSamples( if (verbose_) { // Print the LCS matrices for verbose inspection. - solvers::LCS verbose_lcs = lcs_candidates.at( - SampleIndex::kCurrentLocation); + LCS verbose_lcs = lcs_candidates.at(SampleIndex::kCurrentLocation); std::cout << "A: " << std::endl; - std::cout << verbose_lcs.A_[0] << std::endl; + std::cout << verbose_lcs.A()[0] << std::endl; std::cout << "B: " << std::endl; - std::cout << verbose_lcs.B_[0] << std::endl; + std::cout << verbose_lcs.B()[0] << std::endl; std::cout << "D: " << std::endl; - std::cout << verbose_lcs.D_[0] << std::endl; + std::cout << verbose_lcs.D()[0] << std::endl; std::cout << "d: " << std::endl; - std::cout << verbose_lcs.d_[0] << std::endl; + std::cout << verbose_lcs.d()[0] << std::endl; std::cout << "E: " << std::endl; - std::cout << verbose_lcs.E_[0] << std::endl; + std::cout << verbose_lcs.E()[0] << std::endl; std::cout << "F: " << std::endl; - std::cout << verbose_lcs.F_[0] << std::endl; + std::cout << verbose_lcs.F()[0] << std::endl; std::cout << "H: " << std::endl; - std::cout << verbose_lcs.H_[0] << std::endl; + std::cout << verbose_lcs.H()[0] << std::endl; std::cout << "c: " << std::endl; - std::cout << verbose_lcs.c_[0] << std::endl; + std::cout << verbose_lcs.c()[0] << std::endl; } return std::make_pair(lcs_candidates, lcs_candidates_for_cost); @@ -1456,11 +1959,11 @@ void SamplingC3Controller::UpdateC3ExecutionTrajectory( if (filtered_solve_time_ < (N_ - 1) * dt_) { int last_passed_index = filtered_solve_time_ / dt_; double fraction_to_next_index = - (filtered_solve_time_ / dt_) - (double)last_passed_index; + (filtered_solve_time_ / dt_) - (double)last_passed_index; x_pred_curr_plan_ = - knots.col(last_passed_index) + - fraction_to_next_index * - (knots.col(last_passed_index + 1) - knots.col(last_passed_index)); + knots.col(last_passed_index) + + fraction_to_next_index * + (knots.col(last_passed_index + 1) - knots.col(last_passed_index)); } else { x_pred_curr_plan_ = knots.col(N_ - 1); } @@ -1475,17 +1978,23 @@ void SamplingC3Controller::UpdateC3ExecutionTrajectory( double y_max = sampling_c3_options_.workspace_limits[1][4]; // if ee is close to wall, raise z_height - if ((x_sol[0][0] <= x_min + 0.05 && x_sol[0][0] >= x_min - sampling_c3_options_.workspace_margins) || - (x_sol[0][0] >= x_max - 0.05 && x_sol[0][0] <= x_max + sampling_c3_options_.workspace_margins) || - (x_sol[0][1] <= y_min + 0.05 && x_sol[0][1] >= y_min - sampling_c3_options_.workspace_margins) || - (x_sol[0][1] >= y_max - 0.05 && x_sol[0][1] <= y_max + sampling_c3_options_.workspace_margins)) { - wall_offset = 0.01; + if ((x_sol[0][0] <= x_min + 0.05 && + x_sol[0][0] >= x_min - sampling_c3_options_.workspace_margins) || + (x_sol[0][0] >= x_max - 0.05 && + x_sol[0][0] <= x_max + sampling_c3_options_.workspace_margins) || + (x_sol[0][1] <= y_min + 0.05 && + x_sol[0][1] >= y_min - sampling_c3_options_.workspace_margins) || + (x_sol[0][1] >= y_max - 0.05 && + x_sol[0][1] <= y_max + sampling_c3_options_.workspace_margins)) { + wall_offset = 0.01; } } for (int i = 0; i < N_; i++) { - knots(2, i) = sampling_params_.z_height + wall_offset; // keep ee height constant - knots(5 + 7 * controller_params_.num_objects, i) = 0; // keep ee z-velo constant + knots(2, i) = + sampling_params_.z_height + wall_offset; // keep ee height constant + knots(5 + 7 * controller_params_.num_objects, i) = + 0; // keep ee z-velo constant } // Add end effector position target to LCM Trajectory. @@ -1501,30 +2010,35 @@ void SamplingC3Controller::UpdateC3ExecutionTrajectory( Eigen::MatrixXd ee_orientations = Eigen::MatrixXd::Zero(4, N_); Eigen::Vector3d workspace_center(Eigen::Vector3d::Zero(3)); - workspace_center[0] = (sampling_c3_options_.workspace_limits[0][3] + sampling_c3_options_.workspace_limits[0][4]) / 2; - workspace_center[1] = (sampling_c3_options_.workspace_limits[1][3] + sampling_c3_options_.workspace_limits[1][4]) / 2; - - Eigen::Vector2d max_radius(sampling_c3_options_.workspace_limits[0][4] - - workspace_center[0], sampling_c3_options_.workspace_limits[1][4] - workspace_center[1]); + workspace_center[0] = (sampling_c3_options_.workspace_limits[0][3] + + sampling_c3_options_.workspace_limits[0][4]) / + 2; + workspace_center[1] = (sampling_c3_options_.workspace_limits[1][3] + + sampling_c3_options_.workspace_limits[1][4]) / + 2; + + Eigen::Vector2d max_radius( + sampling_c3_options_.workspace_limits[0][4] - workspace_center[0], + sampling_c3_options_.workspace_limits[1][4] - workspace_center[1]); double max_dist = max_radius.norm(); Eigen::Vector3d direction = ee_position_curr_ - workspace_center; Eigen::Matrix3d rot; - rot << 0, 1, 0, - -1, 0, 0, - 0, 0, 1; + rot << 0, 1, 0, -1, 0, 0, 0, 0, 1; direction[2] = 0; direction = rot * direction; - // If outside of radius, tilt ee so away from workspace center, otherwise set vertical - // Tilt depending on how far from center (for smoothness) - double theta = (direction.norm() / max_dist) * reposition_params_.max_tilt_angle * M_PI / 180.0; + // If outside of radius, tilt ee so away from workspace center, otherwise set + // vertical Tilt depending on how far from center (for smoothness) + double theta = (direction.norm() / max_dist) * + reposition_params_.max_tilt_angle * M_PI / 180.0; direction.normalize(); Eigen::AngleAxisd angle_axis(theta, direction); Eigen::Quaterniond q_rotated(angle_axis); - Eigen::Vector4d q_vec(q_rotated.w(), q_rotated.x(), q_rotated.y(), q_rotated.z()); + Eigen::Vector4d q_vec(q_rotated.w(), q_rotated.x(), q_rotated.y(), + q_rotated.z()); for (int i = 0; i < N_; i++) { ee_orientations.col(i) = q_vec; @@ -1532,12 +2046,13 @@ void SamplingC3Controller::UpdateC3ExecutionTrajectory( LcmTrajectory::Trajectory ee_orientation_traj; ee_orientation_traj.traj_name = "end_effector_orientation_target"; - ee_orientation_traj.datatypes = std::vector(ee_orientations.rows(), "double"); // quaternion + ee_orientation_traj.datatypes = + std::vector(ee_orientations.rows(), "double"); // quaternion ee_orientation_traj.datapoints = ee_orientations; ee_orientation_traj.time_vector = timestamps.cast(); - c3_execution_lcm_traj_.AddTrajectory(ee_orientation_traj.traj_name, ee_orientation_traj); - + c3_execution_lcm_traj_.AddTrajectory(ee_orientation_traj.traj_name, + ee_orientation_traj); // Add end effector force target to LCM Trajectory. // In c3 mode, the end effector forces should match the solved c3 inputs. @@ -1548,7 +2063,7 @@ void SamplingC3Controller::UpdateC3ExecutionTrajectory( LcmTrajectory::Trajectory force_traj; force_traj.traj_name = "end_effector_force_target"; force_traj.datatypes = - std::vector(force_samples.rows(), "double"); + std::vector(force_samples.rows(), "double"); force_traj.datapoints = force_samples; force_traj.time_vector = timestamps.cast(); c3_execution_lcm_traj_.AddTrajectory(force_traj.traj_name, force_traj); @@ -1562,25 +2077,25 @@ void SamplingC3Controller::UpdateRepositioningExecutionTrajectory( const VectorXd& x_lcs, const double& t_context) const { // Get the best sample location. Eigen::Vector3d best_sample_location = - all_sample_locations_[best_sample_index_]; + all_sample_locations_[best_sample_index_]; // Update the previous repositioning target for reference in next loop. prev_repositioning_target_ = best_sample_location; // Generate knot points according to the repositioning strategy. Eigen::MatrixXd knots = Reposition( - n_q_, n_x_, N_, x_lcs, best_sample_location, dt_, is_doing_c3_, - finished_reposition_flag_, reposition_params_, sampling_c3_options_); + n_q_, n_x_, N_, x_lcs, best_sample_location, dt_, is_doing_c3_, + finished_reposition_flag_, reposition_params_, sampling_c3_options_); // Update predicted next state if in this mode. if (!is_doing_c3_) { if (filtered_solve_time_ < (N_ - 1) * dt_) { int last_passed_index = filtered_solve_time_ / dt_; double fraction_to_next_index = - (filtered_solve_time_ / dt_) - (double)last_passed_index; + (filtered_solve_time_ / dt_) - (double)last_passed_index; x_pred_curr_plan_ = - knots.col(last_passed_index) + - fraction_to_next_index * - (knots.col(last_passed_index + 1) - knots.col(last_passed_index)); + knots.col(last_passed_index) + + fraction_to_next_index * + (knots.col(last_passed_index + 1) - knots.col(last_passed_index)); } else { x_pred_curr_plan_ = knots.col(N_ - 1); } @@ -1600,36 +2115,39 @@ void SamplingC3Controller::UpdateRepositioningExecutionTrajectory( repos_execution_lcm_traj_.ClearTrajectories(); repos_execution_lcm_traj_.AddTrajectory(ee_traj.traj_name, ee_traj); - Eigen::MatrixXd ee_orientations = Eigen::MatrixXd::Zero(4, N_); Eigen::Vector3d workspace_center(Eigen::Vector3d::Zero(3)); - workspace_center[0] = (sampling_c3_options_.workspace_limits[0][3] + sampling_c3_options_.workspace_limits[0][4]) / 2; - workspace_center[1] = (sampling_c3_options_.workspace_limits[1][3] + sampling_c3_options_.workspace_limits[1][4]) / 2; - - Eigen::Vector2d max_radius(sampling_c3_options_.workspace_limits[0][4] - - workspace_center[0], sampling_c3_options_.workspace_limits[1][4] - workspace_center[1]); + workspace_center[0] = (sampling_c3_options_.workspace_limits[0][3] + + sampling_c3_options_.workspace_limits[0][4]) / + 2; + workspace_center[1] = (sampling_c3_options_.workspace_limits[1][3] + + sampling_c3_options_.workspace_limits[1][4]) / + 2; + + Eigen::Vector2d max_radius( + sampling_c3_options_.workspace_limits[0][4] - workspace_center[0], + sampling_c3_options_.workspace_limits[1][4] - workspace_center[1]); double max_dist = max_radius.norm(); Eigen::Vector3d direction = ee_position_curr_ - workspace_center; Eigen::Matrix3d rot; - rot << 0, 1, 0, - -1, 0, 0, - 0, 0, 1; + rot << 0, 1, 0, -1, 0, 0, 0, 0, 1; direction[2] = 0; direction = rot * direction; - // If outside of radius, tilt ee so away from workspace center, otherwise set vertical - // Tilt depending on how far from center (for smoothness) - double theta = (direction.norm() / max_dist) * reposition_params_.max_tilt_angle * M_PI / 180.0; + // If outside of radius, tilt ee so away from workspace center, otherwise set + // vertical Tilt depending on how far from center (for smoothness) + double theta = (direction.norm() / max_dist) * + reposition_params_.max_tilt_angle * M_PI / 180.0; direction.normalize(); Eigen::AngleAxisd angle_axis(theta, direction); Eigen::Quaterniond q_rotated(angle_axis); - Eigen::Vector4d q_vec(q_rotated.w(), q_rotated.x(), q_rotated.y(), q_rotated.z()); - + Eigen::Vector4d q_vec(q_rotated.w(), q_rotated.x(), q_rotated.y(), + q_rotated.z()); for (int i = 0; i < N_; i++) { ee_orientations.col(i) = q_vec; @@ -1637,19 +2155,20 @@ void SamplingC3Controller::UpdateRepositioningExecutionTrajectory( LcmTrajectory::Trajectory ee_orientation_traj; ee_orientation_traj.traj_name = "end_effector_orientation_target"; - ee_orientation_traj.datatypes = std::vector(ee_orientations.rows(), "double"); // quaternion + ee_orientation_traj.datatypes = + std::vector(ee_orientations.rows(), "double"); // quaternion ee_orientation_traj.datapoints = ee_orientations; ee_orientation_traj.time_vector = timestamps.cast(); - repos_execution_lcm_traj_.AddTrajectory(ee_orientation_traj.traj_name, ee_orientation_traj); - + repos_execution_lcm_traj_.AddTrajectory(ee_orientation_traj.traj_name, + ee_orientation_traj); // In repositioning mode, the end effector should not exert forces. MatrixXd force_samples = MatrixXd::Zero(3, N_); LcmTrajectory::Trajectory force_traj; force_traj.traj_name = "end_effector_force_target"; force_traj.datatypes = - std::vector(force_samples.rows(), "double"); + std::vector(force_samples.rows(), "double"); force_traj.datapoints = force_samples; force_traj.time_vector = timestamps.cast(); repos_execution_lcm_traj_.AddTrajectory(force_traj.traj_name, force_traj); @@ -1659,10 +2178,8 @@ void SamplingC3Controller::UpdateRepositioningExecutionTrajectory( // Prune outdated samples from a sample buffer, based on object motion. void SamplingC3Controller::PruneOutdatedSamplesFromBuffer( - const Eigen::VectorXd& x_lcs, - int* num_in_buffer, - Eigen::MatrixXd* sample_buffer, - Eigen::VectorXd* sample_costs_buffer, + const Eigen::VectorXd& x_lcs, int* num_in_buffer, + Eigen::MatrixXd* sample_buffer, Eigen::VectorXd* sample_costs_buffer, const double& pos_error_sample_retention, const double& ang_error_sample_retention) const { int n_buffer_length = sample_costs_buffer->size(); @@ -1670,10 +2187,12 @@ void SamplingC3Controller::PruneOutdatedSamplesFromBuffer( std::vector> mask_satisfies_rot; std::vector> mask_satisfies_pos; for (int i = 0; i < controller_params_.num_objects; i++) { - Vector3d object_pos = x_lcs.segment(7 + 7*i, 3); - Eigen::Vector4d object_quat = x_lcs.segment(3 + 7*i, 4).normalized(); - MatrixXd buffer_xyzs = sample_buffer->block(0, 7 + 7*i, n_buffer_length, 3); - MatrixXd buffer_quats = sample_buffer->block(0, 3 + 7*i, n_buffer_length, 4); + Vector3d object_pos = x_lcs.segment(7 + 7 * i, 3); + Eigen::Vector4d object_quat = x_lcs.segment(3 + 7 * i, 4).normalized(); + MatrixXd buffer_xyzs = + sample_buffer->block(0, 7 + 7 * i, n_buffer_length, 3); + MatrixXd buffer_quats = + sample_buffer->block(0, 3 + 7 * i, n_buffer_length, 4); // Compute the angular difference. VectorXd quat_dots = (buffer_quats * object_quat).array().abs(); @@ -1684,7 +2203,8 @@ void SamplingC3Controller::PruneOutdatedSamplesFromBuffer( // Compute the linear difference. MatrixXd pos_deltas = buffer_xyzs.rowwise() - object_pos.transpose(); VectorXd distances = pos_deltas.rowwise().norm(); - mask_satisfies_pos.push_back(distances.array() < pos_error_sample_retention); + mask_satisfies_pos.push_back(distances.array() < + pos_error_sample_retention); } // Keep buffer if none of objects moved @@ -1721,17 +2241,17 @@ void SamplingC3Controller::MaintainSampleBuffers(const VectorXd& x_lcs) const { // prune outdated samples; new samples get added one at a time when the // controller goes from repositioning to C3 mode. PruneOutdatedSamplesFromBuffer( - x_lcs, &num_in_unsuccessful_buffer_, &unsuccessful_sample_buffer_, - &unsuccessful_sample_costs_buffer_, - sampling_params_.unsuccessful_pos_error_sample_retention, - sampling_params_.unsuccessful_ang_error_sample_retention); + x_lcs, &num_in_unsuccessful_buffer_, &unsuccessful_sample_buffer_, + &unsuccessful_sample_costs_buffer_, + sampling_params_.unsuccessful_pos_error_sample_retention, + sampling_params_.unsuccessful_ang_error_sample_retention); // Second, handle the unattempted sample buffer. First, prune outdated // samples. - PruneOutdatedSamplesFromBuffer( - x_lcs, &num_in_buffer_, &sample_buffer_, &sample_costs_buffer_, - sampling_params_.pos_error_sample_retention, - sampling_params_.ang_error_sample_retention); + PruneOutdatedSamplesFromBuffer(x_lcs, &num_in_buffer_, &sample_buffer_, + &sample_costs_buffer_, + sampling_params_.pos_error_sample_retention, + sampling_params_.ang_error_sample_retention); int retained_count = num_in_buffer_; // Third, in preparation for adding new samples stored in @@ -1745,12 +2265,12 @@ void SamplingC3Controller::MaintainSampleBuffers(const VectorXd& x_lcs) const { } if (retained_count + num_to_add > sampling_params_.N_sample_buffer) { int shift_by = - retained_count + num_to_add - sampling_params_.N_sample_buffer; + retained_count + num_to_add - sampling_params_.N_sample_buffer; retained_count -= shift_by; sample_buffer_.block(0, 0, retained_count, n_q_) = - sample_buffer_.block(shift_by, 0, retained_count, n_q_); + sample_buffer_.block(shift_by, 0, retained_count, n_q_); sample_costs_buffer_.segment(0, retained_count) = - sample_costs_buffer_.segment(shift_by, retained_count); + sample_costs_buffer_.segment(shift_by, retained_count); } // Fourth, add the new samples stored in all_sample_locations_ and @@ -1769,15 +2289,15 @@ void SamplingC3Controller::MaintainSampleBuffers(const VectorXd& x_lcs) const { VectorXd new_config = x_lcs.head(n_q_); new_config.head(3) = all_sample_locations_[i - retained_count]; double travel_cost = progress_params_.travel_cost_per_meter * - (new_config.head(2) - x_lcs.head(2)).norm(); + (new_config.head(2) - x_lcs.head(2)).norm(); // Ensure a normalized quaternion is written to the buffer. for (int i = 0; i < controller_params_.num_objects; i++) { - Eigen::Vector4d object_quat = x_lcs.segment(3 + 7*i, 4).normalized(); - new_config.segment(3 + 7*i, 4) = object_quat; + Eigen::Vector4d object_quat = x_lcs.segment(3 + 7 * i, 4).normalized(); + new_config.segment(3 + 7 * i, 4) = object_quat; } sample_buffer_.row(buffer_count) = new_config; sample_costs_buffer_[buffer_count] = - all_sample_costs_[i - retained_count] - travel_cost; + all_sample_costs_[i - retained_count] - travel_cost; buffer_count++; } } @@ -1789,17 +2309,18 @@ void SamplingC3Controller::MaintainSampleBuffers(const VectorXd& x_lcs) const { // Incorporate travel costs for each sample in the buffer. MatrixXd xy_samples = sample_buffer_.block(0, 0, num_in_buffer_, 2); Eigen::Vector2d xy_ref = x_lcs.head(2); - VectorXd travel_costs = progress_params_.travel_cost_per_meter * - (xy_samples.rowwise() - xy_ref.transpose()).rowwise().norm(); + VectorXd travel_costs = + progress_params_.travel_cost_per_meter * + (xy_samples.rowwise() - xy_ref.transpose()).rowwise().norm(); eligible_costs += travel_costs; int lowest_cost_index; double lowest_buffer_cost = eligible_costs.minCoeff(&lowest_cost_index); VectorXd lowest_cost_sample = sample_buffer_.row(lowest_cost_index); sample_buffer_.row(lowest_cost_index) = - sample_buffer_.row(num_in_buffer_ - 1); + sample_buffer_.row(num_in_buffer_ - 1); sample_costs_buffer_[lowest_cost_index] = - sample_costs_buffer_[num_in_buffer_ - 1]; + sample_costs_buffer_[num_in_buffer_ - 1]; sample_buffer_.row(num_in_buffer_ - 1) = lowest_cost_sample; sample_costs_buffer_[num_in_buffer_ - 1] = lowest_buffer_cost; @@ -1811,7 +2332,7 @@ void SamplingC3Controller::MaintainSampleBuffers(const VectorXd& x_lcs) const { // If eligible, augment the current control loop's considered samples with the // best one from the buffer. void SamplingC3Controller::AugmentSamplesWithBuffer( - std::vector>& c3_objects) const { + std::vector>& c3_objects) const { // Add the best from the buffer to the samples, but only if in C3 mode and // only if the best in the buffer is distinct from the current set of samples. if ((is_doing_c3_) && @@ -1819,27 +2340,28 @@ void SamplingC3Controller::AugmentSamplesWithBuffer( // Get the lowest cost from the buffer, incorporating travel cost. double lowest_buffer_cost = sample_costs_buffer_[num_in_buffer_ - 1]; double travel_cost = progress_params_.travel_cost_per_meter * - (sample_buffer_.row(num_in_buffer_ - 1).head(2) - - all_sample_locations_[0].head(2)).norm(); + (sample_buffer_.row(num_in_buffer_ - 1).head(2) - + all_sample_locations_[0].head(2)) + .norm(); lowest_buffer_cost += travel_cost; Vector3d best_buffer_ee_sample = - sample_buffer_.row(num_in_buffer_ - 1).head(3); + sample_buffer_.row(num_in_buffer_ - 1).head(3); // Get the lowest cost from the current set of samples (these already // incoporate travel cost). double lowest_new_cost = - *std::min_element(all_sample_costs_.begin(), all_sample_costs_.end()); + *std::min_element(all_sample_costs_.begin(), all_sample_costs_.end()); std::vector::iterator it = std::min_element( - std::begin(all_sample_costs_), std::end(all_sample_costs_)); + std::begin(all_sample_costs_), std::end(all_sample_costs_)); int lowest_new_cost_index = - (SampleIndex)(std::distance(std::begin(all_sample_costs_), it)); + (SampleIndex)(std::distance(std::begin(all_sample_costs_), it)); Vector3d best_new_ee_sample = all_sample_locations_[lowest_new_cost_index]; // Initialize the buffer plan with something. if (dynamically_feasible_buffer_plan_.size() != N_ + 1) { c3_buffer_plan_ = c3_objects[lowest_new_cost_index]; dynamically_feasible_buffer_plan_ = - all_sample_dynamically_feasible_plans_[lowest_new_cost_index]; + all_sample_dynamically_feasible_plans_[lowest_new_cost_index]; } // If the best in the buffer is from the current set of samples, store the // associated C3 object and dynamically feasible plan, but don't add to the @@ -1848,7 +2370,7 @@ void SamplingC3Controller::AugmentSamplesWithBuffer( ((best_buffer_ee_sample - best_new_ee_sample).norm() < 1e-5)) { c3_buffer_plan_ = c3_objects[lowest_new_cost_index]; dynamically_feasible_buffer_plan_ = - all_sample_dynamically_feasible_plans_[lowest_new_cost_index]; + all_sample_dynamically_feasible_plans_[lowest_new_cost_index]; } // If the best in the buffer is distinct from the current set of samples, // consider it for repositioning by adding it to the set of samples, costs, @@ -1858,7 +2380,7 @@ void SamplingC3Controller::AugmentSamplesWithBuffer( all_sample_locations_.push_back(best_buffer_ee_sample); c3_objects.push_back(c3_buffer_plan_); all_sample_dynamically_feasible_plans_.push_back( - dynamically_feasible_buffer_plan_); + dynamically_feasible_buffer_plan_); } } } @@ -1869,27 +2391,30 @@ void SamplingC3Controller::AddToUnsuccessfulBuffer( // Check if the unsuccessful buffer is going to overflow. if (num_in_unsuccessful_buffer_ == sampling_params_.N_unsuccessful_sample_buffer) { - std::cout<<"!!! WARNING !!! Unsuccessful sample buffer overflow"<& x_lcs_curr, - const BasicVector& x_lcs_final_des, - bool& met_minimum_progress, + const BasicVector& x_lcs_final_des, bool& met_minimum_progress, const bool& print_current_pos_and_rot_cost) const { bool updated_cost = false; bool updated_config_cost = false; bool updated_pos_or_rot = false; - double cost_progress_fraction = -INFINITY; // Negative means progress. + double cost_progress_fraction = -INFINITY; // Negative means progress. std::vector Q_pos_and_rots; for (int i = 0; i < controller_params_.num_objects; i++) { - Q_pos_and_rots.push_back(Q_[0].block(3 + 7*i, 3 + 7*i, 7, 7)); + Q_pos_and_rots.push_back(Q_[0].block(3 + 7 * i, 3 + 7 * i, 7, 7)); } std::vector pos_and_rot_error_vecs; for (int i = 0; i < controller_params_.num_objects; i++) { pos_and_rot_error_vecs.push_back( - x_lcs_curr.segment(3 + 7*i, 7) - x_lcs_final_des.get_value().segment(3 + 7*i, 7) - ); + x_lcs_curr.segment(3 + 7 * i, 7) - + x_lcs_final_des.get_value().segment(3 + 7 * i, 7)); } double curr_pos_and_rot_cost = 0; // accumulate costs across all objects for (int i = 0; i < controller_params_.num_objects; i++) { - curr_pos_and_rot_cost += - pos_and_rot_error_vecs.at(i).transpose() * Q_pos_and_rots.at(i) * pos_and_rot_error_vecs.at(i); + curr_pos_and_rot_cost += pos_and_rot_error_vecs.at(i).transpose() * + Q_pos_and_rots.at(i) * + pos_and_rot_error_vecs.at(i); } // Check for progress along different metrics. @@ -1960,17 +2485,17 @@ void SamplingC3Controller::KeepTrackOfC3ModeProgress( // One of the progress metrics requires a history of object configuration // costs. Maintain this history and check for progress. object_config_cost_history_.push(curr_pos_and_rot_cost); - int max_history_length = - progress_params_.progress_enforced_over_n_loops; + int max_history_length = progress_params_.progress_enforced_over_n_loops; if (object_config_cost_history_.size() > max_history_length) { object_config_cost_history_.pop(); } // Check for progress if the history is full. if (object_config_cost_history_.size() == max_history_length) { // Note: front() is the oldest cost, back() is the most recent. - cost_progress_fraction = // Negative means progress. - ((object_config_cost_history_.back()-object_config_cost_history_.front()) - / object_config_cost_history_.front()); + cost_progress_fraction = // Negative means progress. + ((object_config_cost_history_.back() - + object_config_cost_history_.front()) / + object_config_cost_history_.front()); } if (print_current_pos_and_rot_cost) { @@ -1981,9 +2506,10 @@ void SamplingC3Controller::KeepTrackOfC3ModeProgress( // Keep track of how many control loops have passed since the best seen // progress metric in this mode. ProgressMetric progress_metric = progress_params_.track_c3_progress_via; - if (((progress_metric == ProgressMetric::kC3Cost) && updated_cost) - || ((progress_metric == ProgressMetric::kConfigCost) && updated_config_cost) - || ((progress_metric == ProgressMetric::kPosOrRotCost) && + if (((progress_metric == ProgressMetric::kC3Cost) && updated_cost) || + ((progress_metric == ProgressMetric::kConfigCost) && + updated_config_cost) || + ((progress_metric == ProgressMetric::kPosOrRotCost) && updated_pos_or_rot)) { best_progress_steps_ago_ = 0; } else { @@ -1994,14 +2520,16 @@ void SamplingC3Controller::KeepTrackOfC3ModeProgress( int num_control_loops_to_wait = progress_params_.num_control_loops_to_wait; if (!crossed_cost_switching_threshold_) { num_control_loops_to_wait = - progress_params_.num_control_loops_to_wait_position; + progress_params_.num_control_loops_to_wait_position; } if (progress_metric == ProgressMetric::kConfigCostDrop) { - if (cost_progress_fraction > -progress_params_.progress_enforced_cost_drop) - { met_minimum_progress = false; } + if (cost_progress_fraction > + -progress_params_.progress_enforced_cost_drop) { + met_minimum_progress = false; + } + } else if (best_progress_steps_ago_ > num_control_loops_to_wait) { + met_minimum_progress = false; } - else if (best_progress_steps_ago_ > num_control_loops_to_wait) - { met_minimum_progress = false; } } // Reset the metrics used to track progress in C3 mode. @@ -2036,55 +2564,59 @@ void SamplingC3Controller::OutputC3SolutionCurrPlanActor( c3_solution->time_vector_(i) = base_time + i * dt_; c3_solution->x_sol_.col(i) = z_sol[i].segment(0, n_x_).cast(); c3_solution->lambda_sol_.col(i) = - z_sol[i].segment(n_x_, n_lambda_).cast(); + z_sol[i].segment(n_x_, n_lambda_).cast(); c3_solution->u_sol_.col(i) = - z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); + z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); } MatrixXd knots = MatrixXd::Zero(6, N_); knots.topRows(3) = c3_solution->x_sol_.topRows(3).cast(); knots.bottomRows(3) = - c3_solution->x_sol_.bottomRows(n_v_).topRows(3).cast(); + c3_solution->x_sol_.bottomRows(n_v_).topRows(3).cast(); LcmTrajectory::Trajectory end_effector_traj; end_effector_traj.traj_name = "end_effector_position_target"; end_effector_traj.datatypes = - std::vector(knots.rows(), "double"); + std::vector(knots.rows(), "double"); end_effector_traj.datapoints = knots; end_effector_traj.time_vector = c3_solution->time_vector_.cast(); LcmTrajectory lcm_traj({end_effector_traj}, {"end_effector_position_target"}, "end_effector_position_target", "end_effector_position_target", false); - Eigen::MatrixXd ee_orientations = Eigen::MatrixXd::Zero(4, N_); Eigen::Vector3d workspace_center(Eigen::Vector3d::Zero(3)); - workspace_center[0] = (sampling_c3_options_.workspace_limits[0][3] + sampling_c3_options_.workspace_limits[0][4]) / 2; - workspace_center[1] = (sampling_c3_options_.workspace_limits[1][3] + sampling_c3_options_.workspace_limits[1][4]) / 2; - - Eigen::Vector2d max_radius(sampling_c3_options_.workspace_limits[0][4] - - workspace_center[0], sampling_c3_options_.workspace_limits[1][4] - workspace_center[1]); + workspace_center[0] = (sampling_c3_options_.workspace_limits[0][3] + + sampling_c3_options_.workspace_limits[0][4]) / + 2; + workspace_center[1] = (sampling_c3_options_.workspace_limits[1][3] + + sampling_c3_options_.workspace_limits[1][4]) / + 2; + + Eigen::Vector2d max_radius( + sampling_c3_options_.workspace_limits[0][4] - workspace_center[0], + sampling_c3_options_.workspace_limits[1][4] - workspace_center[1]); double max_dist = max_radius.norm(); Eigen::Vector3d direction = ee_position_curr_ - workspace_center; Eigen::Matrix3d rot; - rot << 0, 1, 0, - -1, 0, 0, - 0, 0, 1; + rot << 0, 1, 0, -1, 0, 0, 0, 0, 1; direction[2] = 0; direction = rot * direction; - // If outside of radius, tilt ee so away from workspace center, otherwise set vertical - // Tilt depending on how far from center (for smoothness) - double theta = (direction.norm() / max_dist) * reposition_params_.max_tilt_angle * M_PI / 180.0; + // If outside of radius, tilt ee so away from workspace center, otherwise set + // vertical Tilt depending on how far from center (for smoothness) + double theta = (direction.norm() / max_dist) * + reposition_params_.max_tilt_angle * M_PI / 180.0; direction.normalize(); Eigen::AngleAxisd angle_axis(theta, direction); Eigen::Quaterniond q_rotated(angle_axis); - Eigen::Vector4d q_vec(q_rotated.w(), q_rotated.x(), q_rotated.y(), q_rotated.z()); + Eigen::Vector4d q_vec(q_rotated.w(), q_rotated.x(), q_rotated.y(), + q_rotated.z()); for (int i = 0; i < N_; i++) { ee_orientations.col(i) = q_vec; @@ -2092,7 +2624,8 @@ void SamplingC3Controller::OutputC3SolutionCurrPlanActor( LcmTrajectory::Trajectory ee_orientation_traj; ee_orientation_traj.traj_name = "end_effector_orientation_target"; - ee_orientation_traj.datatypes = std::vector(ee_orientations.rows(), "double"); // quaternion + ee_orientation_traj.datatypes = + std::vector(ee_orientations.rows(), "double"); // quaternion ee_orientation_traj.datapoints = ee_orientations; ee_orientation_traj.time_vector = c3_solution->time_vector_.cast(); lcm_traj.AddTrajectory(ee_orientation_traj.traj_name, ee_orientation_traj); @@ -2101,7 +2634,7 @@ void SamplingC3Controller::OutputC3SolutionCurrPlanActor( LcmTrajectory::Trajectory force_traj; force_traj.traj_name = "end_effector_force_target"; force_traj.datatypes = - std::vector(force_samples.rows(), "double"); + std::vector(force_samples.rows(), "double"); force_traj.datapoints = force_samples; force_traj.time_vector = c3_solution->time_vector_.cast(); lcm_traj.AddTrajectory(force_traj.traj_name, force_traj); @@ -2127,18 +2660,18 @@ void SamplingC3Controller::OutputC3SolutionCurrPlanObject( c3_solution->time_vector_(i) = base_time + i * dt_; c3_solution->x_sol_.col(i) = z_sol[i].segment(0, n_x_).cast(); c3_solution->lambda_sol_.col(i) = - z_sol[i].segment(n_x_, n_lambda_).cast(); + z_sol[i].segment(n_x_, n_lambda_).cast(); c3_solution->u_sol_.col(i) = - z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); - + z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); } - std::vector knots; for (int i = 0; i < controller_params_.num_objects; i++) { MatrixXd knot = MatrixXd::Zero(6, N_); - knot.topRows(3) = c3_solution->x_sol_.middleRows(7*i + 7, 3).cast(); - knot.bottomRows(3) = c3_solution->x_sol_.middleRows(n_q_ + 6*i + 6, 3).cast(); + knot.topRows(3) = + c3_solution->x_sol_.middleRows(7 * i + 7, 3).cast(); + knot.bottomRows(3) = + c3_solution->x_sol_.middleRows(n_q_ + 6 * i + 6, 3).cast(); knots.push_back(knot); } @@ -2146,20 +2679,20 @@ void SamplingC3Controller::OutputC3SolutionCurrPlanObject( for (int i = 0; i < controller_params_.num_objects; i++) { LcmTrajectory::Trajectory object_traj; object_traj.traj_name = "object_position_target_" + std::to_string(i); - object_traj.datatypes = std::vector(knots.at(i).rows(), "double"); + object_traj.datatypes = + std::vector(knots.at(i).rows(), "double"); object_traj.datapoints = knots.at(i); object_traj.time_vector = c3_solution->time_vector_.cast(); object_trajs.push_back(object_traj); } - std::vector traj_names; for (int i = 0; i < controller_params_.num_objects; i++) { traj_names.push_back("object_position_target_" + std::to_string(i)); } - LcmTrajectory lcm_traj(object_trajs, traj_names, - "object_targets", "object_targets", false); + LcmTrajectory lcm_traj(object_trajs, traj_names, "object_targets", + "object_targets", false); LcmTrajectory::Trajectory object_orientation_traj; // first 3 rows are rpy, last 3 rows are angular velocity @@ -2167,24 +2700,21 @@ void SamplingC3Controller::OutputC3SolutionCurrPlanObject( for (int i = 0; i < controller_params_.num_objects; i++) { MatrixXd orientation_sample = MatrixXd::Zero(4, N_); orientation_sample = - c3_solution->x_sol_.middleRows(3 + 7*i, 4).cast(); + c3_solution->x_sol_.middleRows(3 + 7 * i, 4).cast(); - object_orientation_traj.traj_name = "object_orientation_target_" + std::to_string(i); + object_orientation_traj.traj_name = + "object_orientation_target_" + std::to_string(i); object_orientation_traj.datatypes = - std::vector(orientation_sample.rows(), "double"); + std::vector(orientation_sample.rows(), "double"); object_orientation_traj.datapoints = orientation_sample; object_orientation_traj.time_vector = - c3_solution->time_vector_.cast(); + c3_solution->time_vector_.cast(); lcm_traj.AddTrajectory(object_orientation_traj.traj_name, - object_orientation_traj); - + object_orientation_traj); } - - output->saved_traj = lcm_traj.GenerateLcmObject(); output->utime = context.get_time() * 1e6; - } void SamplingC3Controller::OutputC3SolutionCurrPlan( @@ -2199,9 +2729,9 @@ void SamplingC3Controller::OutputC3SolutionCurrPlan( c3_solution->time_vector_(i) = base_time + i * dt_; c3_solution->x_sol_.col(i) = z_sol[i].segment(0, n_x_).cast(); c3_solution->lambda_sol_.col(i) = - z_sol[i].segment(n_x_, n_lambda_).cast(); + z_sol[i].segment(n_x_, n_lambda_).cast(); c3_solution->u_sol_.col(i) = - z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); + z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); } } @@ -2225,7 +2755,7 @@ void SamplingC3Controller::OutputC3IntermediatesCurrPlan( void SamplingC3Controller::OutputLCSContactJacobianCurrPlan( const drake::systems::Context& context, std::pair>* - lcs_contact_jacobian) const { + lcs_contact_jacobian) const { const TimestampedVector* lcs_x = (TimestampedVector*)this->EvalVectorInput(context, lcs_state_input_port_); @@ -2233,22 +2763,22 @@ void SamplingC3Controller::OutputLCSContactJacobianCurrPlan( UpdateContext(n_q_, n_v_, n_u_, plant_, context_, plant_ad_, context_ad_, lcs_x->get_data()); - C3Options c3_options = sampling_c3_options_.GetC3Options( - crossed_cost_switching_threshold_); + LCSFactoryOptions lcs_factory_options = + sampling_c3_options_.GetLCSFactoryOptions( + crossed_cost_switching_threshold_); // Preprocessing the contact pairs vector> resolved_contact_pairs; - resolved_contact_pairs = LCSFactory::PreProcessor( - plant_, *context_, contact_pairs_, - sampling_c3_options_.resolve_contacts_to, - c3_options.num_friction_directions, verbose_); + resolved_contact_pairs = GetResolvedContactPairs( + plant_, *context_, contact_pairs_, + sampling_c3_options_.resolve_contacts_to, + sampling_c3_options_.num_friction_directions_per_contact, verbose_); // print size of resolved_contact_pairs - *lcs_contact_jacobian = LCSFactory::ComputeContactJacobian( - plant_, *context_, resolved_contact_pairs, - c3_options.mu, sampling_c3_options_.n_lambda_with_tangential, - sampling_c3_options_.num_friction_directions_per_contact, - sampling_c3_options_.starting_index_per_contact_in_lambda_t_vector, contact_model_); + *lcs_contact_jacobian = + LCSFactory(plant_, *context_, plant_ad_, *context_ad_, + resolved_contact_pairs, lcs_factory_options) + .GetContactJacobianAndPoints(); } // Output port handlers for best sample location @@ -2270,29 +2800,29 @@ void SamplingC3Controller::OutputC3SolutionBestPlanActor( c3_solution->time_vector_(i) = base_time + i * dt_; c3_solution->x_sol_.col(i) = z_sol[i].segment(0, n_x_).cast(); c3_solution->lambda_sol_.col(i) = - z_sol[i].segment(n_x_, n_lambda_).cast(); + z_sol[i].segment(n_x_, n_lambda_).cast(); c3_solution->u_sol_.col(i) = - z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); + z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); } for (int i = 0; i < N_; i++) { c3_solution->time_vector_(i) = base_time + i * dt_; c3_solution->x_sol_.col(i) = z_sol[i].segment(0, n_x_).cast(); c3_solution->lambda_sol_.col(i) = - z_sol[i].segment(n_x_, n_lambda_).cast(); + z_sol[i].segment(n_x_, n_lambda_).cast(); c3_solution->u_sol_.col(i) = - z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); + z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); } MatrixXd knots = MatrixXd::Zero(6, N_); knots.topRows(3) = c3_solution->x_sol_.topRows(3).cast(); knots.bottomRows(3) = - c3_solution->x_sol_.bottomRows(n_v_).topRows(3).cast(); + c3_solution->x_sol_.bottomRows(n_v_).topRows(3).cast(); LcmTrajectory::Trajectory end_effector_traj; end_effector_traj.traj_name = "end_effector_position_target"; end_effector_traj.datatypes = - std::vector(knots.rows(), "double"); + std::vector(knots.rows(), "double"); end_effector_traj.datapoints = knots; end_effector_traj.time_vector = c3_solution->time_vector_.cast(); LcmTrajectory lcm_traj({end_effector_traj}, {"end_effector_position_target"}, @@ -2302,40 +2832,45 @@ void SamplingC3Controller::OutputC3SolutionBestPlanActor( Eigen::MatrixXd ee_orientations = Eigen::MatrixXd::Zero(4, N_); Eigen::Vector3d workspace_center(Eigen::Vector3d::Zero(3)); - workspace_center[0] = (sampling_c3_options_.workspace_limits[0][3] + sampling_c3_options_.workspace_limits[0][4]) / 2; - workspace_center[1] = (sampling_c3_options_.workspace_limits[1][3] + sampling_c3_options_.workspace_limits[1][4]) / 2; - - Eigen::Vector2d max_radius(sampling_c3_options_.workspace_limits[0][4] - - workspace_center[0], sampling_c3_options_.workspace_limits[1][4] - workspace_center[1]); + workspace_center[0] = (sampling_c3_options_.workspace_limits[0][3] + + sampling_c3_options_.workspace_limits[0][4]) / + 2; + workspace_center[1] = (sampling_c3_options_.workspace_limits[1][3] + + sampling_c3_options_.workspace_limits[1][4]) / + 2; + + Eigen::Vector2d max_radius( + sampling_c3_options_.workspace_limits[0][4] - workspace_center[0], + sampling_c3_options_.workspace_limits[1][4] - workspace_center[1]); double max_dist = max_radius.norm(); Eigen::Vector3d direction = ee_position_curr_ - workspace_center; Eigen::Matrix3d rot; - rot << 0, 1, 0, - -1, 0, 0, - 0, 0, 1; + rot << 0, 1, 0, -1, 0, 0, 0, 0, 1; direction[2] = 0; direction = rot * direction; - // If outside of radius, tilt ee so away from workspace center, otherwise set vertical - // Tilt depending on how far from center (for smoothness) - double theta = (direction.norm() / max_dist) * reposition_params_.max_tilt_angle * M_PI / 180.0; + // If outside of radius, tilt ee so away from workspace center, otherwise set + // vertical Tilt depending on how far from center (for smoothness) + double theta = (direction.norm() / max_dist) * + reposition_params_.max_tilt_angle * M_PI / 180.0; direction.normalize(); Eigen::AngleAxisd angle_axis(theta, direction); Eigen::Quaterniond q_rotated(angle_axis); - Eigen::Vector4d q_vec(q_rotated.w(), q_rotated.x(), q_rotated.y(), q_rotated.z()); + Eigen::Vector4d q_vec(q_rotated.w(), q_rotated.x(), q_rotated.y(), + q_rotated.z()); for (int i = 0; i < N_; i++) { ee_orientations.col(i) = q_vec; } - LcmTrajectory::Trajectory ee_orientation_traj; ee_orientation_traj.traj_name = "end_effector_orientation_target"; - ee_orientation_traj.datatypes = std::vector(ee_orientations.rows(), "double"); // quaternion + ee_orientation_traj.datatypes = + std::vector(ee_orientations.rows(), "double"); // quaternion ee_orientation_traj.datapoints = ee_orientations; ee_orientation_traj.time_vector = c3_solution->time_vector_.cast(); lcm_traj.AddTrajectory(ee_orientation_traj.traj_name, ee_orientation_traj); @@ -2370,16 +2905,18 @@ void SamplingC3Controller::OutputC3SolutionBestPlanObject( c3_solution->time_vector_(i) = base_time + i * dt_; c3_solution->x_sol_.col(i) = z_sol[i].segment(0, n_x_).cast(); c3_solution->lambda_sol_.col(i) = - z_sol[i].segment(n_x_, n_lambda_).cast(); + z_sol[i].segment(n_x_, n_lambda_).cast(); c3_solution->u_sol_.col(i) = - z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); + z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); } std::vector object_trajs; for (int i = 0; i < controller_params_.num_objects; i++) { MatrixXd knot = MatrixXd::Zero(6, N_); - knot.topRows(3) = c3_solution->x_sol_.middleRows(7*i + 7, 3).cast(); - knot.bottomRows(3) = c3_solution->x_sol_.middleRows(n_q_ + 6*i + 6, 3).cast(); + knot.topRows(3) = + c3_solution->x_sol_.middleRows(7 * i + 7, 3).cast(); + knot.bottomRows(3) = + c3_solution->x_sol_.middleRows(n_q_ + 6 * i + 6, 3).cast(); LcmTrajectory::Trajectory object_traj; object_traj.traj_name = "object_position_target_" + std::to_string(i); object_traj.datatypes = std::vector(knot.rows(), "double"); @@ -2393,32 +2930,29 @@ void SamplingC3Controller::OutputC3SolutionBestPlanObject( for (int i = 0; i < controller_params_.num_objects; i++) { traj_names.push_back("object_position_target_" + std::to_string(i)); } - LcmTrajectory lcm_traj(object_trajs, traj_names, - "object_targets", "object_targets", false); - + LcmTrajectory lcm_traj(object_trajs, traj_names, "object_targets", + "object_targets", false); for (int i = 0; i < controller_params_.num_objects; i++) { LcmTrajectory::Trajectory object_orientation_traj; // first 3 rows are rpy, last 3 rows are angular velocity MatrixXd orientation_sample = MatrixXd::Zero(4, N_); orientation_sample = - c3_solution->x_sol_.middleRows(3 + 7*i, 4).cast(); + c3_solution->x_sol_.middleRows(3 + 7 * i, 4).cast(); - object_orientation_traj.traj_name = "object_orientation_target_" + std::to_string(i); + object_orientation_traj.traj_name = + "object_orientation_target_" + std::to_string(i); object_orientation_traj.datatypes = - std::vector(orientation_sample.rows(), "double"); + std::vector(orientation_sample.rows(), "double"); object_orientation_traj.datapoints = orientation_sample; object_orientation_traj.time_vector = - c3_solution->time_vector_.cast(); + c3_solution->time_vector_.cast(); lcm_traj.AddTrajectory(object_orientation_traj.traj_name, - object_orientation_traj); + object_orientation_traj); } - - output->saved_traj = lcm_traj.GenerateLcmObject(); output->utime = context.get_time() * 1e6; - } void SamplingC3Controller::OutputC3SolutionBestPlan( @@ -2432,9 +2966,9 @@ void SamplingC3Controller::OutputC3SolutionBestPlan( c3_solution->time_vector_(i) = base_time + i * dt_; c3_solution->x_sol_.col(i) = z_sol[i].segment(0, n_x_).cast(); c3_solution->lambda_sol_.col(i) = - z_sol[i].segment(n_x_, n_lambda_).cast(); + z_sol[i].segment(n_x_, n_lambda_).cast(); c3_solution->u_sol_.col(i) = - z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); + z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); } } @@ -2460,8 +2994,8 @@ void SamplingC3Controller::OutputLCSContactJacobianBestPlan( std::pair>* lcs_contact_jacobian) const { const TimestampedVector* lcs_x = - (TimestampedVector*)this->EvalVectorInput(context, - lcs_state_input_port_); + (TimestampedVector*)this->EvalVectorInput(context, + lcs_state_input_port_); // Linearize about state with end effector in sample location. VectorXd x_sample = lcs_x->get_data(); @@ -2469,21 +3003,20 @@ void SamplingC3Controller::OutputLCSContactJacobianBestPlan( UpdateContext(n_q_, n_v_, n_u_, plant_, context_, plant_ad_, context_ad_, x_sample); - C3Options c3_options = sampling_c3_options_.GetC3Options( - crossed_cost_switching_threshold_); + LCSFactoryOptions lcs_factory_options = + sampling_c3_options_.GetLCSFactoryOptions( + crossed_cost_switching_threshold_); // Preprocess the contact pairs. vector> resolved_contact_pairs; - resolved_contact_pairs = LCSFactory::PreProcessor( - plant_, *context_, contact_pairs_, - sampling_c3_options_.resolve_contacts_to, - c3_options.num_friction_directions, verbose_); - - *lcs_contact_jacobian = LCSFactory::ComputeContactJacobian( - plant_, *context_, resolved_contact_pairs, - c3_options.mu, sampling_c3_options_.n_lambda_with_tangential, - sampling_c3_options_.num_friction_directions_per_contact, - sampling_c3_options_.starting_index_per_contact_in_lambda_t_vector, contact_model_); + resolved_contact_pairs = GetResolvedContactPairs( + plant_, *context_, contact_pairs_, + sampling_c3_options_.resolve_contacts_to, + sampling_c3_options_.num_friction_directions_per_contact, verbose_); + *lcs_contact_jacobian = + LCSFactory(plant_, *context_, plant_ad_, *context_ad_, + resolved_contact_pairs, lcs_factory_options) + .GetContactJacobianAndPoints(); // Revert the context. UpdateContext(n_q_, n_v_, n_u_, plant_, context_, plant_ad_, context_ad_, @@ -2496,19 +3029,18 @@ void SamplingC3Controller::OutputC3TrajExecuteActor( dairlib::lcmt_timestamped_saved_traj* output_c3_execution_lcm_traj) const { // Returned trajectory includes EE positions and feed-forward forces. LcmTrajectory::Trajectory end_effector_traj = - c3_execution_lcm_traj_.GetTrajectory("end_effector_position_target"); + c3_execution_lcm_traj_.GetTrajectory("end_effector_position_target"); DRAKE_DEMAND(end_effector_traj.datapoints.rows() == 3); LcmTrajectory lcm_traj({end_effector_traj}, {"end_effector_position_target"}, "end_effector_position_target", "end_effector_position_target", false); - LcmTrajectory::Trajectory ee_orientation_traj = - c3_execution_lcm_traj_.GetTrajectory("end_effector_orientation_target"); + c3_execution_lcm_traj_.GetTrajectory("end_effector_orientation_target"); lcm_traj.AddTrajectory(ee_orientation_traj.traj_name, ee_orientation_traj); LcmTrajectory::Trajectory force_traj = - c3_execution_lcm_traj_.GetTrajectory("end_effector_force_target"); + c3_execution_lcm_traj_.GetTrajectory("end_effector_force_target"); lcm_traj.AddTrajectory(force_traj.traj_name, force_traj); output_c3_execution_lcm_traj->saved_traj = lcm_traj.GenerateLcmObject(); @@ -2521,19 +3053,19 @@ void SamplingC3Controller::OutputReposTrajExecuteActor( const { // Returned trajectory includes EE positions and feed-forward forces. LcmTrajectory::Trajectory end_effector_traj = - repos_execution_lcm_traj_.GetTrajectory("end_effector_position_target"); + repos_execution_lcm_traj_.GetTrajectory("end_effector_position_target"); DRAKE_DEMAND(end_effector_traj.datapoints.rows() == 3); LcmTrajectory lcm_traj({end_effector_traj}, {"end_effector_position_target"}, "end_effector_position_target", "end_effector_position_target", false); LcmTrajectory::Trajectory ee_orientation_traj = - repos_execution_lcm_traj_.GetTrajectory("end_effector_orientation_target"); + repos_execution_lcm_traj_.GetTrajectory( + "end_effector_orientation_target"); lcm_traj.AddTrajectory(ee_orientation_traj.traj_name, ee_orientation_traj); - LcmTrajectory::Trajectory force_traj = - repos_execution_lcm_traj_.GetTrajectory("end_effector_force_target"); + repos_execution_lcm_traj_.GetTrajectory("end_effector_force_target"); lcm_traj.AddTrajectory(force_traj.traj_name, force_traj); output_repos_execution_lcm_traj->saved_traj = lcm_traj.GenerateLcmObject(); @@ -2552,21 +3084,20 @@ void SamplingC3Controller::OutputTrajExecuteActor( // Returned trajectory includes EE positions and feed-forward forces. LcmTrajectory::Trajectory end_effector_traj = - execution_lcm_traj.GetTrajectory("end_effector_position_target"); + execution_lcm_traj.GetTrajectory("end_effector_position_target"); DRAKE_DEMAND(end_effector_traj.datapoints.rows() == 3); LcmTrajectory lcm_traj({end_effector_traj}, {"end_effector_position_target"}, "end_effector_position_target", "end_effector_position_target", false); - LcmTrajectory::Trajectory ee_orientation_traj = - execution_lcm_traj.GetTrajectory("end_effector_orientation_target"); + execution_lcm_traj.GetTrajectory("end_effector_orientation_target"); lcm_traj.AddTrajectory(ee_orientation_traj.traj_name, ee_orientation_traj); MatrixXd force_samples = MatrixXd::Zero(3, N_); LcmTrajectory::Trajectory force_traj = - execution_lcm_traj.GetTrajectory("end_effector_force_target"); + execution_lcm_traj.GetTrajectory("end_effector_force_target"); lcm_traj.AddTrajectory(force_traj.traj_name, force_traj); output_execution_lcm_traj->saved_traj = lcm_traj.GenerateLcmObject(); @@ -2603,17 +3134,16 @@ void SamplingC3Controller::OutputDynamicallyFeasibleCurrPlanActor( dairlib::lcmt_timestamped_saved_traj* dynamically_feasible_curr_plan_actor) const { std::vector dynamically_feasible_traj = - std::vector(N_ + 1, VectorXd::Zero(n_x_)); + std::vector(N_ + 1, VectorXd::Zero(n_x_)); for (int i = 0; i < N_ + 1; i++) { - dynamically_feasible_traj[i] << - all_sample_dynamically_feasible_plans_.at( + dynamically_feasible_traj[i] << all_sample_dynamically_feasible_plans_.at( SampleIndex::kCurrentLocation)[i]; } Eigen::MatrixXd knots = - Eigen::MatrixXd::Zero(3, dynamically_feasible_traj.size()); + Eigen::MatrixXd::Zero(3, dynamically_feasible_traj.size()); Eigen::VectorXd timestamps = - Eigen::VectorXd::Zero(dynamically_feasible_traj.size()); + Eigen::VectorXd::Zero(dynamically_feasible_traj.size()); for (int i = 0; i < dynamically_feasible_traj.size(); i++) { knots.col(i) = dynamically_feasible_traj[i].head(3); timestamps(i) = i; @@ -2634,32 +3164,36 @@ void SamplingC3Controller::OutputDynamicallyFeasibleCurrPlanActor( Eigen::MatrixXd ee_orientations = Eigen::MatrixXd::Zero(4, N_); Eigen::Vector3d workspace_center(Eigen::Vector3d::Zero(3)); - workspace_center[0] = (sampling_c3_options_.workspace_limits[0][3] + sampling_c3_options_.workspace_limits[0][4]) / 2; - workspace_center[1] = (sampling_c3_options_.workspace_limits[1][3] + sampling_c3_options_.workspace_limits[1][4]) / 2; - - Eigen::Vector2d max_radius(sampling_c3_options_.workspace_limits[0][4] - - workspace_center[0], sampling_c3_options_.workspace_limits[1][4] - workspace_center[1]); + workspace_center[0] = (sampling_c3_options_.workspace_limits[0][3] + + sampling_c3_options_.workspace_limits[0][4]) / + 2; + workspace_center[1] = (sampling_c3_options_.workspace_limits[1][3] + + sampling_c3_options_.workspace_limits[1][4]) / + 2; + + Eigen::Vector2d max_radius( + sampling_c3_options_.workspace_limits[0][4] - workspace_center[0], + sampling_c3_options_.workspace_limits[1][4] - workspace_center[1]); double max_dist = max_radius.norm(); Eigen::Vector3d direction = ee_position_curr_ - workspace_center; Eigen::Matrix3d rot; - rot << 0, 1, 0, - -1, 0, 0, - 0, 0, 1; + rot << 0, 1, 0, -1, 0, 0, 0, 0, 1; direction[2] = 0; direction = rot * direction; - // If outside of radius, tilt ee so away from workspace center, otherwise set vertical - // Tilt depending on how far from center (for smoothness) - double theta = (direction.norm() / max_dist) * reposition_params_.max_tilt_angle * M_PI / 180.0; + // If outside of radius, tilt ee so away from workspace center, otherwise set + // vertical Tilt depending on how far from center (for smoothness) + double theta = (direction.norm() / max_dist) * + reposition_params_.max_tilt_angle * M_PI / 180.0; direction.normalize(); Eigen::AngleAxisd angle_axis(theta, direction); Eigen::Quaterniond q_rotated(angle_axis); - Eigen::Vector4d q_vec(q_rotated.w(), q_rotated.x(), q_rotated.y(), q_rotated.z()); - + Eigen::Vector4d q_vec(q_rotated.w(), q_rotated.x(), q_rotated.y(), + q_rotated.z()); for (int i = 0; i < N_; i++) { ee_orientations.col(i) = q_vec; @@ -2667,13 +3201,14 @@ void SamplingC3Controller::OutputDynamicallyFeasibleCurrPlanActor( LcmTrajectory::Trajectory ee_orientation_traj; ee_orientation_traj.traj_name = "end_effector_orientation_target"; - ee_orientation_traj.datatypes = std::vector(ee_orientations.rows(), "double"); // quaternion + ee_orientation_traj.datatypes = + std::vector(ee_orientations.rows(), "double"); // quaternion ee_orientation_traj.datapoints = ee_orientations; ee_orientation_traj.time_vector = timestamps.cast(); ee_traj_lcm.AddTrajectory(ee_orientation_traj.traj_name, ee_orientation_traj); dynamically_feasible_curr_plan_actor->saved_traj = - ee_traj_lcm.GenerateLcmObject(); + ee_traj_lcm.GenerateLcmObject(); dynamically_feasible_curr_plan_actor->utime = context.get_time() * 1e6; } @@ -2683,12 +3218,10 @@ void SamplingC3Controller::OutputDynamicallyFeasibleCurrPlanObject( const drake::systems::Context& context, dairlib::lcmt_timestamped_saved_traj* dynamically_feasible_curr_plan_object) const { - std::vector dynamically_feasible_traj = - std::vector(N_ + 1, VectorXd::Zero(n_x_)); + std::vector(N_ + 1, VectorXd::Zero(n_x_)); for (int i = 0; i < N_ + 1; i++) { - dynamically_feasible_traj[i] << - all_sample_dynamically_feasible_plans_.at( + dynamically_feasible_traj[i] << all_sample_dynamically_feasible_plans_.at( SampleIndex::kCurrentLocation)[i]; } @@ -2697,25 +3230,24 @@ void SamplingC3Controller::OutputDynamicallyFeasibleCurrPlanObject( for (int i = 0; i < controller_params_.num_objects; i++) { Eigen::MatrixXd knot = Eigen::MatrixXd::Zero(7, N_ + 1); Eigen::VectorXd timestamp = - Eigen::VectorXd::Zero(dynamically_feasible_traj.size()); + Eigen::VectorXd::Zero(dynamically_feasible_traj.size()); for (int j = 0; j < dynamically_feasible_traj.size(); j++) { - knot.col(j) = dynamically_feasible_traj[j].segment(3 + 7*i, 7); + knot.col(j) = dynamically_feasible_traj[j].segment(3 + 7 * i, 7); timestamp(j) = j; } knots.push_back(knot); timestamps.push_back(timestamp); } - std::vector object_trajs; for (int i = 0; i < controller_params_.num_objects; i++) { LcmTrajectory::Trajectory object_traj; Eigen::MatrixXd position_sample = Eigen::MatrixXd::Zero(3, N_ + 1); position_sample = knots.at(i).bottomRows(3); - object_traj.traj_name = "object_position_target_" + std::to_string(i); + object_traj.traj_name = "object_position_target_" + std::to_string(i); object_traj.datatypes = - std::vector(position_sample.rows(), "double"); + std::vector(position_sample.rows(), "double"); object_traj.datapoints = position_sample; object_traj.time_vector = timestamps.at(i).cast(); @@ -2727,8 +3259,8 @@ void SamplingC3Controller::OutputDynamicallyFeasibleCurrPlanObject( traj_names.push_back("object_position_target_" + std::to_string(i)); } - LcmTrajectory lcm_traj(object_trajs, traj_names, - "object_target", "object_target", false); + LcmTrajectory lcm_traj(object_trajs, traj_names, "object_target", + "object_target", false); for (int i = 0; i < controller_params_.num_objects; i++) { LcmTrajectory::Trajectory object_orientation_traj; @@ -2736,20 +3268,19 @@ void SamplingC3Controller::OutputDynamicallyFeasibleCurrPlanObject( for (int j = 0; j < controller_params_.num_objects; j++) { orientation_sample = knots.at(i).topRows(4); } - object_orientation_traj.traj_name = "object_orientation_target_" + std::to_string(i); + object_orientation_traj.traj_name = + "object_orientation_target_" + std::to_string(i); object_orientation_traj.datatypes = - std::vector(orientation_sample.rows(), "double"); + std::vector(orientation_sample.rows(), "double"); object_orientation_traj.datapoints = orientation_sample; object_orientation_traj.time_vector = timestamps.at(i).cast(); lcm_traj.AddTrajectory(object_orientation_traj.traj_name, - object_orientation_traj); + object_orientation_traj); } - dynamically_feasible_curr_plan_object->saved_traj = - lcm_traj.GenerateLcmObject(); + lcm_traj.GenerateLcmObject(); dynamically_feasible_curr_plan_object->utime = context.get_time() * 1e6; - } void SamplingC3Controller::OutputDynamicallyFeasibleBestPlanActor( @@ -2757,16 +3288,16 @@ void SamplingC3Controller::OutputDynamicallyFeasibleBestPlanActor( dairlib::lcmt_timestamped_saved_traj* dynamically_feasible_best_plan) const { std::vector dynamically_feasible_traj = - std::vector(N_ + 1, VectorXd::Zero(n_x_)); + std::vector(N_ + 1, VectorXd::Zero(n_x_)); for (int i = 0; i < N_ + 1; i++) { - dynamically_feasible_traj[i] << - all_sample_dynamically_feasible_plans_.at(best_sample_index_)[i]; + dynamically_feasible_traj[i] + << all_sample_dynamically_feasible_plans_.at(best_sample_index_)[i]; } Eigen::MatrixXd knots = - Eigen::MatrixXd::Zero(3, dynamically_feasible_traj.size()); + Eigen::MatrixXd::Zero(3, dynamically_feasible_traj.size()); Eigen::VectorXd timestamps = - Eigen::VectorXd::Zero(dynamically_feasible_traj.size()); + Eigen::VectorXd::Zero(dynamically_feasible_traj.size()); for (int i = 0; i < dynamically_feasible_traj.size(); i++) { knots.col(i) = dynamically_feasible_traj[i].head(3); timestamps(i) = i; @@ -2777,7 +3308,7 @@ void SamplingC3Controller::OutputDynamicallyFeasibleBestPlanActor( position_samples = knots.bottomRows(3); ee_traj.traj_name = "ee_position_target"; ee_traj.datatypes = - std::vector(position_samples.rows(), "double"); + std::vector(position_samples.rows(), "double"); ee_traj.datapoints = position_samples; ee_traj.time_vector = timestamps.cast(); @@ -2787,34 +3318,38 @@ void SamplingC3Controller::OutputDynamicallyFeasibleBestPlanActor( Eigen::MatrixXd ee_orientations = Eigen::MatrixXd::Zero(4, N_); Eigen::Vector3d workspace_center(Eigen::Vector3d::Zero(3)); - workspace_center[0] = (sampling_c3_options_.workspace_limits[0][3] + sampling_c3_options_.workspace_limits[0][4]) / 2; - workspace_center[1] = (sampling_c3_options_.workspace_limits[1][3] + sampling_c3_options_.workspace_limits[1][4]) / 2; - - Eigen::Vector2d max_radius(sampling_c3_options_.workspace_limits[0][4] - - workspace_center[0], sampling_c3_options_.workspace_limits[1][4] - workspace_center[1]); + workspace_center[0] = (sampling_c3_options_.workspace_limits[0][3] + + sampling_c3_options_.workspace_limits[0][4]) / + 2; + workspace_center[1] = (sampling_c3_options_.workspace_limits[1][3] + + sampling_c3_options_.workspace_limits[1][4]) / + 2; + + Eigen::Vector2d max_radius( + sampling_c3_options_.workspace_limits[0][4] - workspace_center[0], + sampling_c3_options_.workspace_limits[1][4] - workspace_center[1]); double max_dist = max_radius.norm(); Eigen::Vector3d direction = ee_position_curr_ - workspace_center; Eigen::Matrix3d rot; - rot << 0, 1, 0, - -1, 0, 0, - 0, 0, 1; + rot << 0, 1, 0, -1, 0, 0, 0, 0, 1; direction[2] = 0; direction = rot * direction; - // If outside of radius, tilt ee so away from workspace center, otherwise set vertical - // Tilt depending on how far from center (for smoothness) - double theta = (direction.norm() / max_dist) * reposition_params_.max_tilt_angle * M_PI / 180.0; + // If outside of radius, tilt ee so away from workspace center, otherwise set + // vertical Tilt depending on how far from center (for smoothness) + double theta = (direction.norm() / max_dist) * + reposition_params_.max_tilt_angle * M_PI / 180.0; direction.normalize(); Eigen::AngleAxisd angle_axis(theta, direction); Eigen::Quaterniond q_rotated(angle_axis); - Eigen::Vector4d q_vec(q_rotated.w(), q_rotated.x(), q_rotated.y(), q_rotated.z()); - + Eigen::Vector4d q_vec(q_rotated.w(), q_rotated.x(), q_rotated.y(), + q_rotated.z()); for (int i = 0; i < N_; i++) { ee_orientations.col(i) = q_vec; @@ -2822,7 +3357,8 @@ void SamplingC3Controller::OutputDynamicallyFeasibleBestPlanActor( LcmTrajectory::Trajectory ee_orientation_traj; ee_orientation_traj.traj_name = "end_effector_orientation_target"; - ee_orientation_traj.datatypes = std::vector(ee_orientations.rows(), "double"); // quaternion + ee_orientation_traj.datatypes = + std::vector(ee_orientations.rows(), "double"); // quaternion ee_orientation_traj.datapoints = ee_orientations; ee_orientation_traj.time_vector = timestamps.cast(); ee_traj_lcm.AddTrajectory(ee_orientation_traj.traj_name, ee_orientation_traj); @@ -2835,23 +3371,22 @@ void SamplingC3Controller::OutputDynamicallyFeasibleBestPlanObject( const drake::systems::Context& context, dairlib::lcmt_timestamped_saved_traj* dynamically_feasible_best_plan) const { - std::vector dynamically_feasible_traj = - std::vector(N_ + 1, VectorXd::Zero(n_x_)); + std::vector(N_ + 1, VectorXd::Zero(n_x_)); for (int i = 0; i < N_ + 1; i++) { - dynamically_feasible_traj[i] << - all_sample_dynamically_feasible_plans_.at(best_sample_index_)[i]; + dynamically_feasible_traj[i] + << all_sample_dynamically_feasible_plans_.at(best_sample_index_)[i]; } std::vector knots; std::vector timestamps; std::vector object_trajs; for (int i = 0; i < controller_params_.num_objects; i++) { Eigen::MatrixXd knot = - Eigen::MatrixXd::Zero(7, dynamically_feasible_traj.size()); + Eigen::MatrixXd::Zero(7, dynamically_feasible_traj.size()); Eigen::VectorXd timestamp = - Eigen::VectorXd::Zero(dynamically_feasible_traj.size()); + Eigen::VectorXd::Zero(dynamically_feasible_traj.size()); for (int j = 0; j < dynamically_feasible_traj.size(); j++) { - knot.col(j) = dynamically_feasible_traj[j].segment(3 + 7*i, 7); + knot.col(j) = dynamically_feasible_traj[j].segment(3 + 7 * i, 7); timestamp(j) = j; } @@ -2860,7 +3395,7 @@ void SamplingC3Controller::OutputDynamicallyFeasibleBestPlanObject( position_sample = knot.bottomRows(3); object_traj.traj_name = "object_position_target_" + std::to_string(i); object_traj.datatypes = - std::vector(position_sample.rows(), "double"); + std::vector(position_sample.rows(), "double"); object_traj.datapoints = position_sample; object_traj.time_vector = timestamp.cast(); @@ -2873,22 +3408,21 @@ void SamplingC3Controller::OutputDynamicallyFeasibleBestPlanObject( for (int i = 0; i < controller_params_.num_objects; i++) { traj_names.push_back("object_position_target_" + std::to_string(i)); } - LcmTrajectory lcm_traj(object_trajs, traj_names, - "object_target", "object_target", false); - + LcmTrajectory lcm_traj(object_trajs, traj_names, "object_target", + "object_target", false); for (int i = 0; i < controller_params_.num_objects; i++) { - LcmTrajectory::Trajectory object_orientation_traj; Eigen::MatrixXd orientation_sample = Eigen::MatrixXd::Zero(4, N_ + 1); orientation_sample = knots.at(i).topRows(4); - object_orientation_traj.traj_name = "object_orientation_target_" + std::to_string(i); + object_orientation_traj.traj_name = + "object_orientation_target_" + std::to_string(i); object_orientation_traj.datatypes = - std::vector(orientation_sample.rows(), "double"); + std::vector(orientation_sample.rows(), "double"); object_orientation_traj.datapoints = orientation_sample; object_orientation_traj.time_vector = timestamps.at(i).cast(); lcm_traj.AddTrajectory(object_orientation_traj.traj_name, - object_orientation_traj); + object_orientation_traj); dynamically_feasible_best_plan->saved_traj = lcm_traj.GenerateLcmObject(); dynamically_feasible_best_plan->utime = context.get_time() * 1e6; @@ -2900,7 +3434,7 @@ void SamplingC3Controller::OutputAllSampleLocations( const drake::systems::Context& context, dairlib::lcmt_timestamped_saved_traj* output_all_sample_locations) const { std::vector sample_locations = std::vector( - all_sample_locations_.begin(), all_sample_locations_.end()); + all_sample_locations_.begin(), all_sample_locations_.end()); // Pad with zeros to make sure the size is max_num_samples_ + 1 for the // visualizer. while (sample_locations.size() < max_num_samples_ + 1) { @@ -2908,7 +3442,7 @@ void SamplingC3Controller::OutputAllSampleLocations( } Eigen::MatrixXd sample_datapoints = - Eigen::MatrixXd::Zero(3, sample_locations.size()); + Eigen::MatrixXd::Zero(3, sample_locations.size()); Eigen::VectorXd timestamps = Eigen::VectorXd::Zero(sample_locations.size()); for (int i = 0; i < sample_locations.size(); i++) { sample_datapoints.col(i) = sample_locations[i]; @@ -2931,7 +3465,7 @@ void SamplingC3Controller::OutputAllSampleCosts( const drake::systems::Context& context, dairlib::lcmt_timestamped_saved_traj* output_all_sample_costs) const { Eigen::MatrixXd cost_datapoints = - Eigen::MatrixXd::Zero(1, all_sample_costs_.size()); + Eigen::MatrixXd::Zero(1, all_sample_costs_.size()); Eigen::VectorXd timestamps = Eigen::VectorXd::Zero(all_sample_costs_.size()); for (int i = 0; i < all_sample_costs_.size(); i++) { @@ -2966,9 +3500,10 @@ void SamplingC3Controller::OutputDebug( // Redundant radio things included in debug message for convenience. const auto& radio_out = - this->EvalInputValue(context, radio_port_); - debug_msg->is_teleop = radio_out->channel[14]; // 14 = teleop - debug_msg->is_force_tracking = !radio_out->channel[11]; // 11 = force tracking + this->EvalInputValue(context, radio_port_); + debug_msg->is_teleop = radio_out->channel[14]; // 14 = teleop + debug_msg->is_force_tracking = + !radio_out->channel[11]; // 11 = force tracking // disabled debug_msg->is_forced_into_c3 = radio_out->channel[12]; // 12 = forced into C3 debug_msg->in_pose_tracking_mode = crossed_cost_switching_threshold_; diff --git a/systems/controllers/sampling_based_c3_controller.h b/systems/controllers/sampling_based_c3_controller.h index ea0bf8d07..2d07bc026 100644 --- a/systems/controllers/sampling_based_c3_controller.h +++ b/systems/controllers/sampling_based_c3_controller.h @@ -5,28 +5,29 @@ #include #include +#include +#include +#include "c3/core/c3.h" +#include "c3/core/c3_options.h" +#include "c3/core/lcs.h" +#include "c3/core/solver_options_io.h" +#include "c3/multibody/lcs_factory.h" +#include "c3/multibody/lcs_factory_options.h" #include "common/find_resource.h" #include "common/update_context.h" #include "dairlib/lcmt_sampling_c3_debug.hpp" #include "dairlib/lcmt_saved_traj.hpp" #include "dairlib/lcmt_timestamped_saved_traj.hpp" +#include "examples/sampling_c3/parameter_headers/progress_params.h" +#include "examples/sampling_c3/parameter_headers/reposition_params.h" #include "examples/sampling_c3/parameter_headers/sampling_c3_controller_params.h" #include "examples/sampling_c3/parameter_headers/sampling_c3_options.h" #include "examples/sampling_c3/parameter_headers/sampling_params.h" -#include "examples/sampling_c3/parameter_headers/reposition_params.h" -#include "examples/sampling_c3/parameter_headers/progress_params.h" #include "lcm/lcm_trajectory.h" -#include "solvers/base_c3.h" -#include "solvers/c3_options.h" #include "solvers/c3_output.h" -#include "solvers/lcs.h" -#include "solvers/lcs_factory.h" -#include "solvers/solver_options_io.h" -#include "systems/framework/timestamped_vector.h" #include "systems/controllers/face.h" -#include -#include +#include "systems/framework/timestamped_vector.h" #include "drake/systems/framework/leaf_system.h" @@ -42,16 +43,15 @@ using drake::math::ExtractValue; using drake::multibody::MultibodyPlant; using drake::systems::BasicVector; using drake::systems::Context; -using systems::TimestampedVector; using systems::Face; +using systems::TimestampedVector; namespace systems { - enum SampleIndex { kCurrentLocation, - kCurrentReposTarget // Only represents current reposition target when in - // reposition mode. + kCurrentReposTarget // Only represents current reposition target when in + // reposition mode. // Could expand this enum if want to reference more samples. }; @@ -64,12 +64,7 @@ enum ModeSwitchReason { kToC3Xbox }; -enum PursuedTargetSource { - kNoTarget, - kPrevious, - kNewSample, - kFromBuffer -}; +enum PursuedTargetSource { kNoTarget, kPrevious, kNewSample, kFromBuffer }; class SamplingC3Controller : public drake::systems::LeafSystem { public: @@ -84,20 +79,16 @@ class SamplingC3Controller : public drake::systems::LeafSystem { SamplingC3ControllerParams controller_params, bool verbose = false); // Input ports - const drake::systems::InputPort& - get_input_port_target() const { + const drake::systems::InputPort& get_input_port_target() const { return this->get_input_port(target_input_port_); } - const drake::systems::InputPort& - get_input_port_final_target() const { + const drake::systems::InputPort& get_input_port_final_target() const { return this->get_input_port(final_target_input_port_); } - const drake::systems::InputPort& - get_input_port_radio() const { + const drake::systems::InputPort& get_input_port_radio() const { return this->get_input_port(radio_port_); } - const drake::systems::InputPort& - get_input_port_lcs_state() const { + const drake::systems::InputPort& get_input_port_lcs_state() const { return this->get_input_port(lcs_state_input_port_); } @@ -166,24 +157,22 @@ class SamplingC3Controller : public drake::systems::LeafSystem { get_output_port_repos_traj_execute_actor() const { return this->get_output_port(repos_traj_execute_actor_port_); } - const drake::systems::OutputPort& - get_output_port_traj_execute_actor() const { + const drake::systems::OutputPort& get_output_port_traj_execute_actor() + const { return this->get_output_port(traj_execute_actor_port_); } - const drake::systems::OutputPort& - get_output_port_is_c3_mode() const { + const drake::systems::OutputPort& get_output_port_is_c3_mode() const { return this->get_output_port(is_c3_mode_port_); } const drake::systems::OutputPort& get_output_port_all_sample_locations() const { return this->get_output_port(all_sample_locations_port_); } - const drake::systems::OutputPort& - get_output_port_all_sample_costs() const { + const drake::systems::OutputPort& get_output_port_all_sample_costs() + const { return this->get_output_port(all_sample_costs_port_); } - const drake::systems::OutputPort& - get_output_port_debug() const { + const drake::systems::OutputPort& get_output_port_debug() const { return this->get_output_port(debug_lcmt_port_); } const drake::systems::OutputPort& @@ -197,7 +186,7 @@ class SamplingC3Controller : public drake::systems::LeafSystem { const drake::systems::OutputPort& get_output_port_unsuccessful_sample_buffer_configurations() const { return this->get_output_port( - unsuccessful_sample_buffer_configurations_port_); + unsuccessful_sample_buffer_configurations_port_); } const drake::systems::OutputPort& get_output_port_unsuccessful_sample_buffer_costs() const { @@ -205,66 +194,79 @@ 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; /// Function for computing one control loop drake::systems::EventStatus ComputePlan( - const drake::systems::Context& context, - drake::systems::DiscreteValues* discrete_state) const; + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; /// Helper functions - solvers::LCS CreatePlaceholderLCS() const; - - void ResolvePredictedEEState( - const bool& is_teleop, drake::VectorX& x_lcs_curr) const; + void ResolvePredictedEEState(const bool& is_teleop, + drake::VectorX& x_lcs_curr) const; void ClampEndEffectorAcceleration(drake::VectorX& x_lcs_curr) const; void CheckForWorkspaceLimitViolations( - const TimestampedVector* lcs_x_curr) const; + const TimestampedVector* lcs_x_curr) const; + + void UpdateCostMatrices(const drake::VectorX& x_lcs_curr, + const BasicVector& x_lcs_des, + const C3Options& c3_options) const; - void UpdateCostMatrices( - const drake::VectorX& x_lcs_curr, - const BasicVector& x_lcs_des, const C3Options& c3_options) const; + std::vector> GetResolvedContactPairs( + const drake::multibody::MultibodyPlant& plant, + const drake::systems::Context& context, + const std::vector>>& contact_geoms, + const std::vector& resolve_contacts_to_list, + std::vector num_friction_directions, bool verbose) const; - std::pair, std::vector> + std::pair, std::vector> CreateLCSObjectsForSamples( - const std::vector& candidate_states, - const drake::VectorX& x_lcs_curr, const C3Options& c3_options, - const C3Options& c3_options_curr_location) const; + const std::vector& candidate_states, + const drake::VectorX& x_lcs_curr, + const LCSFactoryOptions& lcs_factory_options) const; void UpdateC3ExecutionTrajectory(const Eigen::VectorXd& x_lcs, - const double& t_context) const; + const double& t_context) const; void UpdateRepositioningExecutionTrajectory(const Eigen::VectorXd& x_lcs, - const double& t_context) const; + const double& t_context) const; void PruneOutdatedSamplesFromBuffer( - const Eigen::VectorXd& x_lcs, - int* num_in_buffer, - Eigen::MatrixXd* sample_buffer, - Eigen::VectorXd* sample_costs_buffer, - const double& pos_error_sample_retention, - const double& ang_error_sample_retention) const; + const Eigen::VectorXd& x_lcs, int* num_in_buffer, + Eigen::MatrixXd* sample_buffer, Eigen::VectorXd* sample_costs_buffer, + const double& pos_error_sample_retention, + const double& ang_error_sample_retention) const; void MaintainSampleBuffers(const Eigen::VectorXd& x_lcs) const; void AugmentSamplesWithBuffer( - std::vector>& c3_objects) const; + std::vector>& c3_objects) const; void AddToUnsuccessfulBuffer(const Eigen::VectorXd& x_lcs) const; void KeepTrackOfC3ModeProgress( - const drake::VectorX& x_lcs_curr, - const BasicVector& x_lcs_final_des, - bool& reset_progress_cost_buffer, - const bool& print_current_pos_and_rot_cost) const; + const drake::VectorX& x_lcs_curr, + const BasicVector& x_lcs_final_des, + bool& reset_progress_cost_buffer, + const bool& print_current_pos_and_rot_cost) const; void ResetProgressMetrics() const; /// Output port functions void OutputC3SolutionCurrPlan( const drake::systems::Context& context, - C3Output::C3Solution* c3_solution) const; + dairlib::C3Output::C3Solution* c3_solution) const; void OutputC3SolutionCurrPlanActor( const drake::systems::Context& context, dairlib::lcmt_timestamped_saved_traj* output) const; @@ -273,14 +275,14 @@ class SamplingC3Controller : public drake::systems::LeafSystem { dairlib::lcmt_timestamped_saved_traj* output) const; void OutputC3IntermediatesCurrPlan( const drake::systems::Context& context, - C3Output::C3Intermediates* c3_intermediates) const; + dairlib::C3Output::C3Intermediates* c3_intermediates) const; void OutputLCSContactJacobianCurrPlan( const drake::systems::Context& context, std::pair>* lcs_contact_jacobian) const; void OutputC3SolutionBestPlan( const drake::systems::Context& context, - C3Output::C3Solution* c3_solution) const; + dairlib::C3Output::C3Solution* c3_solution) const; void OutputC3SolutionBestPlanActor( const drake::systems::Context& context, dairlib::lcmt_timestamped_saved_traj* output) const; @@ -289,7 +291,7 @@ class SamplingC3Controller : public drake::systems::LeafSystem { dairlib::lcmt_timestamped_saved_traj* output) const; void OutputC3IntermediatesBestPlan( const drake::systems::Context& context, - C3Output::C3Intermediates* c3_intermediates) const; + dairlib::C3Output::C3Intermediates* c3_intermediates) const; void OutputLCSContactJacobianBestPlan( const drake::systems::Context& context, std::pair>* @@ -323,18 +325,15 @@ class SamplingC3Controller : public drake::systems::LeafSystem { void OutputTrajExecuteActor( const drake::systems::Context& context, lcmt_timestamped_saved_traj* execution_lcm_traj) const; - void OutputIsC3Mode( - const drake::systems::Context& context, - dairlib::lcmt_timestamped_saved_traj* is_c3_mode) const; - void OutputDebug( - const drake::systems::Context& context, - dairlib::lcmt_sampling_c3_debug* debug_msg) const; + void OutputIsC3Mode(const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* is_c3_mode) const; + void OutputDebug(const drake::systems::Context& context, + dairlib::lcmt_sampling_c3_debug* debug_msg) const; void OutputSampleBufferConfigurations( const drake::systems::Context& context, Eigen::MatrixXd* sample_buffer_configurations) const; - void OutputSampleBufferCosts( - const drake::systems::Context& context, - Eigen::VectorXd* sample_buffer_costs) const; + void OutputSampleBufferCosts(const drake::systems::Context& context, + Eigen::VectorXd* sample_buffer_costs) const; void OutputUnsuccessfulSampleBufferConfigurations( const drake::systems::Context& context, Eigen::MatrixXd* unsuccessful_sample_buffer_configurations) const; @@ -380,7 +379,8 @@ class SamplingC3Controller : public drake::systems::LeafSystem { drake::systems::OutputPortIndex debug_lcmt_port_; drake::systems::OutputPortIndex sample_buffer_configurations_port_; drake::systems::OutputPortIndex sample_buffer_costs_port_; - drake::systems::OutputPortIndex unsuccessful_sample_buffer_configurations_port_; + drake::systems::OutputPortIndex + unsuccessful_sample_buffer_configurations_port_; drake::systems::OutputPortIndex unsuccessful_sample_buffer_costs_port_; // This plant_ has been made 'not const' so that the context can be updated. @@ -391,7 +391,7 @@ class SamplingC3Controller : public drake::systems::LeafSystem { const std::vector< std::vector>>& contact_pairs_; - solvers::ContactModel contact_model_; + c3::multibody::ContactModel contact_model_; SamplingC3ControllerParams controller_params_; SamplingC3Options sampling_c3_options_; @@ -400,7 +400,7 @@ class SamplingC3Controller : public drake::systems::LeafSystem { SamplingC3ProgressParams progress_params_; SamplingC3GoalParams goal_params_; drake::solvers::SolverOptions solver_options_ = - drake::yaml::LoadYamlFile( + drake::yaml::LoadYamlFile( "solvers/osqp_options_default.yaml") .GetAsSolverOptions(drake::solvers::OsqpSolver::id()); @@ -444,10 +444,10 @@ class SamplingC3Controller : public drake::systems::LeafSystem { mutable Eigen::VectorXd x_pred_curr_plan_; mutable Eigen::VectorXd x_from_last_control_loop_; mutable Eigen::VectorXd x_pred_from_last_control_loop_; - mutable Eigen::Vector3d ee_position_curr_; + mutable Eigen::Vector3d ee_position_curr_; // C3 solution for current location. - mutable std::shared_ptr c3_curr_plan_; + mutable std::shared_ptr c3_curr_plan_; // TODO: these are currently assigned values but go unused -- may be useful if // implementing warm start. mutable std::vector z_sol_curr_plan_; @@ -455,7 +455,7 @@ class SamplingC3Controller : public drake::systems::LeafSystem { mutable std::vector w_curr_plan_; // C3 solution for best sample location. - mutable std::shared_ptr c3_best_plan_; + mutable std::shared_ptr c3_best_plan_; // TODO: these are currently assigned values but go unused -- may be useful if // implementing warm start. mutable std::vector z_sol_best_plan_; @@ -463,7 +463,7 @@ class SamplingC3Controller : public drake::systems::LeafSystem { mutable std::vector w_best_plan_; // C3 solution for best sample in buffer. - mutable std::shared_ptr c3_buffer_plan_; + mutable std::shared_ptr c3_buffer_plan_; mutable std::vector dynamically_feasible_buffer_plan_; // LCS trajectories for C3 or repositioning modes. @@ -473,7 +473,7 @@ class SamplingC3Controller : public drake::systems::LeafSystem { // Samples and associated costs computed in current control loop. mutable std::vector all_sample_locations_; mutable std::vector> - all_sample_dynamically_feasible_plans_; + all_sample_dynamically_feasible_plans_; mutable Eigen::Vector3d prev_repositioning_target_ = Eigen::Vector3d::Zero(); mutable std::vector all_sample_costs_; @@ -489,7 +489,8 @@ class SamplingC3Controller : public drake::systems::LeafSystem { // Unsuccessful sample buffer-related variables. mutable int num_in_unsuccessful_buffer_ = 0; - mutable Eigen::MatrixXd unsuccessful_sample_buffer_; // (num_in_unsuccessful_buffer_ x n_q) + mutable Eigen::MatrixXd + unsuccessful_sample_buffer_; // (num_in_unsuccessful_buffer_ x n_q) mutable Eigen::VectorXd unsuccessful_sample_costs_buffer_; // Miscellaneous sample related variables.