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

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"
 

examples/sampling_c3/anything/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: 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: false  # Penalize (u-u_prev) instead of u.
 num_friction_directions: 2
+spring_stiffness: 0.0  # Not used in C3+.
+final_augmented_cost_scaling: 1000.0
 
 N: 7
 gamma: 1.0  # discount factor on MPC costs
@@ -148,12 +151,12 @@ u_eta_position_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
 
 ### NO NEED TO CHANGE THE BELOW.  Parameters needed for the C3Options struct but
 ### are overwritten by other sampling C3 parameters.
-use_predicted_x0: false   # instead:  use_predicted_x0_c3,
+# use_predicted_x0: false   # instead:  use_predicted_x0_c3,
                           #           use_predicted_x0_repos,
                           #           use_predicted_x0_reset_mechanism
 dt: 0             # instead:  planning_dt_pose, planning_dt_position
-dt_cost: 0
-solve_dt: 0       # unused
+# solve_dt: 0       # unused
+dt_cost: 0      
 mu: []            # instead based on indexing into mu_per_pair_type
 num_contacts: 0   # instead based on summing index of resolve_contacts_to_lists
 # Instead for the below, index into their _list versions.

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/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;