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_c3_options.yaml b/examples/sampling_c3/anything/parameters/sampling_c3_options.yaml index 08d55f8e0..5763f5ce3 100644 --- a/examples/sampling_c3/anything/parameters/sampling_c3_options.yaml +++ b/examples/sampling_c3/anything/parameters/sampling_c3_options.yaml @@ -11,7 +11,7 @@ warm_start: false end_on_qp_step: 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_changes_in_u_across_solves: true # Penalize (u-u_prev) instead of u. num_friction_directions: 2 N: 5 diff --git a/examples/sampling_c3/anything/parameters/sampling_c3plus_options.yaml b/examples/sampling_c3/anything/parameters/sampling_c3plus_options.yaml index 05257c9ca..67b2c0776 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: true # Penalize (u-u_prev) instead of u. num_friction_directions: 2 +spring_stiffness: 0.0 # Not used in C3+. +final_augmented_cost_scaling: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1000, 1000, 1000, 1000, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1000, 1000, 1000, 1000, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] 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 +# solve_dt: 0 # unused dt_cost: 0 -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. @@ -173,4 +176,4 @@ g_eta: [] u_eta_slack: [] u_eta_n: [] u_eta_t: [] -u_eta: [] +u_eta: [] \ No newline at end of file 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/multiyaml_rewrite.py b/examples/sampling_c3/multiyaml_rewrite.py index 5c1f9c0cd..2781947df 100644 --- a/examples/sampling_c3/multiyaml_rewrite.py +++ b/examples/sampling_c3/multiyaml_rewrite.py @@ -249,9 +249,6 @@ def update_c3_options(is_c3_plus, samp_c3_options_yaml_path): samp_c3_options_yaml["q_vector_position"] = q_vector_position - samp_c3_options_yaml["g_x"] = ( - [950] * 3 + [1] * (7 * num_objects) + [0.1] * (3 + 6 * num_objects) - ) samp_c3_options_yaml["g_gamma_list"] = [ [1] * calculate_contacts(num_objects, include_walls * num_objects) ] @@ -263,6 +260,10 @@ def update_c3_options(is_c3_plus, samp_c3_options_yaml_path): ] if is_c3_plus: + n_x = 6 + (13 * num_objects) + n_lambda = 4 * calculate_contacts(num_objects, include_walls * num_objects) - 2 * sum(samp_c3_options_yaml["resolve_as_planar_contacts_list"]) + n_u = 3 + samp_c3_options_yaml["g_x"] = [0] * (3 + 7 * num_objects + 3 + 6 * num_objects) samp_c3_options_yaml["g_eta_slack_list"] = [ [1] * calculate_contacts(num_objects, include_walls * num_objects) ] @@ -278,7 +279,19 @@ def update_c3_options(is_c3_plus, samp_c3_options_yaml_path): samp_c3_options_yaml["g_lambda_list"] = [ [2] * (4 * calculate_contacts(num_objects, include_walls * num_objects)) ] + samp_c3_options_yaml["final_augmented_cost_scaling"] = ( + [1] * n_x + + [1000] * 4 + + [1] * (n_lambda - 4) + + [1] * n_u + + [1000] * 4 + + [1] * (n_lambda - 4) + ) + print(n_x, n_lambda, n_u) else: + samp_c3_options_yaml["g_x"] = ( + [950] * 3 + [1] * (7 * num_objects) + [0.1] * (3 + 6 * num_objects) + ) samp_c3_options_yaml["g_lambda_list"] = [ [0.05] * (4 * calculate_contacts(num_objects, include_walls * num_objects)) ] @@ -336,6 +349,9 @@ def update_c3_options(is_c3_plus, samp_c3_options_yaml_path): ] if is_c3_plus: + n_x = 6 + (13 * num_objects) + n_lambda = 4 * calculate_contacts(num_objects, include_walls * num_objects) - 2 * sum(samp_c3_options_yaml["resolve_as_planar_contacts_list"]) + n_u = 3 samp_c3_options_yaml["g_eta_slack_position_list"] = [ [1] * calculate_contacts(num_objects, include_walls * num_objects) ] @@ -351,14 +367,11 @@ def update_c3_options(is_c3_plus, samp_c3_options_yaml_path): samp_c3_options_yaml["g_eta_position_list"] = [ [1] * (4 * calculate_contacts(num_objects, include_walls * num_objects)) ] - samp_c3_options_yaml["g_x_position"] = ( - [950] * 3 + [1] * (7 * num_objects) + [0.1] * (3 + 6 * num_objects) - ) else: samp_c3_options_yaml["g_lambda_position_list"] = [ [0.005] * (4 * calculate_contacts(num_objects, include_walls * num_objects)) ] - samp_c3_options_yaml["g_x_position"] = [0] * (6 + (13 * num_objects)) + samp_c3_options_yaml["g_x_position"] = [0] * (6 + (13 * num_objects)) samp_c3_options_yaml["u_gamma_position_list"] = [ [1] * calculate_contacts(num_objects, include_walls * num_objects) @@ -551,7 +564,6 @@ def main(): min_max_z = min(max_zs_world) - # Update c3_options is_c3_plus = "plus" in samp_c3_options_yaml_path update_c3_options(is_c3_plus, samp_c3_options_yaml_path) 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 83c87695e..ba325e850 100644 --- a/examples/sampling_c3/parameter_headers/sampling_c3_options.h +++ b/examples/sampling_c3/parameter_headers/sampling_c3_options.h @@ -3,11 +3,19 @@ #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; @@ -109,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)); @@ -192,6 +214,13 @@ 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]; @@ -212,12 +241,12 @@ struct SamplingC3Options : C3Options { num_contacts_for_cost = std::accumulate(resolve_contacts_to_for_cost.begin(), resolve_contacts_to_for_cost.end(), 0); - mu.clear(); + 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]); @@ -227,12 +256,12 @@ struct SamplingC3Options : C3Options { std::tie(num_planar_contacts, num_friction_directions_per_contact) = ProcessPlanarContactInformation(resolve_as_planar_contacts_list, resolve_contacts_to, - num_friction_directions); + num_friction_directions.value()); std::tie(num_planar_contacts_cost, - num_friction_directions_per_contact_cost) = + num_friction_directions_per_contact_for_cost) = ProcessPlanarContactInformation(resolve_as_planar_contacts_list, resolve_contacts_to_for_cost, - num_friction_directions); + num_friction_directions.value()); for (size_t i = 0; i < num_contacts; ++i) { starting_index_per_contact_in_lambda_t_vector.push_back( @@ -243,23 +272,24 @@ struct SamplingC3Options : C3Options { 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)); + 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 * + 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 { @@ -269,11 +299,18 @@ struct SamplingC3Options : C3Options { return c3_options_position; } + 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 (options->contact_model == "stewart_and_trinkle") { + 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(), @@ -286,23 +323,23 @@ struct SamplingC3Options : C3Options { } 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()); + 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_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") { + 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(), @@ -315,17 +352,17 @@ struct SamplingC3Options : C3Options { } 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()); + 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_eta.begin(), - options->u_eta.end()); + u_vector.insert(u_vector.end(), options->u_eta->begin(), + options->u_eta->end()); } } @@ -347,126 +384,125 @@ struct SamplingC3Options : C3Options { 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; + 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* 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; + 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 (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]; + 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]; } - MakePlanarLambdaCost(options); - PopulateCostMatricesFromVectors(options); + + MakePlanarLambdaCost(c3_options, lcs_factory_options); + PopulateCostMatricesFromVectors(c3_options); } - 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]; + 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(options); - PopulateCostMatricesFromVectors(options); + + MakePlanarLambdaCost(c3_options, lcs_factory_options); + PopulateCostMatricesFromVectors(c3_options); } // Convert lambda weights to the planar form: - void MakePlanarLambdaCost(C3Options* options) const { + 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 * + 2 * num_friction_directions.value() * std::accumulate( resolve_contacts_to_lists[num_contacts_index].begin(), resolve_contacts_to_lists[num_contacts_index].begin() + i, @@ -474,37 +510,41 @@ struct SamplingC3Options : C3Options { offset; int erase_end_index_for_lambda = erase_start_index_for_lambda + - 2 * (num_friction_directions - 1) * + 2 * (num_friction_directions.value() - 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); + 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 - 1) * + offset += 2 * (num_friction_directions.value() - 1) * resolve_contacts_to_lists[num_contacts_index][i]; } } 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 2171ddb53..339fbeeb2 100644 --- a/examples/sampling_c3/test/lcm_log_loader.cc +++ b/examples/sampling_c3/test/lcm_log_loader.cc @@ -152,6 +152,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; @@ -243,23 +244,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; @@ -323,13 +324,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]; } @@ -348,7 +349,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]; } @@ -367,13 +368,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]; } @@ -392,7 +393,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]; } @@ -410,31 +411,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]); } @@ -505,12 +506,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/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 b3a643abd..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 "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_plus.h" -#include "solvers/c3_qp.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::C3Plus; -using solvers::C3QP; -using solvers::LCS; -using solvers::LCSFactory; using std::vector; using systems::TimestampedVector; @@ -78,7 +78,7 @@ SamplingC3Controller::SamplingC3Controller( 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. @@ -98,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; @@ -115,58 +115,50 @@ 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_), + lcs_placeholder, C3::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_), + 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_), + 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); + 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_), + lcs_placeholder, C3::CostMatrices(Q_, R_, G_, U_), x_desired_placeholder, c3_options); } else { std::cerr << ("Unknown projection type") << std::endl; @@ -174,13 +166,12 @@ SamplingC3Controller::SamplingC3Controller( } 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); @@ -189,35 +180,52 @@ SamplingC3Controller::SamplingC3Controller( // 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); - c3_best_plan_->AddLinearConstraint(A, c3_options.workspace_limits[i][3], - c3_options.workspace_limits[i][4], 1); - c3_buffer_plan_->AddLinearConstraint(A, c3_options.workspace_limits[i][3], - c3_options.workspace_limits[i][4], - 1); + c3_curr_plan_->AddLinearConstraint( + A, sampling_c3_options_.workspace_limits[i][3], + sampling_c3_options_.workspace_limits[i][4], + c3::ConstraintVariable::STATE); + c3_best_plan_->AddLinearConstraint( + A, sampling_c3_options_.workspace_limits[i][3], + sampling_c3_options_.workspace_limits[i][4], + c3::ConstraintVariable::STATE); + c3_buffer_plan_->AddLinearConstraint( + A, sampling_c3_options_.workspace_limits[i][3], + sampling_c3_options_.workspace_limits[i][4], + c3::ConstraintVariable::STATE); } } for (int i : vector({0, 1})) { 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); - c3_best_plan_->AddLinearConstraint(A, c3_options.u_horizontal_limits[0], - c3_options.u_horizontal_limits[1], 2); - c3_buffer_plan_->AddLinearConstraint(A, c3_options.u_horizontal_limits[0], - c3_options.u_horizontal_limits[1], 2); + c3_curr_plan_->AddLinearConstraint( + A, sampling_c3_options_.u_horizontal_limits[0], + sampling_c3_options_.u_horizontal_limits[1], + c3::ConstraintVariable::INPUT); + c3_best_plan_->AddLinearConstraint( + A, sampling_c3_options_.u_horizontal_limits[0], + sampling_c3_options_.u_horizontal_limits[1], + c3::ConstraintVariable::INPUT); + c3_buffer_plan_->AddLinearConstraint( + 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); - c3_best_plan_->AddLinearConstraint(A, c3_options.u_vertical_limits[0], - c3_options.u_vertical_limits[1], 2); - c3_buffer_plan_->AddLinearConstraint(A, c3_options.u_vertical_limits[0], - c3_options.u_vertical_limits[1], 2); + c3_curr_plan_->AddLinearConstraint( + A, sampling_c3_options_.u_vertical_limits[0], + sampling_c3_options_.u_vertical_limits[1], + c3::ConstraintVariable::INPUT); + c3_best_plan_->AddLinearConstraint( + A, sampling_c3_options_.u_vertical_limits[0], + sampling_c3_options_.u_vertical_limits[1], + c3::ConstraintVariable::INPUT); + c3_buffer_plan_->AddLinearConstraint( + A, sampling_c3_options_.u_vertical_limits[0], + sampling_c3_options_.u_vertical_limits[1], + c3::ConstraintVariable::INPUT); } // Input ports. @@ -523,17 +531,434 @@ SamplingC3Controller::SamplingC3Controller( } } -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); +// 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_)); + + 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( @@ -664,6 +1089,9 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( // crossed_cost_switching_threshold_ flag. 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); @@ -753,12 +1181,11 @@ 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); @@ -766,8 +1193,7 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( std::vector>( num_total_samples, std::vector(N_ + 1, VectorXd::Zero(n_x_))); - std::vector> c3_objects(num_total_samples, - nullptr); + 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_) { @@ -783,24 +1209,22 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( // 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); + 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) { @@ -811,11 +1235,11 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( sampling_c3_options_.workspace_limits[i].segment(0, 3); test_c3_object->AddLinearConstraint( A, - c3_options.workspace_limits[i][3] - + sampling_c3_options_.workspace_limits[i][3] - sampling_c3_options_.workspace_margins, - c3_options.workspace_limits[i][4] + + sampling_c3_options_.workspace_limits[i][4] + sampling_c3_options_.workspace_margins, - 1); + c3::ConstraintVariable::STATE); } // Set object bounds for (int i = 0; i < sampling_c3_options_.workspace_limits.size(); ++i) { @@ -825,11 +1249,11 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( sampling_c3_options_.workspace_limits[i].segment(0, 3); test_c3_object->AddLinearConstraint( A, - c3_options.workspace_limits[i][3] - + sampling_c3_options_.workspace_limits[i][3] - sampling_c3_options_.workspace_margins, - c3_options.workspace_limits[i][4] + + sampling_c3_options_.workspace_limits[i][4] + sampling_c3_options_.workspace_margins, - 1); + c3::ConstraintVariable::STATE); } } } @@ -838,35 +1262,38 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( 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 for (int i : vector({0, 1})) { 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); + test_c3_object->AddLinearConstraint( + 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); + test_c3_object->AddLinearConstraint( + 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; @@ -1133,11 +1560,11 @@ drake::systems::EventStatus SamplingC3Controller::ComputePlan( 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_); @@ -1301,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; @@ -1378,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++) { @@ -1396,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( + 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, 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_); + 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); } @@ -1435,23 +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); @@ -1856,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_) && @@ -2287,23 +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 @@ -2528,22 +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_, diff --git a/systems/controllers/sampling_based_c3_controller.h b/systems/controllers/sampling_based_c3_controller.h index ce479ccfb..2d07bc026 100644 --- a/systems/controllers/sampling_based_c3_controller.h +++ b/systems/controllers/sampling_based_c3_controller.h @@ -8,6 +8,12 @@ #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" @@ -19,12 +25,7 @@ #include "examples/sampling_c3/parameter_headers/sampling_c3_options.h" #include "examples/sampling_c3/parameter_headers/sampling_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/controllers/face.h" #include "systems/framework/timestamped_vector.h" @@ -193,13 +194,22 @@ 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; /// Helper functions - solvers::LCS CreatePlaceholderLCS() const; void ResolvePredictedEEState(const bool& is_teleop, drake::VectorX& x_lcs_curr) const; @@ -213,11 +223,18 @@ class SamplingC3Controller : public drake::systems::LeafSystem { const BasicVector& x_lcs_des, const C3Options& c3_options) const; - std::pair, std::vector> + 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> CreateLCSObjectsForSamples( const std::vector& candidate_states, - const drake::VectorX& x_lcs_curr, const C3Options& c3_options, - const C3Options& c3_options_curr_location) const; + const drake::VectorX& x_lcs_curr, + const LCSFactoryOptions& lcs_factory_options) const; void UpdateC3ExecutionTrajectory(const Eigen::VectorXd& x_lcs, const double& t_context) const; @@ -234,7 +251,7 @@ class SamplingC3Controller : public drake::systems::LeafSystem { 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; @@ -247,8 +264,9 @@ class SamplingC3Controller : public drake::systems::LeafSystem { void ResetProgressMetrics() const; /// Output port functions - void OutputC3SolutionCurrPlan(const drake::systems::Context& context, - C3Output::C3Solution* c3_solution) const; + void OutputC3SolutionCurrPlan( + const drake::systems::Context& context, + dairlib::C3Output::C3Solution* c3_solution) const; void OutputC3SolutionCurrPlanActor( const drake::systems::Context& context, dairlib::lcmt_timestamped_saved_traj* output) const; @@ -257,13 +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; + void OutputC3SolutionBestPlan( + const drake::systems::Context& context, + dairlib::C3Output::C3Solution* c3_solution) const; void OutputC3SolutionBestPlanActor( const drake::systems::Context& context, dairlib::lcmt_timestamped_saved_traj* output) const; @@ -272,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>* @@ -372,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_; @@ -381,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()); @@ -428,7 +447,7 @@ class SamplingC3Controller : public drake::systems::LeafSystem { 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_; @@ -436,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_; @@ -444,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.