fix: optional parameters

Author: Meow404

examples/sampling_c3/anything/parameters/sampling_c3_options.yaml

@@ -123,7 +123,6 @@ use_predicted_x0: false   # instead:  use_predicted_x0_c3,
 dt: 0             # instead:  planning_dt_pose, planning_dt_position
 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.
 g_gamma: []

examples/sampling_c3/anything/parameters/sampling_c3plus_options.yaml

@@ -158,7 +158,6 @@ u_eta_position_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
 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.
 g_gamma: []

examples/sampling_c3/parameter_headers/sampling_c3_options.h

@@ -39,7 +39,6 @@ struct SamplingC3Options : C3Options, LCSFactoryOptions {
 
   int num_planar_contacts;
   int n_lambda_with_tangential;
-  std::vector<int> num_friction_directions_per_contact;
   std::vector<int> starting_index_per_contact_in_lambda_t_vector;
 
   int num_planar_contacts_cost;
@@ -124,7 +123,8 @@ struct SamplingC3Options : C3Options, LCSFactoryOptions {
   std::vector<Eigen::VectorXd>
       workspace_limits;      ///< Workspace boundaries as vectors.
   double workspace_margins;  ///< Margins to be maintained within the workspace.
-  std::vector<double> ee_velocity_limits;  ///< Limits for end-effector velocities.
+  std::vector<double>
+      ee_velocity_limits;  ///< Limits for end-effector velocities.
 
   C3Options c3_options_pose;
   LCSFactoryOptions lcs_factory_options_pose;
@@ -243,10 +243,11 @@ struct SamplingC3Options : C3Options, LCSFactoryOptions {
     num_contacts_for_cost =
         std::accumulate(resolve_contacts_to_for_cost.begin(),
                         resolve_contacts_to_for_cost.end(), 0);
-    mu = std::vector<double>();
+    mu_per_contact = 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.value().insert(mu.value().end(), repeat, mu_per_pair_type[i]);
+      mu_per_contact.value().insert(mu_per_contact.value().end(), repeat,
+                                    mu_per_pair_type[i]);
     }
     mu_for_cost = std::vector<double>();
     for (size_t i = 0; i < mu_per_pair_type.size(); ++i) {
@@ -265,11 +266,11 @@ struct SamplingC3Options : C3Options, LCSFactoryOptions {
                                         resolve_contacts_to_for_cost,
                                         num_friction_directions.value());
 
-    for (size_t i = 0; i < static_cast<size_t>(num_contacts); ++i) {
+    for (size_t i = 0; i < static_cast<size_t>(num_contacts.value()); ++i) {
       starting_index_per_contact_in_lambda_t_vector.push_back(
-          2 * std::accumulate(num_friction_directions_per_contact.begin(),
-                              num_friction_directions_per_contact.begin() + i,
-                              0));
+          2 * std::accumulate(
+                  num_friction_directions_per_contact.value().begin(),
+                  num_friction_directions_per_contact.value().begin() + i, 0));
     }
     for (size_t i = 0; i < static_cast<size_t>(num_contacts_for_cost); ++i) {
       starting_index_per_contact_in_lambda_t_vector_cost.push_back(
@@ -278,15 +279,19 @@ struct SamplingC3Options : C3Options, LCSFactoryOptions {
                   num_friction_directions_per_contact_for_cost.begin() + i, 0));
     }
 
-    n_lambda_with_tangential = 2 * num_friction_directions.value() *
-                                   (num_contacts - num_planar_contacts) +
-                               2 * num_planar_contacts;
+    n_lambda_with_tangential =
+        2 * num_friction_directions.value() *
+            (num_contacts.value() - num_planar_contacts) +
+        2 * num_planar_contacts;
     n_lambda_with_tangential_cost =
         2 * num_friction_directions.value() *
             (num_contacts_for_cost - num_planar_contacts_cost) +
         2 * num_planar_contacts_cost;
 
     // Create C3 options for both pose and position tracking.
+    std::cout
+        << "Setting C3 and LCS factory options for pose and position tracking."
+        << std::endl;
     SetCommonOptions(&c3_options_pose, &lcs_factory_options_pose);
     SetPoseTrackingOptions(&c3_options_pose, &lcs_factory_options_pose);
     SetCommonOptions(&c3_options_position, &lcs_factory_options_position);
@@ -413,7 +418,7 @@ struct SamplingC3Options : C3Options, LCSFactoryOptions {
     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->mu_per_contact = mu_per_contact;
     lcs_factory_options->N = N;
     lcs_factory_options->num_friction_directions_per_contact =
         num_friction_directions_per_contact;
@@ -568,7 +573,7 @@ struct SamplingC3Options : C3Options, LCSFactoryOptions {
     int num_planar_contacts = 0;
     int planar_contact = 1;
     std::vector<int> num_friction_directions_per_contact;
-    for (int i = 0; i < resolve_contacts_to_list.size(); ++i) {
+    for (size_t i = 0; i < resolve_contacts_to_list.size(); ++i) {
       for (int j = 0; j < resolve_contacts_to_list[i]; ++j) {
         num_planar_contacts += (resolve_as_planar_contacts_list[i] ? 1 : 0);
         num_friction_directions_per_contact.push_back(

examples/sampling_c3/test/lcm_log_loader.cc

@@ -464,7 +464,7 @@ int DoMain(int argc, char* argv[]) {
                     << "seconds utime: " << (message.utime) / 1e6
                     << " and event " << "timestamp "
                     << adjusted_utimestamp / 1e6 << std::endl;
-          for (int i = 0;
+          for (size_t i = 0;
                i < message.saved_traj.trajectories[0].datapoints[0].size();
                i++) {
             Eigen::VectorXd sample_location = Eigen::VectorXd::Zero(3);
@@ -486,7 +486,7 @@ int DoMain(int argc, char* argv[]) {
                     << "seconds utime: " << (message.utime) / 1e6
                     << " and event " << "timestamp "
                     << adjusted_utimestamp / 1e6 << std::endl;
-          for (int i = 0;
+          for (size_t i = 0;
                i < message.saved_traj.trajectories[0].datapoints[0].size();
                i++) {
             sample_costs_in_log.push_back(Eigen::VectorXd::Constant(

systems/controllers/sampling_based_c3_controller.cc

@@ -120,8 +120,8 @@ SamplingC3Controller::SamplingC3Controller(
   n_lambda_ = LCSFactory::GetNumContactVariables(
       c3::multibody::GetContactModelMap().at(
           controller_params_.sampling_c3_options.contact_model),
-      sampling_c3_options_.num_contacts,
-      sampling_c3_options_.num_friction_directions_per_contact);
+      sampling_c3_options_.num_contacts.value(),
+      sampling_c3_options_.num_friction_directions_per_contact.value());
 
   // Placeholder LCS will have correct size as it's already determined by the
   // contact model.
@@ -1872,7 +1872,8 @@ SamplingC3Controller::CreateLCSObjectsForSamples(
         GetResolvedContactPairs(
             plant_, *context_, contact_pairs_,
             sampling_c3_options_.resolve_contacts_to,
-            sampling_c3_options_.num_friction_directions_per_contact, verbose_);
+            sampling_c3_options_.num_friction_directions_per_contact.value(),
+            verbose_);
     LCS lcs_object_sample =
         LCSFactory(plant_, *context_, plant_ad_, *context_ad_,
                    resolved_contact_pairs, lcs_factory_options)
@@ -1888,17 +1889,15 @@ SamplingC3Controller::CreateLCSObjectsForSamples(
         verbose_);
     LCSFactoryOptions lcs_factory_options_for_cost = {
         .contact_model = controller_params_.sampling_c3_options.contact_model,
+        .N = N_ * sampling_c3_options_.lcs_dt_resolution,
+        .dt = dt_ / sampling_c3_options_.lcs_dt_resolution,
         .num_contacts = resolved_contact_pairs_for_cost_simulation.size(),
         .spring_stiffness = 0.0,
-        .num_friction_directions = std::nullopt,
         .num_friction_directions_per_contact =
             sampling_c3_options_.num_friction_directions_per_contact_for_cost,
-        .mu = sampling_c3_options_.mu_for_cost,
-        .planar_normal_direction = sampling_c3_options_.planar_normal_direction,
-        .planar_normal_direction_per_contact = std::nullopt,
-        .contact_pair_configs = std::nullopt,
-        .N = N_ * sampling_c3_options_.lcs_dt_resolution,
-        .dt = dt_ / sampling_c3_options_.lcs_dt_resolution};
+        .mu_per_contact = sampling_c3_options_.mu_for_cost,
+        .planar_normal_direction =
+            sampling_c3_options_.planar_normal_direction};
     LCS lcs_object_sample_for_cost_simulation =
         LCSFactory(plant_, *context_, plant_ad_, *context_ad_,
                    resolved_contact_pairs_for_cost_simulation,
@@ -2773,7 +2772,8 @@ void SamplingC3Controller::OutputLCSContactJacobianCurrPlan(
   resolved_contact_pairs = GetResolvedContactPairs(
       plant_, *context_, contact_pairs_,
       sampling_c3_options_.resolve_contacts_to,
-      sampling_c3_options_.num_friction_directions_per_contact, verbose_);
+      sampling_c3_options_.num_friction_directions_per_contact.value(),
+      verbose_);
 
   // print size of resolved_contact_pairs
   *lcs_contact_descriptions =
@@ -3012,7 +3012,8 @@ void SamplingC3Controller::OutputLCSContactJacobianBestPlan(
   resolved_contact_pairs = GetResolvedContactPairs(
       plant_, *context_, contact_pairs_,
       sampling_c3_options_.resolve_contacts_to,
-      sampling_c3_options_.num_friction_directions_per_contact, verbose_);
+      sampling_c3_options_.num_friction_directions_per_contact.value(),
+      verbose_);
   *lcs_contact_descriptions =
       LCSFactory(plant_, *context_, plant_ad_, *context_ad_,
                  resolved_contact_pairs, lcs_factory_options)