rebase(push_anything_dev): fixes after rebase

Author: Meow404

MODULE.bazel

@@ -90,10 +90,14 @@ archive_override(
 )
 
 bazel_dep(name = "c3")
-git_override(
+# git_override(
+#     module_name = "c3",
+#     remote = "https://github.com/DAIRLab/c3.git",
+#     commit = "2bd13495c3306cf7d53992e2495774d1ad454cca"
+# )
+local_path_override(
     module_name = "c3",
-    remote = "https://github.com/DAIRLab/c3.git",
-    commit = "2bd13495c3306cf7d53992e2495774d1ad454cca"
+    path = "/home/stephen/Workspace/DAIR/c3"
 )
 
 INEKF_COMMIT = "297c308e50fa599af92ce3bd5f11d71e2bf8af69"

examples/sampling_c3/anything/parameters/sampling_c3plus_options.yaml

@@ -15,6 +15,7 @@ publish_frequency: 0
 penalize_input_change: false  # Penalize (u-u_prev) instead of u.
 num_friction_directions: 2
 spring_stiffness: 0.0  # Not used in C3+.
+final_augmented_cost_scaling: 1000.0
 
 N: 7
 gamma: 1.0  # discount factor on MPC costs
@@ -155,6 +156,7 @@ u_eta_position_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
                           #           use_predicted_x0_reset_mechanism
 dt: 0             # instead:  planning_dt_pose, planning_dt_position
 # solve_dt: 0       # unused
+dt_cost: 0      
 mu: []            # instead based on indexing into mu_per_pair_type
 num_contacts: 0   # instead based on summing index of resolve_contacts_to_lists
 # Instead for the below, index into their _list versions.

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);
   }
 

examples/sampling_c3/parameter_headers/sampling_c3_options.h

@@ -219,6 +219,8 @@ struct SamplingC3Options : C3Options, LCSFactoryOptions {
     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];
@@ -239,12 +241,12 @@ struct SamplingC3Options : C3Options, LCSFactoryOptions {
     num_contacts_for_cost =
         std::accumulate(resolve_contacts_to_for_cost.begin(),
                         resolve_contacts_to_for_cost.end(), 0);
-    mu.clear();
+    mu = std::vector<double>();
     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<double>();
     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]);
@@ -254,12 +256,12 @@ struct SamplingC3Options : C3Options, LCSFactoryOptions {
     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_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(
@@ -274,11 +276,11 @@ struct SamplingC3Options : C3Options, LCSFactoryOptions {
                   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;
 
@@ -400,14 +402,16 @@ struct SamplingC3Options : C3Options, LCSFactoryOptions {
     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;
+    lcs_factory_options->num_friction_directions_per_contact =
+        num_friction_directions_per_contact;
   }
 
   void SetPositionTrackingOptions(
@@ -446,6 +450,7 @@ struct SamplingC3Options : C3Options, LCSFactoryOptions {
       c3_options->u_eta = u_eta_position_list[num_contacts_index];
     }
 
+    MakePlanarLambdaCost(c3_options, lcs_factory_options);
     PopulateCostMatricesFromVectors(c3_options);
   }
 
@@ -485,94 +490,61 @@ struct SamplingC3Options : C3Options, LCSFactoryOptions {
       c3_options->u_eta = u_eta_list[num_contacts_index];
     }
 
-    MakePlanarLambdaCost(c3_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];
-    }
-    MakePlanarLambdaCost(options);
-    PopulateCostMatricesFromVectors(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,
                     0) -
             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];
       }
     }