fix: parameters and cost scaling

Author: Meow404

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

@@ -12,10 +12,9 @@ scale_lcs: true
 end_on_qp_step: false
 solve_time_filter_alpha: 0.95
 publish_frequency: 0
-penalize_input_change: false  # 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: 1000.0
 
 N: 7
 gamma: 1.0  # discount factor on MPC costs
@@ -88,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]]
@@ -124,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]]
@@ -156,7 +155,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      
+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.
@@ -177,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/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/sampling_c3_options.h

@@ -402,7 +402,10 @@ 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;
+    c3_options->final_augmented_cost_contact_scaling =
+        final_augmented_cost_contact_scaling;
+    c3_options->final_augmented_cost_contact_indices =
+        final_augmented_cost_contact_indices;
 
     lcs_factory_options->contact_model = contact_model;
     lcs_factory_options->num_contacts = num_contacts;