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

@@ -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<double, std::vector<Eigen::VectorXd>> CalcCost(
-      C3CostComputationType cost_type = kSimLCSReplaceC3EEPlan,
-      std::vector<double> Kp_for_ee_pd_rollout = {0.0, 0.0, 0.0},  
-      std::vector<double> 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<Eigen::VectorXd>, std::vector<Eigen::VectorXd>>
-  SimulatePDControl(std::vector<double> Kp_for_ee_pd_rollout = {0.0, 0.0, 0.0},
-                    std::vector<double> 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<double, std::vector<Eigen::VectorXd>> CalcCost(
+//       C3CostComputationType cost_type = kSimLCSReplaceC3EEPlan,
+//       std::vector<double> Kp_for_ee_pd_rollout = {0.0, 0.0, 0.0},  
+//       std::vector<double> 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<Eigen::VectorXd>, std::vector<Eigen::VectorXd>>
+//   SimulatePDControl(std::vector<double> Kp_for_ee_pd_rollout = {0.0, 0.0, 0.0},
+//                     std::vector<double> 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