example(sampling-c3) : Add C3 as an external dependency for C3+

MODULE.bazel

@@ -89,6 +89,13 @@ 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 = "ab5618775ef894725a08df5557e033b2e42583d8"
+)
+
 INEKF_COMMIT = "297c308e50fa599af92ce3bd5f11d71e2bf8af69"
 INEKF_CHECKSUM = "c5a056ce00e1625e52f5a71b1d5c202acd53c1a8c7bca33da458db1e4f3f2edf"
 

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

examples/sampling_c3/anything/parameters/sampling_c3plus_options.yaml

@@ -1,18 +1,20 @@
 ### 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
 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+.
 
 N: 7
 gamma: 1.0  # discount factor on MPC costs
@@ -85,7 +87,7 @@ q_vector: [0.01, 0.01, 0.01, 0.1, 0.1, 0.1, 0.1, 150, 150, 120, 0.1, 0.1, 0.1, 0
 r_vector: [0.01, 0.01, 1]
 
 # Penalty on matching projected variables
-g_x: [950, 950, 950, 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, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
+g_x: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
 g_u: [0, 0, 0]
 g_gamma_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]]
 g_lambda_n_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]]
@@ -121,7 +123,7 @@ q_vector_position: [0.01, 0.01, 0.01, 0, 0, 0, 0, 200, 200, 120, 0, 0, 0, 0, 200
 r_vector_position: [0.01, 0.01, 1]
 
 # Penalty on matching projected variables
-g_x_position: [950, 950, 950, 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, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
+g_x_position: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
 g_u_position: [0, 0, 0]
 g_gamma_position_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]]
 g_lambda_n_position_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]]
@@ -148,12 +150,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.
@@ -174,3 +176,5 @@ u_eta_slack: []
 u_eta_n: []
 u_eta_t: []
 u_eta: []
+final_augmented_cost_contact_scaling: 1000
+final_augmented_cost_contact_indices: [0, 1, 2, 3]

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/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 <drake/geometry/query_object.h>
 #include "systems/controllers/face.h"
 

examples/sampling_c3/jacktoy/parameters/sampling_c3plus_options.yaml

@@ -1,18 +1,21 @@
 ### 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
 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.

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,12 @@ 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_contact_scaling"] = 1000
+        samp_c3_options_yaml["final_augmented_cost_contact_indices"] = [i for i in range(4)]
     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 +342,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 +360,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 +557,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)

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(

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;