From ee174f0daf95a80c213c7694d1d9c6415d684395 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Sat, 23 May 2020 11:36:01 -0400
Subject: [PATCH 01/82] update find_boundary (unfinished)

---
 examples/goldilocks_models/find_boundary.cc | 287 +++++++++++++++++---
 1 file changed, 245 insertions(+), 42 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index d940abff05..a14756c98c 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -54,15 +54,186 @@ using dairlib::FindResourceOrThrow;
 
 
 namespace dairlib::goldilocks_models {
-double boundary_for_one_dimension(int max_iteration,double initial_low,
-    double initial_high, double resolution){
-  int iter = 0;
-  double low = initial_low;
-  double high = initial_high;
-  for (iter = 0; iter <= max_iteration; iter++){
+// Robot models
+DEFINE_int32(robot_option, 0, "0: plannar robot. 1: cassie_fixed_spring");
+// Reduced order models
+DEFINE_int32(rom_option, -1, "");
+
+// inner loop
+DEFINE_string(init_file, "", "Initial Guess for Trajectory Optimization");
+DEFINE_double(major_feasibility_tol, 1e-4,
+"nonlinear constraint violation tol");
+DEFINE_int32(
+    max_inner_iter, 150,
+"Max iteration # for traj opt. Sometimes, snopt takes very small steps "
+"(TODO: find out why), so maybe it's better to stop at some iterations and "
+"resolve again.");
+DEFINE_int32(n_node, -1, "# of nodes for traj opt");
+DEFINE_double(eps_regularization, 1e-8, "Weight of regularization term"); //1e-4
+
+//tasks
+DEFINE_bool(is_zero_touchdown_impact, false,
+"No impact force at fist touchdown");
+DEFINE_bool(is_add_tau_in_cost, true, "Add RoM input in the cost function");
+
+// trajectory optimization for given task and model
+void trajOptGivenModel(double stride_length, double ground_incline,
+    double turning_rate,const string dir,int num,int sample_idx){
+  // Create MBP
+  MultibodyPlant<double> plant(0.0);
+  createMBP(&plant, FLAGS_robot_option);
+
+  // Create autoDiff version of the plant
+  MultibodyPlant<AutoDiffXd> plant_autoDiff(plant);
+  cout << endl;
+
+  // Parameters for the inner loop optimization
+  int max_inner_iter = FLAGS_max_inner_iter;
+  if (FLAGS_robot_option == 0) {
+    max_inner_iter = 300;
+  }
+  double Q = 0; // Cost on velocity
+  double R = 0;  // Cost on input effort
+  double all_cost_scale = 1;
+  setCostWeight(&Q, &R, &all_cost_scale, FLAGS_robot_option);
+  int n_node = 20;
+  if (FLAGS_robot_option == 0) {
+    n_node = 20;
+  } else if (FLAGS_robot_option == 1) {
+    n_node = 20;
+  }
+  if (FLAGS_n_node > 0) n_node = FLAGS_n_node;
+  if (FLAGS_robot_option == 1) {
+    // If the node density is too low, it's harder for SNOPT to converge well.
+    // The ratio of distance per nodes = 0.2/16 is fine for snopt, but
+    // 0.3 / 16 is too high.
+    // However, currently it takes too much time to compute with many nodes, so
+    // we try 0.3/24.
+    double max_distance_per_node = 0.3 / 16;
+    DRAKE_DEMAND((max_stride_length / n_node) <= max_distance_per_node);
+  }
+
+  // Reduced order model parameters
+  int rom_option = (FLAGS_rom_option >= 0) ? FLAGS_rom_option : 0;
+  int n_s = 0;
+  int n_tau = 0;
+  setRomDim(&n_s, &n_tau, rom_option);
+  int n_sDDot = n_s; // Assume that are the same (no quaternion)
+  MatrixXd B_tau = MatrixXd::Zero(n_sDDot, n_tau);
+  setRomBMatrix(&B_tau, rom_option);
+
+  // Reduced order model setup
+  KinematicsExpression<double> kin_expression(n_s, 0, &plant, FLAGS_robot_option);
+  DynamicsExpression dyn_expression(n_sDDot, 0, rom_option, FLAGS_robot_option);
+  VectorXd dummy_q = VectorXd::Ones(plant.num_positions());
+  VectorXd dummy_s = VectorXd::Ones(n_s);
+  int n_feature_s = kin_expression.getFeature(dummy_q).size();
+  int n_feature_sDDot =
+      dyn_expression.getFeature(dummy_s, dummy_s).size();
+  int n_theta_s = n_s * n_feature_s;
+  int n_theta_sDDot = n_sDDot * n_feature_sDDot;
+  VectorXd theta_s(n_theta_s);
+  VectorXd theta_sDDot(n_theta_sDDot);
+
+  // Initial guess of theta
+  theta_s = VectorXd::Zero(n_theta_s);
+  theta_sDDot = VectorXd::Zero(n_theta_sDDot);
+  setInitialTheta(theta_s, theta_sDDot, n_feature_s, rom_option);
+
+  double duration = 0.4;
+  if (FLAGS_robot_option == 0) {
+    duration = 0.746;  // Fix the duration now since we add cost ourselves
+  } else if (FLAGS_robot_option == 1) {
+    duration = 0.4; // 0.4;
+  }
+
+  bool is_get_nomial = false;
+  int max_inner_iter_pass_in = is_get_nominal ? 200 : max_inner_iter;
+
+  string init_file_pass_in = '';
+  string prefix = to_string(num) +  "_" + to_string(sample_idx) + "_";
+
+  // Vectors/Matrices for the outer loop
+  int N_Sample = 1;
+  vector<std::shared_ptr<VectorXd>> w_sol_vec(N_sample);
+  vector<std::shared_ptr<MatrixXd>> H_vec(N_sample);
+  vector<std::shared_ptr<VectorXd>> b_vec(N_sample);
+  vector<std::shared_ptr<VectorXd>> c_vec(N_sample);
+  vector<std::shared_ptr<MatrixXd>> A_vec(N_sample);
+  vector<std::shared_ptr<VectorXd>> lb_vec(N_sample);
+  vector<std::shared_ptr<VectorXd>> ub_vec(N_sample);
+  vector<std::shared_ptr<VectorXd>> y_vec(N_sample);
+  vector<std::shared_ptr<MatrixXd>> B_vec(N_sample);
+  vector<std::shared_ptr<int>> is_success_vec(N_sample);
+  for (int i = 0; i < N_sample; i++) {
+    w_sol_vec[i] = std::make_shared<VectorXd>();
+    H_vec[i] = std::make_shared<MatrixXd>();
+    b_vec[i] = std::make_shared<VectorXd>();
+    c_vec[i] = std::make_shared<VectorXd>();
+    A_vec[i] = std::make_shared<MatrixXd>();
+    lb_vec[i] = std::make_shared<VectorXd>();
+    ub_vec[i] = std::make_shared<VectorXd>();
+    y_vec[i] = std::make_shared<VectorXd>();
+    B_vec[i] = std::make_shared<MatrixXd>();
+    is_success_vec[i] = std::make_shared<int>();
+  }
+
+  vector<std::shared_ptr<int>> thread_finished_vec(N_sample);
+  for (int i = 0; i < N_sample; i++) {
+    thread_finished_vec[i] = std::make_shared<int>(0);
+  }
+
+  bool extend_model_this_iter = false;
+  int n_rerun = 0;
+  cost_threshold_for_update = std::numeric_limits<double>::infinity();
+  int N_rerun = 0;
+
+  //run trajectory optimization
+  trajOptGivenWeights(std::ref(plant), std::ref(plant_autoDiff),
+                      n_s, n_sDDot, n_tau,
+                      n_feature_s, n_feature_sDDot, std::ref(B_tau),
+                      std::ref(theta_s), std::ref(theta_sDDot),
+                      stride_length, ground_incline, turning_rate,
+                      duration, n_node, max_inner_iter_pass_in,
+                      FLAGS_major_feasibility_tol, FLAGS_major_feasibility_tol,
+                      std::ref(dir), init_file_pass_in, prefix,
+                      std::ref(w_sol_vec),
+                      std::ref(A_vec),
+                      std::ref(H_vec),
+                      std::ref(y_vec),
+                      std::ref(lb_vec),
+                      std::ref(ub_vec),
+                      std::ref(b_vec),
+                      std::ref(c_vec),
+                      std::ref(B_vec),
+                      std::ref(is_success_vec),
+                      std::ref(thread_finished_vec),
+                      Q, R, all_cost_scale,
+                      eps_regularization,
+                      is_get_nominal,
+                      FLAGS_is_zero_touchdown_impact,
+                      extend_model_this_iter,
+                      FLAGS_is_add_tau_in_cost,
+                      sample_idx, n_rerun,
+                      cost_threshold_for_update, N_rerun,
+                      rom_option,
+                      FLAGS_robot_option);
+}
+
+//search the max/min ground incline for fixed stride length
+double boundary_for_one_dimension(int max_iteration,double stride_length,
+    double gi_low,double gi_high,double turning_rate,double resolution,
+    const string dir,int num){
+  double low = gi_low;
+  double high = gi_high;
+  int sample_idx = 0;
+  for (sample_idx = 0; sample_idx <= max_iteration; sample_idx++){
     //run trajectory optimization and judge the solution
-    //int sample_success = traj_opt_result();
-    int sample_success = 1;
+    trajOptGivenModel(stride_length, high,
+        turning_rate, dir, num, sample_idx);
+    string prefix = to_string(num) +  "_" + to_string(sample_idx) + "_";
+    int sample_success =
+        (readCSV(dir + prefix + string("is_success.csv")))(0, 0);
     if (sample_success){
       low = high;
       high = 2*high;
@@ -77,55 +248,87 @@ double boundary_for_one_dimension(int max_iteration,double initial_low,
   return low;
 }
 
+//extend range of stride length
+int extend_range(double stride_length_0,int max_iter,double ground_incline_0,
+    double ground_incline_high,double ground_incline_low,
+    double ground_incline_resolution,int boundary_num,int update_direction){
+  //keep updating stride_length and search boundary of ground incline for each
+  //stride length
+  int iter;
+  double stride_length = stride_length_0;
+  for (iter = 0; iter <= max_iter; iter++){
+    //fix stride length and find max ground incline
+    double max_gi = boundary_for_one_dimension(max_iter, stride_length,
+        ground_incline_0, ground_incline_high,
+        turning_rate_0, ground_incline_resolution,
+        dir, boundary_sample_num);
+    VectorXd boundary_point(2);
+    boundary_point<<stride_length,max_gi;
+    writeCSV(dir + to_string(boundary_sample_num) +  "_" +
+        string("max_ground_incline.csv"), boundary_point);
+    boundary_sample_num += 1;
+
+    //find min ground incline
+    double min_gi = boundary_for_one_dimension(max_iter, stride_length,
+        ground_incline_0, ground_incline_low,
+        turning_rate_0, ground_incline_resolution,
+        dir, boundary_sample_num);
+    VectorXd boundary_point(2);
+    boundary_point<<stride_length,min_gi;
+    writeCSV(dir + to_string(boundary_sample_num) +  "_" +
+        string("min_ground_incline.csv"), boundary_point);
+    boundary_sample_num += 1;
+    //if max_gi and min_gi are same, find the boundary of stride length
+    if(min_gi==max_gi){
+      break;
+    }
+    //else continue search
+    else{
+      if(update_direction==0){
+        stride_length += delta_stride_length;
+      }
+      if(update_direction==1){
+        stride_length -= delta_stride_length;
+      }
+    }
+  }
+  return boundary_num;
+}
+
 int find_boundary(int argc, char* argv[]){
   const string dir = "../dairlib_data/goldilocks_models/find_boundary/robot_" +
       to_string(FLAGS_robot_option) + "/";
-  /*
-   * initialize model
-   */
-
-
   /*
    * initialize task space
    */
   double stride_length_0 = 0.2;
-  double stride_length_resolution = 0.01;
-  double ground_incline_0 = 0.1;
+  double delta_stride_length = 0.01;
+
+  double ground_incline_0 = 0;
+  double initial_ground_incline_max = 0.1;
+  double initial_ground_incline_min = -0.1;
   double ground_incline_resolution = 0.01;
-  double turning_rate_0 = 0.1;
-  double turning_rate_resolution = 0.125;
 
-  //iteration setting
-  int max_sl_iter = 200;
-  int min_sl_iter = 200;
-  int max_gi_iter = 200;
-  int min_gi_iter = 200;
+  double turning_rate_0 = 0;
+  double delta_turning_rate = 0;
 
   /*
    * start iteration
    */
-
-  int iter;
+  int max_iter = 50;
   int boundary_sample_num = 0;
+  int extend_direction;//0:decrease the stride length;1:increase stride length
 
-  double sl = 0.2;
-  for (iter = 0; iter <= max_sl_iter; iter++){
-    //fix stride length
-    double sl = stride_length_0;
-
-    //find max ground incline
-    double max_gi = boundary_for_one_dimension(max_gi_iter,0,ground_incline_0,
-        ground_incline_resolution);
-    boundary_sample_num += 1;
-    writeCSV();
-
-    //find min ground incline
-    double min_gi = boundary_for_one_dimension(min_gi_iter,0,-ground_incline_0,
-        ground_incline_resolution);
-    boundary_sample_num += 1;
-    writeCSV(directory + initial_file_name, initial_guess);
-
-  }
+  //search max stride length
+  extend_direction = 1;
+  boundary_sample_num = extend_range(stride_length_0,max_iter, ground_incline_0,
+      initial_ground_incline_max,initial_ground_incline_min,
+      ground_incline_resolution,boundary_sample_num, extend_direction);
+  //search min stride length
+  extend_direction = 0;
+  boundary_sample_num = extend_range(stride_length_0,max_iter, ground_incline_0,
+      initial_ground_incline_max,initial_ground_incline_min,
+      ground_incline_resolution,boundary_sample_num, extend_direction);
 
   return 0;
 }

From b0d0ea322ce4688fb78ed2f178b97c2c55eb262b Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Sat, 23 May 2020 17:28:18 -0400
Subject: [PATCH 02/82] successfully build(unfinished)

---
 examples/goldilocks_models/find_boundary.cc | 160 +++++++++++++++++---
 examples/goldilocks_models/initial_guess.cc |   4 +-
 2 files changed, 140 insertions(+), 24 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index a14756c98c..c3a51b280e 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -76,6 +76,114 @@ DEFINE_bool(is_zero_touchdown_impact, false,
 "No impact force at fist touchdown");
 DEFINE_bool(is_add_tau_in_cost, true, "Add RoM input in the cost function");
 
+void createMBP(MultibodyPlant<double>* plant, int robot_option) {
+  if (robot_option == 0) {
+    Parser parser(plant);
+    std::string full_name = FindResourceOrThrow(
+        "examples/goldilocks_models/PlanarWalkerWithTorso.urdf");
+    parser.AddModelFromFile(full_name);
+    plant->mutable_gravity_field().set_gravity_vector(
+        -9.81 * Eigen::Vector3d::UnitZ());
+    plant->WeldFrames(
+        plant->world_frame(), plant->GetFrameByName("base"),
+        drake::math::RigidTransform<double>());
+    plant->Finalize();
+
+  } else if (robot_option == 1) {
+    Parser parser(plant);
+    string full_name =
+        FindResourceOrThrow("examples/Cassie/urdf/cassie_fixed_springs.urdf");
+    parser.AddModelFromFile(full_name);
+    plant->mutable_gravity_field().set_gravity_vector(
+        -9.81 * Eigen::Vector3d::UnitZ());
+    plant->Finalize();
+  } else {
+    throw std::runtime_error("Should not reach here");
+  }
+}
+void setCostWeight(double* Q, double* R, double* all_cost_scale,
+                   int robot_option) {
+  if (robot_option == 0) {
+    *Q = 1;
+    *R = 0.1;
+    //*all_cost_scale = 1;  // not implemented yet
+  } else if (robot_option == 1) {
+    *Q = 5 * 0.1;
+    *R = 0.1 * 0.01;
+    *all_cost_scale = 0.2/* * 0.12*/;
+  }
+}
+void setRomDim(int* n_s, int* n_tau, int rom_option) {
+  if (rom_option == 0) {
+    // 2D -- lipm
+    *n_s = 2;
+    *n_tau = 0;
+  } else if (rom_option == 1) {
+    // 4D -- lipm + swing foot
+    *n_s = 4;
+    *n_tau = 2;
+  } else if (rom_option == 2) {
+    // 1D -- fix com vertical acceleration
+    *n_s = 1;
+    *n_tau = 0;
+  } else if (rom_option == 3) {
+    // 3D -- fix com vertical acceleration + swing foot
+    *n_s = 3;
+    *n_tau = 2;
+  } else {
+    throw std::runtime_error("Should not reach here");
+  }
+}
+void setRomBMatrix(MatrixXd* B_tau, int rom_option) {
+  if ((rom_option == 0) || (rom_option == 2)) {
+    // passive rom, so we don't need B_tau
+  }
+  else if (rom_option == 1) {
+    DRAKE_DEMAND(B_tau->rows() == 4);
+    (*B_tau)(2, 0) = 1;
+    (*B_tau)(3, 1) = 1;
+  }
+  else if (rom_option == 3) {
+    DRAKE_DEMAND(B_tau->rows() == 3);
+    (*B_tau)(1, 0) = 1;
+    (*B_tau)(2, 1) = 1;
+  } else {
+    throw std::runtime_error("Should not reach here");
+  }
+}
+void setInitialTheta(VectorXd& theta_s, VectorXd& theta_sDDot,
+                     int n_feature_s, int rom_option) {
+  // // Testing intial theta
+  // theta_s = 0.25*VectorXd::Ones(n_theta_s);
+  // theta_sDDot = 0.5*VectorXd::Ones(n_theta_sDDot);
+  // theta_s = VectorXd::Random(n_theta_s);
+  // theta_sDDot = VectorXd::Random(n_theta_sDDot);
+
+  if (rom_option == 0) {
+    // 2D -- lipm
+    theta_s(0) = 1;
+    theta_s(1 + n_feature_s) = 1;
+    theta_sDDot(0) = 1;
+  } else if (rom_option == 1) {
+    // 4D -- lipm + swing foot
+    theta_s(0) = 1;
+    theta_s(1 + n_feature_s) = 1;
+    theta_s(2 + 2 * n_feature_s) = 1;
+    theta_s(3 + 3 * n_feature_s) = 1;
+    theta_sDDot(0) = 1;
+  } else if (rom_option == 2) {
+    // 1D -- fix com vertical acceleration
+    theta_s(1) = 1;
+  } else if (rom_option == 3) {
+    // 3D -- fix com vertical acceleration + swing foot
+    theta_s(1) = 1;
+    theta_s(2 + 1 * n_feature_s) = 1;
+    theta_s(3 + 2 * n_feature_s) = 1;
+  } else {
+    throw std::runtime_error("Should not reach here");
+  }
+}
+
 // trajectory optimization for given task and model
 void trajOptGivenModel(double stride_length, double ground_incline,
     double turning_rate,const string dir,int num,int sample_idx){
@@ -110,7 +218,7 @@ void trajOptGivenModel(double stride_length, double ground_incline,
     // However, currently it takes too much time to compute with many nodes, so
     // we try 0.3/24.
     double max_distance_per_node = 0.3 / 16;
-    DRAKE_DEMAND((max_stride_length / n_node) <= max_distance_per_node);
+//    DRAKE_DEMAND((max_stride_length / n_node) <= max_distance_per_node);
   }
 
   // Reduced order model parameters
@@ -147,14 +255,14 @@ void trajOptGivenModel(double stride_length, double ground_incline,
     duration = 0.4; // 0.4;
   }
 
-  bool is_get_nomial = false;
+  bool is_get_nominal = false;
   int max_inner_iter_pass_in = is_get_nominal ? 200 : max_inner_iter;
 
-  string init_file_pass_in = '';
+  string init_file_pass_in = "";
   string prefix = to_string(num) +  "_" + to_string(sample_idx) + "_";
 
   // Vectors/Matrices for the outer loop
-  int N_Sample = 1;
+  int N_sample = 1;
   vector<std::shared_ptr<VectorXd>> w_sol_vec(N_sample);
   vector<std::shared_ptr<MatrixXd>> H_vec(N_sample);
   vector<std::shared_ptr<VectorXd>> b_vec(N_sample);
@@ -185,7 +293,7 @@ void trajOptGivenModel(double stride_length, double ground_incline,
 
   bool extend_model_this_iter = false;
   int n_rerun = 0;
-  cost_threshold_for_update = std::numeric_limits<double>::infinity();
+  double cost_threshold_for_update = std::numeric_limits<double>::infinity();
   int N_rerun = 0;
 
   //run trajectory optimization
@@ -209,7 +317,7 @@ void trajOptGivenModel(double stride_length, double ground_incline,
                       std::ref(is_success_vec),
                       std::ref(thread_finished_vec),
                       Q, R, all_cost_scale,
-                      eps_regularization,
+                      FLAGS_eps_regularization,
                       is_get_nominal,
                       FLAGS_is_zero_touchdown_impact,
                       extend_model_this_iter,
@@ -249,9 +357,12 @@ double boundary_for_one_dimension(int max_iteration,double stride_length,
 }
 
 //extend range of stride length
-int extend_range(double stride_length_0,int max_iter,double ground_incline_0,
+int extend_range(const string dir,
+    double stride_length_0,int max_iter,double ground_incline_0,
     double ground_incline_high,double ground_incline_low,
-    double ground_incline_resolution,int boundary_num,int update_direction){
+    double ground_incline_resolution,double turning_rate_0,
+    int boundary_num,int update_direction,
+    double delta_stride_length){
   //keep updating stride_length and search boundary of ground incline for each
   //stride length
   int iter;
@@ -261,23 +372,22 @@ int extend_range(double stride_length_0,int max_iter,double ground_incline_0,
     double max_gi = boundary_for_one_dimension(max_iter, stride_length,
         ground_incline_0, ground_incline_high,
         turning_rate_0, ground_incline_resolution,
-        dir, boundary_sample_num);
+        dir, boundary_num);
     VectorXd boundary_point(2);
     boundary_point<<stride_length,max_gi;
-    writeCSV(dir + to_string(boundary_sample_num) +  "_" +
+    writeCSV(dir + to_string(boundary_num) +  "_" +
         string("max_ground_incline.csv"), boundary_point);
-    boundary_sample_num += 1;
+    boundary_num += 1;
 
     //find min ground incline
     double min_gi = boundary_for_one_dimension(max_iter, stride_length,
         ground_incline_0, ground_incline_low,
         turning_rate_0, ground_incline_resolution,
-        dir, boundary_sample_num);
-    VectorXd boundary_point(2);
+        dir, boundary_num);
     boundary_point<<stride_length,min_gi;
-    writeCSV(dir + to_string(boundary_sample_num) +  "_" +
+    writeCSV(dir + to_string(boundary_num) +  "_" +
         string("min_ground_incline.csv"), boundary_point);
-    boundary_sample_num += 1;
+    boundary_num += 1;
     //if max_gi and min_gi are same, find the boundary of stride length
     if(min_gi==max_gi){
       break;
@@ -285,10 +395,10 @@ int extend_range(double stride_length_0,int max_iter,double ground_incline_0,
     //else continue search
     else{
       if(update_direction==0){
-        stride_length += delta_stride_length;
+        stride_length = stride_length+delta_stride_length;
       }
       if(update_direction==1){
-        stride_length -= delta_stride_length;
+        stride_length = stride_length-delta_stride_length;
       }
     }
   }
@@ -310,7 +420,7 @@ int find_boundary(int argc, char* argv[]){
   double ground_incline_resolution = 0.01;
 
   double turning_rate_0 = 0;
-  double delta_turning_rate = 0;
+//  double delta_turning_rate = 0;
 
   /*
    * start iteration
@@ -321,14 +431,20 @@ int find_boundary(int argc, char* argv[]){
 
   //search max stride length
   extend_direction = 1;
-  boundary_sample_num = extend_range(stride_length_0,max_iter, ground_incline_0,
+  boundary_sample_num = extend_range(dir,
+      stride_length_0,max_iter, ground_incline_0,
       initial_ground_incline_max,initial_ground_incline_min,
-      ground_incline_resolution,boundary_sample_num, extend_direction);
+      ground_incline_resolution,turning_rate_0,
+      boundary_sample_num, extend_direction,
+      delta_stride_length);
   //search min stride length
   extend_direction = 0;
-  boundary_sample_num = extend_range(stride_length_0,max_iter, ground_incline_0,
+  boundary_sample_num = extend_range(dir,
+      stride_length_0,max_iter, ground_incline_0,
       initial_ground_incline_max,initial_ground_incline_min,
-      ground_incline_resolution,boundary_sample_num, extend_direction);
+      ground_incline_resolution,turning_rate_0,
+      boundary_sample_num, extend_direction,
+      delta_stride_length);
 
   return 0;
 }
diff --git a/examples/goldilocks_models/initial_guess.cc b/examples/goldilocks_models/initial_guess.cc
index 6aebcb35ab..d75005387a 100644
--- a/examples/goldilocks_models/initial_guess.cc
+++ b/examples/goldilocks_models/initial_guess.cc
@@ -39,7 +39,7 @@ string set_initial_guess(const string directory, int iter, int sample, int total
 * theta_range :decide the range of theta to use in interpolation
 * theta_sclae,gamma_scale :used to scale the theta and gamma in interpolation
 */
-  double theta_range = 0.004;
+  double theta_range = 0.004;//this is tuned by robot_option=1,rom_option=2,3d task space
   int gamma_dimension = 3;
   MatrixXd theta_scale = get_theta_scale(directory, iter);
   MatrixXd gamma_scale = get_gamma_scale(gamma_dimension, min_sl, max_sl, min_gi, max_gi, min_tr, max_tr);
@@ -110,7 +110,7 @@ string set_initial_guess(const string directory, int iter, int sample, int total
     VectorXd past_theta(past_theta_s.rows()+past_theta_sDDot.rows());
     past_theta << past_theta_s,past_theta_sDDot;
     double theta_diff = (past_theta - current_theta).norm() / current_theta.norm();
-    if ((theta_diff < theta_range) && (iter-past_iter<10)) {
+    if (theta_diff < theta_range) {
       //take out corresponding w and calculate the weight for interpolation
       MatrixXd w_gamma;
       VectorXd weight_gamma;

From 7437e3d0b4d0f8ed240d6799965a89e8f5f831c1 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Sun, 24 May 2020 14:21:50 -0400
Subject: [PATCH 03/82] finished search algorithm

---
 examples/goldilocks_models/find_boundary.cc | 81 ++++++++++++++++++---
 1 file changed, 70 insertions(+), 11 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index c3a51b280e..46a12e6751 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -76,6 +76,11 @@ DEFINE_bool(is_zero_touchdown_impact, false,
 "No impact force at fist touchdown");
 DEFINE_bool(is_add_tau_in_cost, true, "Add RoM input in the cost function");
 
+//others
+DEFINE_string(
+    program_name, "",
+"The name of the program (to keep a record for future references)");
+
 void createMBP(MultibodyPlant<double>* plant, int robot_option) {
   if (robot_option == 0) {
     Parser parser(plant);
@@ -292,7 +297,7 @@ void trajOptGivenModel(double stride_length, double ground_incline,
   }
 
   bool extend_model_this_iter = false;
-  int n_rerun = 0;
+  int n_rerun = 1;
   double cost_threshold_for_update = std::numeric_limits<double>::infinity();
   int N_rerun = 0;
 
@@ -328,32 +333,57 @@ void trajOptGivenModel(double stride_length, double ground_incline,
                       FLAGS_robot_option);
 }
 
+//naive test function for search algorithm
+int sample_result(double stride_length,double ground_incline,
+    double turning_rate){
+  int result = 0;
+  if( (ground_incline<0.12) && (ground_incline>-0.12)){
+    if((stride_length<0.25) && (stride_length>0.15)){
+      result = 1;
+    }
+  }
+  return result;
+}
+
+
 //search the max/min ground incline for fixed stride length
 double boundary_for_one_dimension(int max_iteration,double stride_length,
     double gi_low,double gi_high,double turning_rate,double resolution,
     const string dir,int num){
-  double low = gi_low;
-  double high = gi_high;
   int sample_idx = 0;
+  cout << "sample# (rerun #) | stride | incline | turning | init_file | "
+          "Status | Solve time | Cost (tau cost)\n";
   for (sample_idx = 0; sample_idx <= max_iteration; sample_idx++){
     //run trajectory optimization and judge the solution
-    trajOptGivenModel(stride_length, high,
+    trajOptGivenModel(stride_length, gi_high,
         turning_rate, dir, num, sample_idx);
     string prefix = to_string(num) +  "_" + to_string(sample_idx) + "_";
+    //store stride length, ground incline and turning rate
+    writeCSV(dir + prefix +
+        string("stride_length.csv"), stride_length*MatrixXd::Ones(1,1));
+    writeCSV(dir + prefix +
+        string("ground_incline.csv"), gi_high*MatrixXd::Ones(1,1));
+    writeCSV(dir + prefix +
+        string("turning_rate.csv"), turning_rate*MatrixXd::Ones(1,1));
+
     int sample_success =
         (readCSV(dir + prefix + string("is_success.csv")))(0, 0);
+
+//    //test search algorithm
+//    int sample_success = sample_result(stride_length,high,turning_rate);
+
     if (sample_success){
-      low = high;
-      high = 2*high;
+      gi_low = gi_high;
+      gi_high = 2*gi_high;
     }
     else{
-      high = (high+low)/2;
+      gi_high = (gi_high+gi_low)/2;
     }
-    if(abs(high-low) < resolution){
+    if(abs(gi_high-gi_low) < resolution){
       break;
     }
   }
-  return low;
+  return gi_low;
 }
 
 //extend range of stride length
@@ -377,6 +407,9 @@ int extend_range(const string dir,
     boundary_point<<stride_length,max_gi;
     writeCSV(dir + to_string(boundary_num) +  "_" +
         string("max_ground_incline.csv"), boundary_point);
+    cout << "boundary point index | stride length | ground incline"<<endl;
+    cout<<" \t "<<boundary_num<< "\t" <<" | "<<"\t"<<stride_length
+        <<"\t"<<" | "<<"\t"<<max_gi<<endl;
     boundary_num += 1;
 
     //find min ground incline
@@ -387,6 +420,9 @@ int extend_range(const string dir,
     boundary_point<<stride_length,min_gi;
     writeCSV(dir + to_string(boundary_num) +  "_" +
         string("min_ground_incline.csv"), boundary_point);
+    cout << "boundary point index | stride length | ground incline"<<endl;
+    cout<<" \t "<<boundary_num<< "\t" <<" | "<<"\t"<<stride_length
+        <<"\t"<<" | "<<"\t"<<min_gi<<endl;
     boundary_num += 1;
     //if max_gi and min_gi are same, find the boundary of stride length
     if(min_gi==max_gi){
@@ -394,10 +430,10 @@ int extend_range(const string dir,
     }
     //else continue search
     else{
-      if(update_direction==0){
+      if(update_direction==1){
         stride_length = stride_length+delta_stride_length;
       }
-      if(update_direction==1){
+      if(update_direction==0){
         stride_length = stride_length-delta_stride_length;
       }
     }
@@ -406,18 +442,37 @@ int extend_range(const string dir,
 }
 
 int find_boundary(int argc, char* argv[]){
+  gflags::ParseCommandLineFlags(&argc, &argv, true);
+
+  cout << "Trail name: " << FLAGS_program_name << endl;
+  cout << "Git commit hash: " << endl;
+  std::system("git rev-parse HEAD");
+  cout << "Result of \"git diff-index HEAD\":" << endl;
+  std::system("git diff-index HEAD");
+  cout << endl;
+
   const string dir = "../dairlib_data/goldilocks_models/find_boundary/robot_" +
       to_string(FLAGS_robot_option) + "/";
+  if (!CreateFolderIfNotExist(dir)) return 0;
+
   /*
    * initialize task space
    */
+  cout << "\nInitialize task space:\n";
   double stride_length_0 = 0.2;
   double delta_stride_length = 0.01;
+  cout<<"initial stride length "<<stride_length_0<<endl;
+  cout<<"delta stride length "<<delta_stride_length<<endl;
 
   double ground_incline_0 = 0;
   double initial_ground_incline_max = 0.1;
   double initial_ground_incline_min = -0.1;
   double ground_incline_resolution = 0.01;
+  cout<<"max ground incline initially located within "<<ground_incline_0
+      <<" to "<<initial_ground_incline_max<<endl;
+  cout<<"min ground incline initially located within "<<initial_ground_incline_min
+      <<" to "<<ground_incline_0<<endl;
+  cout<<"ground incline resolution "<<ground_incline_resolution<<endl;
 
   double turning_rate_0 = 0;
 //  double delta_turning_rate = 0;
@@ -425,11 +480,13 @@ int find_boundary(int argc, char* argv[]){
   /*
    * start iteration
    */
+  cout << "\nStart iteration:\n";
   int max_iter = 50;
   int boundary_sample_num = 0;
   int extend_direction;//0:decrease the stride length;1:increase stride length
 
   //search max stride length
+  cout<<"search along increasing the stride length"<<endl;
   extend_direction = 1;
   boundary_sample_num = extend_range(dir,
       stride_length_0,max_iter, ground_incline_0,
@@ -437,7 +494,9 @@ int find_boundary(int argc, char* argv[]){
       ground_incline_resolution,turning_rate_0,
       boundary_sample_num, extend_direction,
       delta_stride_length);
+
   //search min stride length
+  cout<<"search along decreasing the stride length"<<endl;
   extend_direction = 0;
   boundary_sample_num = extend_range(dir,
       stride_length_0,max_iter, ground_incline_0,

From 0216ac9f7f94dfecbd8f6e934c866ea545ab1d5e Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Sun, 24 May 2020 20:48:45 -0400
Subject: [PATCH 04/82] debug for sample_index

---
 examples/goldilocks_models/find_boundary.cc | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 46a12e6751..bf4f61d876 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -297,10 +297,10 @@ void trajOptGivenModel(double stride_length, double ground_incline,
   }
 
   bool extend_model_this_iter = false;
-  int n_rerun = 1;
+  int n_rerun = 0;
   double cost_threshold_for_update = std::numeric_limits<double>::infinity();
   int N_rerun = 0;
-
+  sample_idx = 0;
   //run trajectory optimization
   trajOptGivenWeights(std::ref(plant), std::ref(plant_autoDiff),
                       n_s, n_sDDot, n_tau,
@@ -371,13 +371,13 @@ double boundary_for_one_dimension(int max_iteration,double stride_length,
 
 //    //test search algorithm
 //    int sample_success = sample_result(stride_length,high,turning_rate);
-
+    double gi_interval = (gi_high-gi_low)/2;
     if (sample_success){
       gi_low = gi_high;
-      gi_high = 2*gi_high;
+      gi_high = gi_high+gi_interval;
     }
     else{
-      gi_high = (gi_high+gi_low)/2;
+      gi_high = gi_low+gi_interval;
     }
     if(abs(gi_high-gi_low) < resolution){
       break;

From 0919cee6ba27685eae9acf96b113f4822d75917e Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Mon, 25 May 2020 15:29:51 -0400
Subject: [PATCH 05/82] update index for trajectory optimization and use cost
 to judge quality of solutions

---
 examples/goldilocks_models/find_boundary.cc | 106 +++++++++++++-------
 1 file changed, 68 insertions(+), 38 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index bf4f61d876..d587d18dd3 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -76,6 +76,13 @@ DEFINE_bool(is_zero_touchdown_impact, false,
 "No impact force at fist touchdown");
 DEFINE_bool(is_add_tau_in_cost, true, "Add RoM input in the cost function");
 
+//outer loop
+DEFINE_int32(max_outer_iter, 50 , "max number of iterations for searching on each "
+                                "direction of one dimension");
+DEFINE_double(cost_increase_threshold, 0.5, "max increase threshold of cost "
+                                            "used for judging the quality of "
+                                            "solutions");
+
 //others
 DEFINE_string(
     program_name, "",
@@ -191,7 +198,7 @@ void setInitialTheta(VectorXd& theta_s, VectorXd& theta_sDDot,
 
 // trajectory optimization for given task and model
 void trajOptGivenModel(double stride_length, double ground_incline,
-    double turning_rate,const string dir,int num,int sample_idx){
+    double turning_rate,const string dir,int num){
   // Create MBP
   MultibodyPlant<double> plant(0.0);
   createMBP(&plant, FLAGS_robot_option);
@@ -264,6 +271,8 @@ void trajOptGivenModel(double stride_length, double ground_incline,
   int max_inner_iter_pass_in = is_get_nominal ? 200 : max_inner_iter;
 
   string init_file_pass_in = "";
+  //TODO:set initial guess for trajectory optimizations using interpolation
+  int sample_idx = 0;
   string prefix = to_string(num) +  "_" + to_string(sample_idx) + "_";
 
   // Vectors/Matrices for the outer loop
@@ -300,7 +309,6 @@ void trajOptGivenModel(double stride_length, double ground_incline,
   int n_rerun = 0;
   double cost_threshold_for_update = std::numeric_limits<double>::infinity();
   int N_rerun = 0;
-  sample_idx = 0;
   //run trajectory optimization
   trajOptGivenWeights(std::ref(plant), std::ref(plant_autoDiff),
                       n_s, n_sDDot, n_tau,
@@ -334,12 +342,12 @@ void trajOptGivenModel(double stride_length, double ground_incline,
 }
 
 //naive test function for search algorithm
-int sample_result(double stride_length,double ground_incline,
+double sample_result(double stride_length,double ground_incline,
     double turning_rate){
-  int result = 0;
+  double result = std::numeric_limits<double>::infinity();
   if( (ground_incline<0.12) && (ground_incline>-0.12)){
     if((stride_length<0.25) && (stride_length>0.15)){
-      result = 1;
+      result = 0;
     }
   }
   return result;
@@ -349,30 +357,33 @@ int sample_result(double stride_length,double ground_incline,
 //search the max/min ground incline for fixed stride length
 double boundary_for_one_dimension(int max_iteration,double stride_length,
     double gi_low,double gi_high,double turning_rate,double resolution,
-    const string dir,int num){
+    const string dir,int& num,double max_cost){
+  int iter = 0;
   int sample_idx = 0;
   cout << "sample# (rerun #) | stride | incline | turning | init_file | "
           "Status | Solve time | Cost (tau cost)\n";
-  for (sample_idx = 0; sample_idx <= max_iteration; sample_idx++){
-    //run trajectory optimization and judge the solution
-    trajOptGivenModel(stride_length, gi_high,
-        turning_rate, dir, num, sample_idx);
-    string prefix = to_string(num) +  "_" + to_string(sample_idx) + "_";
+  for (iter = 0; iter <= max_iteration; iter++){
     //store stride length, ground incline and turning rate
+    num += 1;
+    string prefix = to_string(num) +  "_" + to_string(sample_idx) + "_";
     writeCSV(dir + prefix +
         string("stride_length.csv"), stride_length*MatrixXd::Ones(1,1));
     writeCSV(dir + prefix +
         string("ground_incline.csv"), gi_high*MatrixXd::Ones(1,1));
     writeCSV(dir + prefix +
         string("turning_rate.csv"), turning_rate*MatrixXd::Ones(1,1));
+    //run trajectory optimization and judge the solution
+    trajOptGivenModel(stride_length, gi_high,
+        turning_rate, dir, num);
 
-    int sample_success =
-        (readCSV(dir + prefix + string("is_success.csv")))(0, 0);
+    double sample_cost =
+        (readCSV(dir + prefix + string("c.csv")))(0, 0);
 
 //    //test search algorithm
-//    int sample_success = sample_result(stride_length,high,turning_rate);
+//    double sample_cost = sample_result(stride_length,gi_high,turning_rate);
+
     double gi_interval = (gi_high-gi_low)/2;
-    if (sample_success){
+    if (sample_cost<max_cost){
       gi_low = gi_high;
       gi_high = gi_high+gi_interval;
     }
@@ -387,12 +398,12 @@ double boundary_for_one_dimension(int max_iteration,double stride_length,
 }
 
 //extend range of stride length
-int extend_range(const string dir,
+void extend_range(const string dir,
     double stride_length_0,int max_iter,double ground_incline_0,
     double ground_incline_high,double ground_incline_low,
     double ground_incline_resolution,double turning_rate_0,
-    int boundary_num,int update_direction,
-    double delta_stride_length){
+    int& boundary_point_idx,int& traj_opt_idx, int update_direction,
+    double delta_stride_length,double max_cost){
   //keep updating stride_length and search boundary of ground incline for each
   //stride length
   int iter;
@@ -402,28 +413,28 @@ int extend_range(const string dir,
     double max_gi = boundary_for_one_dimension(max_iter, stride_length,
         ground_incline_0, ground_incline_high,
         turning_rate_0, ground_incline_resolution,
-        dir, boundary_num);
+        dir, traj_opt_idx,max_cost);
+    boundary_point_idx += 1;
     VectorXd boundary_point(2);
     boundary_point<<stride_length,max_gi;
-    writeCSV(dir + to_string(boundary_num) +  "_" +
+    writeCSV(dir + to_string(boundary_point_idx) +  "_" +
         string("max_ground_incline.csv"), boundary_point);
     cout << "boundary point index | stride length | ground incline"<<endl;
-    cout<<" \t "<<boundary_num<< "\t" <<" | "<<"\t"<<stride_length
+    cout<<" \t "<<boundary_point_idx<< "\t" <<" | "<<"\t"<<stride_length
         <<"\t"<<" | "<<"\t"<<max_gi<<endl;
-    boundary_num += 1;
 
     //find min ground incline
     double min_gi = boundary_for_one_dimension(max_iter, stride_length,
         ground_incline_0, ground_incline_low,
         turning_rate_0, ground_incline_resolution,
-        dir, boundary_num);
+        dir, traj_opt_idx,max_cost);
+    boundary_point_idx += 1;
     boundary_point<<stride_length,min_gi;
-    writeCSV(dir + to_string(boundary_num) +  "_" +
+    writeCSV(dir + to_string(boundary_point_idx) +  "_" +
         string("min_ground_incline.csv"), boundary_point);
     cout << "boundary point index | stride length | ground incline"<<endl;
-    cout<<" \t "<<boundary_num<< "\t" <<" | "<<"\t"<<stride_length
+    cout<<" \t "<<boundary_point_idx<< "\t" <<" | "<<"\t"<<stride_length
         <<"\t"<<" | "<<"\t"<<min_gi<<endl;
-    boundary_num += 1;
     //if max_gi and min_gi are same, find the boundary of stride length
     if(min_gi==max_gi){
       break;
@@ -438,7 +449,6 @@ int extend_range(const string dir,
       }
     }
   }
-  return boundary_num;
 }
 
 int find_boundary(int argc, char* argv[]){
@@ -477,33 +487,53 @@ int find_boundary(int argc, char* argv[]){
   double turning_rate_0 = 0;
 //  double delta_turning_rate = 0;
 
+  /*
+   * Iteration setting
+   */
+  int max_iter = FLAGS_max_outer_iter;
+  double cost_threshold = FLAGS_cost_increase_threshold;
+  int boundary_sample_num = 0;//use this to set the index for boundary point
+  int traj_opt_num = 0;//use this to set the index for Traj Opt
+  int extend_direction;//0:decrease the stride length;1:increase stride length
+
   /*
    * start iteration
    */
+  cout << "\nCalculate Central Point Cost:\n";
+  writeCSV(dir +  to_string(traj_opt_num)  +
+      string("_0_stride_length.csv"), stride_length_0*MatrixXd::Ones(1,1));
+  writeCSV(dir + to_string(traj_opt_num) +
+      string("_0_ground_incline.csv"), ground_incline_0*MatrixXd::Ones(1,1));
+  writeCSV(dir + to_string(traj_opt_num) +
+      string("_0_turning_rate.csv"), turning_rate_0*MatrixXd::Ones(1,1));
+  trajOptGivenModel(stride_length_0, ground_incline_0,
+                    turning_rate_0, dir, traj_opt_num);
+  double nominal_cost = (readCSV(dir + to_string(traj_opt_num) +
+      string("_0_c.csv")))(0, 0);
+  double max_cost = nominal_cost*(1+cost_threshold);
+  cout << "nominal_cost: " <<nominal_cost<<endl;
+  cout << "increase threshold of cost: "<<cost_threshold<<endl;
+  cout << "max permitted cost " <<max_cost<<endl;
+
   cout << "\nStart iteration:\n";
-  int max_iter = 50;
-  int boundary_sample_num = 0;
-  int extend_direction;//0:decrease the stride length;1:increase stride length
 
   //search max stride length
   cout<<"search along increasing the stride length"<<endl;
   extend_direction = 1;
-  boundary_sample_num = extend_range(dir,
-      stride_length_0,max_iter, ground_incline_0,
+  extend_range(dir,stride_length_0,max_iter, ground_incline_0,
       initial_ground_incline_max,initial_ground_incline_min,
       ground_incline_resolution,turning_rate_0,
-      boundary_sample_num, extend_direction,
-      delta_stride_length);
+      boundary_sample_num,traj_opt_num, extend_direction,
+      delta_stride_length,max_cost);
 
   //search min stride length
   cout<<"search along decreasing the stride length"<<endl;
   extend_direction = 0;
-  boundary_sample_num = extend_range(dir,
-      stride_length_0,max_iter, ground_incline_0,
+  extend_range(dir,stride_length_0,max_iter, ground_incline_0,
       initial_ground_incline_max,initial_ground_incline_min,
       ground_incline_resolution,turning_rate_0,
-      boundary_sample_num, extend_direction,
-      delta_stride_length);
+      boundary_sample_num,traj_opt_num, extend_direction,
+      delta_stride_length,max_cost);
 
   return 0;
 }

From a95222395ac7b3c24044ad9dceb65cca75d0cbbe Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Mon, 25 May 2020 22:02:44 -0400
Subject: [PATCH 06/82] set the initial guess for trajecoty optimization

---
 examples/goldilocks_models/find_boundary.cc | 81 ++++++++++++++++++++-
 1 file changed, 78 insertions(+), 3 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index d587d18dd3..ac3faf7e36 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -196,6 +196,75 @@ void setInitialTheta(VectorXd& theta_s, VectorXd& theta_sDDot,
   }
 }
 
+//use interpolation to set the initial guess for the trajectory optimization
+string getInitFileName(const string directory, int traj_opt_num){
+  MatrixXd current_ground_incline = readCSV(directory + to_string(traj_opt_num)
+      + string("_0_ground_incline.csv"));
+  MatrixXd current_stride_length = readCSV(directory + to_string(traj_opt_num)
+      + string("_0_stride_length.csv"));
+  MatrixXd current_turning_rate = readCSV(directory + to_string(traj_opt_num)
+      + string("_0_turning_rate.csv"));
+  int gamma_dimension = 3;
+  VectorXd current_gamma(gamma_dimension);
+  current_gamma << current_ground_incline(0, 0), current_stride_length(0, 0),
+    current_turning_rate(0,0);
+  string initial_file_name;
+  if(traj_opt_num==0){
+    initial_file_name = "";
+  }else{
+    VectorXd initial_guess;
+    //take out corresponding w and calculate the weight for interpolation
+    MatrixXd w_gamma;
+    VectorXd weight_gamma;
+    //paras used to decide gamma scale
+    double delta_sl = 0.015;
+    double delta_gi = 0.05;
+    double delta_tr = 0.125;
+    //calculate the weighted sum of past solutions
+    int sample_num = 0;
+    for (sample_num = 0; sample_num < traj_opt_num; sample_num++) {
+      //check if this sample is success
+      int is_success = (readCSV(directory + to_string(sample_num)
+          + string("_0_is_success.csv")))(0,0);
+      if(is_success == 1) {
+        //extract past gamma
+        MatrixXd past_ground_incline = readCSV(directory + to_string(sample_num)
+            + string("_0_ground_incline.csv"));
+        MatrixXd past_stride_length = readCSV(directory + to_string(sample_num)
+            + string("_0_stride_length.csv"));
+        MatrixXd past_turning_rate = readCSV(directory + to_string(sample_num)
+            + string("_0_turning_rate.csv"));
+        VectorXd past_gamma(gamma_dimension);
+        past_gamma << past_ground_incline(0, 0), past_stride_length(0, 0), past_turning_rate(0, 0);
+        //calculate the weight for each sample using the third power of the difference between gamma
+        VectorXd gamma_scale(gamma_dimension);
+        gamma_scale << 1/delta_gi,1/delta_sl,1.3/delta_tr;
+        VectorXd dif_gamma = (past_gamma - current_gamma).array().abs()*gamma_scale.array();
+        VectorXd dif_gamma2 = dif_gamma.array().pow(2);
+        double distance_gamma =  (dif_gamma.transpose() * dif_gamma2)(0,0);
+        //extract the solution of this sample
+        VectorXd w_to_interpolate = readCSV(directory + to_string(sample_num)
+            + string("_0_w.csv"));
+        //concatenate the weight and solution for further calculation
+        w_gamma.conservativeResize(w_to_interpolate.rows(),w_gamma.cols()+1);
+        w_gamma.col(w_gamma.cols()-1) = w_to_interpolate;
+        weight_gamma.conservativeResize(weight_gamma.rows()+1);
+        weight_gamma(weight_gamma.rows()-1)= 1 /distance_gamma;
+      }
+    }
+
+    //    normalize weight
+    weight_gamma = weight_gamma / weight_gamma.sum();
+    initial_guess = w_gamma * weight_gamma;
+    //    save initial guess and set init file
+    initial_file_name = to_string(traj_opt_num)
+        + string("_0_initial_guess.csv");
+    writeCSV(directory + initial_file_name, initial_guess);
+  }
+
+  return initial_file_name;
+}
+
 // trajectory optimization for given task and model
 void trajOptGivenModel(double stride_length, double ground_incline,
     double turning_rate,const string dir,int num){
@@ -270,8 +339,8 @@ void trajOptGivenModel(double stride_length, double ground_incline,
   bool is_get_nominal = false;
   int max_inner_iter_pass_in = is_get_nominal ? 200 : max_inner_iter;
 
-  string init_file_pass_in = "";
-  //TODO:set initial guess for trajectory optimizations using interpolation
+//  string init_file_pass_in = "";
+  string init_file_pass_in = getInitFileName(dir, num);
   int sample_idx = 0;
   string prefix = to_string(num) +  "_" + to_string(sample_idx) + "_";
 
@@ -469,7 +538,13 @@ int find_boundary(int argc, char* argv[]){
    * initialize task space
    */
   cout << "\nInitialize task space:\n";
-  double stride_length_0 = 0.2;
+  double stride_length_0 = 0;
+  if(FLAGS_robot_option==0){
+    stride_length_0 = 0.3;
+  }
+  else if(FLAGS_robot_option==1){
+      stride_length_0 = 0.2;
+  }
   double delta_stride_length = 0.01;
   cout<<"initial stride length "<<stride_length_0<<endl;
   cout<<"delta stride length "<<delta_stride_length<<endl;

From cd94e5be51d0e23da7accca118a24a1a8daa4cba Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Wed, 27 May 2020 22:06:23 -0400
Subject: [PATCH 07/82] generalize the search algorithm

---
 examples/goldilocks_models/find_boundary.cc | 247 ++++++++------------
 1 file changed, 97 insertions(+), 150 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index ac3faf7e36..2456396426 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -79,9 +79,8 @@ DEFINE_bool(is_add_tau_in_cost, true, "Add RoM input in the cost function");
 //outer loop
 DEFINE_int32(max_outer_iter, 50 , "max number of iterations for searching on each "
                                 "direction of one dimension");
-DEFINE_double(cost_increase_threshold, 0.5, "max increase threshold of cost "
-                                            "used for judging the quality of "
-                                            "solutions");
+DEFINE_double(max_cost_threshold, 20, "max cost used for judging "
+                                       "the quality of solutions");
 
 //others
 DEFINE_string(
@@ -198,16 +197,9 @@ void setInitialTheta(VectorXd& theta_s, VectorXd& theta_sDDot,
 
 //use interpolation to set the initial guess for the trajectory optimization
 string getInitFileName(const string directory, int traj_opt_num){
-  MatrixXd current_ground_incline = readCSV(directory + to_string(traj_opt_num)
-      + string("_0_ground_incline.csv"));
-  MatrixXd current_stride_length = readCSV(directory + to_string(traj_opt_num)
-      + string("_0_stride_length.csv"));
-  MatrixXd current_turning_rate = readCSV(directory + to_string(traj_opt_num)
-      + string("_0_turning_rate.csv"));
   int gamma_dimension = 3;
-  VectorXd current_gamma(gamma_dimension);
-  current_gamma << current_ground_incline(0, 0), current_stride_length(0, 0),
-    current_turning_rate(0,0);
+  VectorXd current_gamma = readCSV(directory + to_string(traj_opt_num)
+                                       + string("_0_gamma.csv"));
   string initial_file_name;
   if(traj_opt_num==0){
     initial_file_name = "";
@@ -228,14 +220,8 @@ string getInitFileName(const string directory, int traj_opt_num){
           + string("_0_is_success.csv")))(0,0);
       if(is_success == 1) {
         //extract past gamma
-        MatrixXd past_ground_incline = readCSV(directory + to_string(sample_num)
-            + string("_0_ground_incline.csv"));
-        MatrixXd past_stride_length = readCSV(directory + to_string(sample_num)
-            + string("_0_stride_length.csv"));
-        MatrixXd past_turning_rate = readCSV(directory + to_string(sample_num)
-            + string("_0_turning_rate.csv"));
-        VectorXd past_gamma(gamma_dimension);
-        past_gamma << past_ground_incline(0, 0), past_stride_length(0, 0), past_turning_rate(0, 0);
+        VectorXd past_gamma = readCSV(directory + to_string(traj_opt_num)
+                                       + string("_0_gamma.csv"));
         //calculate the weight for each sample using the third power of the difference between gamma
         VectorXd gamma_scale(gamma_dimension);
         gamma_scale << 1/delta_gi,1/delta_sl,1.3/delta_tr;
@@ -415,7 +401,7 @@ double sample_result(double stride_length,double ground_incline,
     double turning_rate){
   double result = std::numeric_limits<double>::infinity();
   if( (ground_incline<0.12) && (ground_incline>-0.12)){
-    if((stride_length<0.25) && (stride_length>0.15)){
+    if((stride_length<0.35) && (stride_length>0.25)){
       result = 0;
     }
   }
@@ -423,101 +409,47 @@ double sample_result(double stride_length,double ground_incline,
 }
 
 
-//search the max/min ground incline for fixed stride length
-double boundary_for_one_dimension(int max_iteration,double stride_length,
-    double gi_low,double gi_high,double turning_rate,double resolution,
-    const string dir,int& num,double max_cost){
-  int iter = 0;
+//search the boundary point along one direction
+void boundary_for_one_direction(const string dir,int dims,int max_iteration,
+    VectorXd init_gamma,VectorXd step,double max_cost,
+    int& traj_num,int& boundary_point_idx){
+  int iter;
   int sample_idx = 0;
+  VectorXd new_gamma(dims);
+  VectorXd boundary_point(dims);
   cout << "sample# (rerun #) | stride | incline | turning | init_file | "
           "Status | Solve time | Cost (tau cost)\n";
-  for (iter = 0; iter <= max_iteration; iter++){
+  for (iter = 1; iter <= max_iteration; iter++){
+    new_gamma = init_gamma + iter*step;
     //store stride length, ground incline and turning rate
-    num += 1;
-    string prefix = to_string(num) +  "_" + to_string(sample_idx) + "_";
-    writeCSV(dir + prefix +
-        string("stride_length.csv"), stride_length*MatrixXd::Ones(1,1));
-    writeCSV(dir + prefix +
-        string("ground_incline.csv"), gi_high*MatrixXd::Ones(1,1));
+    traj_num += 1;
+    string prefix = to_string(traj_num) +  "_" + to_string(sample_idx) + "_";
     writeCSV(dir + prefix +
-        string("turning_rate.csv"), turning_rate*MatrixXd::Ones(1,1));
+        string("gamma.csv"), new_gamma);
     //run trajectory optimization and judge the solution
-    trajOptGivenModel(stride_length, gi_high,
-        turning_rate, dir, num);
+    trajOptGivenModel(new_gamma[0], new_gamma[1],
+        new_gamma[2], dir, traj_num);
 
     double sample_cost =
         (readCSV(dir + prefix + string("c.csv")))(0, 0);
 
-//    //test search algorithm
-//    double sample_cost = sample_result(stride_length,gi_high,turning_rate);
-
-    double gi_interval = (gi_high-gi_low)/2;
-    if (sample_cost<max_cost){
-      gi_low = gi_high;
-      gi_high = gi_high+gi_interval;
-    }
-    else{
-      gi_high = gi_low+gi_interval;
-    }
-    if(abs(gi_high-gi_low) < resolution){
+    //test search algorithm
+//    double sample_cost = sample_result(new_gamma[0],new_gamma[1],
+//        new_gamma[2]);
+
+    if(sample_cost>max_cost){
+      boundary_point_idx += 1;
+      boundary_point = new_gamma-step;
+      writeCSV(dir + to_string(boundary_point_idx) +  "_" +
+          string("boundary_point.csv"), boundary_point);
+      cout << "boundary point index | stride length | ground incline"
+              " | turning rate"<<endl;
+      cout<<" \t "<<boundary_point_idx<< "\t" <<" | "<<"\t"<<boundary_point[0]
+          <<"\t"<<" | "<<"\t"<<boundary_point[1]<<"\t"<<" | "<<"\t"<<
+          boundary_point[2]<<endl;
       break;
     }
   }
-  return gi_low;
-}
-
-//extend range of stride length
-void extend_range(const string dir,
-    double stride_length_0,int max_iter,double ground_incline_0,
-    double ground_incline_high,double ground_incline_low,
-    double ground_incline_resolution,double turning_rate_0,
-    int& boundary_point_idx,int& traj_opt_idx, int update_direction,
-    double delta_stride_length,double max_cost){
-  //keep updating stride_length and search boundary of ground incline for each
-  //stride length
-  int iter;
-  double stride_length = stride_length_0;
-  for (iter = 0; iter <= max_iter; iter++){
-    //fix stride length and find max ground incline
-    double max_gi = boundary_for_one_dimension(max_iter, stride_length,
-        ground_incline_0, ground_incline_high,
-        turning_rate_0, ground_incline_resolution,
-        dir, traj_opt_idx,max_cost);
-    boundary_point_idx += 1;
-    VectorXd boundary_point(2);
-    boundary_point<<stride_length,max_gi;
-    writeCSV(dir + to_string(boundary_point_idx) +  "_" +
-        string("max_ground_incline.csv"), boundary_point);
-    cout << "boundary point index | stride length | ground incline"<<endl;
-    cout<<" \t "<<boundary_point_idx<< "\t" <<" | "<<"\t"<<stride_length
-        <<"\t"<<" | "<<"\t"<<max_gi<<endl;
-
-    //find min ground incline
-    double min_gi = boundary_for_one_dimension(max_iter, stride_length,
-        ground_incline_0, ground_incline_low,
-        turning_rate_0, ground_incline_resolution,
-        dir, traj_opt_idx,max_cost);
-    boundary_point_idx += 1;
-    boundary_point<<stride_length,min_gi;
-    writeCSV(dir + to_string(boundary_point_idx) +  "_" +
-        string("min_ground_incline.csv"), boundary_point);
-    cout << "boundary point index | stride length | ground incline"<<endl;
-    cout<<" \t "<<boundary_point_idx<< "\t" <<" | "<<"\t"<<stride_length
-        <<"\t"<<" | "<<"\t"<<min_gi<<endl;
-    //if max_gi and min_gi are same, find the boundary of stride length
-    if(min_gi==max_gi){
-      break;
-    }
-    //else continue search
-    else{
-      if(update_direction==1){
-        stride_length = stride_length+delta_stride_length;
-      }
-      if(update_direction==0){
-        stride_length = stride_length-delta_stride_length;
-      }
-    }
-  }
 }
 
 int find_boundary(int argc, char* argv[]){
@@ -538,9 +470,10 @@ int find_boundary(int argc, char* argv[]){
    * initialize task space
    */
   cout << "\nInitialize task space:\n";
+  int dimensions = 3;//dimension of the task space
   double stride_length_0 = 0;
   if(FLAGS_robot_option==0){
-    stride_length_0 = 0.3;
+    stride_length_0 = 0.25;
   }
   else if(FLAGS_robot_option==1){
       stride_length_0 = 0.2;
@@ -550,65 +483,79 @@ int find_boundary(int argc, char* argv[]){
   cout<<"delta stride length "<<delta_stride_length<<endl;
 
   double ground_incline_0 = 0;
-  double initial_ground_incline_max = 0.1;
-  double initial_ground_incline_min = -0.1;
-  double ground_incline_resolution = 0.01;
-  cout<<"max ground incline initially located within "<<ground_incline_0
-      <<" to "<<initial_ground_incline_max<<endl;
-  cout<<"min ground incline initially located within "<<initial_ground_incline_min
-      <<" to "<<ground_incline_0<<endl;
-  cout<<"ground incline resolution "<<ground_incline_resolution<<endl;
+  double delta_ground_incline = 0.01;
+  cout<<"initial ground incline "<<ground_incline_0<<endl;
+  cout<<"delta ground incline "<<delta_ground_incline<<endl;
 
   double turning_rate_0 = 0;
-//  double delta_turning_rate = 0;
+  double delta_turning_rate = 0;
+  cout<<"initial turning rate "<<turning_rate_0<<endl;
+  cout<<"delta turning rate "<<delta_turning_rate<<endl;
+
+  VectorXd initial_gamma(dimensions);
+  initial_gamma<<stride_length_0,ground_incline_0,turning_rate_0;
+  VectorXd delta(dimensions);
+  delta<<delta_stride_length,delta_ground_incline,delta_turning_rate;
+
 
   /*
    * Iteration setting
    */
   int max_iter = FLAGS_max_outer_iter;
-  double cost_threshold = FLAGS_cost_increase_threshold;
+  double cost_threshold = FLAGS_max_cost_threshold;
   int boundary_sample_num = 0;//use this to set the index for boundary point
   int traj_opt_num = 0;//use this to set the index for Traj Opt
-  int extend_direction;//0:decrease the stride length;1:increase stride length
+  VectorXd extend_direction(dimensions);//the direction of searching
+
+  //create components for each dimension used to decide the direction
+  int sl_comp_num = 3;
+  VectorXd e_sl(sl_comp_num);
+  e_sl<<0,-1,1;
+
+  int gi_comp_num = 3;
+  VectorXd e_gi(gi_comp_num);
+  e_gi<<0,-1,1;
+
+  int tr_comp_num = 1;
+  VectorXd e_tr(tr_comp_num);
+  e_tr<<0;
 
   /*
    * start iteration
    */
-  cout << "\nCalculate Central Point Cost:\n";
-  writeCSV(dir +  to_string(traj_opt_num)  +
-      string("_0_stride_length.csv"), stride_length_0*MatrixXd::Ones(1,1));
-  writeCSV(dir + to_string(traj_opt_num) +
-      string("_0_ground_incline.csv"), ground_incline_0*MatrixXd::Ones(1,1));
-  writeCSV(dir + to_string(traj_opt_num) +
-      string("_0_turning_rate.csv"), turning_rate_0*MatrixXd::Ones(1,1));
-  trajOptGivenModel(stride_length_0, ground_incline_0,
-                    turning_rate_0, dir, traj_opt_num);
-  double nominal_cost = (readCSV(dir + to_string(traj_opt_num) +
-      string("_0_c.csv")))(0, 0);
-  double max_cost = nominal_cost*(1+cost_threshold);
-  cout << "nominal_cost: " <<nominal_cost<<endl;
-  cout << "increase threshold of cost: "<<cost_threshold<<endl;
-  cout << "max permitted cost " <<max_cost<<endl;
-
-  cout << "\nStart iteration:\n";
-
-  //search max stride length
-  cout<<"search along increasing the stride length"<<endl;
-  extend_direction = 1;
-  extend_range(dir,stride_length_0,max_iter, ground_incline_0,
-      initial_ground_incline_max,initial_ground_incline_min,
-      ground_incline_resolution,turning_rate_0,
-      boundary_sample_num,traj_opt_num, extend_direction,
-      delta_stride_length,max_cost);
-
-  //search min stride length
-  cout<<"search along decreasing the stride length"<<endl;
-  extend_direction = 0;
-  extend_range(dir,stride_length_0,max_iter, ground_incline_0,
-      initial_ground_incline_max,initial_ground_incline_min,
-      ground_incline_resolution,turning_rate_0,
-      boundary_sample_num,traj_opt_num, extend_direction,
-      delta_stride_length,max_cost);
+  int dim1,dim2,dim3;
+  VectorXd step(dimensions);
+  //for all the direction, search the boundary
+  for (dim1=0;dim1<sl_comp_num;dim1++){
+    for(dim2=0;dim2<gi_comp_num;dim2++){
+      for(dim3=0;dim3<tr_comp_num;dim3++){
+        extend_direction[0] = e_sl[dim1];
+        extend_direction[1] = e_gi[dim2];
+        extend_direction[2] = e_tr[dim3];
+        //direction elements all zero. evaluate initial sample
+        if((extend_direction[0]==0)&&(extend_direction[1]==0)
+          &&(extend_direction[2]==0)){
+          cout << "\nCalculate Central Point Cost:\n";
+          writeCSV(dir +  to_string(traj_opt_num)  +
+              string("_0_gamma.csv"), initial_gamma);
+          cout << "sample# (rerun #) | stride | incline | turning | init_file | "
+                  "Status | Solve time | Cost (tau cost)\n";
+          trajOptGivenModel(stride_length_0, ground_incline_0,
+                            turning_rate_0, dir, traj_opt_num);
+        }
+        //search along the direction
+        else{
+          //normalize the direction vector
+          cout << "Start searching along direction: ["<<extend_direction[0]
+            <<","<<extend_direction[1]<<","<<extend_direction[2]<<"]"<<endl;
+          extend_direction = extend_direction/extend_direction.norm();
+          step = delta.array()*extend_direction.array();
+          boundary_for_one_direction(dir,dimensions,max_iter,
+          initial_gamma,step,cost_threshold,traj_opt_num,boundary_sample_num);
+        }
+      }
+    }
+  }
 
   return 0;
 }

From e02e64479c8539548a90c817aef9a5859c885a40 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Thu, 28 May 2020 13:06:51 -0400
Subject: [PATCH 08/82] debug for initial guess

---
 examples/goldilocks_models/find_boundary.cc | 118 +++++++++++---------
 1 file changed, 66 insertions(+), 52 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 2456396426..745fad65f1 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -79,7 +79,7 @@ DEFINE_bool(is_add_tau_in_cost, true, "Add RoM input in the cost function");
 //outer loop
 DEFINE_int32(max_outer_iter, 50 , "max number of iterations for searching on each "
                                 "direction of one dimension");
-DEFINE_double(max_cost_threshold, 20, "max cost used for judging "
+DEFINE_double(max_cost_threshold, 15, "max cost used for judging "
                                        "the quality of solutions");
 
 //others
@@ -196,56 +196,62 @@ void setInitialTheta(VectorXd& theta_s, VectorXd& theta_sDDot,
 }
 
 //use interpolation to set the initial guess for the trajectory optimization
-string getInitFileName(const string directory, int traj_opt_num){
+string getInitFileName(const string directory, int traj_opt_num,
+    bool is_rerun){
   int gamma_dimension = 3;
   VectorXd current_gamma = readCSV(directory + to_string(traj_opt_num)
                                        + string("_0_gamma.csv"));
   string initial_file_name;
-  if(traj_opt_num==0){
-    initial_file_name = "";
+  if(is_rerun){
+    initial_file_name = to_string(traj_opt_num)
+        + string("_0_w.csv");;
   }else{
-    VectorXd initial_guess;
-    //take out corresponding w and calculate the weight for interpolation
-    MatrixXd w_gamma;
-    VectorXd weight_gamma;
-    //paras used to decide gamma scale
-    double delta_sl = 0.015;
-    double delta_gi = 0.05;
-    double delta_tr = 0.125;
-    //calculate the weighted sum of past solutions
-    int sample_num = 0;
-    for (sample_num = 0; sample_num < traj_opt_num; sample_num++) {
-      //check if this sample is success
-      int is_success = (readCSV(directory + to_string(sample_num)
-          + string("_0_is_success.csv")))(0,0);
-      if(is_success == 1) {
-        //extract past gamma
-        VectorXd past_gamma = readCSV(directory + to_string(traj_opt_num)
-                                       + string("_0_gamma.csv"));
-        //calculate the weight for each sample using the third power of the difference between gamma
-        VectorXd gamma_scale(gamma_dimension);
-        gamma_scale << 1/delta_gi,1/delta_sl,1.3/delta_tr;
-        VectorXd dif_gamma = (past_gamma - current_gamma).array().abs()*gamma_scale.array();
-        VectorXd dif_gamma2 = dif_gamma.array().pow(2);
-        double distance_gamma =  (dif_gamma.transpose() * dif_gamma2)(0,0);
-        //extract the solution of this sample
-        VectorXd w_to_interpolate = readCSV(directory + to_string(sample_num)
-            + string("_0_w.csv"));
-        //concatenate the weight and solution for further calculation
-        w_gamma.conservativeResize(w_to_interpolate.rows(),w_gamma.cols()+1);
-        w_gamma.col(w_gamma.cols()-1) = w_to_interpolate;
-        weight_gamma.conservativeResize(weight_gamma.rows()+1);
-        weight_gamma(weight_gamma.rows()-1)= 1 /distance_gamma;
+    if(traj_opt_num==0){
+      initial_file_name = "";
+    }else{
+      VectorXd initial_guess;
+      //take out corresponding w and calculate the weight for interpolation
+      MatrixXd w_gamma;
+      VectorXd weight_gamma;
+      //paras used to decide gamma scale
+      double delta_sl = 0.015;
+      double delta_gi = 0.05;
+      double delta_tr = 0.125;
+      //calculate the weighted sum of past solutions
+      int sample_num = 0;
+      for (sample_num = 0; sample_num < traj_opt_num; sample_num++) {
+        //check if this sample is success
+        int is_success = (readCSV(directory + to_string(sample_num)
+                                      + string("_0_is_success.csv")))(0,0);
+        if(is_success == 1) {
+          //extract past gamma
+          VectorXd past_gamma = readCSV(directory + to_string(sample_num)
+                                            + string("_0_gamma.csv"));
+          //calculate the weight for each sample using the third power of the difference between gamma
+          VectorXd gamma_scale(gamma_dimension);
+          gamma_scale << 1/delta_gi,1/delta_sl,1.3/delta_tr;
+          VectorXd dif_gamma = (past_gamma - current_gamma).array().abs()*gamma_scale.array();
+          VectorXd dif_gamma2 = dif_gamma.array().pow(2);
+          double distance_gamma =  (dif_gamma.transpose() * dif_gamma2)(0,0);
+          //extract the solution of this sample
+          VectorXd w_to_interpolate = readCSV(directory + to_string(sample_num)
+                                                  + string("_0_w.csv"));
+          //concatenate the weight and solution for further calculation
+          w_gamma.conservativeResize(w_to_interpolate.rows(),w_gamma.cols()+1);
+          w_gamma.col(w_gamma.cols()-1) = w_to_interpolate;
+          weight_gamma.conservativeResize(weight_gamma.rows()+1);
+          weight_gamma(weight_gamma.rows()-1)= 1 /distance_gamma;
+        }
       }
+      DRAKE_DEMAND(weight_gamma.rows()>0);
+      //    normalize weight
+      weight_gamma = weight_gamma / weight_gamma.sum();
+      initial_guess = w_gamma * weight_gamma;
+      //    save initial guess and set init file
+      initial_file_name = to_string(traj_opt_num)
+          + string("_0_initial_guess.csv");
+      writeCSV(directory + initial_file_name, initial_guess);
     }
-
-    //    normalize weight
-    weight_gamma = weight_gamma / weight_gamma.sum();
-    initial_guess = w_gamma * weight_gamma;
-    //    save initial guess and set init file
-    initial_file_name = to_string(traj_opt_num)
-        + string("_0_initial_guess.csv");
-    writeCSV(directory + initial_file_name, initial_guess);
   }
 
   return initial_file_name;
@@ -253,7 +259,7 @@ string getInitFileName(const string directory, int traj_opt_num){
 
 // trajectory optimization for given task and model
 void trajOptGivenModel(double stride_length, double ground_incline,
-    double turning_rate,const string dir,int num){
+    double turning_rate,const string dir,int num,bool is_rerun){
   // Create MBP
   MultibodyPlant<double> plant(0.0);
   createMBP(&plant, FLAGS_robot_option);
@@ -326,7 +332,7 @@ void trajOptGivenModel(double stride_length, double ground_incline,
   int max_inner_iter_pass_in = is_get_nominal ? 200 : max_inner_iter;
 
 //  string init_file_pass_in = "";
-  string init_file_pass_in = getInitFileName(dir, num);
+  string init_file_pass_in = getInitFileName(dir, num, is_rerun);
   int sample_idx = 0;
   string prefix = to_string(num) +  "_" + to_string(sample_idx) + "_";
 
@@ -415,6 +421,7 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
     int& traj_num,int& boundary_point_idx){
   int iter;
   int sample_idx = 0;
+  bool rerun = false;
   VectorXd new_gamma(dims);
   VectorXd boundary_point(dims);
   cout << "sample# (rerun #) | stride | incline | turning | init_file | "
@@ -428,8 +435,15 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
         string("gamma.csv"), new_gamma);
     //run trajectory optimization and judge the solution
     trajOptGivenModel(new_gamma[0], new_gamma[1],
-        new_gamma[2], dir, traj_num);
-
+        new_gamma[2], dir, traj_num, rerun);
+    //check if snopt find a solution successfully. If not, rerun the Traj Opt
+    int is_success = (readCSV(dir + prefix + string("is_success.csv")))(0, 0);
+    if(is_success==0){
+      rerun = true;
+      trajOptGivenModel(new_gamma[0], new_gamma[1],
+                        new_gamma[2], dir, traj_num,rerun);
+      rerun = false;
+    }
     double sample_cost =
         (readCSV(dir + prefix + string("c.csv")))(0, 0);
 
@@ -473,7 +487,7 @@ int find_boundary(int argc, char* argv[]){
   int dimensions = 3;//dimension of the task space
   double stride_length_0 = 0;
   if(FLAGS_robot_option==0){
-    stride_length_0 = 0.25;
+    stride_length_0 = 0.3;
   }
   else if(FLAGS_robot_option==1){
       stride_length_0 = 0.2;
@@ -541,14 +555,14 @@ int find_boundary(int argc, char* argv[]){
           cout << "sample# (rerun #) | stride | incline | turning | init_file | "
                   "Status | Solve time | Cost (tau cost)\n";
           trajOptGivenModel(stride_length_0, ground_incline_0,
-                            turning_rate_0, dir, traj_opt_num);
+                            turning_rate_0, dir, traj_opt_num, false);
         }
         //search along the direction
         else{
-          //normalize the direction vector
           cout << "Start searching along direction: ["<<extend_direction[0]
             <<","<<extend_direction[1]<<","<<extend_direction[2]<<"]"<<endl;
-          extend_direction = extend_direction/extend_direction.norm();
+          //normalize the direction vector
+          //extend_direction = extend_direction/extend_direction.norm();
           step = delta.array()*extend_direction.array();
           boundary_for_one_direction(dir,dimensions,max_iter,
           initial_gamma,step,cost_threshold,traj_opt_num,boundary_sample_num);

From df4eac545bc5e638cd344afe0baad89c1522be74 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Thu, 28 May 2020 17:02:12 -0400
Subject: [PATCH 09/82] add filter for directions

---
 examples/goldilocks_models/find_boundary.cc | 57 +++++++++++++--------
 1 file changed, 35 insertions(+), 22 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 745fad65f1..d4deac0e20 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -428,6 +428,19 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
           "Status | Solve time | Cost (tau cost)\n";
   for (iter = 1; iter <= max_iteration; iter++){
     new_gamma = init_gamma + iter*step;
+    //if stride length is negative,stop searching
+    if(new_gamma[0]<0){
+      boundary_point_idx += 1;
+      boundary_point = new_gamma-step;
+      writeCSV(dir + to_string(boundary_point_idx) +  "_" +
+          string("boundary_point.csv"), boundary_point);
+      cout << "boundary point index | stride length | ground incline"
+              " | turning rate"<<endl;
+      cout<<" \t "<<boundary_point_idx<< "\t" <<" | "<<"\t"<<boundary_point[0]
+          <<"\t"<<" | "<<"\t"<<boundary_point[1]<<"\t"<<" | "<<"\t"<<
+          boundary_point[2]<<endl;
+      break;
+    }
     //store stride length, ground incline and turning rate
     traj_num += 1;
     string prefix = to_string(traj_num) +  "_" + to_string(sample_idx) + "_";
@@ -487,7 +500,7 @@ int find_boundary(int argc, char* argv[]){
   int dimensions = 3;//dimension of the task space
   double stride_length_0 = 0;
   if(FLAGS_robot_option==0){
-    stride_length_0 = 0.3;
+    stride_length_0 = 0.2;
   }
   else if(FLAGS_robot_option==1){
       stride_length_0 = 0.2;
@@ -522,13 +535,13 @@ int find_boundary(int argc, char* argv[]){
   VectorXd extend_direction(dimensions);//the direction of searching
 
   //create components for each dimension used to decide the direction
-  int sl_comp_num = 3;
+  int sl_comp_num = 5;
   VectorXd e_sl(sl_comp_num);
-  e_sl<<0,-1,1;
+  e_sl<<0,-1,-0.5,0.5,1;
 
-  int gi_comp_num = 3;
+  int gi_comp_num = 5;
   VectorXd e_gi(gi_comp_num);
-  e_gi<<0,-1,1;
+  e_gi<<0,-1,-0.5,0.5,1;
 
   int tr_comp_num = 1;
   VectorXd e_tr(tr_comp_num);
@@ -537,35 +550,35 @@ int find_boundary(int argc, char* argv[]){
   /*
    * start iteration
    */
+  //evaluate initial point
+  cout << "\nCalculate Central Point Cost:\n";
+  writeCSV(dir +  to_string(traj_opt_num)  +
+      string("_0_gamma.csv"), initial_gamma);
+  cout << "sample# (rerun #) | stride | incline | turning | init_file | "
+          "Status | Solve time | Cost (tau cost)\n";
+  trajOptGivenModel(stride_length_0, ground_incline_0,
+                    turning_rate_0, dir, traj_opt_num, false);
+  //rerun the Traj Opt to avoid being stuck in local minimum
+  trajOptGivenModel(stride_length_0, ground_incline_0,
+                    turning_rate_0, dir, traj_opt_num, true);
+
   int dim1,dim2,dim3;
   VectorXd step(dimensions);
   //for all the direction, search the boundary
   for (dim1=0;dim1<sl_comp_num;dim1++){
     for(dim2=0;dim2<gi_comp_num;dim2++){
       for(dim3=0;dim3<tr_comp_num;dim3++){
-        extend_direction[0] = e_sl[dim1];
-        extend_direction[1] = e_gi[dim2];
-        extend_direction[2] = e_tr[dim3];
-        //direction elements all zero. evaluate initial sample
-        if((extend_direction[0]==0)&&(extend_direction[1]==0)
-          &&(extend_direction[2]==0)){
-          cout << "\nCalculate Central Point Cost:\n";
-          writeCSV(dir +  to_string(traj_opt_num)  +
-              string("_0_gamma.csv"), initial_gamma);
-          cout << "sample# (rerun #) | stride | incline | turning | init_file | "
-                  "Status | Solve time | Cost (tau cost)\n";
-          trajOptGivenModel(stride_length_0, ground_incline_0,
-                            turning_rate_0, dir, traj_opt_num, false);
-        }
+        extend_direction << e_sl[dim1],e_gi[dim2],e_tr[dim3];
+        //filter with infinity norm to avoid repetitive searching
         //search along the direction
-        else{
+        if( (extend_direction.lpNorm<Eigen::Infinity>()) >=1){
           cout << "Start searching along direction: ["<<extend_direction[0]
-            <<","<<extend_direction[1]<<","<<extend_direction[2]<<"]"<<endl;
+               <<","<<extend_direction[1]<<","<<extend_direction[2]<<"]"<<endl;
           //normalize the direction vector
           //extend_direction = extend_direction/extend_direction.norm();
           step = delta.array()*extend_direction.array();
           boundary_for_one_direction(dir,dimensions,max_iter,
-          initial_gamma,step,cost_threshold,traj_opt_num,boundary_sample_num);
+                                     initial_gamma,step,cost_threshold,traj_opt_num,boundary_sample_num);
         }
       }
     }

From fdbadbcd731cac2293d3c418ec52f770f01e75cd Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Thu, 28 May 2020 19:18:13 -0400
Subject: [PATCH 10/82] small update

---
 examples/goldilocks_models/find_boundary.cc | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index d4deac0e20..23fce68658 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -428,8 +428,8 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
           "Status | Solve time | Cost (tau cost)\n";
   for (iter = 1; iter <= max_iteration; iter++){
     new_gamma = init_gamma + iter*step;
-    //if stride length is negative,stop searching
-    if(new_gamma[0]<0){
+    //if stride length is negative or zero,stop searching
+    if(new_gamma[0]<=0){
       boundary_point_idx += 1;
       boundary_point = new_gamma-step;
       writeCSV(dir + to_string(boundary_point_idx) +  "_" +

From 96807a50e2b6903dca6eb992f92957fb47d7a03c Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Thu, 28 May 2020 21:41:53 -0400
Subject: [PATCH 11/82] update function used for visualization

---
 examples/goldilocks_models/find_boundary.cc   |   7 +
 .../find_boundary/BUILD.bazel                 | 136 ++++++++
 .../find_boundary/visualize_gait_MBP.cc       | 318 ++++++++++++++++++
 3 files changed, 461 insertions(+)
 create mode 100644 examples/goldilocks_models/find_boundary/BUILD.bazel
 create mode 100644 examples/goldilocks_models/find_boundary/visualize_gait_MBP.cc

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 23fce68658..d4464b3d96 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -530,6 +530,13 @@ int find_boundary(int argc, char* argv[]){
    */
   int max_iter = FLAGS_max_outer_iter;
   double cost_threshold = FLAGS_max_cost_threshold;
+  if(FLAGS_robot_option=0)
+  {
+    cost_threshold = 15;
+  }
+  else{
+    cost_threshold = 10;
+  }
   int boundary_sample_num = 0;//use this to set the index for boundary point
   int traj_opt_num = 0;//use this to set the index for Traj Opt
   VectorXd extend_direction(dimensions);//the direction of searching
diff --git a/examples/goldilocks_models/find_boundary/BUILD.bazel b/examples/goldilocks_models/find_boundary/BUILD.bazel
new file mode 100644
index 0000000000..e44136ded7
--- /dev/null
+++ b/examples/goldilocks_models/find_boundary/BUILD.bazel
@@ -0,0 +1,136 @@
+package(default_visibility = ["//visibility:public"])
+#
+#cc_binary(
+#    name = "visualize_gait_RBP",
+#    srcs = ["visualize_gait_RBP.cc"],
+#    data = glob(["examples/goldilocks_models/PlanarWalkerWithTorso.urdf"]),
+#    deps = [
+#        "//systems/goldilocks_models",
+#        "@drake//:drake_shared_library",
+#        "@gflags",
+#    ],
+#)
+
+cc_binary(
+    name = "visualize_gait_MBP",
+    srcs = ["visualize_gait_MBP.cc"],
+    data = glob(["examples/goldilocks_models/PlanarWalkerWithTorso.urdf"]),
+    deps = [
+        "//common",
+        "//systems/goldilocks_models",
+        "//systems/primitives",
+        "//systems/trajectory_optimization:dircon",
+        "@drake//:drake_shared_library",
+        "@gflags",
+    ],
+)
+
+#cc_library(
+#    name = "goldilocks_model_traj_opt",
+#    srcs = ["goldilocks_model_traj_opt.cc"],
+#    hdrs = ["goldilocks_model_traj_opt.h"],
+#    deps = [
+#        "//common",
+#        "//examples/goldilocks_models/find_models:dynamics_constraint",
+#        "//examples/goldilocks_models/find_models:kinematics_constraint",
+#        "//solvers:optimization_utils",
+#        "//systems/goldilocks_models",
+#        "//systems/primitives",
+#        "//systems/trajectory_optimization:dircon",
+#        "@drake//:drake_shared_library",
+#    ],
+#)
+
+#cc_library(
+#    name = "traj_opt_given_weigths",
+#    srcs = ["traj_opt_given_weigths.cc"],
+#    hdrs = ["traj_opt_given_weigths.h"],
+#    deps = [
+#        ":traj_opt_helper_func",
+#        "//common",
+#        "//examples/goldilocks_models:dynamics_expression",
+#        "//examples/goldilocks_models:goldilocks_utils",
+#        "//examples/goldilocks_models/find_models:goldilocks_model_traj_opt",
+#        "//solvers:nonlinear_constraint",
+#        "//solvers:optimization_utils",
+#        "//systems/goldilocks_models",
+#        "//systems/primitives",
+#        "//systems/trajectory_optimization:dircon",
+#        "@drake//:drake_shared_library",
+#    ],
+#)
+##        "@gurobi//:gurobi_cxx",
+#
+#cc_library(
+#    name = "kinematics_constraint",
+#    srcs = [
+#        "kinematics_constraint.cc",
+#    ],
+#    hdrs = [
+#        "kinematics_constraint.h",
+#    ],
+#    deps = [
+#        "//examples/goldilocks_models:kinematics_expression",
+#        "//multibody:utils",
+#        "@drake//:drake_shared_library",
+#    ],
+#)
+#
+#cc_library(
+#    name = "dynamics_constraint",
+#    srcs = [
+#        "dynamics_constraint.cc",
+#    ],
+#    hdrs = [
+#        "dynamics_constraint.h",
+#    ],
+#    deps = [
+#        "//examples/goldilocks_models:dynamics_expression",
+#        "//examples/goldilocks_models:kinematics_expression",
+#        "//multibody:utils",
+#        "//solvers:nonlinear_constraint",
+#        "//systems/trajectory_optimization:dircon",
+#        "@drake//:drake_shared_library",
+#    ],
+#)
+#
+#cc_binary(
+#    name = "scale_theta",
+#    srcs = ["scale_theta.cc"],
+#    deps = [
+#        "//common",
+#        "//examples/goldilocks_models:dynamics_expression",
+#        "//examples/goldilocks_models:goldilocks_utils",
+#        "//examples/goldilocks_models:kinematics_expression",
+#        "//systems/goldilocks_models",
+#        "@drake//:drake_shared_library",
+#        "@gflags",
+#    ],
+#)
+#
+#cc_binary(
+#    name = "postadd_cost_without_tau",
+#    srcs = ["postadd_cost_without_tau.cc"],
+#    deps = [
+#        "//systems/goldilocks_models",
+#        "@drake//:drake_shared_library",
+#        "@gflags",
+#    ],
+#)
+#
+#cc_library(
+#    name = "traj_opt_helper_func",
+#    srcs = [
+#        "traj_opt_helper_func.cc",
+#    ],
+#    hdrs = [
+#        "traj_opt_helper_func.h",
+#    ],
+#    deps = [
+#        "//common",
+#        "//multibody:utils",
+#        "//solvers:optimization_utils",
+#        "//systems/trajectory_optimization:dircon",
+#        "@drake//:drake_shared_library",
+#    ],
+#)
diff --git a/examples/goldilocks_models/find_boundary/visualize_gait_MBP.cc b/examples/goldilocks_models/find_boundary/visualize_gait_MBP.cc
new file mode 100644
index 0000000000..f80bcfbafa
--- /dev/null
+++ b/examples/goldilocks_models/find_boundary/visualize_gait_MBP.cc
@@ -0,0 +1,318 @@
+#include <gflags/gflags.h>
+
+#include <memory>
+#include <chrono>
+
+#include <string>
+
+#include "drake/systems/analysis/simulator.h"
+#include "drake/systems/framework/diagram.h"
+#include "drake/systems/framework/diagram_builder.h"
+#include "drake/systems/primitives/trajectory_source.h"
+
+#include "drake/lcm/drake_lcm.h"
+
+#include "drake/multibody/parsing/parser.h"
+#include "drake/systems/rendering/multibody_position_to_geometry_pose.h"
+#include "drake/geometry/geometry_visualization.h"
+
+#include "common/find_resource.h"
+#include "systems/primitives/subvector_pass_through.h"
+
+#include "multibody/multibody_utils.h"
+#include "multibody/visualization_utils.h"
+
+#include "drake/systems/analysis/simulator.h"
+#include "drake/systems/framework/diagram.h"
+#include "drake/systems/framework/diagram_builder.h"
+#include "drake/systems/primitives/trajectory_source.h"
+#include "drake/lcm/drake_lcm.h"
+
+#include "drake/common/trajectories/piecewise_polynomial.h"
+
+#include "systems/goldilocks_models/file_utils.h"
+
+using drake::multibody::MultibodyPlant;
+using drake::geometry::SceneGraph;
+using drake::multibody::Body;
+using drake::multibody::Parser;
+using drake::systems::rendering::MultibodyPositionToGeometryPose;
+
+using Eigen::Vector3d;
+using Eigen::VectorXd;
+using Eigen::MatrixXd;
+using Eigen::Matrix3Xd;
+using drake::trajectories::PiecewisePolynomial;
+using drake::MatrixX;
+using std::vector;
+using std::shared_ptr;
+using std::cout;
+using std::endl;
+using std::string;
+using std::to_string;
+
+namespace dairlib {
+
+DEFINE_int32(iter_start, 1, "The iter #");
+DEFINE_int32(iter_end, -1, "The iter #");
+DEFINE_int32(batch, 0, "The batch #");
+DEFINE_double(realtime_factor, 1, "Rate of which the traj is played back");
+DEFINE_int32(n_step, 3, "# of foot steps");
+
+DEFINE_int32(robot_option, 1, "0: plannar robot. 1: cassie_fixed_spring");
+
+DEFINE_bool(construct_cubic, false,
+            "True if you want to construct cubic spline. Old files (before "
+            "2019.12.31) didn't store derivatives information, so this option "
+            "cannot be used on those files.");
+
+    void swapTwoBlocks(MatrixXd* mat, int i_1, int j_1, int i_2, int j_2,
+                       int n_row, int n_col) {
+  MatrixXd temp_block1 = mat->block(i_1, j_1, n_row, n_col);
+  MatrixXd temp_block2 = mat->block(i_2, j_2, n_row, n_col);
+  mat->block(i_1, j_1, n_row, n_col) = temp_block2;
+  mat->block(i_2, j_2, n_row, n_col) = temp_block1;
+}
+
+void visualizeGait(int argc, char* argv[]) {
+  gflags::ParseCommandLineFlags(&argc, &argv, true);
+
+  // parameters
+  int iter_start = FLAGS_iter_start;
+  int iter_end = (FLAGS_iter_end >= FLAGS_iter_start) ?
+                 FLAGS_iter_end : FLAGS_iter_start;
+  int n_step = FLAGS_n_step;  // Should be > 0
+  /*const string directory = "examples/goldilocks_models/find_models/data/robot_"
+                           + to_string(FLAGS_robot_option) + "/";*/
+  const string directory = "../dairlib_data/goldilocks_models/find_boundary/robot_" +
+      to_string(FLAGS_robot_option) + "/";
+
+  // Looping through the iterations
+  for (int iter = iter_start; iter <= iter_end; iter++) {
+    // Read in ground incline
+    double ground_incline = goldilocks_models::readCSV(
+        directory + to_string(iter) + string("_") + to_string(FLAGS_batch) +
+        string("_gamma.csv"))(1);
+
+    // Read in trajecotry
+    VectorXd time_mat;
+    MatrixXd state_mat;
+    MatrixXd statedot_mat;
+    if (!FLAGS_construct_cubic) {
+      time_mat = goldilocks_models::readCSV(
+          directory + to_string(iter) + string("_") + to_string(FLAGS_batch) +
+          string("_time_at_knots.csv"));
+      state_mat = goldilocks_models::readCSV(
+          directory + to_string(iter) + string("_") + to_string(FLAGS_batch) +
+          string("_state_at_knots.csv"));
+    } else {
+      time_mat = goldilocks_models::readCSV(
+          directory + to_string(iter) + string("_") + to_string(FLAGS_batch) +
+          string("_t_cubic_spline.csv"));
+      state_mat = goldilocks_models::readCSV(
+          directory + to_string(iter) + string("_") + to_string(FLAGS_batch) +
+          string("_x_cubic_spline.csv"));
+      statedot_mat = goldilocks_models::readCSV(
+          directory + to_string(iter) + string("_") + to_string(FLAGS_batch) +
+          string("_xdot_cubic_spline.csv"));
+    }
+
+    int n_state = state_mat.rows();
+    int n_q;
+    if (FLAGS_robot_option == 0) {
+      n_q = n_state / 2;
+    } else if (FLAGS_robot_option == 1) {
+      n_q = 19;
+    } else {
+      n_q = -1;
+      DRAKE_DEMAND(false); // Shouldn't come here
+    }
+    int n_node = time_mat.rows();
+    VectorXd ones = VectorXd::Ones(n_node - 1);
+    int translation_size;
+    int translation_start_idx;
+    if (FLAGS_robot_option == 0) {
+      translation_size = 2;
+      translation_start_idx = 0;
+    } else if (FLAGS_robot_option == 1) {
+      translation_size = 3;
+      translation_start_idx = 4;
+    } else {
+      translation_size = -1;
+      translation_start_idx = -1;
+      DRAKE_DEMAND(false); // Shouldn't come here
+    }
+    VectorXd xyz_translation =
+      state_mat.block(translation_start_idx, n_node - 1, translation_size, 1)
+      - state_mat.block(translation_start_idx, 0, translation_size, 1);
+
+    // Concatenate the traj so it has multiple steps
+    // 1. time
+    VectorXd time_mat_cat(n_step * n_node - (n_step - 1));
+    time_mat_cat(0) = 0;
+    for (int i = 0; i < n_step; i++) {
+      time_mat_cat.segment(1 + (n_node - 1)*i, n_node - 1) =
+        time_mat.tail(n_node - 1) + time_mat_cat((n_node - 1) * i) * ones;
+    }
+    // 2. state (and its derivatives)
+    std::vector<MatrixXd> mat_cat;  // first element is state (and second
+                                    // element is its derivatives)
+    mat_cat.push_back(MatrixXd(n_state, n_step * n_node - (n_step - 1)));
+    if (FLAGS_construct_cubic) {
+      mat_cat.push_back(MatrixXd(n_state, n_step * n_node - (n_step - 1)));
+    }
+    mat_cat[0].col(0) = state_mat.col(0);
+    if (FLAGS_construct_cubic) {
+      mat_cat[1].col(0) = statedot_mat.col(0);
+    }
+    for (int i = 0; i < n_step; i++) {
+      // Copy over the data at all knots but the first one
+      mat_cat[0].block(0, 1 + (n_node - 1)*i, n_state, n_node - 1) =
+          state_mat.block(0, 1, n_state, n_node - 1);
+      if (FLAGS_construct_cubic) {
+        mat_cat[1].block(0, 1 + (n_node - 1)*i, n_state, n_node - 1) =
+            statedot_mat.block(0, 1, n_state, n_node - 1);
+      }
+
+      // Translate x and z (only for position not its derivatives)
+      if (FLAGS_robot_option == 0) {
+        for (int j = 0; j < translation_size; j++) {
+          mat_cat[0].block(j, 1 + (n_node - 1)*i, 1, n_node - 1)
+            = state_mat.block(j, 1, 1, n_node - 1)  +
+              i * xyz_translation(j) * ones.transpose();
+        }
+      } else if (FLAGS_robot_option == 1) {
+        if (i > 0) {
+          for (int j = 0; j < translation_size; j++) {
+            if (j == 1) {
+              // It's mirror in x-z plane, so we don't need to translate in y here.
+            } else {
+              mat_cat[0].block(j + translation_start_idx,
+                                  1 + (n_node - 1)*i, 1, n_node - 1)
+                = state_mat.block(j + translation_start_idx, 1, 1, n_node - 1)  +
+                  i * xyz_translation(j) * ones.transpose();
+            }
+          }
+        }
+      }
+
+      // Flip the sign for the even number of stance phase
+      if (i % 2) {
+        if (FLAGS_robot_option == 1) {
+          for (auto& mat_cat_member : mat_cat) {
+            // Quaternion sign should also be flipped.
+            mat_cat_member.block(1, 1 + (n_node - 1) * i, 1, n_node - 1) *= -1;
+            mat_cat_member.block(3, 1 + (n_node - 1) * i, 1, n_node - 1) *= -1;
+            // y should be flipped
+            mat_cat_member.block(5, 1 + (n_node - 1) * i, 1, n_node - 1) *= -1;
+            // Hip roll and yaw sign should also be flipped.
+            mat_cat_member.block(7, 1 + (n_node - 1) * i, 4, n_node - 1) *= -1;
+
+            // We do not flip the sign for the velocity part of state because
+            // it's not used in visualization
+          }
+        }
+      }
+
+      // Swap the leg
+      if (i % 2) {
+        for (auto& mat_cat_member : mat_cat) {
+          if (FLAGS_robot_option == 0) {
+            // Position
+            swapTwoBlocks(&mat_cat_member, 3, 1 + (n_node - 1) * i, 4,
+                          1 + (n_node - 1) * i, 1, n_node - 1);
+            swapTwoBlocks(&mat_cat_member, 5, 1 + (n_node - 1) * i, 6,
+                          1 + (n_node - 1) * i, 1, n_node - 1);
+
+            // We do not swap the velocity part because it's not used in
+            // visualization
+          } else if (FLAGS_robot_option == 1) {
+            // Position
+            swapTwoBlocks(&mat_cat_member, 7, 1 + (n_node - 1) * i, 8,
+                          1 + (n_node - 1) * i, 1, n_node - 1);
+            swapTwoBlocks(&mat_cat_member, 9, 1 + (n_node - 1) * i, 10,
+                          1 + (n_node - 1) * i, 1, n_node - 1);
+            swapTwoBlocks(&mat_cat_member, 11, 1 + (n_node - 1) * i, 12,
+                          1 + (n_node - 1) * i, 1, n_node - 1);
+            swapTwoBlocks(&mat_cat_member, 13, 1 + (n_node - 1) * i, 14,
+                          1 + (n_node - 1) * i, 1, n_node - 1);
+            swapTwoBlocks(&mat_cat_member, 15, 1 + (n_node - 1) * i, 16,
+                          1 + (n_node - 1) * i, 1, n_node - 1);
+            swapTwoBlocks(&mat_cat_member, 17, 1 + (n_node - 1) * i, 18,
+                          1 + (n_node - 1) * i, 1, n_node - 1);
+
+            // We do not swap the velocity part because it's not used in
+            // visualization
+          }
+        }
+      }  // end if (i % 2)
+    }  // end for n_step
+
+    // Create a testing piecewise polynomial
+    std::vector<double> T_breakpoint;
+    for (int i = 0; i < time_mat_cat.size(); i++)
+      T_breakpoint.push_back(time_mat_cat(i));
+    std::vector<MatrixXd> Y;
+    for (int i = 0; i < time_mat_cat.size(); i++)
+      Y.push_back(mat_cat[0].col(i));
+    std::vector<MatrixXd> Y_dot;
+    if (FLAGS_construct_cubic) {
+      for (int i = 0; i < time_mat_cat.size(); i++)
+        Y_dot.push_back(mat_cat[1].col(i));
+    }
+    PiecewisePolynomial<double> pp_xtraj =
+        (FLAGS_construct_cubic)
+            ? PiecewisePolynomial<double>::CubicHermite(T_breakpoint, Y, Y_dot)
+            : PiecewisePolynomial<double>::FirstOrderHold(T_breakpoint, Y);
+
+    // Create MBP
+    drake::systems::DiagramBuilder<double> builder;
+    MultibodyPlant<double> plant(0.0);
+    SceneGraph<double>& scene_graph = *builder.AddSystem<SceneGraph>();
+    Vector3d ground_normal(sin(ground_incline), 0, cos(ground_incline));
+    multibody::addFlatTerrain(&plant, &scene_graph, 0.8, 0.8, ground_normal);
+    Parser parser(&plant, &scene_graph);
+    std::string full_name;
+    if (FLAGS_robot_option == 0) {
+      full_name = FindResourceOrThrow(
+                    "examples/goldilocks_models/PlanarWalkerWithTorso.urdf");
+    } else if (FLAGS_robot_option == 1) {
+      full_name = FindResourceOrThrow(
+                    "examples/Cassie/urdf/cassie_fixed_springs.urdf");
+    }
+    parser.AddModelFromFile(full_name);
+    plant.mutable_gravity_field().set_gravity_vector(
+      -9.81 * Eigen::Vector3d::UnitZ());
+    if (FLAGS_robot_option == 0) {
+      plant.WeldFrames(
+        plant.world_frame(), plant.GetFrameByName("base"),
+        drake::math::RigidTransform<double>());
+    }
+    plant.Finalize();
+
+    // visualizer
+    int n_loops = 1;
+    multibody::connectTrajectoryVisualizer(&plant, &builder, &scene_graph,
+                                           pp_xtraj);
+    auto diagram = builder.Build();
+    // while (true)
+    for (int i = 0; i < n_loops; i++) {
+      drake::systems::Simulator<double> simulator(*diagram);
+      simulator.set_target_realtime_rate(FLAGS_realtime_factor);
+      simulator.Initialize();
+      simulator.AdvanceTo(pp_xtraj.end_time());
+    }
+  }  // end for(int iter...)
+
+
+  return;
+}
+} // dairlib
+
+int main(int argc, char* argv[]) {
+
+  dairlib::visualizeGait(argc, argv);
+
+  return 0;
+}
+

From 0493f08be88d30c328a105c22b86667be1873b1f Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Fri, 29 May 2020 09:44:38 -0400
Subject: [PATCH 12/82] different threshold of cost for different robots

---
 examples/goldilocks_models/find_boundary.cc | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index d4464b3d96..e958632b3d 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -79,8 +79,6 @@ DEFINE_bool(is_add_tau_in_cost, true, "Add RoM input in the cost function");
 //outer loop
 DEFINE_int32(max_outer_iter, 50 , "max number of iterations for searching on each "
                                 "direction of one dimension");
-DEFINE_double(max_cost_threshold, 15, "max cost used for judging "
-                                       "the quality of solutions");
 
 //others
 DEFINE_string(
@@ -529,7 +527,7 @@ int find_boundary(int argc, char* argv[]){
    * Iteration setting
    */
   int max_iter = FLAGS_max_outer_iter;
-  double cost_threshold = FLAGS_max_cost_threshold;
+  double cost_threshold = 10;
   if(FLAGS_robot_option=0)
   {
     cost_threshold = 15;

From 75fd6c2f3a08a8bce1bcd361e0d759b7e48fd6f7 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Sun, 31 May 2020 21:39:15 -0400
Subject: [PATCH 13/82] upadte step size and set a flag to indicate calculating
 nominal cost

---
 examples/goldilocks_models/find_boundary.cc | 69 +++++++++++++++++----
 1 file changed, 57 insertions(+), 12 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index e958632b3d..1fe93c54ba 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -70,6 +70,8 @@ DEFINE_int32(
 "resolve again.");
 DEFINE_int32(n_node, -1, "# of nodes for traj opt");
 DEFINE_double(eps_regularization, 1e-8, "Weight of regularization term"); //1e-4
+DEFINE_bool(is_get_nominal,false,"is calculating the cost without ROM constraints");
+DEFINE_bool(use_optimized_model,false,"read theta from files to apply optimized model");
 
 //tasks
 DEFINE_bool(is_zero_touchdown_impact, false,
@@ -77,7 +79,7 @@ DEFINE_bool(is_zero_touchdown_impact, false,
 DEFINE_bool(is_add_tau_in_cost, true, "Add RoM input in the cost function");
 
 //outer loop
-DEFINE_int32(max_outer_iter, 50 , "max number of iterations for searching on each "
+DEFINE_int32(max_outer_iter, 100 , "max number of iterations for searching on each "
                                 "direction of one dimension");
 
 //others
@@ -193,6 +195,13 @@ void setInitialTheta(VectorXd& theta_s, VectorXd& theta_sDDot,
   }
 }
 
+//read theta from files to use optimized model
+void readThetaFromFiles(const string dir,int theta_idx,
+    VectorXd& theta_s, VectorXd& theta_sDDot){
+  theta_s = readCSV(dir + to_string(theta_idx) + string("_theta_s.csv"));
+  theta_sDDot = readCSV(dir + to_string(theta_idx) + string("_theta_sDDot.csv"));
+}
+
 //use interpolation to set the initial guess for the trajectory optimization
 string getInitFileName(const string directory, int traj_opt_num,
     bool is_rerun){
@@ -317,7 +326,16 @@ void trajOptGivenModel(double stride_length, double ground_incline,
   // Initial guess of theta
   theta_s = VectorXd::Zero(n_theta_s);
   theta_sDDot = VectorXd::Zero(n_theta_sDDot);
-  setInitialTheta(theta_s, theta_sDDot, n_feature_s, rom_option);
+  if(FLAGS_use_optimized_model){
+    //you have to specify the theta to use
+    int theta_idx = 100;
+    const string dir_find_models = "../dairlib_data/goldilocks_models/find_models/robot_" +
+        to_string(FLAGS_robot_option) + "/";
+    readThetaFromFiles(dir_find_models, theta_idx, theta_s, theta_sDDot);
+  }
+  else{
+    setInitialTheta(theta_s, theta_sDDot, n_feature_s, rom_option);
+  }
 
   double duration = 0.4;
   if (FLAGS_robot_option == 0) {
@@ -326,7 +344,7 @@ void trajOptGivenModel(double stride_length, double ground_incline,
     duration = 0.4; // 0.4;
   }
 
-  bool is_get_nominal = false;
+  bool is_get_nominal = FLAGS_is_get_nominal;
   int max_inner_iter_pass_in = is_get_nominal ? 200 : max_inner_iter;
 
 //  string init_file_pass_in = "";
@@ -415,17 +433,28 @@ double sample_result(double stride_length,double ground_incline,
 
 //search the boundary point along one direction
 void boundary_for_one_direction(const string dir,int dims,int max_iteration,
-    VectorXd init_gamma,VectorXd step,double max_cost,
-    int& traj_num,int& boundary_point_idx){
+    VectorXd init_gamma,VectorXd step_direction, VectorXd step_size,
+    double max_cost,int& traj_num,int& boundary_point_idx){
   int iter;
   int sample_idx = 0;
   bool rerun = false;
   VectorXd new_gamma(dims);
+  VectorXd last_gamma = init_gamma;
   VectorXd boundary_point(dims);
+  VectorXd step = step_size.array()*step_direction.array();
+  double decay_factor;//take a large step at the beginning
   cout << "sample# (rerun #) | stride | incline | turning | init_file | "
           "Status | Solve time | Cost (tau cost)\n";
   for (iter = 1; iter <= max_iteration; iter++){
-    new_gamma = init_gamma + iter*step;
+    decay_factor = 2*pow(0.95,iter);
+    if(decay_factor>1){
+      new_gamma = last_gamma+decay_factor*step;
+      last_gamma = new_gamma;
+    }
+    else{
+      new_gamma = last_gamma+step;
+      last_gamma = new_gamma;
+    }
     //if stride length is negative or zero,stop searching
     if(new_gamma[0]<=0){
       boundary_point_idx += 1;
@@ -475,6 +504,7 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
       break;
     }
   }
+  //TODO:check the adjacent sample to avoid being stuck in local minimum
 }
 
 int find_boundary(int argc, char* argv[]){
@@ -526,15 +556,30 @@ int find_boundary(int argc, char* argv[]){
   /*
    * Iteration setting
    */
+  cout << "\nIteration setting:\n";
+  cout<<"get nominal cost: "<<FLAGS_is_get_nominal<<endl;
+  cout<<"use optimized model: "<<FLAGS_use_optimized_model<<endl;
   int max_iter = FLAGS_max_outer_iter;
-  double cost_threshold = 10;
-  if(FLAGS_robot_option=0)
+  //TODO:decide the threshold under different situation
+  double cost_threshold = 15;
+  if(FLAGS_robot_option==0)
   {
-    cost_threshold = 15;
+    if(FLAGS_is_get_nominal){
+      cost_threshold = 25;
+    }
+    else{
+      cost_threshold = 20;
+    }
   }
   else{
-    cost_threshold = 10;
+    if(FLAGS_is_get_nominal){
+      cost_threshold = 20;
+    }
+    else{
+      cost_threshold = 15;
+    }
   }
+  cout<<"cost_threshold "<<cost_threshold<<endl;
   int boundary_sample_num = 0;//use this to set the index for boundary point
   int traj_opt_num = 0;//use this to set the index for Traj Opt
   VectorXd extend_direction(dimensions);//the direction of searching
@@ -581,9 +626,9 @@ int find_boundary(int argc, char* argv[]){
                <<","<<extend_direction[1]<<","<<extend_direction[2]<<"]"<<endl;
           //normalize the direction vector
           //extend_direction = extend_direction/extend_direction.norm();
-          step = delta.array()*extend_direction.array();
           boundary_for_one_direction(dir,dimensions,max_iter,
-                                     initial_gamma,step,cost_threshold,traj_opt_num,boundary_sample_num);
+              initial_gamma,extend_direction,delta,
+              cost_threshold,traj_opt_num,boundary_sample_num);
         }
       }
     }

From cbc356b4c4e73576d3ee8efd917e5d04751fb10e Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Mon, 1 Jun 2020 13:56:05 -0400
Subject: [PATCH 14/82] check the adjacent sample cost

---
 examples/goldilocks_models/find_boundary.cc | 51 ++++++++++++++++++---
 1 file changed, 44 insertions(+), 7 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 1fe93c54ba..7919928304 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -204,14 +204,22 @@ void readThetaFromFiles(const string dir,int theta_idx,
 
 //use interpolation to set the initial guess for the trajectory optimization
 string getInitFileName(const string directory, int traj_opt_num,
-    bool is_rerun){
+    bool is_rerun,int rerun_traj_idx=-1){
   int gamma_dimension = 3;
   VectorXd current_gamma = readCSV(directory + to_string(traj_opt_num)
                                        + string("_0_gamma.csv"));
   string initial_file_name;
   if(is_rerun){
-    initial_file_name = to_string(traj_opt_num)
-        + string("_0_w.csv");;
+    //if not specify which Traj Opt result to use, use its own result to rerun;
+    //else use the specified one.
+    if(rerun_traj_idx==-1) {
+      initial_file_name = to_string(traj_opt_num)
+          + string("_0_w.csv");
+    }
+    else{
+      initial_file_name = to_string(rerun_traj_idx)
+          + string("_0_w.csv");
+    }
   }else{
     if(traj_opt_num==0){
       initial_file_name = "";
@@ -266,7 +274,8 @@ string getInitFileName(const string directory, int traj_opt_num,
 
 // trajectory optimization for given task and model
 void trajOptGivenModel(double stride_length, double ground_incline,
-    double turning_rate,const string dir,int num,bool is_rerun){
+    double turning_rate,const string dir,int num,bool is_rerun,
+    int initial_guess_idx=-1){
   // Create MBP
   MultibodyPlant<double> plant(0.0);
   createMBP(&plant, FLAGS_robot_option);
@@ -348,7 +357,8 @@ void trajOptGivenModel(double stride_length, double ground_incline,
   int max_inner_iter_pass_in = is_get_nominal ? 200 : max_inner_iter;
 
 //  string init_file_pass_in = "";
-  string init_file_pass_in = getInitFileName(dir, num, is_rerun);
+  string init_file_pass_in = getInitFileName(dir, num, is_rerun,
+      initial_guess_idx);
   int sample_idx = 0;
   string prefix = to_string(num) +  "_" + to_string(sample_idx) + "_";
 
@@ -442,6 +452,7 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
   VectorXd last_gamma = init_gamma;
   VectorXd boundary_point(dims);
   VectorXd step = step_size.array()*step_direction.array();
+  VectorXd cost_list;
   double decay_factor;//take a large step at the beginning
   cout << "sample# (rerun #) | stride | incline | turning | init_file | "
           "Status | Solve time | Cost (tau cost)\n";
@@ -486,6 +497,9 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
     }
     double sample_cost =
         (readCSV(dir + prefix + string("c.csv")))(0, 0);
+    //save the trajectory optimization index and corresponding cost for further use
+    cost_list.conservativeResize(cost_list.rows()+1, 2);
+    cost_list.row(cost_list.rows()-1)<<traj_num,sample_cost;
 
     //test search algorithm
 //    double sample_cost = sample_result(new_gamma[0],new_gamma[1],
@@ -504,7 +518,30 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
       break;
     }
   }
-  //TODO:check the adjacent sample to avoid being stuck in local minimum
+  cout << "\nStart checking the cost:\n";
+  //check the adjacent sample to avoid being stuck in local minimum
+  for(iter=cost_list.rows()-2;iter>=1;iter--){
+    //if cost is larger than adjacent sample, rerun with adjacent sample result
+    if( (cost_list(iter,1) > 1.2*cost_list(iter-1,1)) &&
+      (cost_list(iter,1) > 1.2*cost_list(iter+1,1)) ){
+      VectorXd gamma_to_rerun = readCSV(dir + to_string(iter)
+                                           + string("_0_gamma.csv"));
+      //choose the result of sample with lower cost as initial guess
+      if(cost_list(iter-1,1)<cost_list(iter+1,1)){
+        trajOptGivenModel(gamma_to_rerun[0], gamma_to_rerun[1],
+                          gamma_to_rerun[2], dir, iter, true, iter-1);
+      }
+      else{
+        trajOptGivenModel(gamma_to_rerun[0], gamma_to_rerun[1],
+                          gamma_to_rerun[2], dir, iter, true, iter+1);
+      }
+      //update cost list
+      cost_list(iter,1) = readCSV(dir + to_string(iter)
+                                      + string("_0_c.csv"))(0,0);
+    }
+  }
+  writeCSV(dir +  to_string(boundary_point_idx)  +
+      string("_cost_list.csv"), cost_list);
 }
 
 int find_boundary(int argc, char* argv[]){
@@ -561,7 +598,7 @@ int find_boundary(int argc, char* argv[]){
   cout<<"use optimized model: "<<FLAGS_use_optimized_model<<endl;
   int max_iter = FLAGS_max_outer_iter;
   //TODO:decide the threshold under different situation
-  double cost_threshold = 15;
+  double cost_threshold = 20;
   if(FLAGS_robot_option==0)
   {
     if(FLAGS_is_get_nominal){

From c24403f4006336192c05d8470e93aeebd903bbdb Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Tue, 2 Jun 2020 16:26:14 -0400
Subject: [PATCH 15/82] update method of using adjacent sample to judge the
 solution

---
 examples/goldilocks_models/find_boundary.cc | 38 ++++++++++++++-------
 1 file changed, 25 insertions(+), 13 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 7919928304..7794bd8570 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -79,7 +79,7 @@ DEFINE_bool(is_zero_touchdown_impact, false,
 DEFINE_bool(is_add_tau_in_cost, true, "Add RoM input in the cost function");
 
 //outer loop
-DEFINE_int32(max_outer_iter, 100 , "max number of iterations for searching on each "
+DEFINE_int32(max_outer_iter, 150 , "max number of iterations for searching on each "
                                 "direction of one dimension");
 
 //others
@@ -447,17 +447,16 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
     double max_cost,int& traj_num,int& boundary_point_idx){
   int iter;
   int sample_idx = 0;
-  bool rerun = false;
   VectorXd new_gamma(dims);
   VectorXd last_gamma = init_gamma;
   VectorXd boundary_point(dims);
   VectorXd step = step_size.array()*step_direction.array();
-  VectorXd cost_list;
+  MatrixXd cost_list;
   double decay_factor;//take a large step at the beginning
   cout << "sample# (rerun #) | stride | incline | turning | init_file | "
           "Status | Solve time | Cost (tau cost)\n";
   for (iter = 1; iter <= max_iteration; iter++){
-    decay_factor = 2*pow(0.95,iter);
+    decay_factor = 2.5*pow(0.95,iter);
     if(decay_factor>1){
       new_gamma = last_gamma+decay_factor*step;
       last_gamma = new_gamma;
@@ -486,17 +485,25 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
         string("gamma.csv"), new_gamma);
     //run trajectory optimization and judge the solution
     trajOptGivenModel(new_gamma[0], new_gamma[1],
-        new_gamma[2], dir, traj_num, rerun);
+        new_gamma[2], dir, traj_num, false);
     //check if snopt find a solution successfully. If not, rerun the Traj Opt
     int is_success = (readCSV(dir + prefix + string("is_success.csv")))(0, 0);
     if(is_success==0){
-      rerun = true;
       trajOptGivenModel(new_gamma[0], new_gamma[1],
-                        new_gamma[2], dir, traj_num,rerun);
-      rerun = false;
+                        new_gamma[2], dir, traj_num,true);
     }
     double sample_cost =
         (readCSV(dir + prefix + string("c.csv")))(0, 0);
+    // without a good initial guess, the initial point is easily stuck in a local minimum
+    // use the first sample to judge the solution of initial point
+    if(iter==1){
+      double initial_cost =
+          (readCSV(dir + string("0_0_c.csv")))(0, 0);
+      if(initial_cost>1.2*sample_cost){
+        trajOptGivenModel(init_gamma[0], init_gamma[1],
+                          init_gamma[2], dir, 0,true,traj_num);
+      }
+    }
     //save the trajectory optimization index and corresponding cost for further use
     cost_list.conservativeResize(cost_list.rows()+1, 2);
     cost_list.row(cost_list.rows()-1)<<traj_num,sample_cost;
@@ -520,20 +527,24 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
   }
   cout << "\nStart checking the cost:\n";
   //check the adjacent sample to avoid being stuck in local minimum
+  int traj_idx;
   for(iter=cost_list.rows()-2;iter>=1;iter--){
+    traj_idx = cost_list(iter,0);
     //if cost is larger than adjacent sample, rerun with adjacent sample result
     if( (cost_list(iter,1) > 1.2*cost_list(iter-1,1)) &&
       (cost_list(iter,1) > 1.2*cost_list(iter+1,1)) ){
-      VectorXd gamma_to_rerun = readCSV(dir + to_string(iter)
+      VectorXd gamma_to_rerun = readCSV(dir + to_string(traj_idx)
                                            + string("_0_gamma.csv"));
       //choose the result of sample with lower cost as initial guess
       if(cost_list(iter-1,1)<cost_list(iter+1,1)){
         trajOptGivenModel(gamma_to_rerun[0], gamma_to_rerun[1],
-                          gamma_to_rerun[2], dir, iter, true, iter-1);
+                          gamma_to_rerun[2], dir, traj_idx,
+                          true, traj_idx-1);
       }
       else{
         trajOptGivenModel(gamma_to_rerun[0], gamma_to_rerun[1],
-                          gamma_to_rerun[2], dir, iter, true, iter+1);
+                          gamma_to_rerun[2], dir, traj_idx,
+                          true, traj_idx+1);
       }
       //update cost list
       cost_list(iter,1) = readCSV(dir + to_string(iter)
@@ -542,6 +553,7 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
   }
   writeCSV(dir +  to_string(boundary_point_idx)  +
       string("_cost_list.csv"), cost_list);
+  cout << "\nFinish checking the cost:\n";
 }
 
 int find_boundary(int argc, char* argv[]){
@@ -602,10 +614,10 @@ int find_boundary(int argc, char* argv[]){
   if(FLAGS_robot_option==0)
   {
     if(FLAGS_is_get_nominal){
-      cost_threshold = 25;
+      cost_threshold = 35;
     }
     else{
-      cost_threshold = 20;
+      cost_threshold = 30;
     }
   }
   else{

From cd775eaff0797ded4603a3875f97adf3068fac8a Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Tue, 2 Jun 2020 16:31:08 -0400
Subject: [PATCH 16/82] change the scale option for snope with cassie

---
 examples/goldilocks_models/find_boundary.cc                 | 6 +++---
 .../goldilocks_models/find_models/traj_opt_given_weigths.cc | 2 +-
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 7794bd8570..e70ab36492 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -610,7 +610,7 @@ int find_boundary(int argc, char* argv[]){
   cout<<"use optimized model: "<<FLAGS_use_optimized_model<<endl;
   int max_iter = FLAGS_max_outer_iter;
   //TODO:decide the threshold under different situation
-  double cost_threshold = 20;
+  double cost_threshold = 30;
   if(FLAGS_robot_option==0)
   {
     if(FLAGS_is_get_nominal){
@@ -622,10 +622,10 @@ int find_boundary(int argc, char* argv[]){
   }
   else{
     if(FLAGS_is_get_nominal){
-      cost_threshold = 20;
+      cost_threshold = 35;
     }
     else{
-      cost_threshold = 15;
+      cost_threshold = 30;
     }
   }
   cout<<"cost_threshold "<<cost_threshold<<endl;
diff --git a/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc b/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc
index 8e5bb3ca01..0c7657c6a7 100644
--- a/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc
+++ b/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc
@@ -1821,7 +1821,7 @@ void cassieTrajOpt(const MultibodyPlant<double> & plant,
                            0);  // 0
   trajopt->SetSolverOption(
       drake::solvers::SnoptSolver::id(), "Scale option",
-      0);  // snopt doc said try 2 if seeing snopta exit 40
+      2);  // snopt doc said try 2 if seeing snopta exit 40
   trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(),
                            "Major optimality tolerance",
                            major_optimality_tol);  // target nonlinear constraint violation

From fc0eb300d2f8c1dda22548a172c89a99e3417af7 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Fri, 5 Jun 2020 16:00:35 -0400
Subject: [PATCH 17/82] update theta_index

---
 examples/goldilocks_models/find_boundary.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index e70ab36492..4ee43700fb 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -337,7 +337,7 @@ void trajOptGivenModel(double stride_length, double ground_incline,
   theta_sDDot = VectorXd::Zero(n_theta_sDDot);
   if(FLAGS_use_optimized_model){
     //you have to specify the theta to use
-    int theta_idx = 100;
+    int theta_idx = 200;
     const string dir_find_models = "../dairlib_data/goldilocks_models/find_models/robot_" +
         to_string(FLAGS_robot_option) + "/";
     readThetaFromFiles(dir_find_models, theta_idx, theta_s, theta_sDDot);

From 78ff1da3f6284b7a1db40be92b1742800b36f522 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Fri, 5 Jun 2020 22:56:35 -0400
Subject: [PATCH 18/82] update theta_index

---
 examples/goldilocks_models/find_boundary.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 4ee43700fb..8c9112769d 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -337,7 +337,7 @@ void trajOptGivenModel(double stride_length, double ground_incline,
   theta_sDDot = VectorXd::Zero(n_theta_sDDot);
   if(FLAGS_use_optimized_model){
     //you have to specify the theta to use
-    int theta_idx = 200;
+    int theta_idx = 500;
     const string dir_find_models = "../dairlib_data/goldilocks_models/find_models/robot_" +
         to_string(FLAGS_robot_option) + "/";
     readThetaFromFiles(dir_find_models, theta_idx, theta_s, theta_sDDot);

From 94d16b12ff4271a5936c5c489c4460e86f9bc22e Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Sat, 6 Jun 2020 10:16:29 -0400
Subject: [PATCH 19/82] set theta index as a flag

---
 examples/goldilocks_models/find_boundary.cc | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 8c9112769d..00203baf75 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -72,6 +72,7 @@ DEFINE_int32(n_node, -1, "# of nodes for traj opt");
 DEFINE_double(eps_regularization, 1e-8, "Weight of regularization term"); //1e-4
 DEFINE_bool(is_get_nominal,false,"is calculating the cost without ROM constraints");
 DEFINE_bool(use_optimized_model,false,"read theta from files to apply optimized model");
+DEFINE_int32(theta_index,-1,"# of optimized model to use");
 
 //tasks
 DEFINE_bool(is_zero_touchdown_impact, false,
@@ -337,7 +338,9 @@ void trajOptGivenModel(double stride_length, double ground_incline,
   theta_sDDot = VectorXd::Zero(n_theta_sDDot);
   if(FLAGS_use_optimized_model){
     //you have to specify the theta to use
-    int theta_idx = 500;
+    DRAKE_DEMAND(FLAGS_theta_index>=0);
+    int theta_idx = FLAGS_theta_index;
+
     const string dir_find_models = "../dairlib_data/goldilocks_models/find_models/robot_" +
         to_string(FLAGS_robot_option) + "/";
     readThetaFromFiles(dir_find_models, theta_idx, theta_s, theta_sDDot);

From 3db7c30f19d80eb996af75003ee7b8deef836a9c Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Sat, 6 Jun 2020 11:31:35 -0400
Subject: [PATCH 20/82] cout more information

---
 examples/goldilocks_models/find_boundary.cc | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 00203baf75..fb79c52276 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -573,6 +573,13 @@ int find_boundary(int argc, char* argv[]){
       to_string(FLAGS_robot_option) + "/";
   if (!CreateFolderIfNotExist(dir)) return 0;
 
+  /*
+   * cout basic program information
+   */
+  cout << "\nBasic information:\n";
+  cout << "robot_option" << FLAGS_robot_option << endl;
+  cout << "rom_option" << FLAGS_rom_option << endl;
+
   /*
    * initialize task space
    */
@@ -611,6 +618,7 @@ int find_boundary(int argc, char* argv[]){
   cout << "\nIteration setting:\n";
   cout<<"get nominal cost: "<<FLAGS_is_get_nominal<<endl;
   cout<<"use optimized model: "<<FLAGS_use_optimized_model<<endl;
+  cout<<"optimized model index: "<<FLAGS_theta_index<<endl;
   int max_iter = FLAGS_max_outer_iter;
   //TODO:decide the threshold under different situation
   double cost_threshold = 30;

From b85ff1b12a84aafc04ce5c7cb78cf76613714636 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Mon, 8 Jun 2020 15:27:57 -0400
Subject: [PATCH 21/82] set the snopt scale option as 2 for five_link robot

---
 examples/goldilocks_models/find_boundary.cc                   | 4 ++--
 .../goldilocks_models/find_models/traj_opt_given_weigths.cc   | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index fb79c52276..e8ce9a2219 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -577,8 +577,8 @@ int find_boundary(int argc, char* argv[]){
    * cout basic program information
    */
   cout << "\nBasic information:\n";
-  cout << "robot_option" << FLAGS_robot_option << endl;
-  cout << "rom_option" << FLAGS_rom_option << endl;
+  cout << "robot_option " << FLAGS_robot_option << endl;
+  cout << "rom_option " << FLAGS_rom_option << endl;
 
   /*
    * initialize task space
diff --git a/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc b/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc
index 0c7657c6a7..fb7e21ad5b 100644
--- a/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc
+++ b/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc
@@ -1276,7 +1276,7 @@ void fiveLinkRobotTrajOpt(const MultibodyPlant<double> & plant,
   trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(), "Verify level",
                            0);
   trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(), "Scale option",
-                           0);  // 0 // snopt doc said try 2 if seeing snopta exit 40
+                           2);  // 0 // snopt doc said try 2 if seeing snopta exit 40
   trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(),
                            "Major optimality tolerance", 1e-4);  // target nonlinear constraint violation
   trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(),

From cb8f1f7642b3a8b69ba4d5055951481ebb330581 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Mon, 8 Jun 2020 15:38:58 -0400
Subject: [PATCH 22/82] change the snopt option back to 0 for five_link robot

---
 .../goldilocks_models/find_models/traj_opt_given_weigths.cc     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc b/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc
index fb7e21ad5b..b303e45a5c 100644
--- a/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc
+++ b/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc
@@ -1821,7 +1821,7 @@ void cassieTrajOpt(const MultibodyPlant<double> & plant,
                            0);  // 0
   trajopt->SetSolverOption(
       drake::solvers::SnoptSolver::id(), "Scale option",
-      2);  // snopt doc said try 2 if seeing snopta exit 40
+      0);  // snopt doc said try 2 if seeing snopta exit 40
   trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(),
                            "Major optimality tolerance",
                            major_optimality_tol);  // target nonlinear constraint violation

From 904ccd598305cadf79913db193c99aefb2e25b35 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Mon, 8 Jun 2020 16:28:00 -0400
Subject: [PATCH 23/82] now the snopt option for five_link is 0 and for cassie
 is 2

---
 .../goldilocks_models/find_models/traj_opt_given_weigths.cc   | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc b/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc
index b303e45a5c..0c7657c6a7 100644
--- a/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc
+++ b/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc
@@ -1276,7 +1276,7 @@ void fiveLinkRobotTrajOpt(const MultibodyPlant<double> & plant,
   trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(), "Verify level",
                            0);
   trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(), "Scale option",
-                           2);  // 0 // snopt doc said try 2 if seeing snopta exit 40
+                           0);  // 0 // snopt doc said try 2 if seeing snopta exit 40
   trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(),
                            "Major optimality tolerance", 1e-4);  // target nonlinear constraint violation
   trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(),
@@ -1821,7 +1821,7 @@ void cassieTrajOpt(const MultibodyPlant<double> & plant,
                            0);  // 0
   trajopt->SetSolverOption(
       drake::solvers::SnoptSolver::id(), "Scale option",
-      0);  // snopt doc said try 2 if seeing snopta exit 40
+      2);  // snopt doc said try 2 if seeing snopta exit 40
   trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(),
                            "Major optimality tolerance",
                            major_optimality_tol);  // target nonlinear constraint violation

From fc64093f5a4a3350c576c0bf00327375df99d670 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Mon, 8 Jun 2020 17:51:55 -0400
Subject: [PATCH 24/82] set good initial guess for central point while using
 optimized model

---
 examples/goldilocks_models/find_boundary.cc | 62 ++++++++++++++++++++-
 1 file changed, 61 insertions(+), 1 deletion(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index e8ce9a2219..8993a47128 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -223,8 +223,68 @@ string getInitFileName(const string directory, int traj_opt_num,
     }
   }else{
     if(traj_opt_num==0){
-      initial_file_name = "";
+      //specify initial guess if using optimized model
+      //use solutions during ROM optimization process to calculate the initial guess
+      if(FLAGS_use_optimized_model){
+        const string dir_find_models = "../dairlib_data/goldilocks_models/find_models/robot_" +
+            to_string(FLAGS_robot_option) + "/";
+        VectorXd initial_guess;
+        //take out corresponding w and calculate the weight for interpolation
+        MatrixXd w_gamma;
+        VectorXd weight_gamma;
+        //paras used to decide gamma scale
+        double delta_sl = 0.015;
+        double delta_gi = 0.05;
+        double delta_tr = 0.125;
+        //calculate the weighted sum of solutions
+        int theta_idx = FLAGS_theta_index;
+        int sample_num = 0;
+        while(file_exist(dir_find_models + to_string(theta_idx)+ '_' +
+        to_string(sample_num)+ string("_is_success.csv"))){
+          //check if this sample is success
+          int is_success = (readCSV(dir_find_models + to_string(theta_idx)+ '_'+
+              to_string(sample_num)+ string("_is_success.csv")))(0,0);
+          if(is_success == 1) {
+            //extract past gamma
+            MatrixXd past_ground_incline = readCSV(dir_find_models + to_string(theta_idx) + string("_")
+                + to_string(sample_num) + string("_ground_incline.csv"));
+            MatrixXd past_stride_length = readCSV(dir_find_models + to_string(theta_idx) + string("_")
+                + to_string(sample_num) + string("_stride_length.csv"));
+            MatrixXd past_turning_rate = readCSV(dir_find_models + to_string(theta_idx) + string("_")
+                + to_string(sample_num) + string("_turning_rate.csv"));
+            VectorXd past_gamma(gamma_dimension);
+            past_gamma << past_ground_incline(0, 0), past_stride_length(0, 0), past_turning_rate(0, 0);
+            //calculate the weight for each sample using the 3-norm of the difference between gamma
+            VectorXd gamma_scale(gamma_dimension);
+            gamma_scale << 1/delta_gi,1/delta_sl,1.3/delta_tr;
+            VectorXd dif_gamma = (past_gamma - current_gamma).array().abs()*gamma_scale.array();
+            VectorXd dif_gamma2 = dif_gamma.array().pow(2);
+            double distance_gamma =  (dif_gamma.transpose() * dif_gamma2)(0,0);
+            //extract the solution of this sample
+            VectorXd w_to_interpolate = readCSV(dir_find_models + to_string(theta_idx)+ '_'+
+                to_string(sample_num) + string("_w.csv"));
+            //concatenate the weight and solution for further calculation
+            w_gamma.conservativeResize(w_to_interpolate.rows(),w_gamma.cols()+1);
+            w_gamma.col(w_gamma.cols()-1) = w_to_interpolate;
+            weight_gamma.conservativeResize(weight_gamma.rows()+1);
+            weight_gamma(weight_gamma.rows()-1)= 1 /distance_gamma;
+          }
+          sample_num = sample_num+1;
+        }
+        DRAKE_DEMAND(weight_gamma.rows()>0);
+        //    normalize weight
+        weight_gamma = weight_gamma / weight_gamma.sum();
+        initial_guess = w_gamma * weight_gamma;
+        //    save initial guess and set init file
+        initial_file_name = to_string(traj_opt_num)
+            + string("_0_initial_guess.csv");
+        writeCSV(directory + initial_file_name, initial_guess);
+      }
+      else{
+        initial_file_name = "";
+      }
     }else{
+      //use past solutions to calculate interpolated initial guess
       VectorXd initial_guess;
       //take out corresponding w and calculate the weight for interpolation
       MatrixXd w_gamma;

From 6d1c11f8fd8e33da376c55b04b71dc711e31392c Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Mon, 8 Jun 2020 18:16:47 -0400
Subject: [PATCH 25/82] set the snopt option as 2 for five_link robot

---
 .../goldilocks_models/find_models/traj_opt_given_weigths.cc     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc b/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc
index 0c7657c6a7..fb7e21ad5b 100644
--- a/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc
+++ b/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc
@@ -1276,7 +1276,7 @@ void fiveLinkRobotTrajOpt(const MultibodyPlant<double> & plant,
   trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(), "Verify level",
                            0);
   trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(), "Scale option",
-                           0);  // 0 // snopt doc said try 2 if seeing snopta exit 40
+                           2);  // 0 // snopt doc said try 2 if seeing snopta exit 40
   trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(),
                            "Major optimality tolerance", 1e-4);  // target nonlinear constraint violation
   trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(),

From b728da8b7851fdabeeaad93825ab60e8dc197184 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Tue, 9 Jun 2020 17:32:24 -0400
Subject: [PATCH 26/82] update central point evaluation to make sure getting a
 good solution

---
 examples/goldilocks_models/find_boundary.cc | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 8993a47128..c1407f5ae1 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -728,9 +728,13 @@ int find_boundary(int argc, char* argv[]){
           "Status | Solve time | Cost (tau cost)\n";
   trajOptGivenModel(stride_length_0, ground_incline_0,
                     turning_rate_0, dir, traj_opt_num, false);
-  //rerun the Traj Opt to avoid being stuck in local minimum
-  trajOptGivenModel(stride_length_0, ground_incline_0,
-                    turning_rate_0, dir, traj_opt_num, true);
+  //make sure solution found for the initial point
+  int init_is_success = (readCSV(dir + string("0_0_is_success.csv")))(0,0);
+  while(!init_is_success){
+    trajOptGivenModel(stride_length_0, ground_incline_0,
+                      turning_rate_0, dir, traj_opt_num, true);
+    init_is_success = (readCSV(dir + string("0_0_is_success.csv")))(0,0);
+  }
 
   int dim1,dim2,dim3;
   VectorXd step(dimensions);

From d249465ef8b693f0ed4d498f0969fd7c8a547d70 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Fri, 12 Jun 2020 13:40:21 -0400
Subject: [PATCH 27/82] save more information in boundary_point

---
 examples/goldilocks_models/find_boundary.cc          | 6 ++++++
 examples/goldilocks_models/find_goldilocks_models.cc | 2 +-
 2 files changed, 7 insertions(+), 1 deletion(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index c1407f5ae1..8d016924fa 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -513,6 +513,7 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
   VectorXd new_gamma(dims);
   VectorXd last_gamma = init_gamma;
   VectorXd boundary_point(dims);
+  VectorXd boundary_point_infor(dims+2);
   VectorXd step = step_size.array()*step_direction.array();
   MatrixXd cost_list;
   double decay_factor;//take a large step at the beginning
@@ -532,6 +533,9 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
     if(new_gamma[0]<=0){
       boundary_point_idx += 1;
       boundary_point = new_gamma-step;
+      double boundary_point_cost = (readCSV(dir + to_string(traj_num) +  "_" +
+          to_string(sample_idx) + "_" + string("c.csv")))(0, 0);
+      boundary_point_infor << traj_num,boundary_point,boundary_point_cost;
       writeCSV(dir + to_string(boundary_point_idx) +  "_" +
           string("boundary_point.csv"), boundary_point);
       cout << "boundary point index | stride length | ground incline"
@@ -578,6 +582,8 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
     if(sample_cost>max_cost){
       boundary_point_idx += 1;
       boundary_point = new_gamma-step;
+      double boundary_point_cost = sample_cost;
+      boundary_point_infor << traj_num,boundary_point,boundary_point_cost;
       writeCSV(dir + to_string(boundary_point_idx) +  "_" +
           string("boundary_point.csv"), boundary_point);
       cout << "boundary point index | stride length | ground incline"
diff --git a/examples/goldilocks_models/find_goldilocks_models.cc b/examples/goldilocks_models/find_goldilocks_models.cc
index 1e6ccc0874..1a71cca3e6 100644
--- a/examples/goldilocks_models/find_goldilocks_models.cc
+++ b/examples/goldilocks_models/find_goldilocks_models.cc
@@ -1509,7 +1509,7 @@ int findGoldilocksModels(int argc, char* argv[]) {
     } else {
       delta_stride_length = 0.1;
     }
-    stride_length_0 = 0.2;  //0.15
+    stride_length_0 = 0.3;  //0.15
   } else {
     throw std::runtime_error("Should not reach here");
     delta_stride_length = 0;

From 1f52bf08d0a63162a0e33420346c8bf65499dc76 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Wed, 17 Jun 2020 09:49:27 -0400
Subject: [PATCH 28/82] add more reruns for each sample to make sure getting a
 successful solution

---
 examples/goldilocks_models/find_boundary.cc | 14 ++++++++++----
 1 file changed, 10 insertions(+), 4 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 8d016924fa..5673e5e041 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -554,10 +554,16 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
     trajOptGivenModel(new_gamma[0], new_gamma[1],
         new_gamma[2], dir, traj_num, false);
     //check if snopt find a solution successfully. If not, rerun the Traj Opt
-    int is_success = (readCSV(dir + prefix + string("is_success.csv")))(0, 0);
-    if(is_success==0){
-      trajOptGivenModel(new_gamma[0], new_gamma[1],
-                        new_gamma[2], dir, traj_num,true);
+    int max_rerun_num = 5;
+    for(iter=0;iter<5;iter++){
+      int is_success = (readCSV(dir + prefix + string("is_success.csv")))(0, 0);
+      if(is_success==0){
+        trajOptGivenModel(new_gamma[0], new_gamma[1],
+                          new_gamma[2], dir, traj_num,true);
+      }
+      else{
+        break;
+      }
     }
     double sample_cost =
         (readCSV(dir + prefix + string("c.csv")))(0, 0);

From 4c3dd90bc3be242fc022f10395c34457b05a5cb0 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Wed, 17 Jun 2020 12:04:46 -0400
Subject: [PATCH 29/82] try different methods to rerun to make sure all samples
 get successful solutions

---
 examples/goldilocks_models/find_boundary.cc   | 35 +++++++++++++++---
 .../find_models/traj_opt_given_weigths.cc     | 36 +++++++++++++------
 .../find_models/traj_opt_given_weigths.h      |  2 +-
 3 files changed, 57 insertions(+), 16 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 5673e5e041..8080cdfd0a 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -336,7 +336,7 @@ string getInitFileName(const string directory, int traj_opt_num,
 // trajectory optimization for given task and model
 void trajOptGivenModel(double stride_length, double ground_incline,
     double turning_rate,const string dir,int num,bool is_rerun,
-    int initial_guess_idx=-1){
+    int initial_guess_idx=-1,bool turn_on_scaling=true){
   // Create MBP
   MultibodyPlant<double> plant(0.0);
   createMBP(&plant, FLAGS_robot_option);
@@ -488,7 +488,8 @@ void trajOptGivenModel(double stride_length, double ground_incline,
                       sample_idx, n_rerun,
                       cost_threshold_for_update, N_rerun,
                       rom_option,
-                      FLAGS_robot_option);
+                      FLAGS_robot_option,
+                      turn_on_scaling);
 }
 
 //naive test function for search algorithm
@@ -554,9 +555,10 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
     trajOptGivenModel(new_gamma[0], new_gamma[1],
         new_gamma[2], dir, traj_num, false);
     //check if snopt find a solution successfully. If not, rerun the Traj Opt
-    int max_rerun_num = 5;
-    for(iter=0;iter<5;iter++){
-      int is_success = (readCSV(dir + prefix + string("is_success.csv")))(0, 0);
+    int max_rerun_num = 4;
+    int is_success;
+    for(iter=0;iter<max_rerun_num;iter++){
+      is_success = (readCSV(dir + prefix + string("is_success.csv")))(0, 0);
       if(is_success==0){
         trajOptGivenModel(new_gamma[0], new_gamma[1],
                           new_gamma[2], dir, traj_num,true);
@@ -565,6 +567,29 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
         break;
       }
     }
+    //if snopt still can't find a solution, try to use adjacent sample to help
+    for(iter=0;iter<max_rerun_num;iter++){
+      is_success = (readCSV(dir + prefix + string("is_success.csv")))(0, 0);
+      if(is_success==0){
+        trajOptGivenModel(new_gamma[0], new_gamma[1],
+                          new_gamma[2], dir, traj_num,true,traj_num-1);
+      }
+      else{
+        break;
+      }
+    }
+    //if snopt still failed to find a solution,turn off the scaling option
+    // and try again
+    for(iter=0;iter<max_rerun_num;iter++){
+      is_success = (readCSV(dir + prefix + string("is_success.csv")))(0, 0);
+      if(is_success==0){
+        trajOptGivenModel(new_gamma[0], new_gamma[1],
+                          new_gamma[2], dir, traj_num,true,-1,false);
+      }
+      else{
+        break;
+      }
+    }
     double sample_cost =
         (readCSV(dir + prefix + string("c.csv")))(0, 0);
     // without a good initial guess, the initial point is easily stuck in a local minimum
diff --git a/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc b/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc
index fb7e21ad5b..90bff37aab 100644
--- a/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc
+++ b/examples/goldilocks_models/find_models/traj_opt_given_weigths.cc
@@ -1132,7 +1132,8 @@ void fiveLinkRobotTrajOpt(const MultibodyPlant<double> & plant,
                           int sample_idx, int n_rerun,
                           double cost_threshold_for_update, int N_rerun,
                           int rom_option,
-                          int robot_option) {
+                          int robot_option,
+                          bool turn_on_scaling) {
   map<string, int> pos_map = multibody::makeNameToPositionsMap(plant);
   map<string, int> vel_map = multibody::makeNameToVelocitiesMap(plant);
   map<string, int> input_map = multibody::makeNameToActuatorsMap(plant);
@@ -1275,8 +1276,15 @@ void fiveLinkRobotTrajOpt(const MultibodyPlant<double> & plant,
                            "Iterations limit", 100000);  // QP subproblems
   trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(), "Verify level",
                            0);
-  trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(), "Scale option",
-                           2);  // 0 // snopt doc said try 2 if seeing snopta exit 40
+  if(turn_on_scaling){
+    trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(), "Scale option",
+                             2);
+  }
+  else{
+    trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(), "Scale option",
+                             0);
+  }
+  // 0 // snopt doc said try 2 if seeing snopta exit 40
   trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(),
                            "Major optimality tolerance", 1e-4);  // target nonlinear constraint violation
   trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(),
@@ -1543,7 +1551,7 @@ void cassieTrajOpt(const MultibodyPlant<double> & plant,
                    bool is_add_tau_in_cost,
                    int sample_idx, int n_rerun,
                    double cost_threshold_for_update, int N_rerun,
-                   int rom_option, int robot_option) {
+                   int rom_option, int robot_option,bool turn_on_scaling) {
   // Dircon parameter
   double minimum_timestep = 0.01;
   DRAKE_DEMAND(duration / (n_node - 1) >= minimum_timestep);
@@ -1819,9 +1827,17 @@ void cassieTrajOpt(const MultibodyPlant<double> & plant,
                            "Iterations limit", 100000);  // QP subproblems
   trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(), "Verify level",
                            0);  // 0
-  trajopt->SetSolverOption(
-      drake::solvers::SnoptSolver::id(), "Scale option",
-      2);  // snopt doc said try 2 if seeing snopta exit 40
+  if(turn_on_scaling){
+    trajopt->SetSolverOption(
+        drake::solvers::SnoptSolver::id(), "Scale option",
+        2);
+  }
+  else{
+    trajopt->SetSolverOption(
+        drake::solvers::SnoptSolver::id(), "Scale option",
+        0);
+  }
+  // snopt doc said try 2 if seeing snopta exit 40
   trajopt->SetSolverOption(drake::solvers::SnoptSolver::id(),
                            "Major optimality tolerance",
                            major_optimality_tol);  // target nonlinear constraint violation
@@ -2526,7 +2542,7 @@ void trajOptGivenWeights(const MultibodyPlant<double> & plant,
                          bool is_add_tau_in_cost,
                          int sample_idx, int n_rerun,
                          double cost_threshold_for_update, int N_rerun,
-                         int rom_option, int robot_option) {
+                         int rom_option, int robot_option,bool turn_on_scaling) {
 
   //Testing
 //  if (sample_idx == 0) {
@@ -2569,7 +2585,7 @@ void trajOptGivenWeights(const MultibodyPlant<double> & plant,
                          is_get_nominal, is_zero_touchdown_impact,
                          extend_model, is_add_tau_in_cost,
                          sample_idx, n_rerun, cost_threshold_for_update, N_rerun,
-                         rom_option, robot_option);
+                         rom_option, robot_option,turn_on_scaling);
   } else if (robot_option == 1) {
     cassieTrajOpt(plant, plant_autoDiff,
                   n_s, n_sDDot, n_tau, n_feature_s, n_feature_sDDot, B_tau,
@@ -2592,7 +2608,7 @@ void trajOptGivenWeights(const MultibodyPlant<double> & plant,
                   is_get_nominal, is_zero_touchdown_impact,
                   extend_model, is_add_tau_in_cost,
                   sample_idx, n_rerun, cost_threshold_for_update, N_rerun,
-                  rom_option, robot_option);
+                  rom_option, robot_option,turn_on_scaling);
   }
 
   // For multithreading purpose. Indicate this function has ended.
diff --git a/examples/goldilocks_models/find_models/traj_opt_given_weigths.h b/examples/goldilocks_models/find_models/traj_opt_given_weigths.h
index 1c0f70681c..b545edb8a1 100644
--- a/examples/goldilocks_models/find_models/traj_opt_given_weigths.h
+++ b/examples/goldilocks_models/find_models/traj_opt_given_weigths.h
@@ -72,7 +72,7 @@ void trajOptGivenWeights(
     bool extend_model,
     bool is_add_tau_in_cost,
     int sample_idx, int n_rerun, double cost_threshold_for_update, int N_rerun,
-    int rom_option, int robot_option);
+    int rom_option, int robot_option,bool turn_on_scaling);
 
 void addRegularization(bool is_get_nominal, double eps_reg,
                        GoldilocksModelTrajOpt& gm_traj_opt);

From 00b5247afe2ac7ddcb0f8ea26ea0f9d57b7ce181 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Wed, 17 Jun 2020 22:51:32 -0400
Subject: [PATCH 30/82] debug for rerun

---
 examples/goldilocks_models/find_boundary.cc | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 8080cdfd0a..86302ff861 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -555,9 +555,10 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
     trajOptGivenModel(new_gamma[0], new_gamma[1],
         new_gamma[2], dir, traj_num, false);
     //check if snopt find a solution successfully. If not, rerun the Traj Opt
+    int rerun = 0;
     int max_rerun_num = 4;
     int is_success;
-    for(iter=0;iter<max_rerun_num;iter++){
+    for(rerun=0;rerun<max_rerun_num;rerun++){
       is_success = (readCSV(dir + prefix + string("is_success.csv")))(0, 0);
       if(is_success==0){
         trajOptGivenModel(new_gamma[0], new_gamma[1],
@@ -568,7 +569,7 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
       }
     }
     //if snopt still can't find a solution, try to use adjacent sample to help
-    for(iter=0;iter<max_rerun_num;iter++){
+    for(rerun=0;rerun<max_rerun_num;rerun++){
       is_success = (readCSV(dir + prefix + string("is_success.csv")))(0, 0);
       if(is_success==0){
         trajOptGivenModel(new_gamma[0], new_gamma[1],
@@ -580,7 +581,7 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
     }
     //if snopt still failed to find a solution,turn off the scaling option
     // and try again
-    for(iter=0;iter<max_rerun_num;iter++){
+    for(rerun=0;rerun<max_rerun_num;rerun++){
       is_success = (readCSV(dir + prefix + string("is_success.csv")))(0, 0);
       if(is_success==0){
         trajOptGivenModel(new_gamma[0], new_gamma[1],

From 864100bab31479079998c5d1bc3e3a05a6f9acb9 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Wed, 17 Jun 2020 22:54:00 -0400
Subject: [PATCH 31/82] add program name

---
 examples/goldilocks_models/find_boundary.cc | 1 +
 1 file changed, 1 insertion(+)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 86302ff861..146482261d 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -675,6 +675,7 @@ int find_boundary(int argc, char* argv[]){
    * cout basic program information
    */
   cout << "\nBasic information:\n";
+  cout << FLAGS_program_name << endl;
   cout << "robot_option " << FLAGS_robot_option << endl;
   cout << "rom_option " << FLAGS_rom_option << endl;
 

From 29512ad7d92816b9dfe0b91a4ee85cdf914e9054 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Wed, 17 Jun 2020 23:02:21 -0400
Subject: [PATCH 32/82] save boundary point infor

---
 examples/goldilocks_models/find_boundary.cc | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 146482261d..4fbeb3eaa5 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -538,7 +538,7 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
           to_string(sample_idx) + "_" + string("c.csv")))(0, 0);
       boundary_point_infor << traj_num,boundary_point,boundary_point_cost;
       writeCSV(dir + to_string(boundary_point_idx) +  "_" +
-          string("boundary_point.csv"), boundary_point);
+          string("boundary_point.csv"), boundary_point_infor);
       cout << "boundary point index | stride length | ground incline"
               " | turning rate"<<endl;
       cout<<" \t "<<boundary_point_idx<< "\t" <<" | "<<"\t"<<boundary_point[0]
@@ -617,7 +617,7 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
       double boundary_point_cost = sample_cost;
       boundary_point_infor << traj_num,boundary_point,boundary_point_cost;
       writeCSV(dir + to_string(boundary_point_idx) +  "_" +
-          string("boundary_point.csv"), boundary_point);
+          string("boundary_point.csv"), boundary_point_infor);
       cout << "boundary point index | stride length | ground incline"
               " | turning rate"<<endl;
       cout<<" \t "<<boundary_point_idx<< "\t" <<" | "<<"\t"<<boundary_point[0]

From 242508912f29a9148eb07da57c64ba41d65d7501 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Thu, 18 Jun 2020 12:12:36 -0400
Subject: [PATCH 33/82] make sure all samples success while checking cost

---
 examples/goldilocks_models/find_boundary.cc | 24 +++++++++++++++++++++
 1 file changed, 24 insertions(+)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 4fbeb3eaa5..70c24b0f20 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -641,11 +641,35 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
         trajOptGivenModel(gamma_to_rerun[0], gamma_to_rerun[1],
                           gamma_to_rerun[2], dir, traj_idx,
                           true, traj_idx-1);
+        //make sure this sample success
+        for(rerun=0;rerun<max_rerun_num;rerun++){
+          is_success = (readCSV(dir + to_string(traj_idx) +
+              string("_0_is_success.csv")))(0, 0);
+          if(is_success==0){
+            trajOptGivenModel(new_gamma[0], new_gamma[1],
+                              new_gamma[2], dir, traj_idx,true);
+          }
+          else{
+            break;
+          }
+        }
       }
       else{
         trajOptGivenModel(gamma_to_rerun[0], gamma_to_rerun[1],
                           gamma_to_rerun[2], dir, traj_idx,
                           true, traj_idx+1);
+        //make sure this sample success
+        for(rerun=0;rerun<max_rerun_num;rerun++){
+          is_success = (readCSV(dir + to_string(traj_idx) +
+              string("_0_is_success.csv")))(0, 0);
+          if(is_success==0){
+            trajOptGivenModel(new_gamma[0], new_gamma[1],
+                              new_gamma[2], dir, traj_idx,true);
+          }
+          else{
+            break;
+          }
+        }
       }
       //update cost list
       cost_list(iter,1) = readCSV(dir + to_string(iter)

From 4b568b80c04cb52d67c24e8ab38fae3817e50058 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Thu, 18 Jun 2020 12:15:41 -0400
Subject: [PATCH 34/82] move definition ofseveral variables

---
 examples/goldilocks_models/find_boundary.cc | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 70c24b0f20..5246a989d0 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -511,6 +511,9 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
     double max_cost,int& traj_num,int& boundary_point_idx){
   int iter;
   int sample_idx = 0;
+  int rerun = 0;
+  int max_rerun_num = 4;
+  int is_success;
   VectorXd new_gamma(dims);
   VectorXd last_gamma = init_gamma;
   VectorXd boundary_point(dims);
@@ -555,9 +558,6 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
     trajOptGivenModel(new_gamma[0], new_gamma[1],
         new_gamma[2], dir, traj_num, false);
     //check if snopt find a solution successfully. If not, rerun the Traj Opt
-    int rerun = 0;
-    int max_rerun_num = 4;
-    int is_success;
     for(rerun=0;rerun<max_rerun_num;rerun++){
       is_success = (readCSV(dir + prefix + string("is_success.csv")))(0, 0);
       if(is_success==0){

From 5668fd3c387f1fae87eedbe1781728d98992ee56 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Thu, 25 Jun 2020 20:01:41 -0400
Subject: [PATCH 35/82] fix using the solutions from ROM optimization to set
 initial guesses.Still need to organize the program to manage the tasks after
 merging with goldilocks-model-dev in the future

---
 examples/goldilocks_models/find_boundary.cc | 42 ++++++++++-----------
 1 file changed, 21 insertions(+), 21 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 5246a989d0..2808895580 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -206,9 +206,27 @@ void readThetaFromFiles(const string dir,int theta_idx,
 //use interpolation to set the initial guess for the trajectory optimization
 string getInitFileName(const string directory, int traj_opt_num,
     bool is_rerun,int rerun_traj_idx=-1){
-  int gamma_dimension = 3;
   VectorXd current_gamma = readCSV(directory + to_string(traj_opt_num)
                                        + string("_0_gamma.csv"));
+  int gamma_dimension = current_gamma.size();
+  VectorXd gamma_scale(gamma_dimension);
+  //paras used to decide gamma scale
+  if(FLAGS_robot_option==0)
+  {
+    double delta_sl = 0.015;
+    double delta_gi = 0.05;
+    double delta_v = 0.02;
+    gamma_scale << 1/delta_sl,1/delta_gi,1/delta_v;
+
+  }
+  else if(FLAGS_robot_option==1)
+  {
+    double delta_sl = 0.015;
+    double delta_gi = 0.05;
+    double delta_tr = 0.125;
+    double delta_v = 0.04;
+    gamma_scale << 1/delta_sl,1/delta_gi,1/delta_v,1.3/delta_tr;
+  }
   string initial_file_name;
   if(is_rerun){
     //if not specify which Traj Opt result to use, use its own result to rerun;
@@ -232,10 +250,6 @@ string getInitFileName(const string directory, int traj_opt_num,
         //take out corresponding w and calculate the weight for interpolation
         MatrixXd w_gamma;
         VectorXd weight_gamma;
-        //paras used to decide gamma scale
-        double delta_sl = 0.015;
-        double delta_gi = 0.05;
-        double delta_tr = 0.125;
         //calculate the weighted sum of solutions
         int theta_idx = FLAGS_theta_index;
         int sample_num = 0;
@@ -246,17 +260,9 @@ string getInitFileName(const string directory, int traj_opt_num,
               to_string(sample_num)+ string("_is_success.csv")))(0,0);
           if(is_success == 1) {
             //extract past gamma
-            MatrixXd past_ground_incline = readCSV(dir_find_models + to_string(theta_idx) + string("_")
-                + to_string(sample_num) + string("_ground_incline.csv"));
-            MatrixXd past_stride_length = readCSV(dir_find_models + to_string(theta_idx) + string("_")
-                + to_string(sample_num) + string("_stride_length.csv"));
-            MatrixXd past_turning_rate = readCSV(dir_find_models + to_string(theta_idx) + string("_")
-                + to_string(sample_num) + string("_turning_rate.csv"));
-            VectorXd past_gamma(gamma_dimension);
-            past_gamma << past_ground_incline(0, 0), past_stride_length(0, 0), past_turning_rate(0, 0);
+            VectorXd past_gamma = readCSV(dir_find_models + to_string(theta_idx) + string("_")
+                + to_string(sample_num) + string("_task.csv"));
             //calculate the weight for each sample using the 3-norm of the difference between gamma
-            VectorXd gamma_scale(gamma_dimension);
-            gamma_scale << 1/delta_gi,1/delta_sl,1.3/delta_tr;
             VectorXd dif_gamma = (past_gamma - current_gamma).array().abs()*gamma_scale.array();
             VectorXd dif_gamma2 = dif_gamma.array().pow(2);
             double distance_gamma =  (dif_gamma.transpose() * dif_gamma2)(0,0);
@@ -289,10 +295,6 @@ string getInitFileName(const string directory, int traj_opt_num,
       //take out corresponding w and calculate the weight for interpolation
       MatrixXd w_gamma;
       VectorXd weight_gamma;
-      //paras used to decide gamma scale
-      double delta_sl = 0.015;
-      double delta_gi = 0.05;
-      double delta_tr = 0.125;
       //calculate the weighted sum of past solutions
       int sample_num = 0;
       for (sample_num = 0; sample_num < traj_opt_num; sample_num++) {
@@ -304,8 +306,6 @@ string getInitFileName(const string directory, int traj_opt_num,
           VectorXd past_gamma = readCSV(directory + to_string(sample_num)
                                             + string("_0_gamma.csv"));
           //calculate the weight for each sample using the third power of the difference between gamma
-          VectorXd gamma_scale(gamma_dimension);
-          gamma_scale << 1/delta_gi,1/delta_sl,1.3/delta_tr;
           VectorXd dif_gamma = (past_gamma - current_gamma).array().abs()*gamma_scale.array();
           VectorXd dif_gamma2 = dif_gamma.array().pow(2);
           double distance_gamma =  (dif_gamma.transpose() * dif_gamma2)(0,0);

From b58251ba3cf5c47d7796ad16a3376868f3a48304 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Sun, 28 Jun 2020 16:19:29 -0400
Subject: [PATCH 36/82] parts of the modification

---
 examples/goldilocks_models/find_boundary.cc   | 138 +++++++++---------
 .../find_models/traj_opt_given_weigths.h      |   2 +-
 2 files changed, 67 insertions(+), 73 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 2808895580..bc78a80c12 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -338,6 +338,7 @@ void trajOptGivenModel(double stride_length, double ground_incline,
     double turning_rate,const string dir,int num,bool is_rerun,
     int initial_guess_idx=-1,bool turn_on_scaling=true){
   // Create MBP
+  drake::logging::set_log_level("err");  // ignore warnings about joint limits
   MultibodyPlant<double> plant(0.0);
   createMBP(&plant, FLAGS_robot_option);
 
@@ -345,6 +346,9 @@ void trajOptGivenModel(double stride_length, double ground_incline,
   MultibodyPlant<AutoDiffXd> plant_autoDiff(plant);
   cout << endl;
 
+  //Task Setting
+  Task task();
+
   // Parameters for the inner loop optimization
   int max_inner_iter = FLAGS_max_inner_iter;
   if (FLAGS_robot_option == 0) {
@@ -354,48 +358,54 @@ void trajOptGivenModel(double stride_length, double ground_incline,
   double R = 0;  // Cost on input effort
   double all_cost_scale = 1;
   setCostWeight(&Q, &R, &all_cost_scale, FLAGS_robot_option);
-  int n_node = 20;
-  if (FLAGS_robot_option == 0) {
-    n_node = 20;
-  } else if (FLAGS_robot_option == 1) {
-    n_node = 20;
-  }
-  if (FLAGS_n_node > 0) n_node = FLAGS_n_node;
-  if (FLAGS_robot_option == 1) {
-    // If the node density is too low, it's harder for SNOPT to converge well.
-    // The ratio of distance per nodes = 0.2/16 is fine for snopt, but
-    // 0.3 / 16 is too high.
-    // However, currently it takes too much time to compute with many nodes, so
-    // we try 0.3/24.
-    double max_distance_per_node = 0.3 / 16;
-//    DRAKE_DEMAND((max_stride_length / n_node) <= max_distance_per_node);
+  // Inner loop setup
+  vector<int> n_node_vec(N_sample, 20);
+  if (!FLAGS_fix_node_number) {
+    for (int sample_idx = 0; sample_idx < N_sample; sample_idx++) {
+      task.set(task_gen.NewTask(sample_idx, true));
+      double duration = task.get("stride length") / task.get("velocity");
+      n_node_vec[sample_idx] = int(FLAGS_node_density * duration);
+      cout << n_node_vec[sample_idx] << ", ";
+    } cout << endl;
+    cout << "WARNING: we will only add adjacent samples to the list if it has "
+            "the same number of nodes\n";
   }
+  InnerLoopSetting inner_loop_setting = InnerLoopSetting();
+  inner_loop_setting.Q_double = Q;
+  inner_loop_setting.R_double = R;
+  inner_loop_setting.eps_reg = FLAGS_eps_regularization;
+  inner_loop_setting.all_cost_scale = all_cost_scale;
+  inner_loop_setting.is_add_tau_in_cost = FLAGS_is_add_tau_in_cost;
+  inner_loop_setting.is_zero_touchdown_impact = FLAGS_is_zero_touchdown_impact;
+  inner_loop_setting.max_iter = max_inner_iter;
+  inner_loop_setting.major_optimality_tol = FLAGS_major_optimality_tol;
+  inner_loop_setting.major_feasibility_tol = FLAGS_major_feasibility_tol;
+  inner_loop_setting.snopt_scaling = FLAGS_snopt_scaling;
+  inner_loop_setting.directory = dir;
 
   // Reduced order model parameters
-  int rom_option = (FLAGS_rom_option >= 0) ? FLAGS_rom_option : 0;
-  int n_s = 0;
+  int n_y = 0;
   int n_tau = 0;
-  setRomDim(&n_s, &n_tau, rom_option);
-  int n_sDDot = n_s; // Assume that are the same (no quaternion)
-  MatrixXd B_tau = MatrixXd::Zero(n_sDDot, n_tau);
-  setRomBMatrix(&B_tau, rom_option);
+  setRomDim(&n_y, &n_tau, FLAGS_rom_option);
+  int n_yddot = n_y; // Assume that are the same (no quaternion)
+  MatrixXd B_tau = MatrixXd::Zero(n_yddot, n_tau);
+  setRomBMatrix(&B_tau, FLAGS_rom_option);
+  writeCSV(dir + string("B_tau.csv"), B_tau);
 
   // Reduced order model setup
-  KinematicsExpression<double> kin_expression(n_s, 0, &plant, FLAGS_robot_option);
-  DynamicsExpression dyn_expression(n_sDDot, 0, rom_option, FLAGS_robot_option);
+  KinematicsExpression<double> kin_expression(n_y, 0, &plant, FLAGS_robot_option);
+  DynamicsExpression dyn_expression(n_yddot, 0, FLAGS_rom_option, FLAGS_robot_option);
   VectorXd dummy_q = VectorXd::Ones(plant.num_positions());
-  VectorXd dummy_s = VectorXd::Ones(n_s);
-  int n_feature_s = kin_expression.getFeature(dummy_q).size();
-  int n_feature_sDDot =
+  VectorXd dummy_s = VectorXd::Ones(n_y);
+  int n_feature_y = kin_expression.getFeature(dummy_q).size();
+  int n_feature_yddot =
       dyn_expression.getFeature(dummy_s, dummy_s).size();
-  int n_theta_s = n_s * n_feature_s;
-  int n_theta_sDDot = n_sDDot * n_feature_sDDot;
-  VectorXd theta_s(n_theta_s);
-  VectorXd theta_sDDot(n_theta_sDDot);
+  int n_theta_y = n_y * n_feature_y;
+  int n_theta_yddot = n_yddot * n_feature_yddot;
 
   // Initial guess of theta
-  theta_s = VectorXd::Zero(n_theta_s);
-  theta_sDDot = VectorXd::Zero(n_theta_sDDot);
+  theta_y = VectorXd::Zero(n_theta_y);
+  theta_yDDot = VectorXd::Zero(n_theta_yDDot);
   if(FLAGS_use_optimized_model){
     //you have to specify the theta to use
     DRAKE_DEMAND(FLAGS_theta_index>=0);
@@ -403,18 +413,14 @@ void trajOptGivenModel(double stride_length, double ground_incline,
 
     const string dir_find_models = "../dairlib_data/goldilocks_models/find_models/robot_" +
         to_string(FLAGS_robot_option) + "/";
-    readThetaFromFiles(dir_find_models, theta_idx, theta_s, theta_sDDot);
+    readThetaFromFiles(dir_find_models, theta_idx, theta_y, theta_yDDot);
   }
   else{
-    setInitialTheta(theta_s, theta_sDDot, n_feature_s, rom_option);
+    setInitialTheta(theta_y, theta_yDDot, n_feature_y, rom_option);
   }
 
-  double duration = 0.4;
-  if (FLAGS_robot_option == 0) {
-    duration = 0.746;  // Fix the duration now since we add cost ourselves
-  } else if (FLAGS_robot_option == 1) {
-    duration = 0.4; // 0.4;
-  }
+  RomData rom = RomData(n_y, n_tau, n_feature_y, n_feature_yddot, B_tau,
+                        theta_y, theta_yddot);
 
   bool is_get_nominal = FLAGS_is_get_nominal;
   int max_inner_iter_pass_in = is_get_nominal ? 200 : max_inner_iter;
@@ -425,8 +431,14 @@ void trajOptGivenModel(double stride_length, double ground_incline,
   int sample_idx = 0;
   string prefix = to_string(num) +  "_" + to_string(sample_idx) + "_";
 
+  inner_loop_setting.n_node = n_node_vec[sample_idx];
+  inner_loop_setting.max_iter = max_inner_iter_pass_in;
+  inner_loop_setting.prefix = prefix;
+  inner_loop_setting.init_file = init_file_pass_in;
+
   // Vectors/Matrices for the outer loop
   int N_sample = 1;
+  SubQpData QPs(N_sample);
   vector<std::shared_ptr<VectorXd>> w_sol_vec(N_sample);
   vector<std::shared_ptr<MatrixXd>> H_vec(N_sample);
   vector<std::shared_ptr<VectorXd>> b_vec(N_sample);
@@ -460,36 +472,18 @@ void trajOptGivenModel(double stride_length, double ground_incline,
   double cost_threshold_for_update = std::numeric_limits<double>::infinity();
   int N_rerun = 0;
   //run trajectory optimization
-  trajOptGivenWeights(std::ref(plant), std::ref(plant_autoDiff),
-                      n_s, n_sDDot, n_tau,
-                      n_feature_s, n_feature_sDDot, std::ref(B_tau),
-                      std::ref(theta_s), std::ref(theta_sDDot),
-                      stride_length, ground_incline, turning_rate,
-                      duration, n_node, max_inner_iter_pass_in,
-                      FLAGS_major_feasibility_tol, FLAGS_major_feasibility_tol,
-                      std::ref(dir), init_file_pass_in, prefix,
-                      std::ref(w_sol_vec),
-                      std::ref(A_vec),
-                      std::ref(H_vec),
-                      std::ref(y_vec),
-                      std::ref(lb_vec),
-                      std::ref(ub_vec),
-                      std::ref(b_vec),
-                      std::ref(c_vec),
-                      std::ref(B_vec),
-                      std::ref(is_success_vec),
-                      std::ref(thread_finished_vec),
-                      Q, R, all_cost_scale,
-                      FLAGS_eps_regularization,
-                      is_get_nominal,
-                      FLAGS_is_zero_touchdown_impact,
-                      extend_model_this_iter,
-                      FLAGS_is_add_tau_in_cost,
-                      sample_idx, n_rerun,
-                      cost_threshold_for_update, N_rerun,
-                      rom_option,
-                      FLAGS_robot_option,
-                      turn_on_scaling);
+  void trajOptGivenWeights(
+      std::ref(plant),
+      std::ref(plant_autoDiff),
+      std::ref(rom),
+      inner_loop_setting,
+      task,//
+      std::ref(QPs),//
+      std::ref(thread_finished_vec),
+      is_get_nominal,
+      extend_model_this_iter,
+      sample_idx, n_rerun, cost_threshold_for_update, N_rerun,
+      rom_option, FLAGS_robot_option);
 }
 
 //naive test function for search algorithm
@@ -700,8 +694,8 @@ int find_boundary(int argc, char* argv[]){
    */
   cout << "\nBasic information:\n";
   cout << FLAGS_program_name << endl;
-  cout << "robot_option " << FLAGS_robot_option << endl;
-  cout << "rom_option " << FLAGS_rom_option << endl;
+  cout << "robot_option: " << FLAGS_robot_option << endl;
+  cout << "rom_option: " << FLAGS_rom_option << endl;
 
   /*
    * initialize task space
diff --git a/examples/goldilocks_models/find_models/traj_opt_given_weigths.h b/examples/goldilocks_models/find_models/traj_opt_given_weigths.h
index 1794794562..5bbeddaddc 100644
--- a/examples/goldilocks_models/find_models/traj_opt_given_weigths.h
+++ b/examples/goldilocks_models/find_models/traj_opt_given_weigths.h
@@ -56,7 +56,7 @@ void trajOptGivenWeights(
     bool is_get_nominal,
     bool extend_model,
     int sample_idx, int n_rerun, double cost_threshold_for_update, int N_rerun,
-    int rom_option, int robot_option,bool turn_on_scaling);
+    int rom_option, int robot_option);
 
 void addRegularization(bool is_get_nominal, double eps_reg,
                        GoldilocksModelTrajOpt& gm_traj_opt);

From 8029d52f57af5eeb830d1fb8c85c5529edab5e71 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Mon, 29 Jun 2020 15:43:50 -0400
Subject: [PATCH 37/82] Task need to be set later

---
 examples/goldilocks_models/find_boundary.cc | 53 +++++----------------
 1 file changed, 11 insertions(+), 42 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index bc78a80c12..2233ecfc9f 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -334,8 +334,7 @@ string getInitFileName(const string directory, int traj_opt_num,
 }
 
 // trajectory optimization for given task and model
-void trajOptGivenModel(double stride_length, double ground_incline,
-    double turning_rate,const string dir,int num,bool is_rerun,
+void trajOptGivenModel(Task task, const string dir,int num,bool is_rerun,
     int initial_guess_idx=-1,bool turn_on_scaling=true){
   // Create MBP
   drake::logging::set_log_level("err");  // ignore warnings about joint limits
@@ -346,9 +345,6 @@ void trajOptGivenModel(double stride_length, double ground_incline,
   MultibodyPlant<AutoDiffXd> plant_autoDiff(plant);
   cout << endl;
 
-  //Task Setting
-  Task task();
-
   // Parameters for the inner loop optimization
   int max_inner_iter = FLAGS_max_inner_iter;
   if (FLAGS_robot_option == 0) {
@@ -359,17 +355,6 @@ void trajOptGivenModel(double stride_length, double ground_incline,
   double all_cost_scale = 1;
   setCostWeight(&Q, &R, &all_cost_scale, FLAGS_robot_option);
   // Inner loop setup
-  vector<int> n_node_vec(N_sample, 20);
-  if (!FLAGS_fix_node_number) {
-    for (int sample_idx = 0; sample_idx < N_sample; sample_idx++) {
-      task.set(task_gen.NewTask(sample_idx, true));
-      double duration = task.get("stride length") / task.get("velocity");
-      n_node_vec[sample_idx] = int(FLAGS_node_density * duration);
-      cout << n_node_vec[sample_idx] << ", ";
-    } cout << endl;
-    cout << "WARNING: we will only add adjacent samples to the list if it has "
-            "the same number of nodes\n";
-  }
   InnerLoopSetting inner_loop_setting = InnerLoopSetting();
   inner_loop_setting.Q_double = Q;
   inner_loop_setting.R_double = R;
@@ -380,7 +365,7 @@ void trajOptGivenModel(double stride_length, double ground_incline,
   inner_loop_setting.max_iter = max_inner_iter;
   inner_loop_setting.major_optimality_tol = FLAGS_major_optimality_tol;
   inner_loop_setting.major_feasibility_tol = FLAGS_major_feasibility_tol;
-  inner_loop_setting.snopt_scaling = FLAGS_snopt_scaling;
+  inner_loop_setting.snopt_scaling = turn_on_scaling;
   inner_loop_setting.directory = dir;
 
   // Reduced order model parameters
@@ -431,7 +416,7 @@ void trajOptGivenModel(double stride_length, double ground_incline,
   int sample_idx = 0;
   string prefix = to_string(num) +  "_" + to_string(sample_idx) + "_";
 
-  inner_loop_setting.n_node = n_node_vec[sample_idx];
+  inner_loop_setting.n_node = 20;//fix number of nodes
   inner_loop_setting.max_iter = max_inner_iter_pass_in;
   inner_loop_setting.prefix = prefix;
   inner_loop_setting.init_file = init_file_pass_in;
@@ -439,28 +424,6 @@ void trajOptGivenModel(double stride_length, double ground_incline,
   // Vectors/Matrices for the outer loop
   int N_sample = 1;
   SubQpData QPs(N_sample);
-  vector<std::shared_ptr<VectorXd>> w_sol_vec(N_sample);
-  vector<std::shared_ptr<MatrixXd>> H_vec(N_sample);
-  vector<std::shared_ptr<VectorXd>> b_vec(N_sample);
-  vector<std::shared_ptr<VectorXd>> c_vec(N_sample);
-  vector<std::shared_ptr<MatrixXd>> A_vec(N_sample);
-  vector<std::shared_ptr<VectorXd>> lb_vec(N_sample);
-  vector<std::shared_ptr<VectorXd>> ub_vec(N_sample);
-  vector<std::shared_ptr<VectorXd>> y_vec(N_sample);
-  vector<std::shared_ptr<MatrixXd>> B_vec(N_sample);
-  vector<std::shared_ptr<int>> is_success_vec(N_sample);
-  for (int i = 0; i < N_sample; i++) {
-    w_sol_vec[i] = std::make_shared<VectorXd>();
-    H_vec[i] = std::make_shared<MatrixXd>();
-    b_vec[i] = std::make_shared<VectorXd>();
-    c_vec[i] = std::make_shared<VectorXd>();
-    A_vec[i] = std::make_shared<MatrixXd>();
-    lb_vec[i] = std::make_shared<VectorXd>();
-    ub_vec[i] = std::make_shared<VectorXd>();
-    y_vec[i] = std::make_shared<VectorXd>();
-    B_vec[i] = std::make_shared<MatrixXd>();
-    is_success_vec[i] = std::make_shared<int>();
-  }
 
   vector<std::shared_ptr<int>> thread_finished_vec(N_sample);
   for (int i = 0; i < N_sample; i++) {
@@ -477,8 +440,8 @@ void trajOptGivenModel(double stride_length, double ground_incline,
       std::ref(plant_autoDiff),
       std::ref(rom),
       inner_loop_setting,
-      task,//
-      std::ref(QPs),//
+      task,
+      std::ref(QPs),
       std::ref(thread_finished_vec),
       is_get_nominal,
       extend_model_this_iter,
@@ -701,6 +664,12 @@ int find_boundary(int argc, char* argv[]){
    * initialize task space
    */
   cout << "\nInitialize task space:\n";
+  Task task
+  if(FLAGS_robot_option==0)
+  {
+    int dimensions = 3;
+    Task
+  }
   int dimensions = 3;//dimension of the task space
   double stride_length_0 = 0;
   if(FLAGS_robot_option==0){

From 4f85ab592f641215b5a1a6d8221e14ad8ec17ed7 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Mon, 29 Jun 2020 16:32:14 -0400
Subject: [PATCH 38/82] add class

---
 examples/goldilocks_models/find_boundary.cc        | 14 ++++++++++----
 .../find_boundary/SearchSetting.cc                 |  4 ++++
 .../find_boundary/SearchSetting.h                  |  4 ++++
 examples/goldilocks_models/task.h                  |  1 +
 4 files changed, 19 insertions(+), 4 deletions(-)
 create mode 100644 examples/goldilocks_models/find_boundary/SearchSetting.cc
 create mode 100644 examples/goldilocks_models/find_boundary/SearchSetting.h

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 2233ecfc9f..4c5033152e 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -664,13 +664,19 @@ int find_boundary(int argc, char* argv[]){
    * initialize task space
    */
   cout << "\nInitialize task space:\n";
-  Task task
+  Task task;
+  SearchSetting search_setting;
   if(FLAGS_robot_option==0)
   {
-    int dimensions = 3;
-    Task
+    task = Task({"stride length", "ground incline",
+                 "velocity"});
   }
-  int dimensions = 3;//dimension of the task space
+  else{
+    vector<string> task_names = {"stride length", "ground incline",
+                                 "velocity", "turning rate"};
+    task = Task(task_names);
+  }
+  int dimensions = task.dim();//dimension of the task space
   double stride_length_0 = 0;
   if(FLAGS_robot_option==0){
     stride_length_0 = 0.2;
diff --git a/examples/goldilocks_models/find_boundary/SearchSetting.cc b/examples/goldilocks_models/find_boundary/SearchSetting.cc
new file mode 100644
index 0000000000..52285c9c31
--- /dev/null
+++ b/examples/goldilocks_models/find_boundary/SearchSetting.cc
@@ -0,0 +1,4 @@
+//
+// Created by jianshu on 6/29/20.
+//
+
diff --git a/examples/goldilocks_models/find_boundary/SearchSetting.h b/examples/goldilocks_models/find_boundary/SearchSetting.h
new file mode 100644
index 0000000000..52285c9c31
--- /dev/null
+++ b/examples/goldilocks_models/find_boundary/SearchSetting.h
@@ -0,0 +1,4 @@
+//
+// Created by jianshu on 6/29/20.
+//
+
diff --git a/examples/goldilocks_models/task.h b/examples/goldilocks_models/task.h
index cb174f7e2a..9076d8554f 100644
--- a/examples/goldilocks_models/task.h
+++ b/examples/goldilocks_models/task.h
@@ -39,6 +39,7 @@ class Task {
   double get(const string& name) const {
     return task_.at(name_to_index_map_.at(name));
   }
+  int dim() {return task_dim_};
   const std::vector<double>& get() const { return task_; }
   void set(const std::vector<double>& values) {
     DRAKE_DEMAND(values.size() == (unsigned)task_dim_);

From 2cfd8262d05cd8d1304dabc271a32d02f7dbf496 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Mon, 29 Jun 2020 23:41:33 -0400
Subject: [PATCH 39/82] add search_setting into main functions

---
 examples/goldilocks_models/BUILD.bazel        |  19 +++
 examples/goldilocks_models/find_boundary.cc   |  63 ++++-----
 .../find_boundary/BUILD.bazel                 | 133 ++----------------
 .../find_boundary/SearchSetting.cc            |   4 -
 .../find_boundary/SearchSetting.h             |   4 -
 .../find_boundary/search_setting.cc           |  23 +++
 .../find_boundary/search_setting.h            |  33 +++++
 7 files changed, 115 insertions(+), 164 deletions(-)
 delete mode 100644 examples/goldilocks_models/find_boundary/SearchSetting.cc
 delete mode 100644 examples/goldilocks_models/find_boundary/SearchSetting.h
 create mode 100644 examples/goldilocks_models/find_boundary/search_setting.cc
 create mode 100644 examples/goldilocks_models/find_boundary/search_setting.h

diff --git a/examples/goldilocks_models/BUILD.bazel b/examples/goldilocks_models/BUILD.bazel
index d06ef97599..0c33f085d3 100644
--- a/examples/goldilocks_models/BUILD.bazel
+++ b/examples/goldilocks_models/BUILD.bazel
@@ -19,6 +19,25 @@ cc_binary(
     ],
 )
 
+cc_binary(
+    name = "find_boundary",
+    srcs = ["find_boundary.cc"],
+    deps = [
+        ":dynamics_expression",
+        ":goldilocks_utils",
+        ":kinematics_expression",
+        ":task",
+        "//common:eigen_utils",
+        "//examples/goldilocks_models/find_boundary:search_setting",
+        "//examples/goldilocks_models/find_models:initial_guess",
+        "//examples/goldilocks_models/find_models:traj_opt_given_weigths",
+        "//systems/goldilocks_models",
+        "//systems/trajectory_optimization:dircon",
+        "@drake//:drake_shared_library",
+        "@gflags",
+    ],
+)
+
 cc_binary(
     name = "plan_with_RoM_FoM",
     srcs = ["plan_with_RoM_FoM.cc"],
diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 4c5033152e..19db707354 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -3,7 +3,6 @@
 //
 
 #include <gflags/gflags.h>
-#include <stdio.h>  // For removing files
 #include <thread>  // multi-threading
 #include <chrono>
 #include <ctime>
@@ -12,23 +11,24 @@
 #include <utility>  // std::pair, std::make_pair
 #include <bits/stdc++.h>  // system call
 #include <cmath>
-#include <numeric> // std::accumulate
 #include <tuple>
 #include <Eigen/QR>  // CompleteOrthogonalDecomposition
 
 #include "drake/multibody/parsing/parser.h"
-#include "drake/solvers/choose_best_solver.h"
 #include "drake/solvers/mathematical_program.h"
 #include "drake/solvers/snopt_solver.h"
 #include "drake/solvers/solve.h"
 
+#include "common/eigen_utils.h"
 #include "common/find_resource.h"
 #include "examples/goldilocks_models/dynamics_expression.h"
 #include "examples/goldilocks_models/find_models/traj_opt_given_weigths.h"
-#include "examples/goldilocks_models/kinematics_expression.h"
 #include "examples/goldilocks_models/goldilocks_utils.h"
-#include "examples/goldilocks_models/initial_guess.h"
+#include "examples/goldilocks_models/find_models/initial_guess.h"
+#include "examples/goldilocks_models/kinematics_expression.h"
+#include "examples/goldilocks_models/task.h"
 #include "systems/goldilocks_models/file_utils.h"
+#include "examples/goldilocks_models/find_boundary/search_setting.h"
 
 using std::cin;
 using std::cout;
@@ -664,44 +664,35 @@ int find_boundary(int argc, char* argv[]){
    * initialize task space
    */
   cout << "\nInitialize task space:\n";
-  Task task;
-  SearchSetting search_setting;
+  Task task;//Traj Opt tasks
+  SearchSetting search_setting;//Settings of searching the task space
   if(FLAGS_robot_option==0)
   {
     task = Task({"stride length", "ground incline",
                  "velocity"});
+    search_setting = SearchSetting(3,{"stride length", "ground incline",
+                                      "velocity"},{0.25,0,0.4},
+                                          {0.01,0.01,0});
   }
   else{
-    vector<string> task_names = {"stride length", "ground incline",
-                                 "velocity", "turning rate"};
-    task = Task(task_names);
-  }
-  int dimensions = task.dim();//dimension of the task space
-  double stride_length_0 = 0;
-  if(FLAGS_robot_option==0){
-    stride_length_0 = 0.2;
+    task = Task({"stride length", "ground incline",
+                 "velocity", "turning rate"});
+    search_setting = SearchSetting(4,{"stride length", "ground incline",
+                                      "velocity","turning rate"},
+                                   {0.3,0,0.5,0},{0.01,0.01,0,0});
   }
-  else if(FLAGS_robot_option==1){
-      stride_length_0 = 0.2;
+  //cout initial point information
+  int dim = 0;
+  for(dim = 0;dim<search_setting.search_dim();dim++)
+  {
+    cout<<"initial "<<search_setting.names()[dim]<<
+    ": "+to_string(search_setting.task_0()[dim])<<endl;
   }
-  double delta_stride_length = 0.01;
-  cout<<"initial stride length "<<stride_length_0<<endl;
-  cout<<"delta stride length "<<delta_stride_length<<endl;
-
-  double ground_incline_0 = 0;
-  double delta_ground_incline = 0.01;
-  cout<<"initial ground incline "<<ground_incline_0<<endl;
-  cout<<"delta ground incline "<<delta_ground_incline<<endl;
-
-  double turning_rate_0 = 0;
-  double delta_turning_rate = 0;
-  cout<<"initial turning rate "<<turning_rate_0<<endl;
-  cout<<"delta turning rate "<<delta_turning_rate<<endl;
 
-  VectorXd initial_gamma(dimensions);
-  initial_gamma<<stride_length_0,ground_incline_0,turning_rate_0;
-  VectorXd delta(dimensions);
-  delta<<delta_stride_length,delta_ground_incline,delta_turning_rate;
+  VectorXd initial_task = Eigen::Map<VectorXd>(search_setting.task_0().data(),
+      search_setting.task_0().size());
+  VectorXd delta_task = Eigen::Map<VectorXd>(search_setting.task_delta().data(),
+      search_setting.task_delta().size());
 
 
   /*
@@ -732,8 +723,8 @@ int find_boundary(int argc, char* argv[]){
     }
   }
   cout<<"cost_threshold "<<cost_threshold<<endl;
-  int boundary_sample_num = 0;//use this to set the index for boundary point
-  int traj_opt_num = 0;//use this to set the index for Traj Opt
+  int boundary_sample_num = 0;//use this to record the index of boundary point
+  int traj_opt_num = 0;//use this to record the index of Traj Opt
   VectorXd extend_direction(dimensions);//the direction of searching
 
   //create components for each dimension used to decide the direction
diff --git a/examples/goldilocks_models/find_boundary/BUILD.bazel b/examples/goldilocks_models/find_boundary/BUILD.bazel
index e44136ded7..8b8dc9e817 100644
--- a/examples/goldilocks_models/find_boundary/BUILD.bazel
+++ b/examples/goldilocks_models/find_boundary/BUILD.bazel
@@ -1,15 +1,4 @@
 package(default_visibility = ["//visibility:public"])
-#
-#cc_binary(
-#    name = "visualize_gait_RBP",
-#    srcs = ["visualize_gait_RBP.cc"],
-#    data = glob(["examples/goldilocks_models/PlanarWalkerWithTorso.urdf"]),
-#    deps = [
-#        "//systems/goldilocks_models",
-#        "@drake//:drake_shared_library",
-#        "@gflags",
-#    ],
-#)
 
 cc_binary(
     name = "visualize_gait_MBP",
@@ -25,112 +14,16 @@ cc_binary(
     ],
 )
 
-#cc_library(
-#    name = "goldilocks_model_traj_opt",
-#    srcs = ["goldilocks_model_traj_opt.cc"],
-#    hdrs = ["goldilocks_model_traj_opt.h"],
-#    deps = [
-#        "//common",
-#        "//examples/goldilocks_models/find_models:dynamics_constraint",
-#        "//examples/goldilocks_models/find_models:kinematics_constraint",
-#        "//solvers:optimization_utils",
-#        "//systems/goldilocks_models",
-#        "//systems/primitives",
-#        "//systems/trajectory_optimization:dircon",
-#        "@drake//:drake_shared_library",
-#    ],
-#)
-
-#cc_library(
-#    name = "traj_opt_given_weigths",
-#    srcs = ["traj_opt_given_weigths.cc"],
-#    hdrs = ["traj_opt_given_weigths.h"],
-#    deps = [
-#        ":traj_opt_helper_func",
-#        "//common",
-#        "//examples/goldilocks_models:dynamics_expression",
-#        "//examples/goldilocks_models:goldilocks_utils",
-#        "//examples/goldilocks_models/find_models:goldilocks_model_traj_opt",
-#        "//solvers:nonlinear_constraint",
-#        "//solvers:optimization_utils",
-#        "//systems/goldilocks_models",
-#        "//systems/primitives",
-#        "//systems/trajectory_optimization:dircon",
-#        "@drake//:drake_shared_library",
-#    ],
-#)
-##        "@gurobi//:gurobi_cxx",
-#
-#cc_library(
-#    name = "kinematics_constraint",
-#    srcs = [
-#        "kinematics_constraint.cc",
-#    ],
-#    hdrs = [
-#        "kinematics_constraint.h",
-#    ],
-#    deps = [
-#        "//examples/goldilocks_models:kinematics_expression",
-#        "//multibody:utils",
-#        "@drake//:drake_shared_library",
-#    ],
-#)
-#
-#cc_library(
-#    name = "dynamics_constraint",
-#    srcs = [
-#        "dynamics_constraint.cc",
-#    ],
-#    hdrs = [
-#        "dynamics_constraint.h",
-#    ],
-#    deps = [
-#        "//examples/goldilocks_models:dynamics_expression",
-#        "//examples/goldilocks_models:kinematics_expression",
-#        "//multibody:utils",
-#        "//solvers:nonlinear_constraint",
-#        "//systems/trajectory_optimization:dircon",
-#        "@drake//:drake_shared_library",
-#    ],
-#)
-#
-#cc_binary(
-#    name = "scale_theta",
-#    srcs = ["scale_theta.cc"],
-#    deps = [
-#        "//common",
-#        "//examples/goldilocks_models:dynamics_expression",
-#        "//examples/goldilocks_models:goldilocks_utils",
-#        "//examples/goldilocks_models:kinematics_expression",
-#        "//systems/goldilocks_models",
-#        "@drake//:drake_shared_library",
-#        "@gflags",
-#    ],
-#)
-#
-#cc_binary(
-#    name = "postadd_cost_without_tau",
-#    srcs = ["postadd_cost_without_tau.cc"],
-#    deps = [
-#        "//systems/goldilocks_models",
-#        "@drake//:drake_shared_library",
-#        "@gflags",
-#    ],
-#)
-#
-#cc_library(
-#    name = "traj_opt_helper_func",
-#    srcs = [
-#        "traj_opt_helper_func.cc",
-#    ],
-#    hdrs = [
-#        "traj_opt_helper_func.h",
-#    ],
-#    deps = [
-#        "//common",
-#        "//multibody:utils",
-#        "//solvers:optimization_utils",
-#        "//systems/trajectory_optimization:dircon",
-#        "@drake//:drake_shared_library",
-#    ],
-#)
+cc_library(
+    name = "search_setting",
+    srcs = [
+        "search_setting.cc",
+    ],
+    hdrs = [
+        "search_setting.h",
+    ],
+    deps = [
+        "//common",
+        "@drake//:drake_shared_library",
+    ],
+)
diff --git a/examples/goldilocks_models/find_boundary/SearchSetting.cc b/examples/goldilocks_models/find_boundary/SearchSetting.cc
deleted file mode 100644
index 52285c9c31..0000000000
--- a/examples/goldilocks_models/find_boundary/SearchSetting.cc
+++ /dev/null
@@ -1,4 +0,0 @@
-//
-// Created by jianshu on 6/29/20.
-//
-
diff --git a/examples/goldilocks_models/find_boundary/SearchSetting.h b/examples/goldilocks_models/find_boundary/SearchSetting.h
deleted file mode 100644
index 52285c9c31..0000000000
--- a/examples/goldilocks_models/find_boundary/SearchSetting.h
+++ /dev/null
@@ -1,4 +0,0 @@
-//
-// Created by jianshu on 6/29/20.
-//
-
diff --git a/examples/goldilocks_models/find_boundary/search_setting.cc b/examples/goldilocks_models/find_boundary/search_setting.cc
new file mode 100644
index 0000000000..88dfa6ad8b
--- /dev/null
+++ b/examples/goldilocks_models/find_boundary/search_setting.cc
@@ -0,0 +1,23 @@
+//
+// Created by jianshu on 6/29/20.
+//
+#include "examples/goldilocks_models/find_boundary/search_setting.h"
+
+
+namespace dairlib {
+namespace goldilocks_models {
+
+SearchSetting::SearchSetting(int search_dim,
+                             std::vector <string> names,
+                             std::vector<double> task_0,
+                             std::vector<double> task_delta)
+    : search_dim_(search_dim),
+      names_(names),
+      task_0_(task_0),
+      task_delta_(task_delta) {
+
+}
+
+
+}
+}
\ No newline at end of file
diff --git a/examples/goldilocks_models/find_boundary/search_setting.h b/examples/goldilocks_models/find_boundary/search_setting.h
new file mode 100644
index 0000000000..e0a6a44727
--- /dev/null
+++ b/examples/goldilocks_models/find_boundary/search_setting.h
@@ -0,0 +1,33 @@
+//
+// Created by jianshu on 6/29/20.
+//
+#include <string>
+
+namespace dairlib {
+namespace goldilocks_models {
+
+class SearchSetting {
+ public:
+  // Default constructor
+  SearchSetting(){};
+  SearchSetting(int search_dim, std::vector<string> names,
+      std::vector<double> task_0,std::vector<double> task_delta);
+
+  //getters
+  int search_dim(){return search_dim_};
+  std::vector<string> names(){return names_};
+  std::vector<double> task_0{return task_0_};
+  std::vector<double> task_delta{return task_delta_};
+
+  //setters
+  void SetExtendComponents(){}
+
+ private:
+  int search_dim_;
+  std::vector<string> names_;
+  std::vector<double> task_0_;
+  std:vector<double> task_delta_;
+
+
+}
+}

From 6bbc3256bfe93dd603fb97d2d4ec62655f6553fe Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Tue, 30 Jun 2020 12:26:46 -0400
Subject: [PATCH 40/82] unfinished class searchsetting

---
 examples/goldilocks_models/find_boundary.cc   | 63 +++++++++++--------
 .../find_boundary/search_setting.cc           |  9 +++
 .../find_boundary/search_setting.h            | 27 +++++---
 3 files changed, 63 insertions(+), 36 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 19db707354..1d3df345b4 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -657,8 +657,10 @@ int find_boundary(int argc, char* argv[]){
    */
   cout << "\nBasic information:\n";
   cout << FLAGS_program_name << endl;
-  cout << "robot_option: " << FLAGS_robot_option << endl;
-  cout << "rom_option: " << FLAGS_rom_option << endl;
+  int robot_option = FLAGS_robot_option;
+  int rom_potion = FLAGS_rom_option;
+  cout << "robot_option: " << robot_option << endl;
+  cout << "rom_option: " << rom_option << endl;
 
   /*
    * initialize task space
@@ -666,7 +668,7 @@ int find_boundary(int argc, char* argv[]){
   cout << "\nInitialize task space:\n";
   Task task;//Traj Opt tasks
   SearchSetting search_setting;//Settings of searching the task space
-  if(FLAGS_robot_option==0)
+  if(robot_option==0)
   {
     task = Task({"stride length", "ground incline",
                  "velocity"});
@@ -674,7 +676,7 @@ int find_boundary(int argc, char* argv[]){
                                       "velocity"},{0.25,0,0.4},
                                           {0.01,0.01,0});
   }
-  else{
+  else if(robot_option==1){
     task = Task({"stride length", "ground incline",
                  "velocity", "turning rate"});
     search_setting = SearchSetting(4,{"stride length", "ground incline",
@@ -689,11 +691,10 @@ int find_boundary(int argc, char* argv[]){
     ": "+to_string(search_setting.task_0()[dim])<<endl;
   }
 
-  VectorXd initial_task = Eigen::Map<VectorXd>(search_setting.task_0().data(),
-      search_setting.task_0().size());
-  VectorXd delta_task = Eigen::Map<VectorXd>(search_setting.task_delta().data(),
-      search_setting.task_delta().size());
-
+//  VectorXd initial_task = Eigen::Map<VectorXd>(search_setting.task_0().data(),
+//      search_setting.task_0().size());
+//  VectorXd delta_task = Eigen::Map<VectorXd>(search_setting.task_delta().data(),
+//      search_setting.task_delta().size());
 
   /*
    * Iteration setting
@@ -705,7 +706,7 @@ int find_boundary(int argc, char* argv[]){
   int max_iter = FLAGS_max_outer_iter;
   //TODO:decide the threshold under different situation
   double cost_threshold = 30;
-  if(FLAGS_robot_option==0)
+  if(robot_option==0)
   {
     if(FLAGS_is_get_nominal){
       cost_threshold = 35;
@@ -714,7 +715,7 @@ int find_boundary(int argc, char* argv[]){
       cost_threshold = 30;
     }
   }
-  else{
+  else if(robot_option==1){
     if(FLAGS_is_get_nominal){
       cost_threshold = 35;
     }
@@ -723,30 +724,38 @@ int find_boundary(int argc, char* argv[]){
     }
   }
   cout<<"cost_threshold "<<cost_threshold<<endl;
-  int boundary_sample_num = 0;//use this to record the index of boundary point
-  int traj_opt_num = 0;//use this to record the index of Traj Opt
-  VectorXd extend_direction(dimensions);//the direction of searching
 
   //create components for each dimension used to decide the direction
-  int sl_comp_num = 5;
-  VectorXd e_sl(sl_comp_num);
-  e_sl<<0,-1,-0.5,0.5,1;
-
-  int gi_comp_num = 5;
-  VectorXd e_gi(gi_comp_num);
-  e_gi<<0,-1,-0.5,0.5,1;
-
-  int tr_comp_num = 1;
-  VectorXd e_tr(tr_comp_num);
-  e_tr<<0;
+  if(robot_option==0){
+    search_setting.SetExtendComponents("stride length",5,{0,-1,0.5,0.5,1});
+    search_setting.SetExtendComponents("ground incline",5,{0,-1,0.5,0.5,1});
+    search_setting.SetExtendComponents("velocity",1,{0});
+  }
+  else if(robot_option==1)
+  {
+    search_setting.SetExtendComponents("stride length",5,{0,-1,0.5,0.5,1});
+    search_setting.SetExtendComponents("ground incline",5,{0,-1,0.5,0.5,1});
+    search_setting.SetExtendComponents("velocity",1,{0});
+    search_setting.SetExtendComponents("turning rate",1,{0});
+  }
 
   /*
    * start iteration
    */
+  int boundary_sample_num = 0;//use this to record the index of boundary point
+  int traj_opt_num = 0;//use this to record the index of Traj Opt
   //evaluate initial point
-  cout << "\nCalculate Central Point Cost:\n";
+  VectorXd initial_task = Eigen::Map<VectorXd>(search_setting.task_0().data(),
+      search_setting.task_0().size());
   writeCSV(dir +  to_string(traj_opt_num)  +
-      string("_0_gamma.csv"), initial_gamma);
+      string("_0_task.csv"), initial_task);
+  cout << "\nCalculate Central Point Cost:\n";
+  if(robot_option==0)
+  {
+    cout << "sample# (rerun #) | stride | incline | velocity | init_file | "
+            "Status | Solve time | Cost (tau cost)\n";
+  }
+
   cout << "sample# (rerun #) | stride | incline | turning | init_file | "
           "Status | Solve time | Cost (tau cost)\n";
   trajOptGivenModel(stride_length_0, ground_incline_0,
diff --git a/examples/goldilocks_models/find_boundary/search_setting.cc b/examples/goldilocks_models/find_boundary/search_setting.cc
index 88dfa6ad8b..d5f6df2c7c 100644
--- a/examples/goldilocks_models/find_boundary/search_setting.cc
+++ b/examples/goldilocks_models/find_boundary/search_setting.cc
@@ -16,8 +16,17 @@ SearchSetting::SearchSetting(int search_dim,
       task_0_(task_0),
       task_delta_(task_delta) {
 
+  for (unsigned int i = 0; i < names.size(); i++) {
+    name_to_index_map_[names[i]] = i;
+  }
 }
 
+void SearchSetting::SetExtendComponents(string task_name,int n_element,
+                         vector<double> element){
+  DRAKE_DEMAND(n_element==element.size());
+  n_elements_[name_to_index_map_[task_name]] = n_element;
+  elements_[name_to_index_map_[task_name]] = element;
+}
 
 }
 }
\ No newline at end of file
diff --git a/examples/goldilocks_models/find_boundary/search_setting.h b/examples/goldilocks_models/find_boundary/search_setting.h
index e0a6a44727..3be8186b0d 100644
--- a/examples/goldilocks_models/find_boundary/search_setting.h
+++ b/examples/goldilocks_models/find_boundary/search_setting.h
@@ -2,6 +2,9 @@
 // Created by jianshu on 6/29/20.
 //
 #include <string>
+#include "drake/common/drake_assert.h"
+
+using std::vector
 
 namespace dairlib {
 namespace goldilocks_models {
@@ -10,23 +13,29 @@ class SearchSetting {
  public:
   // Default constructor
   SearchSetting(){};
-  SearchSetting(int search_dim, std::vector<string> names,
-      std::vector<double> task_0,std::vector<double> task_delta);
+  SearchSetting(int search_dim, vector<string> names,
+      vector<double> task_0,vector<double> task_delta);
 
   //getters
   int search_dim(){return search_dim_};
-  std::vector<string> names(){return names_};
-  std::vector<double> task_0{return task_0_};
-  std::vector<double> task_delta{return task_delta_};
+  vector<string> names(){return names_};
+  vector<double> task_0(){return task_0_};
+  vector<double> task_delta(){return task_delta_};
+  int index(string task_name){return name_to_index_map_[task_name]};
 
   //setters
-  void SetExtendComponents(){}
+  void SetExtendComponents(string task_name,int n_element,
+      vector<double> element);
+}
 
  private:
   int search_dim_;
-  std::vector<string> names_;
-  std::vector<double> task_0_;
-  std:vector<double> task_delta_;
+  vector<string> names_;
+  vector<double> task_0_;
+  vector<double> task_delta_;
+  std::unordered_map<string, int> name_to_index_map_;
+  vector<int> n_elements_;
+  vector<vector<double>> elements_;
 
 
 }

From 16bf89e758ad00df246ce2461a86d34c5cb4ec2e Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Tue, 30 Jun 2020 12:35:57 -0400
Subject: [PATCH 41/82] fix small typo

---
 examples/goldilocks_models/find_models/initial_guess.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/goldilocks_models/find_models/initial_guess.cc b/examples/goldilocks_models/find_models/initial_guess.cc
index 2be359cb36..1226e728ae 100644
--- a/examples/goldilocks_models/find_models/initial_guess.cc
+++ b/examples/goldilocks_models/find_models/initial_guess.cc
@@ -159,7 +159,7 @@ string SetInitialGuessByInterpolation(const string& directory, int iter,
       VectorXd past_theta_s =
           readCSV(directory + to_string(past_iter) + string("_theta_y.csv"));
       VectorXd past_theta_sDDot = readCSV(directory + to_string(past_iter) +
-                                          string("_theta_yDDot.csv"));
+                                          string("_theta_yddot.csv"));
       VectorXd past_theta(past_theta_s.rows() + past_theta_sDDot.rows());
       past_theta << past_theta_s, past_theta_sDDot;
       double theta_diff =

From e1ce142196a8ae9c1de2656ae104582e042e384e Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Wed, 1 Jul 2020 14:30:49 -0400
Subject: [PATCH 42/82] Finished modifying the whole program following the
 change of find_goldilocks_models

---
 examples/goldilocks_models/BUILD.bazel        |   2 +-
 examples/goldilocks_models/find_boundary.cc   | 431 ++++++++----------
 .../BUILD.bazel                               |   0
 .../search_setting.cc                         |   8 +-
 .../search_setting.h                          |  26 +-
 .../visualize_gait_MBP.cc                     |   2 +-
 examples/goldilocks_models/task.h             |   4 +-
 7 files changed, 222 insertions(+), 251 deletions(-)
 rename examples/goldilocks_models/{find_boundary => find_boundary_utils}/BUILD.bazel (100%)
 rename examples/goldilocks_models/{find_boundary => find_boundary_utils}/search_setting.cc (77%)
 rename examples/goldilocks_models/{find_boundary => find_boundary_utils}/search_setting.h (57%)
 rename examples/goldilocks_models/{find_boundary => find_boundary_utils}/visualize_gait_MBP.cc (99%)

diff --git a/examples/goldilocks_models/BUILD.bazel b/examples/goldilocks_models/BUILD.bazel
index 0c33f085d3..2a1d70621f 100644
--- a/examples/goldilocks_models/BUILD.bazel
+++ b/examples/goldilocks_models/BUILD.bazel
@@ -28,7 +28,7 @@ cc_binary(
         ":kinematics_expression",
         ":task",
         "//common:eigen_utils",
-        "//examples/goldilocks_models/find_boundary:search_setting",
+        "//examples/goldilocks_models/find_boundary_utils:search_setting",
         "//examples/goldilocks_models/find_models:initial_guess",
         "//examples/goldilocks_models/find_models:traj_opt_given_weigths",
         "//systems/goldilocks_models",
diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 1d3df345b4..5c35ed5797 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -28,7 +28,7 @@
 #include "examples/goldilocks_models/kinematics_expression.h"
 #include "examples/goldilocks_models/task.h"
 #include "systems/goldilocks_models/file_utils.h"
-#include "examples/goldilocks_models/find_boundary/search_setting.h"
+#include "examples/goldilocks_models/find_boundary_utils/search_setting.h"
 
 using std::cin;
 using std::cout;
@@ -61,6 +61,8 @@ DEFINE_int32(rom_option, -1, "");
 
 // inner loop
 DEFINE_string(init_file, "", "Initial Guess for Trajectory Optimization");
+DEFINE_double(major_optimality_tol, 1e-4,
+"tolerance for optimality condition (complementarity gap)");
 DEFINE_double(major_feasibility_tol, 1e-4,
 "nonlinear constraint violation tol");
 DEFINE_int32(
@@ -198,16 +200,16 @@ void setInitialTheta(VectorXd& theta_s, VectorXd& theta_sDDot,
 
 //read theta from files to use optimized model
 void readThetaFromFiles(const string dir,int theta_idx,
-    VectorXd& theta_s, VectorXd& theta_sDDot){
-  theta_s = readCSV(dir + to_string(theta_idx) + string("_theta_s.csv"));
-  theta_sDDot = readCSV(dir + to_string(theta_idx) + string("_theta_sDDot.csv"));
+    VectorXd& theta_y, VectorXd& theta_yddot){
+  theta_y = readCSV(dir + to_string(theta_idx) + string("_theta_y.csv"));
+  theta_yddot = readCSV(dir + to_string(theta_idx) + string("_theta_yddot.csv"));
 }
-
+////TODO:MODIFY THIS FUNCTION!
 //use interpolation to set the initial guess for the trajectory optimization
 string getInitFileName(const string directory, int traj_opt_num,
     bool is_rerun,int rerun_traj_idx=-1){
   VectorXd current_gamma = readCSV(directory + to_string(traj_opt_num)
-                                       + string("_0_gamma.csv"));
+                                       + string("_0_task.csv"));
   int gamma_dimension = current_gamma.size();
   VectorXd gamma_scale(gamma_dimension);
   //paras used to decide gamma scale
@@ -229,7 +231,7 @@ string getInitFileName(const string directory, int traj_opt_num,
   }
   string initial_file_name;
   if(is_rerun){
-    //if not specify which Traj Opt result to use, use its own result to rerun;
+    //if not specify which Traj Opt solution to use, use its own result to rerun;
     //else use the specified one.
     if(rerun_traj_idx==-1) {
       initial_file_name = to_string(traj_opt_num)
@@ -240,47 +242,32 @@ string getInitFileName(const string directory, int traj_opt_num,
           + string("_0_w.csv");
     }
   }else{
+    VectorXd initial_guess;
+    //take out corresponding w and calculate the weight for interpolation
+    MatrixXd w_gamma;
+    VectorXd weight_gamma;
+    int sample_num = 0;
+    string prefix;
+    // initial guess for initial point
     if(traj_opt_num==0){
       //specify initial guess if using optimized model
       //use solutions during ROM optimization process to calculate the initial guess
       if(FLAGS_use_optimized_model){
         const string dir_find_models = "../dairlib_data/goldilocks_models/find_models/robot_" +
             to_string(FLAGS_robot_option) + "/";
-        VectorXd initial_guess;
-        //take out corresponding w and calculate the weight for interpolation
-        MatrixXd w_gamma;
-        VectorXd weight_gamma;
         //calculate the weighted sum of solutions
         int theta_idx = FLAGS_theta_index;
-        int sample_num = 0;
         while(file_exist(dir_find_models + to_string(theta_idx)+ '_' +
         to_string(sample_num)+ string("_is_success.csv"))){
-          //check if this sample is success
-          int is_success = (readCSV(dir_find_models + to_string(theta_idx)+ '_'+
-              to_string(sample_num)+ string("_is_success.csv")))(0,0);
-          if(is_success == 1) {
-            //extract past gamma
-            VectorXd past_gamma = readCSV(dir_find_models + to_string(theta_idx) + string("_")
-                + to_string(sample_num) + string("_task.csv"));
-            //calculate the weight for each sample using the 3-norm of the difference between gamma
-            VectorXd dif_gamma = (past_gamma - current_gamma).array().abs()*gamma_scale.array();
-            VectorXd dif_gamma2 = dif_gamma.array().pow(2);
-            double distance_gamma =  (dif_gamma.transpose() * dif_gamma2)(0,0);
-            //extract the solution of this sample
-            VectorXd w_to_interpolate = readCSV(dir_find_models + to_string(theta_idx)+ '_'+
-                to_string(sample_num) + string("_w.csv"));
-            //concatenate the weight and solution for further calculation
-            w_gamma.conservativeResize(w_to_interpolate.rows(),w_gamma.cols()+1);
-            w_gamma.col(w_gamma.cols()-1) = w_to_interpolate;
-            weight_gamma.conservativeResize(weight_gamma.rows()+1);
-            weight_gamma(weight_gamma.rows()-1)= 1 /distance_gamma;
-          }
+          prefix = to_string(theta_idx)+"_"+to_string(sample_num);
+          //calculate interpolation weight and extract solutions
+          InterpolateAmongDifferentTasks(dir_find_models, prefix,
+              current_gamma,gamma_scale,weight_gamma,w_gamma);
           sample_num = sample_num+1;
         }
-        DRAKE_DEMAND(weight_gamma.rows()>0);
-        //    normalize weight
-        weight_gamma = weight_gamma / weight_gamma.sum();
-        initial_guess = w_gamma * weight_gamma;
+        //calculate interpolated initial guess
+        initial_guess = CalculateInterpolation(weight_gamma,
+            w_gamma);
         //    save initial guess and set init file
         initial_file_name = to_string(traj_opt_num)
             + string("_0_initial_guess.csv");
@@ -291,38 +278,14 @@ string getInitFileName(const string directory, int traj_opt_num,
       }
     }else{
       //use past solutions to calculate interpolated initial guess
-      VectorXd initial_guess;
-      //take out corresponding w and calculate the weight for interpolation
-      MatrixXd w_gamma;
-      VectorXd weight_gamma;
       //calculate the weighted sum of past solutions
-      int sample_num = 0;
       for (sample_num = 0; sample_num < traj_opt_num; sample_num++) {
-        //check if this sample is success
-        int is_success = (readCSV(directory + to_string(sample_num)
-                                      + string("_0_is_success.csv")))(0,0);
-        if(is_success == 1) {
-          //extract past gamma
-          VectorXd past_gamma = readCSV(directory + to_string(sample_num)
-                                            + string("_0_gamma.csv"));
-          //calculate the weight for each sample using the third power of the difference between gamma
-          VectorXd dif_gamma = (past_gamma - current_gamma).array().abs()*gamma_scale.array();
-          VectorXd dif_gamma2 = dif_gamma.array().pow(2);
-          double distance_gamma =  (dif_gamma.transpose() * dif_gamma2)(0,0);
-          //extract the solution of this sample
-          VectorXd w_to_interpolate = readCSV(directory + to_string(sample_num)
-                                                  + string("_0_w.csv"));
-          //concatenate the weight and solution for further calculation
-          w_gamma.conservativeResize(w_to_interpolate.rows(),w_gamma.cols()+1);
-          w_gamma.col(w_gamma.cols()-1) = w_to_interpolate;
-          weight_gamma.conservativeResize(weight_gamma.rows()+1);
-          weight_gamma(weight_gamma.rows()-1)= 1 /distance_gamma;
-        }
+        prefix = to_string(sample_num)+"_"+to_string(0);
+        InterpolateAmongDifferentTasks(directory, prefix,
+            current_gamma,gamma_scale,weight_gamma,w_gamma);
       }
-      DRAKE_DEMAND(weight_gamma.rows()>0);
-      //    normalize weight
-      weight_gamma = weight_gamma / weight_gamma.sum();
-      initial_guess = w_gamma * weight_gamma;
+      initial_guess = CalculateInterpolation(weight_gamma,
+                                             w_gamma);
       //    save initial guess and set init file
       initial_file_name = to_string(traj_opt_num)
           + string("_0_initial_guess.csv");
@@ -343,7 +306,6 @@ void trajOptGivenModel(Task task, const string dir,int num,bool is_rerun,
 
   // Create autoDiff version of the plant
   MultibodyPlant<AutoDiffXd> plant_autoDiff(plant);
-  cout << endl;
 
   // Parameters for the inner loop optimization
   int max_inner_iter = FLAGS_max_inner_iter;
@@ -389,8 +351,8 @@ void trajOptGivenModel(Task task, const string dir,int num,bool is_rerun,
   int n_theta_yddot = n_yddot * n_feature_yddot;
 
   // Initial guess of theta
-  theta_y = VectorXd::Zero(n_theta_y);
-  theta_yDDot = VectorXd::Zero(n_theta_yDDot);
+  VectorXd theta_y = VectorXd::Zero(n_theta_y);
+  VectorXd theta_yddot = VectorXd::Zero(n_theta_yddot);
   if(FLAGS_use_optimized_model){
     //you have to specify the theta to use
     DRAKE_DEMAND(FLAGS_theta_index>=0);
@@ -398,10 +360,10 @@ void trajOptGivenModel(Task task, const string dir,int num,bool is_rerun,
 
     const string dir_find_models = "../dairlib_data/goldilocks_models/find_models/robot_" +
         to_string(FLAGS_robot_option) + "/";
-    readThetaFromFiles(dir_find_models, theta_idx, theta_y, theta_yDDot);
+    readThetaFromFiles(dir_find_models, theta_idx, theta_y, theta_yddot);
   }
   else{
-    setInitialTheta(theta_y, theta_yDDot, n_feature_y, rom_option);
+    setInitialTheta(theta_y, theta_yddot, n_feature_y, FLAGS_rom_option);
   }
 
   RomData rom = RomData(n_y, n_tau, n_feature_y, n_feature_yddot, B_tau,
@@ -435,7 +397,7 @@ void trajOptGivenModel(Task task, const string dir,int num,bool is_rerun,
   double cost_threshold_for_update = std::numeric_limits<double>::infinity();
   int N_rerun = 0;
   //run trajectory optimization
-  void trajOptGivenWeights(
+  trajOptGivenWeights(
       std::ref(plant),
       std::ref(plant_autoDiff),
       std::ref(rom),
@@ -446,143 +408,140 @@ void trajOptGivenModel(Task task, const string dir,int num,bool is_rerun,
       is_get_nominal,
       extend_model_this_iter,
       sample_idx, n_rerun, cost_threshold_for_update, N_rerun,
-      rom_option, FLAGS_robot_option);
+      FLAGS_rom_option, FLAGS_robot_option);
 }
 
-//naive test function for search algorithm
-double sample_result(double stride_length,double ground_incline,
-    double turning_rate){
-  double result = std::numeric_limits<double>::infinity();
-  if( (ground_incline<0.12) && (ground_incline>-0.12)){
-    if((stride_length<0.35) && (stride_length>0.25)){
-      result = 0;
-    }
+//Save boundary point information
+void SaveBoundaryPointInfor(const string dir,int boundary_point_index,
+    int traj_num,const VectorXd& boundary_point){
+  VectorXd boundary_point_infor(boundary_point.rows()+2);
+  double boundary_point_cost = (readCSV(dir + to_string(traj_num) +
+      string("_0_c.csv")))(0, 0);
+  boundary_point_infor << traj_num,boundary_point_cost,boundary_point;
+  writeCSV(dir + to_string(boundary_point_index) +  "_" +
+      string("boundary_point.csv"), boundary_point_infor);
+  cout << "boundary point index | stride length | ground incline"
+          " | velocity | turning rate"<<endl;
+  cout << " \t "<<boundary_point_index;
+  for(int i=0;i<boundary_point.rows();i++)
+  {
+    cout<<"\t" <<" | "<<"\t"<<boundary_point[i];
   }
-  return result;
+  cout<<endl;
 }
 
+//check the solution of trajectory optimization and rerun if necessary
+void CheckSolution(const Task& task, const string dir,int traj_num){
+  int rerun = 0;
+  int max_rerun_num = 4;
+  int is_success;
+  //check if snopt find a solution successfully. If not, rerun the Traj Opt
+  for(rerun=0;rerun<max_rerun_num;rerun++){
+    is_success = (readCSV(dir + to_string(traj_num) +
+        string("_0_is_success.csv")))(0, 0);
+    if(is_success==0){
+      trajOptGivenModel(task, dir, traj_num,true);
+    }
+    else{
+      break;
+    }
+  }
+  //if snopt still can't find a solution, try to use adjacent sample to help
+  for(rerun=0;rerun<max_rerun_num;rerun++){
+    is_success = (readCSV(dir + to_string(traj_num) +
+        string("_0_is_success.csv")))(0, 0);
+    if(is_success==0){
+      trajOptGivenModel(task, dir, traj_num,true,traj_num-1);
+    }
+    else{
+      break;
+    }
+  }
+  //if snopt still failed to find a solution,turn off the scaling option
+  // and try again
+  for(rerun=0;rerun<max_rerun_num;rerun++){
+    is_success = (readCSV(dir + to_string(traj_num) +
+        string("_0_is_success.csv")))(0, 0);
+    if(is_success==0){
+      trajOptGivenModel(task, dir, traj_num,true,-1,false);
+    }
+    else{
+      break;
+    }
+  }
+}
 
 //search the boundary point along one direction
-void boundary_for_one_direction(const string dir,int dims,int max_iteration,
-    VectorXd init_gamma,VectorXd step_direction, VectorXd step_size,
+void boundary_for_one_direction(const string dir,int max_iteration,
+    const Task& initial_task,const VectorXd& step_direction, const VectorXd& step_size,
     double max_cost,int& traj_num,int& boundary_point_idx){
   int iter;
-  int sample_idx = 0;
-  int rerun = 0;
-  int max_rerun_num = 4;
-  int is_success;
-  VectorXd new_gamma(dims);
-  VectorXd last_gamma = init_gamma;
-  VectorXd boundary_point(dims);
-  VectorXd boundary_point_infor(dims+2);
+  Task task=initial_task;//create a copy for future use
+  VectorXd new_task(task.dim());
+  VectorXd last_task = Eigen::Map<const VectorXd>(task.get().data(),
+      task.get().size());
+  VectorXd boundary_point(task.dim());
   VectorXd step = step_size.array()*step_direction.array();
   MatrixXd cost_list;
   double decay_factor;//take a large step at the beginning
-  cout << "sample# (rerun #) | stride | incline | turning | init_file | "
-          "Status | Solve time | Cost (tau cost)\n";
+  cout << "sample# (rerun #) | stride | incline | velocity | turning rate | "
+          "init_file | Status | Solve time | Cost (tau cost)\n";
   for (iter = 1; iter <= max_iteration; iter++){
     decay_factor = 2.5*pow(0.95,iter);
+    //if decay_factor is larger than 1, use it to decrease the step size;
+    //otherwise directly use the step size.
     if(decay_factor>1){
-      new_gamma = last_gamma+decay_factor*step;
-      last_gamma = new_gamma;
+      new_task = last_task+decay_factor*step;
+      last_task = new_task;
     }
     else{
-      new_gamma = last_gamma+step;
-      last_gamma = new_gamma;
+      new_task = last_task+step;
+      last_task = new_task;
     }
     //if stride length is negative or zero,stop searching
-    if(new_gamma[0]<=0){
+    if(new_task[0]<=0){
       boundary_point_idx += 1;
-      boundary_point = new_gamma-step;
-      double boundary_point_cost = (readCSV(dir + to_string(traj_num) +  "_" +
-          to_string(sample_idx) + "_" + string("c.csv")))(0, 0);
-      boundary_point_infor << traj_num,boundary_point,boundary_point_cost;
-      writeCSV(dir + to_string(boundary_point_idx) +  "_" +
-          string("boundary_point.csv"), boundary_point_infor);
-      cout << "boundary point index | stride length | ground incline"
-              " | turning rate"<<endl;
-      cout<<" \t "<<boundary_point_idx<< "\t" <<" | "<<"\t"<<boundary_point[0]
-          <<"\t"<<" | "<<"\t"<<boundary_point[1]<<"\t"<<" | "<<"\t"<<
-          boundary_point[2]<<endl;
+      boundary_point = new_task-step;
+      SaveBoundaryPointInfor(dir,boundary_point_idx,
+          traj_num,boundary_point);
       break;
     }
-    //store stride length, ground incline and turning rate
+    //start searching
+    //set tasks
+    vector<double> new_task_vector(new_task.data(),
+        new_task.data()+new_task.size());
+    task.set(new_task_vector);
+    //save tasks
     traj_num += 1;
-    string prefix = to_string(traj_num) +  "_" + to_string(sample_idx) + "_";
-    writeCSV(dir + prefix +
-        string("gamma.csv"), new_gamma);
+    writeCSV(dir + to_string(traj_num) +
+        string("_0_task.csv"), new_task);
     //run trajectory optimization and judge the solution
-    trajOptGivenModel(new_gamma[0], new_gamma[1],
-        new_gamma[2], dir, traj_num, false);
-    //check if snopt find a solution successfully. If not, rerun the Traj Opt
-    for(rerun=0;rerun<max_rerun_num;rerun++){
-      is_success = (readCSV(dir + prefix + string("is_success.csv")))(0, 0);
-      if(is_success==0){
-        trajOptGivenModel(new_gamma[0], new_gamma[1],
-                          new_gamma[2], dir, traj_num,true);
-      }
-      else{
-        break;
-      }
-    }
-    //if snopt still can't find a solution, try to use adjacent sample to help
-    for(rerun=0;rerun<max_rerun_num;rerun++){
-      is_success = (readCSV(dir + prefix + string("is_success.csv")))(0, 0);
-      if(is_success==0){
-        trajOptGivenModel(new_gamma[0], new_gamma[1],
-                          new_gamma[2], dir, traj_num,true,traj_num-1);
-      }
-      else{
-        break;
-      }
-    }
-    //if snopt still failed to find a solution,turn off the scaling option
-    // and try again
-    for(rerun=0;rerun<max_rerun_num;rerun++){
-      is_success = (readCSV(dir + prefix + string("is_success.csv")))(0, 0);
-      if(is_success==0){
-        trajOptGivenModel(new_gamma[0], new_gamma[1],
-                          new_gamma[2], dir, traj_num,true,-1,false);
-      }
-      else{
-        break;
-      }
-    }
+    trajOptGivenModel(task, dir, traj_num, false);
+    CheckSolution(task,dir,traj_num);
     double sample_cost =
-        (readCSV(dir + prefix + string("c.csv")))(0, 0);
+        (readCSV(dir + to_string(traj_num) + string("_0_c.csv")))(0, 0);
     // without a good initial guess, the initial point is easily stuck in a local minimum
     // use the first sample to judge the solution of initial point
     if(iter==1){
       double initial_cost =
           (readCSV(dir + string("0_0_c.csv")))(0, 0);
       if(initial_cost>1.2*sample_cost){
-        trajOptGivenModel(init_gamma[0], init_gamma[1],
-                          init_gamma[2], dir, 0,true,traj_num);
+        trajOptGivenModel(initial_task, dir, 0,true,traj_num);
       }
     }
     //save the trajectory optimization index and corresponding cost for further use
     cost_list.conservativeResize(cost_list.rows()+1, 2);
     cost_list.row(cost_list.rows()-1)<<traj_num,sample_cost;
 
-    //test search algorithm
-//    double sample_cost = sample_result(new_gamma[0],new_gamma[1],
-//        new_gamma[2]);
-
     if(sample_cost>max_cost){
       boundary_point_idx += 1;
-      boundary_point = new_gamma-step;
-      double boundary_point_cost = sample_cost;
-      boundary_point_infor << traj_num,boundary_point,boundary_point_cost;
-      writeCSV(dir + to_string(boundary_point_idx) +  "_" +
-          string("boundary_point.csv"), boundary_point_infor);
-      cout << "boundary point index | stride length | ground incline"
-              " | turning rate"<<endl;
-      cout<<" \t "<<boundary_point_idx<< "\t" <<" | "<<"\t"<<boundary_point[0]
-          <<"\t"<<" | "<<"\t"<<boundary_point[1]<<"\t"<<" | "<<"\t"<<
-          boundary_point[2]<<endl;
+      boundary_point = new_task-step;
+      SaveBoundaryPointInfor(dir,boundary_point_idx,
+                             traj_num,boundary_point);
       break;
     }
   }
+
   cout << "\nStart checking the cost:\n";
   //check the adjacent sample to avoid being stuck in local minimum
   int traj_idx;
@@ -591,45 +550,26 @@ void boundary_for_one_direction(const string dir,int dims,int max_iteration,
     //if cost is larger than adjacent sample, rerun with adjacent sample result
     if( (cost_list(iter,1) > 1.2*cost_list(iter-1,1)) &&
       (cost_list(iter,1) > 1.2*cost_list(iter+1,1)) ){
-      VectorXd gamma_to_rerun = readCSV(dir + to_string(traj_idx)
-                                           + string("_0_gamma.csv"));
+      VectorXd task_to_rerun = readCSV(dir + to_string(traj_idx)
+                                           + string("_0_task.csv"));
+      vector<double> task_vector(task_to_rerun.data(),
+          task_to_rerun.data()+task_to_rerun.size());
+      task.set(task_vector);
       //choose the result of sample with lower cost as initial guess
       if(cost_list(iter-1,1)<cost_list(iter+1,1)){
-        trajOptGivenModel(gamma_to_rerun[0], gamma_to_rerun[1],
-                          gamma_to_rerun[2], dir, traj_idx,
+        trajOptGivenModel(task, dir, traj_idx,
                           true, traj_idx-1);
         //make sure this sample success
-        for(rerun=0;rerun<max_rerun_num;rerun++){
-          is_success = (readCSV(dir + to_string(traj_idx) +
-              string("_0_is_success.csv")))(0, 0);
-          if(is_success==0){
-            trajOptGivenModel(new_gamma[0], new_gamma[1],
-                              new_gamma[2], dir, traj_idx,true);
-          }
-          else{
-            break;
-          }
-        }
+        CheckSolution(task,dir,traj_idx);
       }
       else{
-        trajOptGivenModel(gamma_to_rerun[0], gamma_to_rerun[1],
-                          gamma_to_rerun[2], dir, traj_idx,
+        trajOptGivenModel(task, dir, traj_idx,
                           true, traj_idx+1);
         //make sure this sample success
-        for(rerun=0;rerun<max_rerun_num;rerun++){
-          is_success = (readCSV(dir + to_string(traj_idx) +
-              string("_0_is_success.csv")))(0, 0);
-          if(is_success==0){
-            trajOptGivenModel(new_gamma[0], new_gamma[1],
-                              new_gamma[2], dir, traj_idx,true);
-          }
-          else{
-            break;
-          }
-        }
+        CheckSolution(task,dir,traj_idx);
       }
       //update cost list
-      cost_list(iter,1) = readCSV(dir + to_string(iter)
+      cost_list(iter,1) = readCSV(dir + to_string(traj_idx)
                                       + string("_0_c.csv"))(0,0);
     }
   }
@@ -658,7 +598,7 @@ int find_boundary(int argc, char* argv[]){
   cout << "\nBasic information:\n";
   cout << FLAGS_program_name << endl;
   int robot_option = FLAGS_robot_option;
-  int rom_potion = FLAGS_rom_option;
+  int rom_option = FLAGS_rom_option;
   cout << "robot_option: " << robot_option << endl;
   cout << "rom_option: " << rom_option << endl;
 
@@ -674,28 +614,23 @@ int find_boundary(int argc, char* argv[]){
                  "velocity"});
     search_setting = SearchSetting(3,{"stride length", "ground incline",
                                       "velocity"},{0.25,0,0.4},
-                                          {0.01,0.01,0});
+                                          {0.01,0.01,0.02});
   }
   else if(robot_option==1){
     task = Task({"stride length", "ground incline",
                  "velocity", "turning rate"});
     search_setting = SearchSetting(4,{"stride length", "ground incline",
                                       "velocity","turning rate"},
-                                   {0.3,0,0.5,0},{0.01,0.01,0,0});
+                                   {0.3,0,0.5,0},{0.01,0.01,0.02,0.01});
   }
   //cout initial point information
   int dim = 0;
-  for(dim = 0;dim<search_setting.search_dim();dim++)
+  for(dim = 0;dim<search_setting.task_dim();dim++)
   {
     cout<<"initial "<<search_setting.names()[dim]<<
     ": "+to_string(search_setting.task_0()[dim])<<endl;
   }
 
-//  VectorXd initial_task = Eigen::Map<VectorXd>(search_setting.task_0().data(),
-//      search_setting.task_0().size());
-//  VectorXd delta_task = Eigen::Map<VectorXd>(search_setting.task_delta().data(),
-//      search_setting.task_delta().size());
-
   /*
    * Iteration setting
    */
@@ -725,7 +660,9 @@ int find_boundary(int argc, char* argv[]){
   }
   cout<<"cost_threshold "<<cost_threshold<<endl;
 
-  //create components for each dimension used to decide the direction
+  // create components for each dimension used to decide the direction
+  // considering we can only visualize 2D landscape, we only search within
+  // 2D space and fix other dimension
   if(robot_option==0){
     search_setting.SetExtendComponents("stride length",5,{0,-1,0.5,0.5,1});
     search_setting.SetExtendComponents("ground incline",5,{0,-1,0.5,0.5,1});
@@ -747,44 +684,70 @@ int find_boundary(int argc, char* argv[]){
   //evaluate initial point
   VectorXd initial_task = Eigen::Map<VectorXd>(search_setting.task_0().data(),
       search_setting.task_0().size());
+  task.set(search_setting.task_0());
   writeCSV(dir +  to_string(traj_opt_num)  +
       string("_0_task.csv"), initial_task);
   cout << "\nCalculate Central Point Cost:\n";
-  if(robot_option==0)
-  {
-    cout << "sample# (rerun #) | stride | incline | velocity | init_file | "
-            "Status | Solve time | Cost (tau cost)\n";
-  }
-
-  cout << "sample# (rerun #) | stride | incline | turning | init_file | "
-          "Status | Solve time | Cost (tau cost)\n";
-  trajOptGivenModel(stride_length_0, ground_incline_0,
-                    turning_rate_0, dir, traj_opt_num, false);
+  cout << "sample# (rerun #) | stride | incline | velocity | turning rate | "
+          "init_file | Status | Solve time | Cost (tau cost)\n";
+  trajOptGivenModel(task, dir, traj_opt_num, false);
   //make sure solution found for the initial point
   int init_is_success = (readCSV(dir + string("0_0_is_success.csv")))(0,0);
   while(!init_is_success){
-    trajOptGivenModel(stride_length_0, ground_incline_0,
-                      turning_rate_0, dir, traj_opt_num, true);
+    trajOptGivenModel(task, dir, traj_opt_num, true);
     init_is_success = (readCSV(dir + string("0_0_is_success.csv")))(0,0);
   }
 
-  int dim1,dim2,dim3;
-  VectorXd step(dimensions);
-  //for all the direction, search the boundary
-  for (dim1=0;dim1<sl_comp_num;dim1++){
-    for(dim2=0;dim2<gi_comp_num;dim2++){
-      for(dim3=0;dim3<tr_comp_num;dim3++){
-        extend_direction << e_sl[dim1],e_gi[dim2],e_tr[dim3];
-        //filter with infinity norm to avoid repetitive searching
-        //search along the direction
-        if( (extend_direction.lpNorm<Eigen::Infinity>()) >=1){
-          cout << "Start searching along direction: ["<<extend_direction[0]
-               <<","<<extend_direction[1]<<","<<extend_direction[2]<<"]"<<endl;
-          //normalize the direction vector
-          //extend_direction = extend_direction/extend_direction.norm();
-          boundary_for_one_direction(dir,dimensions,max_iter,
-              initial_gamma,extend_direction,delta,
-              cost_threshold,traj_opt_num,boundary_sample_num);
+  // find which task dimension to search
+  dim = 0;
+  VectorXd extend_direction;
+  if(robot_option==0)
+  {
+    VectorXd extend_direction(3);
+  }
+  else if(robot_option==1)
+  {
+    VectorXd extend_direction(4);
+  }
+  int ele1,ele2,ele3,ele4;
+  // search the boundary
+  VectorXd task_delta = Eigen::Map<VectorXd>(search_setting.task_delta().data(),
+      search_setting.task_delta().size());
+  for (ele1=0;ele1<search_setting.get_n_element(0);ele1++){
+    for(ele2=0;ele2<search_setting.get_n_element(1);ele2++){
+      for(ele3=0;ele3<search_setting.get_n_element(2);ele3++){
+        if(robot_option==0) {
+          extend_direction << search_setting.get_element(0,ele1),
+              search_setting.get_element(1,ele2),
+              search_setting.get_element(2,ele3);
+          //filter with infinity norm to avoid repetitive searching
+          //search along the direction
+          if ((extend_direction.lpNorm<Eigen::Infinity>()) >= 1) {
+            cout << "Start searching along direction: [" << extend_direction[0]
+                 << "," << extend_direction[1] << "," << extend_direction[2]
+                 << "]" << endl;
+            boundary_for_one_direction(dir,max_iter,
+                task, extend_direction, task_delta,
+                cost_threshold,traj_opt_num,boundary_sample_num);
+          }
+        }
+        else if(robot_option==1){
+          for(ele4=0;ele4<search_setting.get_n_element(3);ele4++){
+            extend_direction << search_setting.get_element(0,ele1),
+                search_setting.get_element(1,ele2),
+                search_setting.get_element(2,ele3),
+                search_setting.get_element(3,ele4);
+            //filter with infinity norm to avoid repetitive searching
+            //search along the direction
+            if ((extend_direction.lpNorm<Eigen::Infinity>()) >= 1) {
+              cout << "Start searching along direction: [" << extend_direction[0]
+                   << "," << extend_direction[1] << "," << extend_direction[2]
+                   << "," << extend_direction[3] << "]" << endl;
+              boundary_for_one_direction(dir,max_iter,
+                  task, extend_direction, task_delta,
+                  cost_threshold,traj_opt_num,boundary_sample_num);
+            }
+          }
         }
       }
     }
diff --git a/examples/goldilocks_models/find_boundary/BUILD.bazel b/examples/goldilocks_models/find_boundary_utils/BUILD.bazel
similarity index 100%
rename from examples/goldilocks_models/find_boundary/BUILD.bazel
rename to examples/goldilocks_models/find_boundary_utils/BUILD.bazel
diff --git a/examples/goldilocks_models/find_boundary/search_setting.cc b/examples/goldilocks_models/find_boundary_utils/search_setting.cc
similarity index 77%
rename from examples/goldilocks_models/find_boundary/search_setting.cc
rename to examples/goldilocks_models/find_boundary_utils/search_setting.cc
index d5f6df2c7c..63f84be117 100644
--- a/examples/goldilocks_models/find_boundary/search_setting.cc
+++ b/examples/goldilocks_models/find_boundary_utils/search_setting.cc
@@ -1,17 +1,17 @@
 //
 // Created by jianshu on 6/29/20.
 //
-#include "examples/goldilocks_models/find_boundary/search_setting.h"
+#include "examples/goldilocks_models/find_boundary_utils/search_setting.h"
 
 
 namespace dairlib {
 namespace goldilocks_models {
 
-SearchSetting::SearchSetting(int search_dim,
+SearchSetting::SearchSetting(int task_dim,
                              std::vector <string> names,
                              std::vector<double> task_0,
                              std::vector<double> task_delta)
-    : search_dim_(search_dim),
+    : task_dim_(task_dim),
       names_(names),
       task_0_(task_0),
       task_delta_(task_delta) {
@@ -23,7 +23,7 @@ SearchSetting::SearchSetting(int search_dim,
 
 void SearchSetting::SetExtendComponents(string task_name,int n_element,
                          vector<double> element){
-  DRAKE_DEMAND(n_element==element.size());
+  DRAKE_DEMAND((unsigned)n_element==element.size());
   n_elements_[name_to_index_map_[task_name]] = n_element;
   elements_[name_to_index_map_[task_name]] = element;
 }
diff --git a/examples/goldilocks_models/find_boundary/search_setting.h b/examples/goldilocks_models/find_boundary_utils/search_setting.h
similarity index 57%
rename from examples/goldilocks_models/find_boundary/search_setting.h
rename to examples/goldilocks_models/find_boundary_utils/search_setting.h
index 3be8186b0d..1ff1ee069d 100644
--- a/examples/goldilocks_models/find_boundary/search_setting.h
+++ b/examples/goldilocks_models/find_boundary_utils/search_setting.h
@@ -2,9 +2,12 @@
 // Created by jianshu on 6/29/20.
 //
 #include <string>
+#include <vector>
+#include <unordered_map>
 #include "drake/common/drake_assert.h"
 
-using std::vector
+using std::vector;
+using std::string;
 
 namespace dairlib {
 namespace goldilocks_models {
@@ -13,30 +16,33 @@ class SearchSetting {
  public:
   // Default constructor
   SearchSetting(){};
-  SearchSetting(int search_dim, vector<string> names,
+  SearchSetting(int task_dim, vector<string> names,
       vector<double> task_0,vector<double> task_delta);
 
   //getters
-  int search_dim(){return search_dim_};
-  vector<string> names(){return names_};
-  vector<double> task_0(){return task_0_};
-  vector<double> task_delta(){return task_delta_};
-  int index(string task_name){return name_to_index_map_[task_name]};
+  int task_dim(){return task_dim_;}
+  vector<string> names(){return names_;}
+  vector<double> task_0(){return task_0_;}
+  vector<double> task_delta(){return task_delta_;}
+  int index(string task_name){return name_to_index_map_[task_name];}
+  int get_n_element(int task_dim){return n_elements_[task_dim];}
+  double get_element(int task_index,int element_index){
+    return elements_[task_index][element_index];
+  }
 
   //setters
   void SetExtendComponents(string task_name,int n_element,
       vector<double> element);
-}
 
  private:
-  int search_dim_;
+  int task_dim_;
   vector<string> names_;
   vector<double> task_0_;
   vector<double> task_delta_;
   std::unordered_map<string, int> name_to_index_map_;
   vector<int> n_elements_;
   vector<vector<double>> elements_;
-
+};
 
 }
 }
diff --git a/examples/goldilocks_models/find_boundary/visualize_gait_MBP.cc b/examples/goldilocks_models/find_boundary_utils/visualize_gait_MBP.cc
similarity index 99%
rename from examples/goldilocks_models/find_boundary/visualize_gait_MBP.cc
rename to examples/goldilocks_models/find_boundary_utils/visualize_gait_MBP.cc
index f80bcfbafa..9007eb5e8e 100644
--- a/examples/goldilocks_models/find_boundary/visualize_gait_MBP.cc
+++ b/examples/goldilocks_models/find_boundary_utils/visualize_gait_MBP.cc
@@ -84,7 +84,7 @@ void visualizeGait(int argc, char* argv[]) {
   int n_step = FLAGS_n_step;  // Should be > 0
   /*const string directory = "examples/goldilocks_models/find_models/data/robot_"
                            + to_string(FLAGS_robot_option) + "/";*/
-  const string directory = "../dairlib_data/goldilocks_models/find_boundary/robot_" +
+  const string directory = "../dairlib_data/goldilocks_models/find_boundary_utils/robot_" +
       to_string(FLAGS_robot_option) + "/";
 
   // Looping through the iterations
diff --git a/examples/goldilocks_models/task.h b/examples/goldilocks_models/task.h
index e9142735b8..b93d63b1d8 100644
--- a/examples/goldilocks_models/task.h
+++ b/examples/goldilocks_models/task.h
@@ -34,12 +34,14 @@ class Task {
       name_to_index_map_[names[i]] = i;
     }
   }
+  //Default constructor;
+  Task(){};
 
   // Getters and setters
   double get(const string& name) const {
     return task_.at(name_to_index_map_.at(name));
   }
-  int dim() {return task_dim_};
+  int dim() {return task_dim_;}
   const std::vector<double>& get() const { return task_; }
   void set(const std::vector<double>& values) {
     DRAKE_DEMAND(values.size() == (unsigned)task_dim_);

From bd6e0270b8ab6eb69c950f06bbe72c5e5ba29d37 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Wed, 1 Jul 2020 14:36:58 -0400
Subject: [PATCH 43/82] delete one line

---
 examples/goldilocks_models/find_boundary.cc | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 5c35ed5797..8edda50cf1 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -204,7 +204,8 @@ void readThetaFromFiles(const string dir,int theta_idx,
   theta_y = readCSV(dir + to_string(theta_idx) + string("_theta_y.csv"));
   theta_yddot = readCSV(dir + to_string(theta_idx) + string("_theta_yddot.csv"));
 }
-////TODO:MODIFY THIS FUNCTION!
+
+
 //use interpolation to set the initial guess for the trajectory optimization
 string getInitFileName(const string directory, int traj_opt_num,
     bool is_rerun,int rerun_traj_idx=-1){

From 937f58c4da20b74a1fe887d33e63a2220c62e57b Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Wed, 1 Jul 2020 14:43:06 -0400
Subject: [PATCH 44/82] add several const in the program

---
 examples/goldilocks_models/find_models/initial_guess.cc | 8 ++++----
 examples/goldilocks_models/find_models/initial_guess.h  | 8 ++++----
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/examples/goldilocks_models/find_models/initial_guess.cc b/examples/goldilocks_models/find_models/initial_guess.cc
index 1226e728ae..440925f9d4 100644
--- a/examples/goldilocks_models/find_models/initial_guess.cc
+++ b/examples/goldilocks_models/find_models/initial_guess.cc
@@ -44,8 +44,8 @@ VectorXd GetGammaScale(const TasksGenerator* task_gen) {
 
 // calculate the interpolation weight; update weight vector and solution matrix
 void InterpolateAmongDifferentTasks(const string& dir, string prefix,
-                                    VectorXd current_gamma,
-                                    VectorXd gamma_scale,
+                                    const VectorXd& current_gamma,
+                                    const VectorXd& gamma_scale,
                                     VectorXd& weight_vector,
                                     MatrixXd& solution_matrix) {
   // check if this sample is success
@@ -71,8 +71,8 @@ void InterpolateAmongDifferentTasks(const string& dir, string prefix,
 }
 
 // calculate interpolated initial guess using weight vector and solution matrix
-VectorXd CalculateInterpolation(VectorXd weight_vector,
-                                MatrixXd solution_matrix) {
+VectorXd CalculateInterpolation(VectorXd& weight_vector,
+                                const MatrixXd& solution_matrix) {
   DRAKE_DEMAND(weight_vector.rows() > 0);
   // normalize weight
   weight_vector.normalize();
diff --git a/examples/goldilocks_models/find_models/initial_guess.h b/examples/goldilocks_models/find_models/initial_guess.h
index 7ebdd3b7a7..085ff0b1c2 100644
--- a/examples/goldilocks_models/find_models/initial_guess.h
+++ b/examples/goldilocks_models/find_models/initial_guess.h
@@ -21,11 +21,11 @@ std::string SetInitialGuessByInterpolation(const std::string& directory,
 Eigen::VectorXd GetThetaScale(const std::string& directory, int iter);
 Eigen::VectorXd GetGammaScale(const TasksGenerator* task_gen);
 // utility functions
-Eigen::VectorXd CalculateInterpolation(Eigen::VectorXd weight_vector,
-                                       Eigen::MatrixXd solution_matrix);
+Eigen::VectorXd CalculateInterpolation(Eigen::VectorXd& weight_vector,
+                                       const Eigen::MatrixXd& solution_matrix);
 void InterpolateAmongDifferentTasks(const std::string& dir, string prefix,
-                                    Eigen::VectorXd current_gamma,
-                                    Eigen::VectorXd gamma_scale,
+                                    const Eigen::VectorXd& current_gamma,
+                                    const Eigen::VectorXd& gamma_scale,
                                     Eigen::VectorXd& weight_vector,
                                     Eigen::MatrixXd& solution_matrix);
 }  // namespace dairlib::goldilocks_models

From b9b3e82bfffe9f21ce6db17d6ac7ffdd098da56a Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Wed, 1 Jul 2020 15:20:50 -0400
Subject: [PATCH 45/82] Build completely while it stops at first traj opt

---
 examples/goldilocks_models/find_boundary.cc        | 14 +++++++++++++-
 .../find_boundary_utils/search_setting.cc          |  6 +++---
 .../find_boundary_utils/search_setting.h           |  3 ++-
 3 files changed, 18 insertions(+), 5 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 8edda50cf1..bcee6a570b 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -379,7 +379,7 @@ void trajOptGivenModel(Task task, const string dir,int num,bool is_rerun,
   int sample_idx = 0;
   string prefix = to_string(num) +  "_" + to_string(sample_idx) + "_";
 
-  inner_loop_setting.n_node = 20;//fix number of nodes
+  inner_loop_setting.n_node = (FLAGS_n_node>0)? FLAGS_n_node : 20;//fix number of nodes
   inner_loop_setting.max_iter = max_inner_iter_pass_in;
   inner_loop_setting.prefix = prefix;
   inner_loop_setting.init_file = init_file_pass_in;
@@ -387,6 +387,10 @@ void trajOptGivenModel(Task task, const string dir,int num,bool is_rerun,
   // Vectors/Matrices for the outer loop
   int N_sample = 1;
   SubQpData QPs(N_sample);
+  // reset is_success_vec before trajectory optimization
+  for (int i = 0; i < N_sample; i++) {
+    *(QPs.is_success_vec[i]) = 0;
+  }
 
   vector<std::shared_ptr<int>> thread_finished_vec(N_sample);
   for (int i = 0; i < N_sample; i++) {
@@ -676,6 +680,14 @@ int find_boundary(int argc, char* argv[]){
     search_setting.SetExtendComponents("velocity",1,{0});
     search_setting.SetExtendComponents("turning rate",1,{0});
   }
+  cout<<"search along: ";
+  for (dim=0;dim<search_setting.task_dim();dim++)
+  {
+    if(search_setting.get_n_element(dim)>1){
+      cout<<search_setting.names()[dim]<<" ";
+    }
+  }
+  cout<<endl;
 
   /*
    * start iteration
diff --git a/examples/goldilocks_models/find_boundary_utils/search_setting.cc b/examples/goldilocks_models/find_boundary_utils/search_setting.cc
index 63f84be117..899afd4d37 100644
--- a/examples/goldilocks_models/find_boundary_utils/search_setting.cc
+++ b/examples/goldilocks_models/find_boundary_utils/search_setting.cc
@@ -21,11 +21,11 @@ SearchSetting::SearchSetting(int task_dim,
   }
 }
 
-void SearchSetting::SetExtendComponents(string task_name,int n_element,
+void SearchSetting::SetExtendComponents(const string task_name,int n_element,
                          vector<double> element){
   DRAKE_DEMAND((unsigned)n_element==element.size());
-  n_elements_[name_to_index_map_[task_name]] = n_element;
-  elements_[name_to_index_map_[task_name]] = element;
+  n_elements_.push_back(n_element);
+  elements_.push_back(element);
 }
 
 }
diff --git a/examples/goldilocks_models/find_boundary_utils/search_setting.h b/examples/goldilocks_models/find_boundary_utils/search_setting.h
index 1ff1ee069d..0f8238a97f 100644
--- a/examples/goldilocks_models/find_boundary_utils/search_setting.h
+++ b/examples/goldilocks_models/find_boundary_utils/search_setting.h
@@ -1,6 +1,7 @@
 //
 // Created by jianshu on 6/29/20.
 //
+#include <iostream>
 #include <string>
 #include <vector>
 #include <unordered_map>
@@ -31,7 +32,7 @@ class SearchSetting {
   }
 
   //setters
-  void SetExtendComponents(string task_name,int n_element,
+  void SetExtendComponents(const string task_name,int n_element,
       vector<double> element);
 
  private:

From 00f3904a076dde7e968fd09116a014b70602d15c Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Wed, 1 Jul 2020 20:01:46 -0400
Subject: [PATCH 46/82] fix the bug related redefine variable

---
 examples/goldilocks_models/find_boundary.cc          | 12 ++++--------
 .../find_boundary_utils/search_setting.h             |  2 +-
 2 files changed, 5 insertions(+), 9 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index bcee6a570b..ada08630e4 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -713,18 +713,14 @@ int find_boundary(int argc, char* argv[]){
 
   // find which task dimension to search
   dim = 0;
-  VectorXd extend_direction;
-  if(robot_option==0)
-  {
-    VectorXd extend_direction(3);
-  }
-  else if(robot_option==1)
+  VectorXd extend_direction(3);
+  if(robot_option==1)
   {
-    VectorXd extend_direction(4);
+    extend_direction.conservativeResize(extend_direction.rows() + 1);
   }
   int ele1,ele2,ele3,ele4;
   // search the boundary
-  VectorXd task_delta = Eigen::Map<VectorXd>(search_setting.task_delta().data(),
+  VectorXd task_delta = Eigen::Map<const VectorXd>(search_setting.task_delta().data(),
       search_setting.task_delta().size());
   for (ele1=0;ele1<search_setting.get_n_element(0);ele1++){
     for(ele2=0;ele2<search_setting.get_n_element(1);ele2++){
diff --git a/examples/goldilocks_models/find_boundary_utils/search_setting.h b/examples/goldilocks_models/find_boundary_utils/search_setting.h
index 0f8238a97f..e8480e2de8 100644
--- a/examples/goldilocks_models/find_boundary_utils/search_setting.h
+++ b/examples/goldilocks_models/find_boundary_utils/search_setting.h
@@ -24,7 +24,7 @@ class SearchSetting {
   int task_dim(){return task_dim_;}
   vector<string> names(){return names_;}
   vector<double> task_0(){return task_0_;}
-  vector<double> task_delta(){return task_delta_;}
+  const vector<double> task_delta(){return task_delta_;}
   int index(string task_name){return name_to_index_map_[task_name];}
   int get_n_element(int task_dim){return n_elements_[task_dim];}
   double get_element(int task_index,int element_index){

From 26378a14488b0403d08d7652f24678f25c48fa45 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Thu, 2 Jul 2020 11:20:50 -0400
Subject: [PATCH 47/82] small changes related to initial_guess.cc

---
 examples/goldilocks_models/find_models/initial_guess.cc | 6 ++----
 examples/goldilocks_models/find_models/initial_guess.h  | 2 +-
 2 files changed, 3 insertions(+), 5 deletions(-)

diff --git a/examples/goldilocks_models/find_models/initial_guess.cc b/examples/goldilocks_models/find_models/initial_guess.cc
index 440925f9d4..4ea60e1856 100644
--- a/examples/goldilocks_models/find_models/initial_guess.cc
+++ b/examples/goldilocks_models/find_models/initial_guess.cc
@@ -71,13 +71,11 @@ void InterpolateAmongDifferentTasks(const string& dir, string prefix,
 }
 
 // calculate interpolated initial guess using weight vector and solution matrix
-VectorXd CalculateInterpolation(VectorXd& weight_vector,
+VectorXd CalculateInterpolation(const VectorXd& weight_vector,
                                 const MatrixXd& solution_matrix) {
   DRAKE_DEMAND(weight_vector.rows() > 0);
-  // normalize weight
-  weight_vector.normalize();
   // interpolation
-  VectorXd interpolated_solution = solution_matrix * weight_vector;
+  VectorXd interpolated_solution = solution_matrix * weight_vector.normalized();
   return interpolated_solution;
 }
 
diff --git a/examples/goldilocks_models/find_models/initial_guess.h b/examples/goldilocks_models/find_models/initial_guess.h
index 085ff0b1c2..d101fa7e10 100644
--- a/examples/goldilocks_models/find_models/initial_guess.h
+++ b/examples/goldilocks_models/find_models/initial_guess.h
@@ -21,7 +21,7 @@ std::string SetInitialGuessByInterpolation(const std::string& directory,
 Eigen::VectorXd GetThetaScale(const std::string& directory, int iter);
 Eigen::VectorXd GetGammaScale(const TasksGenerator* task_gen);
 // utility functions
-Eigen::VectorXd CalculateInterpolation(Eigen::VectorXd& weight_vector,
+Eigen::VectorXd CalculateInterpolation(const Eigen::VectorXd& weight_vector,
                                        const Eigen::MatrixXd& solution_matrix);
 void InterpolateAmongDifferentTasks(const std::string& dir, string prefix,
                                     const Eigen::VectorXd& current_gamma,

From 7a349ce4b9a2e61bb08de2f58d69dd7efa27822e Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Thu, 2 Jul 2020 14:21:13 -0400
Subject: [PATCH 48/82] modify the class SearchSetting and add the python file
 used for plotting

---
 examples/goldilocks_models/find_boundary.cc   |  41 +--
 .../find_boundary_utils/BUILD.bazel           |  14 -
 .../find_boundary_utils/compare_boundary.py   | 164 +++++++++
 .../find_boundary_utils/plot_landscape.py     | 141 ++++++++
 .../find_boundary_utils/search_setting.cc     |  16 +-
 .../find_boundary_utils/search_setting.h      |  25 +-
 .../find_boundary_utils/visualize_gait_MBP.cc | 318 ------------------
 .../goldilocks_models/find_models/BUILD.bazel |   1 -
 .../find_models/initial_guess.cc              |   3 -
 .../find_models/initial_guess.h               |   3 -
 examples/goldilocks_models/task.h             |   2 +-
 11 files changed, 337 insertions(+), 391 deletions(-)
 create mode 100644 examples/goldilocks_models/find_boundary_utils/compare_boundary.py
 create mode 100644 examples/goldilocks_models/find_boundary_utils/plot_landscape.py
 delete mode 100644 examples/goldilocks_models/find_boundary_utils/visualize_gait_MBP.cc

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index ada08630e4..504b630447 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -1,7 +1,3 @@
-//
-// Created by jianshu on 5/20/20.
-//
-
 #include <gflags/gflags.h>
 #include <thread>  // multi-threading
 #include <chrono>
@@ -613,20 +609,34 @@ int find_boundary(int argc, char* argv[]){
   cout << "\nInitialize task space:\n";
   Task task;//Traj Opt tasks
   SearchSetting search_setting;//Settings of searching the task space
+  // create components for each dimension used to decide the direction
+  // considering we can only visualize 2D landscape, we only search within
+  // 2D space and fix other dimension
   if(robot_option==0)
   {
+    vector<vector<double>> elements{
+        {0,-1,0.5,0.5,1}, //stride length
+        {0,-1,0.5,0.5,1}, //ground incline
+        {0} //velocity
+    };
     task = Task({"stride length", "ground incline",
                  "velocity"});
     search_setting = SearchSetting(3,{"stride length", "ground incline",
                                       "velocity"},{0.25,0,0.4},
-                                          {0.01,0.01,0.02});
+                                          {0.01,0.01,0.02},elements);
   }
   else if(robot_option==1){
+    vector<vector<double>> elements{
+        {0,-1,0.5,0.5,1}, //stride length
+        {0,-1,0.5,0.5,1}, //ground incline
+        {0}, //velocity
+        {0} //turning rate
+    };
     task = Task({"stride length", "ground incline",
                  "velocity", "turning rate"});
     search_setting = SearchSetting(4,{"stride length", "ground incline",
                                       "velocity","turning rate"},
-                                   {0.3,0,0.5,0},{0.01,0.01,0.02,0.01});
+                                   {0.3,0,0.5,0},{0.01,0.01,0.02,0.01},elements);
   }
   //cout initial point information
   int dim = 0;
@@ -665,21 +675,6 @@ int find_boundary(int argc, char* argv[]){
   }
   cout<<"cost_threshold "<<cost_threshold<<endl;
 
-  // create components for each dimension used to decide the direction
-  // considering we can only visualize 2D landscape, we only search within
-  // 2D space and fix other dimension
-  if(robot_option==0){
-    search_setting.SetExtendComponents("stride length",5,{0,-1,0.5,0.5,1});
-    search_setting.SetExtendComponents("ground incline",5,{0,-1,0.5,0.5,1});
-    search_setting.SetExtendComponents("velocity",1,{0});
-  }
-  else if(robot_option==1)
-  {
-    search_setting.SetExtendComponents("stride length",5,{0,-1,0.5,0.5,1});
-    search_setting.SetExtendComponents("ground incline",5,{0,-1,0.5,0.5,1});
-    search_setting.SetExtendComponents("velocity",1,{0});
-    search_setting.SetExtendComponents("turning rate",1,{0});
-  }
   cout<<"search along: ";
   for (dim=0;dim<search_setting.task_dim();dim++)
   {
@@ -695,8 +690,8 @@ int find_boundary(int argc, char* argv[]){
   int boundary_sample_num = 0;//use this to record the index of boundary point
   int traj_opt_num = 0;//use this to record the index of Traj Opt
   //evaluate initial point
-  VectorXd initial_task = Eigen::Map<VectorXd>(search_setting.task_0().data(),
-      search_setting.task_0().size());
+  VectorXd initial_task = Eigen::Map<const VectorXd>
+      (search_setting.task_0().data(),search_setting.task_0().size());
   task.set(search_setting.task_0());
   writeCSV(dir +  to_string(traj_opt_num)  +
       string("_0_task.csv"), initial_task);
diff --git a/examples/goldilocks_models/find_boundary_utils/BUILD.bazel b/examples/goldilocks_models/find_boundary_utils/BUILD.bazel
index 8b8dc9e817..5aa98f3ea6 100644
--- a/examples/goldilocks_models/find_boundary_utils/BUILD.bazel
+++ b/examples/goldilocks_models/find_boundary_utils/BUILD.bazel
@@ -1,19 +1,5 @@
 package(default_visibility = ["//visibility:public"])
 
-cc_binary(
-    name = "visualize_gait_MBP",
-    srcs = ["visualize_gait_MBP.cc"],
-    data = glob(["examples/goldilocks_models/PlanarWalkerWithTorso.urdf"]),
-    deps = [
-        "//common",
-        "//systems/goldilocks_models",
-        "//systems/primitives",
-        "//systems/trajectory_optimization:dircon",
-        "@drake//:drake_shared_library",
-        "@gflags",
-    ],
-)
-
 cc_library(
     name = "search_setting",
     srcs = [
diff --git a/examples/goldilocks_models/find_boundary_utils/compare_boundary.py b/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
new file mode 100644
index 0000000000..b3520134d5
--- /dev/null
+++ b/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
@@ -0,0 +1,164 @@
+import matplotlib.pyplot as plt
+import matplotlib
+import numpy as np
+from mpl_toolkits.mplot3d import Axes3D
+from scipy.linalg import solve
+import torch
+from torch import tensor
+import csv
+import os
+
+robot_option = 1
+file_dir = '/Users/jason-hu/'
+
+# optimization setting
+if robot_option == 0:
+    min_gi1 = -0.325
+    max_gi1 = 0.275
+    min_sl1 = 0.0925
+    max_sl1 = 0.3925
+else:
+    min_gi1 = -0.325
+    max_gi1 = 0.275
+    min_sl1 = 0.1425
+    max_sl1 = 0.4425
+plot_large_range = 1
+
+if robot_option == 0:
+    min_gi2 = -0.175
+    max_gi2 = 0.125
+    min_sl2 = 0.1675
+    max_sl2 = 0.3175
+else:
+    min_gi2 = -0.175
+    max_gi2 = 0.125
+    min_sl2 = 0.2175
+    max_sl2 = 0.3675
+plot_small_range = 1
+
+if robot_option == 0:
+    robot = 'five_link/'
+    dir1 = file_dir+'dairlib_data/find_boundary/' + robot + 'robot_' + str(robot_option) + \
+           '_nominal/'
+    dir2 = file_dir+'dairlib_data/find_boundary/' + robot + 'robot_' + str(robot_option) + \
+           '_initial_model/'
+    dir3 = file_dir+'dairlib_data/find_boundary/' + robot + 'large_task_space/' + 'robot_' + str(robot_option) + \
+           '_large_task_space_iter1000/'
+    dir4 = file_dir+'dairlib_data/find_boundary/' + robot + 'large_task_space/' + 'robot_' + str(robot_option) + \
+           '_large_task_space_iter2000/'
+    dir5 = file_dir+'dairlib_data/find_boundary/' + robot + 'large_task_space/' + 'robot_' + str(robot_option) + \
+           '_large_task_space_iter3000_with_scaling/'
+    dir6 = file_dir+'dairlib_data/find_boundary/' + robot + 'small_task_space/' + 'robot_' + str(robot_option) + \
+           '_small_task_space_iter500/'
+    dir7 = file_dir+'dairlib_data/find_boundary/' + robot + 'small_task_space/' + 'robot_' + str(robot_option) + \
+           '_small_task_space_iter1000/'
+    dir8 = file_dir+'dairlib_data/find_boundary/' + robot + 'small_task_space/' + 'robot_' + str(robot_option) + \
+           '_small_task_space_iter2000_with_scaling/'
+else:
+    if robot_option == 1:
+        robot = 'cassie/'
+        dir1 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/'+'robot_' + str(robot_option) + \
+               '_2d_nominal/'
+        dir2 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+               '_2d_initial_model/'
+        dir3 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+               '_small_iter300/'
+        dir4 = file_dir + 'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+               '_large_iter150/'
+        dir5 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+               '_2d_initial_model/'
+        dir6 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+               '_small_iter200/'
+        dir7 = file_dir + 'dairlib_data/find_boundary/' + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+               '_large_iter100/'
+
+
+x = []
+y = []
+z = []
+dir = dir4
+dir_compare = dir3
+i = 0
+found_same_sample = False
+while os.path.isfile(dir+str(i)+'_'+str(0)+'_gamma.csv'):
+    gamma = np.genfromtxt(dir + str(i) + '_' + str(0) + '_gamma.csv', delimiter=",")
+    cost = float(np.genfromtxt(dir + str(i) + '_' + str(0) + '_c.csv', delimiter=","))
+    j = 0
+    # search if the intersected samples
+    while os.path.isfile(dir_compare+str(j)+'_'+str(0)+'_gamma.csv'):
+        gamma_compare = np.genfromtxt(dir_compare + str(j) + '_' + str(0) + '_gamma.csv', delimiter=",")
+        if np.array_equal(gamma, gamma_compare):
+            found_same_sample = True
+            # if np.genfromtxt(dir + str(i) + '_' + str(0) + '_is_success.csv', delimiter=",") == 1:
+            #     if np.genfromtxt(dir_compare + str(j) + '_' + str(0) + '_is_success.csv', delimiter=",") == 1:
+            compare_cost = float(np.genfromtxt(dir_compare + str(j) + '_' + str(0) + '_c.csv', delimiter=","))
+            x.append(gamma[0])
+            y.append(gamma[1])
+            if cost > compare_cost:
+                z.append(1.5)
+            else:
+                z.append(0.5)
+            break
+        j = j+1
+    if found_same_sample:
+        found_same_sample = False
+    else:
+        x.append(gamma[0])
+        y.append(gamma[1])
+        z.append(-1)
+    i = i+1
+# search another side
+i = 0
+while os.path.isfile(dir_compare+str(i)+'_'+str(0)+'_gamma.csv'):
+    gamma_compare = np.genfromtxt(dir_compare + str(i) + '_' + str(0) + '_gamma.csv', delimiter=",")
+    j = 0
+    # search if the intersected samples
+    while os.path.isfile(dir+str(j)+'_'+str(0)+'_gamma.csv'):
+        gamma = np.genfromtxt(dir + str(j) + '_' + str(0) + '_gamma.csv', delimiter=",")
+        if np.array_equal(gamma, gamma_compare):
+            found_same_sample = True
+            break
+        j = j+1
+    if found_same_sample:
+        found_same_sample = False
+    else:
+        x.append(gamma_compare[0])
+        y.append(gamma_compare[1])
+        z.append(1.5)
+    i = i+1
+
+
+print(np.shape(x))
+print(np.shape(y))
+print(np.shape(z))
+
+fig, ax = plt.subplots()
+
+# discrete color map
+
+levels = [0, 1, 2]
+colors = ['green', 'blue']
+cmap, norm = matplotlib.colors.from_levels_and_colors(levels, colors)
+cmap.set_over('yellow')
+cmap.set_under('red')
+surf = ax.tricontourf(x, y, z, cmap=cmap, norm=norm, levels=levels, extend='both')
+cbar = fig.colorbar(surf, shrink=0.5, aspect=6, extend='both')
+cbar.ax.set_yticklabels(['0', '1', 'Infinity'])
+
+ax.set_xlabel('Stride length')
+ax.set_ylabel('Ground incline')
+ax.set_title('Compare two cost landscapes')
+
+# optimization range
+# large range
+if plot_large_range:
+    x_line = np.array([min_sl1, max_sl1, max_sl1, min_sl1, min_sl1])
+    y_line = np.array([min_gi1, min_gi1, max_gi1, max_gi1, min_gi1])
+    ax.plot(x_line, y_line, 'black', linewidth=3)
+# small range
+if plot_small_range:
+    x_line2 = np.array([min_sl2, max_sl2, max_sl2, min_sl2, min_sl2])
+    y_line2 = np.array([min_gi2, min_gi2, max_gi2, max_gi2, min_gi2])
+    ax.plot(x_line2, y_line2, 'black', linewidth=3)
+
+plt.show()
\ No newline at end of file
diff --git a/examples/goldilocks_models/find_boundary_utils/plot_landscape.py b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
new file mode 100644
index 0000000000..945525add9
--- /dev/null
+++ b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
@@ -0,0 +1,141 @@
+import matplotlib.pyplot as plt
+import matplotlib
+import numpy as np
+from mpl_toolkits.mplot3d import Axes3D
+from scipy.linalg import solve
+import torch
+from torch import tensor
+import csv
+import os
+
+robot_option = 1
+normalized_cost = True
+file_dir = '/Users/jason-hu/'
+
+# optimization setting
+if robot_option == 0:
+    min_gi1 = -0.325
+    max_gi1 = 0.275
+    min_sl1 = 0.0925
+    max_sl1 = 0.3925
+
+    min_gi2 = -0.175
+    max_gi2 = 0.125
+    min_sl2 = 0.1675
+    max_sl2 = 0.3175
+plot_large_range = 1
+if robot_option == 1:
+    min_gi1 = -0.325
+    max_gi1 = 0.275
+    min_sl1 = 0.1425
+    max_sl1 = 0.4425
+
+    min_gi2 = -0.175
+    max_gi2 = 0.125
+    min_sl2 = 0.2175
+    max_sl2 = 0.3675
+plot_small_range = 1
+
+if robot_option == 0:
+    robot = 'five_link/'
+    dir1 = file_dir+'dairlib_data/find_boundary/' + robot + 'robot_' + str(robot_option) + \
+           '_nominal/'
+    dir2 = file_dir+'dairlib_data/find_boundary/' + robot + 'robot_' + str(robot_option) + \
+           '_initial_model/'
+    dir3 = file_dir+'dairlib_data/find_boundary/' + robot + 'robot_' + str(robot_option) + \
+           '_large_task_space_iter1000/'
+    dir4 = file_dir+'dairlib_data/find_boundary/' + robot + 'robot_' + str(robot_option) + \
+           '_large_task_space_iter2000/'
+    dir5 = file_dir+'dairlib_data/find_boundary/' + robot + 'robot_' + str(robot_option) + \
+           '_large_task_space_iter3000_with_scaling/'
+    dir6 = file_dir+'dairlib_data/find_boundary/' + robot + 'robot_' + str(robot_option) + \
+           '_small_task_space_iter500/'
+    dir7 = file_dir+'dairlib_data/find_boundary/' + robot + 'robot_' + str(robot_option) + \
+           '_small_task_space_iter1000/'
+    dir8 = file_dir+'dairlib_data/find_boundary/' + robot + 'robot_' + str(robot_option) + \
+           '_small_task_space_iter2000_with_scaling/'
+if robot_option == 1:
+    robot = 'cassie/'
+    dir1 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+           '_2d_nominal/'
+    dir2 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+           '_2d_initial_model/'
+    dir3 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+           '_small_iter300/'
+    dir4 = file_dir + 'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+           '_large_iter150/'
+    dir5 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+           '_2d_initial_model/'
+    dir6 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+           '_small_iter200/'
+    dir7 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+           '_large_iter100/'
+
+
+i = 0
+x = []
+y = []
+z = []
+dir = dir4
+dir_nominal = dir3
+found_same_sample = False
+while os.path.isfile(dir+str(i)+'_'+str(0)+'_gamma.csv'):
+    gamma = np.genfromtxt(dir + str(i) + '_' + str(0) + '_gamma.csv', delimiter=",")
+    cost = float(np.genfromtxt(dir + str(i) + '_' + str(0) + '_c.csv', delimiter=","))
+    if np.genfromtxt(dir + str(i) + '_' + str(0) + '_is_success.csv', delimiter=",") == 1:
+        if normalized_cost:
+            j = 0
+            # search the nominal cost map
+            while os.path.isfile(dir_nominal+str(j)+'_'+str(0)+'_gamma.csv'):
+                gamma_nominal = np.genfromtxt(dir_nominal + str(j) + '_' + str(0) + '_gamma.csv', delimiter=",")
+                if np.array_equal(gamma, gamma_nominal):
+                    found_same_sample = True
+                    if np.genfromtxt(dir_nominal + str(j) + '_' + str(0) + '_is_success.csv', delimiter=",") == 1:
+                        nominal_cost = float(np.genfromtxt(dir_nominal + str(j) + '_' + str(0) + '_c.csv', delimiter=","))
+                        if cost/nominal_cost < 1.5:
+                            x.append(gamma[0])
+                            y.append(gamma[1])
+                            z.append(cost/nominal_cost)
+                    break
+                j = j+1
+            if found_same_sample:
+                found_same_sample = False
+            else:
+                x.append(gamma[0])
+                y.append(gamma[1])
+                z.append(0)
+        else:
+            x.append(gamma[0])
+            y.append(gamma[1])
+            z.append(cost)
+    i = i+1
+
+fig, ax = plt.subplots()
+
+print(np.shape(x))
+print(np.shape(y))
+print(np.shape(z))
+
+# continuous color map
+surf = ax.tricontourf(x, y, z)
+fig.colorbar(surf, shrink=0.5, aspect=6)
+ax.set_xlabel('Stride length')
+ax.set_ylabel('Ground incline')
+if normalized_cost:
+    ax.set_title('normalized cost landscape')
+else:
+    ax.set_title('cost landscape')
+
+# optimization range
+# large range
+if plot_large_range == 1:
+    x_line = np.array([min_sl1, max_sl1, max_sl1, min_sl1, min_sl1])
+    y_line = np.array([min_gi1, min_gi1, max_gi1, max_gi1, min_gi1])
+    ax.plot(x_line, y_line, 'black', linewidth=3)
+# small range
+if plot_small_range == 1:
+    x_line2 = np.array([min_sl2, max_sl2, max_sl2, min_sl2, min_sl2])
+    y_line2 = np.array([min_gi2, min_gi2, max_gi2, max_gi2, min_gi2])
+    ax.plot(x_line2, y_line2, 'black', linewidth=3)
+
+plt.show()
\ No newline at end of file
diff --git a/examples/goldilocks_models/find_boundary_utils/search_setting.cc b/examples/goldilocks_models/find_boundary_utils/search_setting.cc
index 899afd4d37..4c0855ff65 100644
--- a/examples/goldilocks_models/find_boundary_utils/search_setting.cc
+++ b/examples/goldilocks_models/find_boundary_utils/search_setting.cc
@@ -1,6 +1,3 @@
-//
-// Created by jianshu on 6/29/20.
-//
 #include "examples/goldilocks_models/find_boundary_utils/search_setting.h"
 
 
@@ -10,23 +7,18 @@ namespace goldilocks_models {
 SearchSetting::SearchSetting(int task_dim,
                              std::vector <string> names,
                              std::vector<double> task_0,
-                             std::vector<double> task_delta)
+                             std::vector<double> task_delta,
+                             std::vector<std::vector<double>> elements)
     : task_dim_(task_dim),
       names_(names),
       task_0_(task_0),
-      task_delta_(task_delta) {
+      task_delta_(task_delta),
+      elements_(elements){
 
   for (unsigned int i = 0; i < names.size(); i++) {
     name_to_index_map_[names[i]] = i;
   }
 }
 
-void SearchSetting::SetExtendComponents(const string task_name,int n_element,
-                         vector<double> element){
-  DRAKE_DEMAND((unsigned)n_element==element.size());
-  n_elements_.push_back(n_element);
-  elements_.push_back(element);
-}
-
 }
 }
\ No newline at end of file
diff --git a/examples/goldilocks_models/find_boundary_utils/search_setting.h b/examples/goldilocks_models/find_boundary_utils/search_setting.h
index e8480e2de8..30709db845 100644
--- a/examples/goldilocks_models/find_boundary_utils/search_setting.h
+++ b/examples/goldilocks_models/find_boundary_utils/search_setting.h
@@ -1,6 +1,3 @@
-//
-// Created by jianshu on 6/29/20.
-//
 #include <iostream>
 #include <string>
 #include <vector>
@@ -18,30 +15,26 @@ class SearchSetting {
   // Default constructor
   SearchSetting(){};
   SearchSetting(int task_dim, vector<string> names,
-      vector<double> task_0,vector<double> task_delta);
+      vector<double> task_0,vector<double> task_delta,
+      vector<vector<double>> elements);
 
   //getters
-  int task_dim(){return task_dim_;}
-  vector<string> names(){return names_;}
-  vector<double> task_0(){return task_0_;}
-  const vector<double> task_delta(){return task_delta_;}
-  int index(string task_name){return name_to_index_map_[task_name];}
-  int get_n_element(int task_dim){return n_elements_[task_dim];}
-  double get_element(int task_index,int element_index){
+  int task_dim() const {return task_dim_;}
+  const vector<string>& names() const {return names_;}
+  const vector<double>& task_0() const {return task_0_;}
+  const vector<double>& task_delta() const {return task_delta_;}
+  int index(string task_name) const {return name_to_index_map_.at(task_name);}
+  int get_n_element(int task_dim) const {return elements_[task_dim].size();}
+  double get_element(int task_index,int element_index) const {
     return elements_[task_index][element_index];
   }
 
-  //setters
-  void SetExtendComponents(const string task_name,int n_element,
-      vector<double> element);
-
  private:
   int task_dim_;
   vector<string> names_;
   vector<double> task_0_;
   vector<double> task_delta_;
   std::unordered_map<string, int> name_to_index_map_;
-  vector<int> n_elements_;
   vector<vector<double>> elements_;
 };
 
diff --git a/examples/goldilocks_models/find_boundary_utils/visualize_gait_MBP.cc b/examples/goldilocks_models/find_boundary_utils/visualize_gait_MBP.cc
deleted file mode 100644
index 9007eb5e8e..0000000000
--- a/examples/goldilocks_models/find_boundary_utils/visualize_gait_MBP.cc
+++ /dev/null
@@ -1,318 +0,0 @@
-#include <gflags/gflags.h>
-
-#include <memory>
-#include <chrono>
-
-#include <string>
-
-#include "drake/systems/analysis/simulator.h"
-#include "drake/systems/framework/diagram.h"
-#include "drake/systems/framework/diagram_builder.h"
-#include "drake/systems/primitives/trajectory_source.h"
-
-#include "drake/lcm/drake_lcm.h"
-
-#include "drake/multibody/parsing/parser.h"
-#include "drake/systems/rendering/multibody_position_to_geometry_pose.h"
-#include "drake/geometry/geometry_visualization.h"
-
-#include "common/find_resource.h"
-#include "systems/primitives/subvector_pass_through.h"
-
-#include "multibody/multibody_utils.h"
-#include "multibody/visualization_utils.h"
-
-#include "drake/systems/analysis/simulator.h"
-#include "drake/systems/framework/diagram.h"
-#include "drake/systems/framework/diagram_builder.h"
-#include "drake/systems/primitives/trajectory_source.h"
-#include "drake/lcm/drake_lcm.h"
-
-#include "drake/common/trajectories/piecewise_polynomial.h"
-
-#include "systems/goldilocks_models/file_utils.h"
-
-using drake::multibody::MultibodyPlant;
-using drake::geometry::SceneGraph;
-using drake::multibody::Body;
-using drake::multibody::Parser;
-using drake::systems::rendering::MultibodyPositionToGeometryPose;
-
-using Eigen::Vector3d;
-using Eigen::VectorXd;
-using Eigen::MatrixXd;
-using Eigen::Matrix3Xd;
-using drake::trajectories::PiecewisePolynomial;
-using drake::MatrixX;
-using std::vector;
-using std::shared_ptr;
-using std::cout;
-using std::endl;
-using std::string;
-using std::to_string;
-
-namespace dairlib {
-
-DEFINE_int32(iter_start, 1, "The iter #");
-DEFINE_int32(iter_end, -1, "The iter #");
-DEFINE_int32(batch, 0, "The batch #");
-DEFINE_double(realtime_factor, 1, "Rate of which the traj is played back");
-DEFINE_int32(n_step, 3, "# of foot steps");
-
-DEFINE_int32(robot_option, 1, "0: plannar robot. 1: cassie_fixed_spring");
-
-DEFINE_bool(construct_cubic, false,
-            "True if you want to construct cubic spline. Old files (before "
-            "2019.12.31) didn't store derivatives information, so this option "
-            "cannot be used on those files.");
-
-    void swapTwoBlocks(MatrixXd* mat, int i_1, int j_1, int i_2, int j_2,
-                       int n_row, int n_col) {
-  MatrixXd temp_block1 = mat->block(i_1, j_1, n_row, n_col);
-  MatrixXd temp_block2 = mat->block(i_2, j_2, n_row, n_col);
-  mat->block(i_1, j_1, n_row, n_col) = temp_block2;
-  mat->block(i_2, j_2, n_row, n_col) = temp_block1;
-}
-
-void visualizeGait(int argc, char* argv[]) {
-  gflags::ParseCommandLineFlags(&argc, &argv, true);
-
-  // parameters
-  int iter_start = FLAGS_iter_start;
-  int iter_end = (FLAGS_iter_end >= FLAGS_iter_start) ?
-                 FLAGS_iter_end : FLAGS_iter_start;
-  int n_step = FLAGS_n_step;  // Should be > 0
-  /*const string directory = "examples/goldilocks_models/find_models/data/robot_"
-                           + to_string(FLAGS_robot_option) + "/";*/
-  const string directory = "../dairlib_data/goldilocks_models/find_boundary_utils/robot_" +
-      to_string(FLAGS_robot_option) + "/";
-
-  // Looping through the iterations
-  for (int iter = iter_start; iter <= iter_end; iter++) {
-    // Read in ground incline
-    double ground_incline = goldilocks_models::readCSV(
-        directory + to_string(iter) + string("_") + to_string(FLAGS_batch) +
-        string("_gamma.csv"))(1);
-
-    // Read in trajecotry
-    VectorXd time_mat;
-    MatrixXd state_mat;
-    MatrixXd statedot_mat;
-    if (!FLAGS_construct_cubic) {
-      time_mat = goldilocks_models::readCSV(
-          directory + to_string(iter) + string("_") + to_string(FLAGS_batch) +
-          string("_time_at_knots.csv"));
-      state_mat = goldilocks_models::readCSV(
-          directory + to_string(iter) + string("_") + to_string(FLAGS_batch) +
-          string("_state_at_knots.csv"));
-    } else {
-      time_mat = goldilocks_models::readCSV(
-          directory + to_string(iter) + string("_") + to_string(FLAGS_batch) +
-          string("_t_cubic_spline.csv"));
-      state_mat = goldilocks_models::readCSV(
-          directory + to_string(iter) + string("_") + to_string(FLAGS_batch) +
-          string("_x_cubic_spline.csv"));
-      statedot_mat = goldilocks_models::readCSV(
-          directory + to_string(iter) + string("_") + to_string(FLAGS_batch) +
-          string("_xdot_cubic_spline.csv"));
-    }
-
-    int n_state = state_mat.rows();
-    int n_q;
-    if (FLAGS_robot_option == 0) {
-      n_q = n_state / 2;
-    } else if (FLAGS_robot_option == 1) {
-      n_q = 19;
-    } else {
-      n_q = -1;
-      DRAKE_DEMAND(false); // Shouldn't come here
-    }
-    int n_node = time_mat.rows();
-    VectorXd ones = VectorXd::Ones(n_node - 1);
-    int translation_size;
-    int translation_start_idx;
-    if (FLAGS_robot_option == 0) {
-      translation_size = 2;
-      translation_start_idx = 0;
-    } else if (FLAGS_robot_option == 1) {
-      translation_size = 3;
-      translation_start_idx = 4;
-    } else {
-      translation_size = -1;
-      translation_start_idx = -1;
-      DRAKE_DEMAND(false); // Shouldn't come here
-    }
-    VectorXd xyz_translation =
-      state_mat.block(translation_start_idx, n_node - 1, translation_size, 1)
-      - state_mat.block(translation_start_idx, 0, translation_size, 1);
-
-    // Concatenate the traj so it has multiple steps
-    // 1. time
-    VectorXd time_mat_cat(n_step * n_node - (n_step - 1));
-    time_mat_cat(0) = 0;
-    for (int i = 0; i < n_step; i++) {
-      time_mat_cat.segment(1 + (n_node - 1)*i, n_node - 1) =
-        time_mat.tail(n_node - 1) + time_mat_cat((n_node - 1) * i) * ones;
-    }
-    // 2. state (and its derivatives)
-    std::vector<MatrixXd> mat_cat;  // first element is state (and second
-                                    // element is its derivatives)
-    mat_cat.push_back(MatrixXd(n_state, n_step * n_node - (n_step - 1)));
-    if (FLAGS_construct_cubic) {
-      mat_cat.push_back(MatrixXd(n_state, n_step * n_node - (n_step - 1)));
-    }
-    mat_cat[0].col(0) = state_mat.col(0);
-    if (FLAGS_construct_cubic) {
-      mat_cat[1].col(0) = statedot_mat.col(0);
-    }
-    for (int i = 0; i < n_step; i++) {
-      // Copy over the data at all knots but the first one
-      mat_cat[0].block(0, 1 + (n_node - 1)*i, n_state, n_node - 1) =
-          state_mat.block(0, 1, n_state, n_node - 1);
-      if (FLAGS_construct_cubic) {
-        mat_cat[1].block(0, 1 + (n_node - 1)*i, n_state, n_node - 1) =
-            statedot_mat.block(0, 1, n_state, n_node - 1);
-      }
-
-      // Translate x and z (only for position not its derivatives)
-      if (FLAGS_robot_option == 0) {
-        for (int j = 0; j < translation_size; j++) {
-          mat_cat[0].block(j, 1 + (n_node - 1)*i, 1, n_node - 1)
-            = state_mat.block(j, 1, 1, n_node - 1)  +
-              i * xyz_translation(j) * ones.transpose();
-        }
-      } else if (FLAGS_robot_option == 1) {
-        if (i > 0) {
-          for (int j = 0; j < translation_size; j++) {
-            if (j == 1) {
-              // It's mirror in x-z plane, so we don't need to translate in y here.
-            } else {
-              mat_cat[0].block(j + translation_start_idx,
-                                  1 + (n_node - 1)*i, 1, n_node - 1)
-                = state_mat.block(j + translation_start_idx, 1, 1, n_node - 1)  +
-                  i * xyz_translation(j) * ones.transpose();
-            }
-          }
-        }
-      }
-
-      // Flip the sign for the even number of stance phase
-      if (i % 2) {
-        if (FLAGS_robot_option == 1) {
-          for (auto& mat_cat_member : mat_cat) {
-            // Quaternion sign should also be flipped.
-            mat_cat_member.block(1, 1 + (n_node - 1) * i, 1, n_node - 1) *= -1;
-            mat_cat_member.block(3, 1 + (n_node - 1) * i, 1, n_node - 1) *= -1;
-            // y should be flipped
-            mat_cat_member.block(5, 1 + (n_node - 1) * i, 1, n_node - 1) *= -1;
-            // Hip roll and yaw sign should also be flipped.
-            mat_cat_member.block(7, 1 + (n_node - 1) * i, 4, n_node - 1) *= -1;
-
-            // We do not flip the sign for the velocity part of state because
-            // it's not used in visualization
-          }
-        }
-      }
-
-      // Swap the leg
-      if (i % 2) {
-        for (auto& mat_cat_member : mat_cat) {
-          if (FLAGS_robot_option == 0) {
-            // Position
-            swapTwoBlocks(&mat_cat_member, 3, 1 + (n_node - 1) * i, 4,
-                          1 + (n_node - 1) * i, 1, n_node - 1);
-            swapTwoBlocks(&mat_cat_member, 5, 1 + (n_node - 1) * i, 6,
-                          1 + (n_node - 1) * i, 1, n_node - 1);
-
-            // We do not swap the velocity part because it's not used in
-            // visualization
-          } else if (FLAGS_robot_option == 1) {
-            // Position
-            swapTwoBlocks(&mat_cat_member, 7, 1 + (n_node - 1) * i, 8,
-                          1 + (n_node - 1) * i, 1, n_node - 1);
-            swapTwoBlocks(&mat_cat_member, 9, 1 + (n_node - 1) * i, 10,
-                          1 + (n_node - 1) * i, 1, n_node - 1);
-            swapTwoBlocks(&mat_cat_member, 11, 1 + (n_node - 1) * i, 12,
-                          1 + (n_node - 1) * i, 1, n_node - 1);
-            swapTwoBlocks(&mat_cat_member, 13, 1 + (n_node - 1) * i, 14,
-                          1 + (n_node - 1) * i, 1, n_node - 1);
-            swapTwoBlocks(&mat_cat_member, 15, 1 + (n_node - 1) * i, 16,
-                          1 + (n_node - 1) * i, 1, n_node - 1);
-            swapTwoBlocks(&mat_cat_member, 17, 1 + (n_node - 1) * i, 18,
-                          1 + (n_node - 1) * i, 1, n_node - 1);
-
-            // We do not swap the velocity part because it's not used in
-            // visualization
-          }
-        }
-      }  // end if (i % 2)
-    }  // end for n_step
-
-    // Create a testing piecewise polynomial
-    std::vector<double> T_breakpoint;
-    for (int i = 0; i < time_mat_cat.size(); i++)
-      T_breakpoint.push_back(time_mat_cat(i));
-    std::vector<MatrixXd> Y;
-    for (int i = 0; i < time_mat_cat.size(); i++)
-      Y.push_back(mat_cat[0].col(i));
-    std::vector<MatrixXd> Y_dot;
-    if (FLAGS_construct_cubic) {
-      for (int i = 0; i < time_mat_cat.size(); i++)
-        Y_dot.push_back(mat_cat[1].col(i));
-    }
-    PiecewisePolynomial<double> pp_xtraj =
-        (FLAGS_construct_cubic)
-            ? PiecewisePolynomial<double>::CubicHermite(T_breakpoint, Y, Y_dot)
-            : PiecewisePolynomial<double>::FirstOrderHold(T_breakpoint, Y);
-
-    // Create MBP
-    drake::systems::DiagramBuilder<double> builder;
-    MultibodyPlant<double> plant(0.0);
-    SceneGraph<double>& scene_graph = *builder.AddSystem<SceneGraph>();
-    Vector3d ground_normal(sin(ground_incline), 0, cos(ground_incline));
-    multibody::addFlatTerrain(&plant, &scene_graph, 0.8, 0.8, ground_normal);
-    Parser parser(&plant, &scene_graph);
-    std::string full_name;
-    if (FLAGS_robot_option == 0) {
-      full_name = FindResourceOrThrow(
-                    "examples/goldilocks_models/PlanarWalkerWithTorso.urdf");
-    } else if (FLAGS_robot_option == 1) {
-      full_name = FindResourceOrThrow(
-                    "examples/Cassie/urdf/cassie_fixed_springs.urdf");
-    }
-    parser.AddModelFromFile(full_name);
-    plant.mutable_gravity_field().set_gravity_vector(
-      -9.81 * Eigen::Vector3d::UnitZ());
-    if (FLAGS_robot_option == 0) {
-      plant.WeldFrames(
-        plant.world_frame(), plant.GetFrameByName("base"),
-        drake::math::RigidTransform<double>());
-    }
-    plant.Finalize();
-
-    // visualizer
-    int n_loops = 1;
-    multibody::connectTrajectoryVisualizer(&plant, &builder, &scene_graph,
-                                           pp_xtraj);
-    auto diagram = builder.Build();
-    // while (true)
-    for (int i = 0; i < n_loops; i++) {
-      drake::systems::Simulator<double> simulator(*diagram);
-      simulator.set_target_realtime_rate(FLAGS_realtime_factor);
-      simulator.Initialize();
-      simulator.AdvanceTo(pp_xtraj.end_time());
-    }
-  }  // end for(int iter...)
-
-
-  return;
-}
-} // dairlib
-
-int main(int argc, char* argv[]) {
-
-  dairlib::visualizeGait(argc, argv);
-
-  return 0;
-}
-
diff --git a/examples/goldilocks_models/find_models/BUILD.bazel b/examples/goldilocks_models/find_models/BUILD.bazel
index 01987924e7..d01e0b00e3 100644
--- a/examples/goldilocks_models/find_models/BUILD.bazel
+++ b/examples/goldilocks_models/find_models/BUILD.bazel
@@ -138,7 +138,6 @@ cc_library(
     ],
 )
 
-#edited by jianshu
 cc_library(
     name = "initial_guess",
     srcs = [
diff --git a/examples/goldilocks_models/find_models/initial_guess.cc b/examples/goldilocks_models/find_models/initial_guess.cc
index 1226e728ae..26e50edc7c 100644
--- a/examples/goldilocks_models/find_models/initial_guess.cc
+++ b/examples/goldilocks_models/find_models/initial_guess.cc
@@ -1,6 +1,3 @@
-//
-// Created by jianshu on 3/25/20.
-//
 #include "examples/goldilocks_models/find_models/initial_guess.h"
 
 using std::cout;
diff --git a/examples/goldilocks_models/find_models/initial_guess.h b/examples/goldilocks_models/find_models/initial_guess.h
index 7ebdd3b7a7..628adfcf9e 100644
--- a/examples/goldilocks_models/find_models/initial_guess.h
+++ b/examples/goldilocks_models/find_models/initial_guess.h
@@ -1,6 +1,3 @@
-//
-// Created by jianshu on 3/25/20.
-//
 #include <iostream>
 #include <random>
 #include <Eigen/Dense>
diff --git a/examples/goldilocks_models/task.h b/examples/goldilocks_models/task.h
index b93d63b1d8..361b7dfe8b 100644
--- a/examples/goldilocks_models/task.h
+++ b/examples/goldilocks_models/task.h
@@ -41,7 +41,7 @@ class Task {
   double get(const string& name) const {
     return task_.at(name_to_index_map_.at(name));
   }
-  int dim() {return task_dim_;}
+  const int dim() {return task_dim_;}
   const std::vector<double>& get() const { return task_; }
   void set(const std::vector<double>& values) {
     DRAKE_DEMAND(values.size() == (unsigned)task_dim_);

From c60f1e316f02a36e75d9f3b1843a468443f2ce06 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Thu, 2 Jul 2020 20:05:59 -0400
Subject: [PATCH 49/82] fix the bug in initial_guess_test.cc

---
 .../goldilocks_models/find_models/BUILD.bazel |  1 -
 .../find_models/initial_guess.cc              |  5 ++-
 .../find_models/initial_guess.h               |  3 +-
 .../find_models/initial_guess_test.cc         | 44 ++++++++++++-------
 4 files changed, 32 insertions(+), 21 deletions(-)

diff --git a/examples/goldilocks_models/find_models/BUILD.bazel b/examples/goldilocks_models/find_models/BUILD.bazel
index 01987924e7..d01e0b00e3 100644
--- a/examples/goldilocks_models/find_models/BUILD.bazel
+++ b/examples/goldilocks_models/find_models/BUILD.bazel
@@ -138,7 +138,6 @@ cc_library(
     ],
 )
 
-#edited by jianshu
 cc_library(
     name = "initial_guess",
     srcs = [
diff --git a/examples/goldilocks_models/find_models/initial_guess.cc b/examples/goldilocks_models/find_models/initial_guess.cc
index 4ea60e1856..81224c2b3c 100644
--- a/examples/goldilocks_models/find_models/initial_guess.cc
+++ b/examples/goldilocks_models/find_models/initial_guess.cc
@@ -83,7 +83,8 @@ string SetInitialGuessByInterpolation(const string& directory, int iter,
                                       int sample,
                                       const TasksGenerator* task_gen,
                                       const Task& task, const RomData& rom,
-                                      bool use_database, int robot) {
+                                      bool use_database, int robot,
+                                      bool is_test) {
   /* define some parameters used in interpolation
    * theta_range :decide the range of theta to use in interpolation
    * theta_sclae,gamma_scale :used to scale the theta and gamma in interpolation
@@ -162,7 +163,7 @@ string SetInitialGuessByInterpolation(const string& directory, int iter,
       past_theta << past_theta_s, past_theta_sDDot;
       double theta_diff =
           (past_theta - current_theta).norm() / current_theta.norm();
-      if ((theta_diff < theta_range)) {
+      if ( (theta_diff < theta_range) || (is_test) ) {
         // take out corresponding solution and store it in each column of
         // w_gamma calculate the interpolation weight and store it in
         // weight_gamma
diff --git a/examples/goldilocks_models/find_models/initial_guess.h b/examples/goldilocks_models/find_models/initial_guess.h
index d101fa7e10..a1a66854d6 100644
--- a/examples/goldilocks_models/find_models/initial_guess.h
+++ b/examples/goldilocks_models/find_models/initial_guess.h
@@ -16,7 +16,8 @@ std::string SetInitialGuessByInterpolation(const std::string& directory,
                                            int iter, int sample,
                                            const TasksGenerator* task_gen,
                                            const Task& task, const RomData& rom,
-                                           bool use_database, int robot);
+                                           bool use_database, int robot,
+                                           bool is_test=false);
 // set scale for theta and gamma
 Eigen::VectorXd GetThetaScale(const std::string& directory, int iter);
 Eigen::VectorXd GetGammaScale(const TasksGenerator* task_gen);
diff --git a/examples/goldilocks_models/find_models/initial_guess_test.cc b/examples/goldilocks_models/find_models/initial_guess_test.cc
index dfc26f2575..2fbed859f8 100644
--- a/examples/goldilocks_models/find_models/initial_guess_test.cc
+++ b/examples/goldilocks_models/find_models/initial_guess_test.cc
@@ -8,7 +8,7 @@ class InitialGuessTest : public ::testing::Test {};
 
 int test_initial_guess(int iter, int sample, int robot) {
   // create test data and save it
-  int use_database = false;
+  bool use_database = false;
   // create task_gen
   GridTasksGenerator task_gen_grid;
   if (robot == 0) {
@@ -32,16 +32,26 @@ int test_initial_guess(int iter, int sample, int robot) {
 
   const string dir =
       "../dairlib_data/goldilocks_models/find_models/robot_1_test/";
-  if (!CreateFolderIfNotExist(dir, false)) return 0;
+  //create folder if it doesn't exist
+  if(!folder_exist(dir)){
+    cout<<"Test folder doesn't exist"<<endl;
+    std::string string_for_system_call = "mkdir -p " + dir;
+    if (system(string_for_system_call.c_str()) == -1) {
+      printf("Error creating directory!n");
+      return 0;
+    } else {
+      cout << "Test folder has been created: " << dir << endl;
+    }
+  }
 
   // for each iteration, create theta_s and theta_sDDot
   int iteration = 0;
   for (iteration = 0; iteration <= iter; iteration++) {
-    VectorXd theta_y = VectorXd::Random(2);
-    VectorXd theta_yDDot = VectorXd::Random(2);
+    VectorXd theta_y = VectorXd::Ones(2)+VectorXd::Random(2);
+    VectorXd theta_yddot = VectorXd::Ones(2)+VectorXd::Random(2);
     writeCSV(dir + to_string(iteration) + string("_theta_y.csv"), theta_y);
-    writeCSV(dir + to_string(iteration) + string("_theta_yDDot.csv"),
-             theta_yDDot);
+    writeCSV(dir + to_string(iteration) + string("_theta_yddot.csv"),
+             theta_yddot);
     // for each sample, create gamma, is_success and w
     int sample = 0;
     int dim = 0;
@@ -55,29 +65,29 @@ int test_initial_guess(int iter, int sample, int robot) {
         std::default_random_engine re;
         gamma[dim] = dist(re);
       }
-      writeCSV(dir + prefix + string("_task.csv"), gamma);
-      bool is_success = 1;
+      writeCSV(dir + prefix + string("task.csv"), gamma);
+      int is_success = 1;
       writeCSV(dir + prefix + string("is_success.csv"),
                is_success * MatrixXd::Ones(1, 1));
       VectorXd w = VectorXd::Random(20);
       writeCSV(dir + prefix + string("w.csv"), w);
     }
   }
-
+  bool is_test= true;
   string initial_file = SetInitialGuessByInterpolation(
-      dir, iter, sample, task_gen, task, rom, use_database, robot);
-  return 0;
+      dir, iter, sample, task_gen, task, rom, use_database, robot, is_test);
+  return 1;
 }
 
 TEST_F(InitialGuessTest, DifferentIter) {
-  EXPECT_EQ(0, test_initial_guess(10, 0, 0));
-  EXPECT_EQ(0, test_initial_guess(15, 0, 0));
-  EXPECT_EQ(0, test_initial_guess(20, 0, 0));
+  EXPECT_EQ(1, test_initial_guess(10, 0, 0));
+  EXPECT_EQ(1, test_initial_guess(15, 0, 0));
+  EXPECT_EQ(1, test_initial_guess(20, 0, 0));
 }
 TEST_F(InitialGuessTest, DifferentRobot) {
-  EXPECT_EQ(0, test_initial_guess(10, 0, 1));
-  EXPECT_EQ(0, test_initial_guess(15, 0, 1));
-  EXPECT_EQ(0, test_initial_guess(20, 0, 1));
+  EXPECT_EQ(1, test_initial_guess(10, 0, 1));
+  EXPECT_EQ(1, test_initial_guess(15, 0, 1));
+  EXPECT_EQ(1, test_initial_guess(20, 0, 1));
 }
 
 }  // namespace dairlib::goldilocks_models

From e037f8bb92bcb691089a96425c147345c50e5620 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Fri, 3 Jul 2020 10:35:11 -0400
Subject: [PATCH 50/82] change parts of the codes

---
 examples/goldilocks_models/find_boundary.cc   |  7 +---
 .../find_boundary_utils/compare_boundary.py   | 33 +++++++++----------
 .../find_boundary_utils/search_setting.h      |  2 +-
 3 files changed, 18 insertions(+), 24 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 504b630447..301f728f25 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -707,12 +707,7 @@ int find_boundary(int argc, char* argv[]){
   }
 
   // find which task dimension to search
-  dim = 0;
-  VectorXd extend_direction(3);
-  if(robot_option==1)
-  {
-    extend_direction.conservativeResize(extend_direction.rows() + 1);
-  }
+  VectorXd extend_direction(search_setting.task_dim());
   int ele1,ele2,ele3,ele4;
   // search the boundary
   VectorXd task_delta = Eigen::Map<const VectorXd>(search_setting.task_delta().data(),
diff --git a/examples/goldilocks_models/find_boundary_utils/compare_boundary.py b/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
index b3520134d5..8a60b1852b 100644
--- a/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
+++ b/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
@@ -54,23 +54,22 @@
            '_small_task_space_iter1000/'
     dir8 = file_dir+'dairlib_data/find_boundary/' + robot + 'small_task_space/' + 'robot_' + str(robot_option) + \
            '_small_task_space_iter2000_with_scaling/'
-else:
-    if robot_option == 1:
-        robot = 'cassie/'
-        dir1 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/'+'robot_' + str(robot_option) + \
-               '_2d_nominal/'
-        dir2 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-               '_2d_initial_model/'
-        dir3 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-               '_small_iter300/'
-        dir4 = file_dir + 'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-               '_large_iter150/'
-        dir5 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-               '_2d_initial_model/'
-        dir6 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-               '_small_iter200/'
-        dir7 = file_dir + 'dairlib_data/find_boundary/' + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-               '_large_iter100/'
+if robot_option == 1:
+    robot = 'cassie/'
+    dir1 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/'+'robot_' + str(robot_option) + \
+           '_2d_nominal/'
+    dir2 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+           '_2d_initial_model/'
+    dir3 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+           '_small_iter300/'
+    dir4 = file_dir + 'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+           '_large_iter150/'
+    dir5 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+           '_2d_initial_model/'
+    dir6 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+           '_small_iter200/'
+    dir7 = file_dir + 'dairlib_data/find_boundary/' + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+           '_large_iter100/'
 
 
 x = []
diff --git a/examples/goldilocks_models/find_boundary_utils/search_setting.h b/examples/goldilocks_models/find_boundary_utils/search_setting.h
index 30709db845..e464f3abf8 100644
--- a/examples/goldilocks_models/find_boundary_utils/search_setting.h
+++ b/examples/goldilocks_models/find_boundary_utils/search_setting.h
@@ -24,7 +24,7 @@ class SearchSetting {
   const vector<double>& task_0() const {return task_0_;}
   const vector<double>& task_delta() const {return task_delta_;}
   int index(string task_name) const {return name_to_index_map_.at(task_name);}
-  int get_n_element(int task_dim) const {return elements_[task_dim].size();}
+  int get_n_element(int task_index) const {return elements_[task_index].size();}
   double get_element(int task_index,int element_index) const {
     return elements_[task_index][element_index];
   }

From 8ac40cff667205ba40ceddafaaa764961be2997b Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Fri, 3 Jul 2020 10:46:17 -0400
Subject: [PATCH 51/82] Modifying the codes as requested

---
 .../goldilocks_models/find_models/initial_guess.cc    |  5 ++---
 .../goldilocks_models/find_models/initial_guess.h     |  3 +--
 .../find_models/initial_guess_test.cc                 | 11 ++++++-----
 3 files changed, 9 insertions(+), 10 deletions(-)

diff --git a/examples/goldilocks_models/find_models/initial_guess.cc b/examples/goldilocks_models/find_models/initial_guess.cc
index 81224c2b3c..eca159b7b4 100644
--- a/examples/goldilocks_models/find_models/initial_guess.cc
+++ b/examples/goldilocks_models/find_models/initial_guess.cc
@@ -83,8 +83,7 @@ string SetInitialGuessByInterpolation(const string& directory, int iter,
                                       int sample,
                                       const TasksGenerator* task_gen,
                                       const Task& task, const RomData& rom,
-                                      bool use_database, int robot,
-                                      bool is_test) {
+                                      bool use_database, int robot) {
   /* define some parameters used in interpolation
    * theta_range :decide the range of theta to use in interpolation
    * theta_sclae,gamma_scale :used to scale the theta and gamma in interpolation
@@ -163,7 +162,7 @@ string SetInitialGuessByInterpolation(const string& directory, int iter,
       past_theta << past_theta_s, past_theta_sDDot;
       double theta_diff =
           (past_theta - current_theta).norm() / current_theta.norm();
-      if ( (theta_diff < theta_range) || (is_test) ) {
+      if ( (theta_diff < theta_range) ) {
         // take out corresponding solution and store it in each column of
         // w_gamma calculate the interpolation weight and store it in
         // weight_gamma
diff --git a/examples/goldilocks_models/find_models/initial_guess.h b/examples/goldilocks_models/find_models/initial_guess.h
index a1a66854d6..d101fa7e10 100644
--- a/examples/goldilocks_models/find_models/initial_guess.h
+++ b/examples/goldilocks_models/find_models/initial_guess.h
@@ -16,8 +16,7 @@ std::string SetInitialGuessByInterpolation(const std::string& directory,
                                            int iter, int sample,
                                            const TasksGenerator* task_gen,
                                            const Task& task, const RomData& rom,
-                                           bool use_database, int robot,
-                                           bool is_test=false);
+                                           bool use_database, int robot);
 // set scale for theta and gamma
 Eigen::VectorXd GetThetaScale(const std::string& directory, int iter);
 Eigen::VectorXd GetGammaScale(const TasksGenerator* task_gen);
diff --git a/examples/goldilocks_models/find_models/initial_guess_test.cc b/examples/goldilocks_models/find_models/initial_guess_test.cc
index 2fbed859f8..8be9c93fea 100644
--- a/examples/goldilocks_models/find_models/initial_guess_test.cc
+++ b/examples/goldilocks_models/find_models/initial_guess_test.cc
@@ -27,7 +27,8 @@ int test_initial_guess(int iter, int sample, int robot) {
   task.set(task_gen->NewTask(sample));
   // create rom
   RomData rom = RomData(1, 2, 2, 2);
-  VectorXd current_theta = VectorXd::Random(4);
+  //make sure difference between thetas within theta_range
+  VectorXd current_theta = VectorXd::Random(4)/10000+VectorXd::Ones(4);
   rom.SetTheta(current_theta);
 
   const string dir =
@@ -47,8 +48,9 @@ int test_initial_guess(int iter, int sample, int robot) {
   // for each iteration, create theta_s and theta_sDDot
   int iteration = 0;
   for (iteration = 0; iteration <= iter; iteration++) {
-    VectorXd theta_y = VectorXd::Ones(2)+VectorXd::Random(2);
-    VectorXd theta_yddot = VectorXd::Ones(2)+VectorXd::Random(2);
+    //make sure difference between thetas within theta_range
+    VectorXd theta_y = VectorXd::Ones(2)+VectorXd::Random(2)/10000;
+    VectorXd theta_yddot = VectorXd::Ones(2)+VectorXd::Random(2)/10000;
     writeCSV(dir + to_string(iteration) + string("_theta_y.csv"), theta_y);
     writeCSV(dir + to_string(iteration) + string("_theta_yddot.csv"),
              theta_yddot);
@@ -73,9 +75,8 @@ int test_initial_guess(int iter, int sample, int robot) {
       writeCSV(dir + prefix + string("w.csv"), w);
     }
   }
-  bool is_test= true;
   string initial_file = SetInitialGuessByInterpolation(
-      dir, iter, sample, task_gen, task, rom, use_database, robot, is_test);
+      dir, iter, sample, task_gen, task, rom, use_database, robot);
   return 1;
 }
 

From de1a6d44fec686eca2af405ef4a06183125a36ec Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Fri, 3 Jul 2020 16:45:07 -0400
Subject: [PATCH 52/82] generalize the search function

---
 examples/goldilocks_models/find_boundary.cc   | 115 ++++++++----------
 .../find_boundary_utils/compare_boundary.py   |  13 +-
 ...dscape.py => plot_landscape_comparison.py} |  36 ++----
 3 files changed, 71 insertions(+), 93 deletions(-)
 rename examples/goldilocks_models/find_boundary_utils/{plot_landscape.py => plot_landscape_comparison.py} (90%)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 301f728f25..1f78bdad6a 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -195,7 +195,7 @@ void setInitialTheta(VectorXd& theta_s, VectorXd& theta_sDDot,
 }
 
 //read theta from files to use optimized model
-void readThetaFromFiles(const string dir,int theta_idx,
+void ReadThetaFromFiles(const string dir,int theta_idx,
     VectorXd& theta_y, VectorXd& theta_yddot){
   theta_y = readCSV(dir + to_string(theta_idx) + string("_theta_y.csv"));
   theta_yddot = readCSV(dir + to_string(theta_idx) + string("_theta_yddot.csv"));
@@ -294,7 +294,7 @@ string getInitFileName(const string directory, int traj_opt_num,
 }
 
 // trajectory optimization for given task and model
-void trajOptGivenModel(Task task, const string dir,int num,bool is_rerun,
+void TrajOptGivenModel(Task task, const string dir,int num,bool is_rerun,
     int initial_guess_idx=-1,bool turn_on_scaling=true){
   // Create MBP
   drake::logging::set_log_level("err");  // ignore warnings about joint limits
@@ -357,7 +357,7 @@ void trajOptGivenModel(Task task, const string dir,int num,bool is_rerun,
 
     const string dir_find_models = "../dairlib_data/goldilocks_models/find_models/robot_" +
         to_string(FLAGS_robot_option) + "/";
-    readThetaFromFiles(dir_find_models, theta_idx, theta_y, theta_yddot);
+    ReadThetaFromFiles(dir_find_models, theta_idx, theta_y, theta_yddot);
   }
   else{
     setInitialTheta(theta_y, theta_yddot, n_feature_y, FLAGS_rom_option);
@@ -441,7 +441,7 @@ void CheckSolution(const Task& task, const string dir,int traj_num){
     is_success = (readCSV(dir + to_string(traj_num) +
         string("_0_is_success.csv")))(0, 0);
     if(is_success==0){
-      trajOptGivenModel(task, dir, traj_num,true);
+      TrajOptGivenModel(task, dir, traj_num,true);
     }
     else{
       break;
@@ -452,7 +452,7 @@ void CheckSolution(const Task& task, const string dir,int traj_num){
     is_success = (readCSV(dir + to_string(traj_num) +
         string("_0_is_success.csv")))(0, 0);
     if(is_success==0){
-      trajOptGivenModel(task, dir, traj_num,true,traj_num-1);
+      TrajOptGivenModel(task, dir, traj_num,true,traj_num-1);
     }
     else{
       break;
@@ -464,7 +464,7 @@ void CheckSolution(const Task& task, const string dir,int traj_num){
     is_success = (readCSV(dir + to_string(traj_num) +
         string("_0_is_success.csv")))(0, 0);
     if(is_success==0){
-      trajOptGivenModel(task, dir, traj_num,true,-1,false);
+      TrajOptGivenModel(task, dir, traj_num,true,-1,false);
     }
     else{
       break;
@@ -473,7 +473,7 @@ void CheckSolution(const Task& task, const string dir,int traj_num){
 }
 
 //search the boundary point along one direction
-void boundary_for_one_direction(const string dir,int max_iteration,
+void BoundaryForOneDirection(const string dir,int max_iteration,
     const Task& initial_task,const VectorXd& step_direction, const VectorXd& step_size,
     double max_cost,int& traj_num,int& boundary_point_idx){
   int iter;
@@ -517,7 +517,7 @@ void boundary_for_one_direction(const string dir,int max_iteration,
     writeCSV(dir + to_string(traj_num) +
         string("_0_task.csv"), new_task);
     //run trajectory optimization and judge the solution
-    trajOptGivenModel(task, dir, traj_num, false);
+    TrajOptGivenModel(task, dir, traj_num, false);
     CheckSolution(task,dir,traj_num);
     double sample_cost =
         (readCSV(dir + to_string(traj_num) + string("_0_c.csv")))(0, 0);
@@ -527,7 +527,7 @@ void boundary_for_one_direction(const string dir,int max_iteration,
       double initial_cost =
           (readCSV(dir + string("0_0_c.csv")))(0, 0);
       if(initial_cost>1.2*sample_cost){
-        trajOptGivenModel(initial_task, dir, 0,true,traj_num);
+        TrajOptGivenModel(initial_task, dir, 0,true,traj_num);
       }
     }
     //save the trajectory optimization index and corresponding cost for further use
@@ -558,13 +558,13 @@ void boundary_for_one_direction(const string dir,int max_iteration,
       task.set(task_vector);
       //choose the result of sample with lower cost as initial guess
       if(cost_list(iter-1,1)<cost_list(iter+1,1)){
-        trajOptGivenModel(task, dir, traj_idx,
+        TrajOptGivenModel(task, dir, traj_idx,
                           true, traj_idx-1);
         //make sure this sample success
         CheckSolution(task,dir,traj_idx);
       }
       else{
-        trajOptGivenModel(task, dir, traj_idx,
+        TrajOptGivenModel(task, dir, traj_idx,
                           true, traj_idx+1);
         //make sure this sample success
         CheckSolution(task,dir,traj_idx);
@@ -579,6 +579,36 @@ void boundary_for_one_direction(const string dir,int max_iteration,
   cout << "\nFinish checking the cost:\n";
 }
 
+//Decide Search Vector and serach the task space
+void SearchTaskSpace(int index,const SearchSetting& search_setting,
+    VectorXd& extend_direction,const string dir,int max_iter,
+    const Task& task,const VectorXd& task_delta,double cost_threshold,
+    int& traj_opt_num,int& boundary_sample_num){
+  if(index<search_setting.task_dim())
+  {
+    int ele=0;
+    for (ele=0;ele<search_setting.get_n_element(index);ele++){
+      extend_direction[index] = search_setting.get_element(index,ele);
+      SearchTaskSpace(index+1,search_setting,extend_direction,dir,max_iter,
+      task,task_delta,cost_threshold,traj_opt_num,boundary_sample_num);
+    }
+  }
+  else{
+    //filter with infinity norm to avoid repetitive searching
+    //search along the direction
+    if ((extend_direction.lpNorm<Eigen::Infinity>()) >= 1) {
+      cout << "Start searching along direction: [";
+      for (int i =0;i<search_setting.task_dim();i++) {
+        cout << extend_direction[i] << " \t ";
+      }
+      cout<< "]" << endl;
+      BoundaryForOneDirection(dir,max_iter,
+                              task, extend_direction, task_delta,
+                              cost_threshold,traj_opt_num,boundary_sample_num);
+    }
+  }
+}
+
 int find_boundary(int argc, char* argv[]){
   gflags::ParseCommandLineFlags(&argc, &argv, true);
 
@@ -615,8 +645,8 @@ int find_boundary(int argc, char* argv[]){
   if(robot_option==0)
   {
     vector<vector<double>> elements{
-        {0,-1,0.5,0.5,1}, //stride length
-        {0,-1,0.5,0.5,1}, //ground incline
+        {0,-1,-0.5,0.5,1}, //stride length
+        {0,-1,-0.5,0.5,1}, //ground incline
         {0} //velocity
     };
     task = Task({"stride length", "ground incline",
@@ -627,8 +657,8 @@ int find_boundary(int argc, char* argv[]){
   }
   else if(robot_option==1){
     vector<vector<double>> elements{
-        {0,-1,0.5,0.5,1}, //stride length
-        {0,-1,0.5,0.5,1}, //ground incline
+        {0,-1,-0.5,0.5,1}, //stride length
+        {0,-1,-0.5,0.5,1}, //ground incline
         {0}, //velocity
         {0} //turning rate
     };
@@ -698,60 +728,21 @@ int find_boundary(int argc, char* argv[]){
   cout << "\nCalculate Central Point Cost:\n";
   cout << "sample# (rerun #) | stride | incline | velocity | turning rate | "
           "init_file | Status | Solve time | Cost (tau cost)\n";
-  trajOptGivenModel(task, dir, traj_opt_num, false);
+  TrajOptGivenModel(task, dir, traj_opt_num, false);
   //make sure solution found for the initial point
   int init_is_success = (readCSV(dir + string("0_0_is_success.csv")))(0,0);
   while(!init_is_success){
-    trajOptGivenModel(task, dir, traj_opt_num, true);
+    TrajOptGivenModel(task, dir, traj_opt_num, true);
     init_is_success = (readCSV(dir + string("0_0_is_success.csv")))(0,0);
   }
 
-  // find which task dimension to search
-  VectorXd extend_direction(search_setting.task_dim());
-  int ele1,ele2,ele3,ele4;
   // search the boundary
-  VectorXd task_delta = Eigen::Map<const VectorXd>(search_setting.task_delta().data(),
-      search_setting.task_delta().size());
-  for (ele1=0;ele1<search_setting.get_n_element(0);ele1++){
-    for(ele2=0;ele2<search_setting.get_n_element(1);ele2++){
-      for(ele3=0;ele3<search_setting.get_n_element(2);ele3++){
-        if(robot_option==0) {
-          extend_direction << search_setting.get_element(0,ele1),
-              search_setting.get_element(1,ele2),
-              search_setting.get_element(2,ele3);
-          //filter with infinity norm to avoid repetitive searching
-          //search along the direction
-          if ((extend_direction.lpNorm<Eigen::Infinity>()) >= 1) {
-            cout << "Start searching along direction: [" << extend_direction[0]
-                 << "," << extend_direction[1] << "," << extend_direction[2]
-                 << "]" << endl;
-            boundary_for_one_direction(dir,max_iter,
-                task, extend_direction, task_delta,
-                cost_threshold,traj_opt_num,boundary_sample_num);
-          }
-        }
-        else if(robot_option==1){
-          for(ele4=0;ele4<search_setting.get_n_element(3);ele4++){
-            extend_direction << search_setting.get_element(0,ele1),
-                search_setting.get_element(1,ele2),
-                search_setting.get_element(2,ele3),
-                search_setting.get_element(3,ele4);
-            //filter with infinity norm to avoid repetitive searching
-            //search along the direction
-            if ((extend_direction.lpNorm<Eigen::Infinity>()) >= 1) {
-              cout << "Start searching along direction: [" << extend_direction[0]
-                   << "," << extend_direction[1] << "," << extend_direction[2]
-                   << "," << extend_direction[3] << "]" << endl;
-              boundary_for_one_direction(dir,max_iter,
-                  task, extend_direction, task_delta,
-                  cost_threshold,traj_opt_num,boundary_sample_num);
-            }
-          }
-        }
-      }
-    }
-  }
-
+  VectorXd extend_direction(search_setting.task_dim());
+  dim=0;
+  VectorXd task_delta = Eigen::Map<const VectorXd>(
+      search_setting.task_delta().data(),search_setting.task_delta().size());
+  SearchTaskSpace(dim,search_setting,extend_direction,dir,max_iter,
+      task,task_delta,cost_threshold,traj_opt_num,boundary_sample_num);
   return 0;
 }
 }
diff --git a/examples/goldilocks_models/find_boundary_utils/compare_boundary.py b/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
index 8a60b1852b..91c21790e2 100644
--- a/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
+++ b/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
@@ -1,11 +1,14 @@
+"""
+This function is used for comparing two cost landscape and plot the landscape with discrete color map.
+1.For every point in cost landscape 1 (C1), we check if this point exists in cost landscape 2 (C2):
+If it exists in C2, we compare the costs between them and set the corresponding value for this point.
+Else, we set the value zero for this point.
+2.For every point in C2, we do the same thing while we ignore the intersected point, and we set a large
+value for point existing in C2 rather than in C1.
+"""
 import matplotlib.pyplot as plt
 import matplotlib
 import numpy as np
-from mpl_toolkits.mplot3d import Axes3D
-from scipy.linalg import solve
-import torch
-from torch import tensor
-import csv
 import os
 
 robot_option = 1
diff --git a/examples/goldilocks_models/find_boundary_utils/plot_landscape.py b/examples/goldilocks_models/find_boundary_utils/plot_landscape_comparison.py
similarity index 90%
rename from examples/goldilocks_models/find_boundary_utils/plot_landscape.py
rename to examples/goldilocks_models/find_boundary_utils/plot_landscape_comparison.py
index 945525add9..2c6ad3273d 100644
--- a/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
+++ b/examples/goldilocks_models/find_boundary_utils/plot_landscape_comparison.py
@@ -1,11 +1,5 @@
 import matplotlib.pyplot as plt
-import matplotlib
 import numpy as np
-from mpl_toolkits.mplot3d import Axes3D
-from scipy.linalg import solve
-import torch
-from torch import tensor
-import csv
 import os
 
 robot_option = 1
@@ -13,27 +7,17 @@
 file_dir = '/Users/jason-hu/'
 
 # optimization setting
-if robot_option == 0:
-    min_gi1 = -0.325
-    max_gi1 = 0.275
-    min_sl1 = 0.0925
-    max_sl1 = 0.3925
-
-    min_gi2 = -0.175
-    max_gi2 = 0.125
-    min_sl2 = 0.1675
-    max_sl2 = 0.3175
+# large range
+min_gi1 = -0.325
+max_gi1 = 0.275
+min_sl1 = 0.0925
+max_sl1 = 0.3925
+# small range
+min_gi2 = -0.175
+max_gi2 = 0.125
+min_sl2 = 0.1675
+max_sl2 = 0.3175
 plot_large_range = 1
-if robot_option == 1:
-    min_gi1 = -0.325
-    max_gi1 = 0.275
-    min_sl1 = 0.1425
-    max_sl1 = 0.4425
-
-    min_gi2 = -0.175
-    max_gi2 = 0.125
-    min_sl2 = 0.2175
-    max_sl2 = 0.3675
 plot_small_range = 1
 
 if robot_option == 0:

From df786fa92761bb913cd47dc53a91a8a47f9dd5e9 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Sat, 4 Jul 2020 10:50:08 -0400
Subject: [PATCH 53/82] shrink the delta for velocity to 0.005 for test

---
 examples/goldilocks_models/find_goldilocks_models.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/goldilocks_models/find_goldilocks_models.cc b/examples/goldilocks_models/find_goldilocks_models.cc
index 85f4b3d325..ebe32d5734 100644
--- a/examples/goldilocks_models/find_goldilocks_models.cc
+++ b/examples/goldilocks_models/find_goldilocks_models.cc
@@ -1480,7 +1480,7 @@ int findGoldilocksModels(int argc, char* argv[]) {
           4, {"stride length", "ground incline", "velocity", "turning rate"},
           {FLAGS_N_sample_sl, FLAGS_N_sample_gi, FLAGS_N_sample_v,
            FLAGS_N_sample_tr},
-          {0.3, 0, 0.5, 0}, {0.015, 0.05, 0.04, 0.125}, FLAGS_is_stochastic);
+          {0.3, 0, 0.5, 0}, {0.015, 0.05, 0.005, 0.125}, FLAGS_is_stochastic);
     } else {
       throw std::runtime_error("Should not reach here");
       task_gen_grid = GridTasksGenerator();

From 9ef90a5eb017c439283ec857b36b67596a1893d1 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Sun, 5 Jul 2020 12:03:13 -0400
Subject: [PATCH 54/82] add function of self-defined triangular surf plot

---
 examples/goldilocks_models/find_boundary.cc   |   2 +
 .../find_boundary_utils/poly_color_plot.py    | 164 ++++++++++++++++++
 2 files changed, 166 insertions(+)
 create mode 100644 examples/goldilocks_models/find_boundary_utils/poly_color_plot.py

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 1f78bdad6a..8a07bb4603 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -574,6 +574,8 @@ void BoundaryForOneDirection(const string dir,int max_iteration,
                                       + string("_0_c.csv"))(0,0);
     }
   }
+  writeCSV(dir + to_string(boundary_point+idx) +
+    string("_searching_direction.csv"), step_direction);
   writeCSV(dir +  to_string(boundary_point_idx)  +
       string("_cost_list.csv"), cost_list);
   cout << "\nFinish checking the cost:\n";
diff --git a/examples/goldilocks_models/find_boundary_utils/poly_color_plot.py b/examples/goldilocks_models/find_boundary_utils/poly_color_plot.py
new file mode 100644
index 0000000000..5a0047a5e9
--- /dev/null
+++ b/examples/goldilocks_models/find_boundary_utils/poly_color_plot.py
@@ -0,0 +1,164 @@
+"""
+Given points on two adjacent lines, fill the region between two lines with the color corresponding to the cost.
+Input: points on two lines
+"""
+import matplotlib.pyplot as plt
+import numpy as np
+import os
+
+robot_option = 1
+file_dir = '/Users/jason-hu/'
+if robot_option == 1:
+    robot = 'cassie/'
+    dir1 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/'+'robot_' + str(robot_option) + \
+           '_2d_nominal/'
+    dir2 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+           '_2d_initial_model/'
+
+# number of searching directions
+n_direction = 16
+
+def decidecolor(cost):
+    if cost < 1:
+        color = "green"
+        if cost == 0:
+            color = "red"
+    if cost >= 1:
+        color = "blue"
+        if cost > 1.5:
+            color = "yellow"
+    return color
+
+
+# x1, y1, x2, y2, x3, y3 are scalar
+def tricolorplot(x1, y1, x2, y2, x3, y3, color):
+    X = np.array([x1, x2, x3, x1])
+    Y = np.array([y1, y2, y3, y1])
+    XY = np.ones([X.shape[0], 2])
+    XY[:, 0] = X
+    XY[:, 1] = Y
+    t1 = plt.Polygon(XY, color=color)
+    plt.gca().add_patch(t1)
+    plt.plot(X, Y, color=color)
+
+
+# x1, y1, z1, x2, y2, z2 are array
+def polycolorplot(x1, y1, z1, x2, y2, z2):
+
+    # use i,j to represent the index of the two points on two lines;
+    i = 1
+    j = 1
+    # plot two triangles
+    while (i < x1.shape[0]) & (j < x2.shape[0]):
+        average_cost = (z1[i-1]+z1[i]+z2[j-1]) / 3
+        color = decidecolor(average_cost)
+        tricolorplot(x1[i-1], y1[i-1], x1[i], y1[i], x2[j-1], y2[j-1], color)
+
+        average_cost = (z1[i]+z2[i-1]+z2[i]) / 3
+        color = decidecolor(average_cost)
+        tricolorplot(x1[i], y1[i], x2[j-1], y2[j-1], x2[j], y2[j], color)
+
+        i = i+1
+        j = j+1
+    # after point on one line reach the edge
+    if i == x1.shape[0]:
+        while j < x2.shape[0]:
+            average_cost = (z1[i-1] + z2[j-1] + z2[j]) / 3
+            color = decidecolor(average_cost)
+            tricolorplot(x1[i-1], y1[i-1], x2[j-1], y2[j-1], x2[j], y2[j], color)
+            j = j+1
+    if j == x2.shape[0]:
+        while i < x1.shape[0]:
+            average_cost = (z1[i-1] + z1[i] + z2[j-1]) / 3
+            color = decidecolor(average_cost)
+            tricolorplot(x1[i-1], y1[i-1], x1[i], y1[i], x2[j-1], y2[j-1], color)
+            i = i+1
+
+    plt.show()
+
+
+def extractadjacentline(dir):
+    adj_direction = np.zeros([n_direction])
+    for i in range(n_direction):
+        min_sin = 1
+        for j in range(n_direction):
+            line1 = np.genfromtxt(dir + str(i) + '_' + str(0) + '_searching_direction.csv', delimiter=",")
+            line2 = np.genfromtxt(dir + str(j) + '_' + str(0) + '_searching_direction.csv', delimiter=",")
+            sin = np.cross(line1, line2) / (np.linalg.norm(line1) * np.linalg.norm(line2))
+            cos = np.dot(line1, line2) / (np.linalg.norm(line1) * np.linalg.norm(line2))
+            # restrict the direction
+            if (cos > 0) & (sin > 0):
+                # find the adjacent pair
+                if sin < min_sin:
+                    adj_index = j
+                    min_sin = sin
+        adj_direction[i] = adj_index
+    return adj_direction
+
+
+def process_data_from_direction(i, dir1, dir2):
+    data_dir1 = np.genfromtxt(dir1 + str(i) + '_' + str(0) + '_cost_list.csv', delimiter=",")
+    data_dir2 = np.genfromtxt(dir2 + str(i) + '_' + str(0) + '_cost_list.csv', delimiter=",")
+    # choose the longer line
+    if data_dir1.shape[0] >= data_dir2.shape[0]:
+        num_large = data_dir1.shape[0]
+        num_small = data_dir2.shape[0]
+    else:
+        num_large = data_dir2.shape[0]
+        num_small = data_dir1.shape[0]
+    # need to add central point on the points list
+    task0 = np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_task.csv', delimiter=",")
+    x0 = task0[0]
+    y0 = task0[1]
+    cost1 = np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_c.csv', delimiter=",")
+    cost2 = np.genfromtxt(dir2 + str(0) + '_' + str(0) + '_c.csv', delimiter=",")
+    if cost1 > cost2:
+        z0 = 1.5
+    else:
+        z0 = 0.5
+    # process the points on the line
+    x = np.zeros([num_large])
+    y = np.zeros([num_large])
+    z = np.zeros([num_large])
+    # set the value for intersected parts
+    # Note:decide which column of the task to plot according to the task dimensions
+    # Eg. column index 0 corresponds to stride length
+    for j in range(num_small):
+        task = np.genfromtxt(dir1 + str(data_dir1[j, 0]) + '_' + str(0) + '_task.csv', delimiter=",")
+        x[j] = task[0]
+        y[j] = task[1]
+        cost1 = data_dir1[j,1]
+        cost2 = data_dir2[j,1]
+        if cost1 > cost2:
+            z[j] = 1.5
+        else:
+            z[j] = 0.5
+    for j in range(num_small, num_large):
+        if data_dir1.shape[0] >= data_dir2.shape[0]:
+            # extended range
+            z[j] = 0
+        else:
+            # shrunk range
+            z[j] = 2
+    x = np.concatenate([np.array([x0]), x])
+    y = np.concatenate([np.array([y0]), y])
+    z = np.concatenate([np.array([z0]), z])
+
+    return x,y,z
+
+
+def generateplot(dir1, dir2, adj_index):
+    for i in range(n_direction):
+        # process data on first line
+        x1, y1, z1 = process_data_from_direction(i, dir1, dir2)
+        # process data on adjacent line
+        x2, y2, z2 = process_data_from_direction(adj_index[i], dir1, dir2)
+        # plot
+        polycolorplot(x1, y1, z1, x2, y2, z2)
+
+
+def main():
+    adjacent = extractadjacentline(dir1)
+    generateplot(dir1, dir2, adjacent)
+
+

From 819b99a8b793111e498e63a4330879193351d1f3 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Sun, 5 Jul 2020 12:52:20 -0400
Subject: [PATCH 55/82] add flags to decide which dimension to search

---
 examples/goldilocks_models/find_boundary.cc | 24 ++++++++++++---------
 1 file changed, 14 insertions(+), 10 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 8a07bb4603..8b3339272c 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -80,6 +80,10 @@ DEFINE_bool(is_add_tau_in_cost, true, "Add RoM input in the cost function");
 //outer loop
 DEFINE_int32(max_outer_iter, 150 , "max number of iterations for searching on each "
                                 "direction of one dimension");
+DEFINE_bool(search_sl,true,"decide whether to search the stride length");
+DEFINE_bool(search_gi,true,"decide whether to search the ground incline");
+DEFINE_bool(search_v,false,"decide whether to search the velocity");
+DEFINE_bool(search_tr,false,"decide whether to search the turning rate");
 
 //others
 DEFINE_string(
@@ -574,7 +578,7 @@ void BoundaryForOneDirection(const string dir,int max_iteration,
                                       + string("_0_c.csv"))(0,0);
     }
   }
-  writeCSV(dir + to_string(boundary_point+idx) +
+  writeCSV(dir + to_string(boundary_point_idx) +
     string("_searching_direction.csv"), step_direction);
   writeCSV(dir +  to_string(boundary_point_idx)  +
       string("_cost_list.csv"), cost_list);
@@ -644,13 +648,14 @@ int find_boundary(int argc, char* argv[]){
   // create components for each dimension used to decide the direction
   // considering we can only visualize 2D landscape, we only search within
   // 2D space and fix other dimension
+  vector<double> search_elements = {0,-1,-0.5,0.5,1};
+  vector<double> non_search_elements = {0};
   if(robot_option==0)
   {
     vector<vector<double>> elements{
-        {0,-1,-0.5,0.5,1}, //stride length
-        {0,-1,-0.5,0.5,1}, //ground incline
-        {0} //velocity
-    };
+      FLAGS_search_sl ? search_elements : non_search_elements,
+      FLAGS_search_gi ? search_elements : non_search_elements,
+      FLAGS_search_v ? search_elements : non_search_elements};
     task = Task({"stride length", "ground incline",
                  "velocity"});
     search_setting = SearchSetting(3,{"stride length", "ground incline",
@@ -659,11 +664,10 @@ int find_boundary(int argc, char* argv[]){
   }
   else if(robot_option==1){
     vector<vector<double>> elements{
-        {0,-1,-0.5,0.5,1}, //stride length
-        {0,-1,-0.5,0.5,1}, //ground incline
-        {0}, //velocity
-        {0} //turning rate
-    };
+      FLAGS_search_sl ? search_elements : non_search_elements,
+      FLAGS_search_gi ? search_elements : non_search_elements,
+      FLAGS_search_v ? search_elements : non_search_elements,
+      FLAGS_search_tr ? search_elements : non_search_elements};
     task = Task({"stride length", "ground incline",
                  "velocity", "turning rate"});
     search_setting = SearchSetting(4,{"stride length", "ground incline",

From 72400e3df815b53755007758184c30b3f5c2bfb0 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Sun, 5 Jul 2020 18:59:55 -0400
Subject: [PATCH 56/82] fix a bug in initial_guess.cc

---
 examples/goldilocks_models/find_models/initial_guess.cc | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/examples/goldilocks_models/find_models/initial_guess.cc b/examples/goldilocks_models/find_models/initial_guess.cc
index eca159b7b4..d9b7123632 100644
--- a/examples/goldilocks_models/find_models/initial_guess.cc
+++ b/examples/goldilocks_models/find_models/initial_guess.cc
@@ -120,8 +120,10 @@ string SetInitialGuessByInterpolation(const string& directory, int iter,
 
     // calculate the weighted sum of past solutions
     int sample_num = 0;
-    string prefix = to_string(sample_num) + "_0";
-    while (file_exist(data_dir + prefix + string("_is_success.csv"))) {
+    string prefix;
+    while (file_exist(data_dir + to_string(sample_num) + "_0"
+         + string("_is_success.csv"))) {
+      prefix = to_string(sample_num) + "_0";
       InterpolateAmongDifferentTasks(data_dir, prefix, current_gamma,
                                      gamma_scale, weight_gamma, w_gamma);
       sample_num = sample_num + 1;

From abf9e880bf27fc8b3559535b823ab85acb749978 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Sun, 5 Jul 2020 19:43:00 -0400
Subject: [PATCH 57/82] fix a bug in initial_guess.cc;Wrongly use the
 normalize()

---
 examples/goldilocks_models/find_boundary.cc             | 2 +-
 examples/goldilocks_models/find_models/initial_guess.cc | 5 +++--
 examples/goldilocks_models/find_models/initial_guess.h  | 2 +-
 3 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 8b3339272c..282718b5d8 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -227,7 +227,7 @@ string getInitFileName(const string directory, int traj_opt_num,
     double delta_sl = 0.015;
     double delta_gi = 0.05;
     double delta_tr = 0.125;
-    double delta_v = 0.04;
+    double delta_v = 0.02;
     gamma_scale << 1/delta_sl,1/delta_gi,1/delta_v,1.3/delta_tr;
   }
   string initial_file_name;
diff --git a/examples/goldilocks_models/find_models/initial_guess.cc b/examples/goldilocks_models/find_models/initial_guess.cc
index cb1c4a5c31..5e1ae5649b 100644
--- a/examples/goldilocks_models/find_models/initial_guess.cc
+++ b/examples/goldilocks_models/find_models/initial_guess.cc
@@ -68,11 +68,12 @@ void InterpolateAmongDifferentTasks(const string& dir, string prefix,
 }
 
 // calculate interpolated initial guess using weight vector and solution matrix
-VectorXd CalculateInterpolation(const VectorXd& weight_vector,
+VectorXd CalculateInterpolation(VectorXd& weight_vector,
                                 const MatrixXd& solution_matrix) {
   DRAKE_DEMAND(weight_vector.rows() > 0);
   // interpolation
-  VectorXd interpolated_solution = solution_matrix * weight_vector.normalized();
+  weight_vector = weight_vector/weight_vector.sum();
+  VectorXd interpolated_solution = solution_matrix * weight_vector;
   return interpolated_solution;
 }
 
diff --git a/examples/goldilocks_models/find_models/initial_guess.h b/examples/goldilocks_models/find_models/initial_guess.h
index 3546c98423..f8e1ee2dec 100644
--- a/examples/goldilocks_models/find_models/initial_guess.h
+++ b/examples/goldilocks_models/find_models/initial_guess.h
@@ -18,7 +18,7 @@ std::string SetInitialGuessByInterpolation(const std::string& directory,
 Eigen::VectorXd GetThetaScale(const std::string& directory, int iter);
 Eigen::VectorXd GetGammaScale(const TasksGenerator* task_gen);
 // utility functions
-Eigen::VectorXd CalculateInterpolation(const Eigen::VectorXd& weight_vector,
+Eigen::VectorXd CalculateInterpolation(Eigen::VectorXd& weight_vector,
                                        const Eigen::MatrixXd& solution_matrix);
 void InterpolateAmongDifferentTasks(const std::string& dir, string prefix,
                                     const Eigen::VectorXd& current_gamma,

From 0990cbf9568bb7ff553e238c7154fb888814d19d Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Sun, 5 Jul 2020 19:45:08 -0400
Subject: [PATCH 58/82] fix a bug in initial_guess.cc;Wringly use the function
 normalized()

---
 examples/goldilocks_models/find_models/initial_guess.cc | 3 ++-
 examples/goldilocks_models/find_models/initial_guess.h  | 2 +-
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/examples/goldilocks_models/find_models/initial_guess.cc b/examples/goldilocks_models/find_models/initial_guess.cc
index d9b7123632..56494c03f2 100644
--- a/examples/goldilocks_models/find_models/initial_guess.cc
+++ b/examples/goldilocks_models/find_models/initial_guess.cc
@@ -71,10 +71,11 @@ void InterpolateAmongDifferentTasks(const string& dir, string prefix,
 }
 
 // calculate interpolated initial guess using weight vector and solution matrix
-VectorXd CalculateInterpolation(const VectorXd& weight_vector,
+VectorXd CalculateInterpolation(VectorXd& weight_vector,
                                 const MatrixXd& solution_matrix) {
   DRAKE_DEMAND(weight_vector.rows() > 0);
   // interpolation
+  weight_vector = weight_vector/weight_vector.sum();
   VectorXd interpolated_solution = solution_matrix * weight_vector.normalized();
   return interpolated_solution;
 }
diff --git a/examples/goldilocks_models/find_models/initial_guess.h b/examples/goldilocks_models/find_models/initial_guess.h
index d101fa7e10..085ff0b1c2 100644
--- a/examples/goldilocks_models/find_models/initial_guess.h
+++ b/examples/goldilocks_models/find_models/initial_guess.h
@@ -21,7 +21,7 @@ std::string SetInitialGuessByInterpolation(const std::string& directory,
 Eigen::VectorXd GetThetaScale(const std::string& directory, int iter);
 Eigen::VectorXd GetGammaScale(const TasksGenerator* task_gen);
 // utility functions
-Eigen::VectorXd CalculateInterpolation(const Eigen::VectorXd& weight_vector,
+Eigen::VectorXd CalculateInterpolation(Eigen::VectorXd& weight_vector,
                                        const Eigen::MatrixXd& solution_matrix);
 void InterpolateAmongDifferentTasks(const std::string& dir, string prefix,
                                     const Eigen::VectorXd& current_gamma,

From a02f0397067a09e1c83213172d612528d7cbddcc Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Sun, 5 Jul 2020 19:47:15 -0400
Subject: [PATCH 59/82] modify

---
 examples/goldilocks_models/find_models/initial_guess.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/goldilocks_models/find_models/initial_guess.cc b/examples/goldilocks_models/find_models/initial_guess.cc
index 56494c03f2..a7d43a980b 100644
--- a/examples/goldilocks_models/find_models/initial_guess.cc
+++ b/examples/goldilocks_models/find_models/initial_guess.cc
@@ -76,7 +76,7 @@ VectorXd CalculateInterpolation(VectorXd& weight_vector,
   DRAKE_DEMAND(weight_vector.rows() > 0);
   // interpolation
   weight_vector = weight_vector/weight_vector.sum();
-  VectorXd interpolated_solution = solution_matrix * weight_vector.normalized();
+  VectorXd interpolated_solution = solution_matrix * weight_vector;
   return interpolated_solution;
 }
 

From 38b502c669d08093199aa114e32c7f9758043474 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Mon, 6 Jul 2020 12:56:59 -0400
Subject: [PATCH 60/82] adjust the delta for tasks

---
 examples/goldilocks_models/find_boundary.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 282718b5d8..a5ad960220 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -672,7 +672,7 @@ int find_boundary(int argc, char* argv[]){
                  "velocity", "turning rate"});
     search_setting = SearchSetting(4,{"stride length", "ground incline",
                                       "velocity","turning rate"},
-                                   {0.3,0,0.5,0},{0.01,0.01,0.02,0.01},elements);
+                                   {0.3,0,0.5,0},{0.01,0.01,0.01,0.02},elements);
   }
   //cout initial point information
   int dim = 0;

From ee8fa46aacfa569a50761cc23e76435ce0403e51 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Tue, 7 Jul 2020 15:51:54 -0400
Subject: [PATCH 61/82] minor change

---
 examples/goldilocks_models/find_models/initial_guess.cc | 7 +++----
 examples/goldilocks_models/find_models/initial_guess.h  | 2 +-
 2 files changed, 4 insertions(+), 5 deletions(-)

diff --git a/examples/goldilocks_models/find_models/initial_guess.cc b/examples/goldilocks_models/find_models/initial_guess.cc
index a7d43a980b..70c8bba8f3 100644
--- a/examples/goldilocks_models/find_models/initial_guess.cc
+++ b/examples/goldilocks_models/find_models/initial_guess.cc
@@ -71,12 +71,11 @@ void InterpolateAmongDifferentTasks(const string& dir, string prefix,
 }
 
 // calculate interpolated initial guess using weight vector and solution matrix
-VectorXd CalculateInterpolation(VectorXd& weight_vector,
+VectorXd CalculateInterpolation(const VectorXd& weight_vector,
                                 const MatrixXd& solution_matrix) {
   DRAKE_DEMAND(weight_vector.rows() > 0);
-  // interpolation
-  weight_vector = weight_vector/weight_vector.sum();
-  VectorXd interpolated_solution = solution_matrix * weight_vector;
+  // normalize the weight vector by L1 norm and interpolate
+  VectorXd interpolated_solution = solution_matrix * weight_vector/weight_vector.sum();
   return interpolated_solution;
 }
 
diff --git a/examples/goldilocks_models/find_models/initial_guess.h b/examples/goldilocks_models/find_models/initial_guess.h
index 085ff0b1c2..d101fa7e10 100644
--- a/examples/goldilocks_models/find_models/initial_guess.h
+++ b/examples/goldilocks_models/find_models/initial_guess.h
@@ -21,7 +21,7 @@ std::string SetInitialGuessByInterpolation(const std::string& directory,
 Eigen::VectorXd GetThetaScale(const std::string& directory, int iter);
 Eigen::VectorXd GetGammaScale(const TasksGenerator* task_gen);
 // utility functions
-Eigen::VectorXd CalculateInterpolation(Eigen::VectorXd& weight_vector,
+Eigen::VectorXd CalculateInterpolation(const Eigen::VectorXd& weight_vector,
                                        const Eigen::MatrixXd& solution_matrix);
 void InterpolateAmongDifferentTasks(const std::string& dir, string prefix,
                                     const Eigen::VectorXd& current_gamma,

From 7ac34d711f7baae66b5e3b8bf42f4d1f4e8a71f1 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshu@seas.upenn.edu>
Date: Thu, 9 Jul 2020 10:11:47 -0400
Subject: [PATCH 62/82] set the cost threshold same for nominal and usual case

---
 examples/goldilocks_models/find_boundary.cc | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index a5ad960220..f1058bb327 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -695,7 +695,7 @@ int find_boundary(int argc, char* argv[]){
   if(robot_option==0)
   {
     if(FLAGS_is_get_nominal){
-      cost_threshold = 35;
+      cost_threshold = 30;
     }
     else{
       cost_threshold = 30;
@@ -703,7 +703,7 @@ int find_boundary(int argc, char* argv[]){
   }
   else if(robot_option==1){
     if(FLAGS_is_get_nominal){
-      cost_threshold = 35;
+      cost_threshold = 30;
     }
     else{
       cost_threshold = 30;

From 47065daf0ef8916d2d63e8cec3ea353f7dd5e981 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshuhu98@gmail.com>
Date: Fri, 10 Jul 2020 09:16:00 -0400
Subject: [PATCH 63/82] fix a bug for providing initial guess for first sample
 in a line

---
 examples/goldilocks_models/find_boundary.cc | 20 ++++++++++++++------
 1 file changed, 14 insertions(+), 6 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index f1058bb327..ef2321cd69 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -436,9 +436,10 @@ void SaveBoundaryPointInfor(const string dir,int boundary_point_index,
 }
 
 //check the solution of trajectory optimization and rerun if necessary
-void CheckSolution(const Task& task, const string dir,int traj_num){
+void CheckSolution(const Task& task, const string dir, int traj_num,
+    int iteration){
   int rerun = 0;
-  int max_rerun_num = 4;
+  int max_rerun_num = 5;
   int is_success;
   //check if snopt find a solution successfully. If not, rerun the Traj Opt
   for(rerun=0;rerun<max_rerun_num;rerun++){
@@ -452,11 +453,18 @@ void CheckSolution(const Task& task, const string dir,int traj_num){
     }
   }
   //if snopt still can't find a solution, try to use adjacent sample to help
+  //if this is iteration 1, we should use the central point to help
   for(rerun=0;rerun<max_rerun_num;rerun++){
     is_success = (readCSV(dir + to_string(traj_num) +
         string("_0_is_success.csv")))(0, 0);
     if(is_success==0){
-      TrajOptGivenModel(task, dir, traj_num,true,traj_num-1);
+      if(iteration==1)
+      {
+        TrajOptGivenModel(task, dir, traj_num,true,0);
+      }
+      else{
+        TrajOptGivenModel(task, dir, traj_num,true,traj_num-1);
+      }
     }
     else{
       break;
@@ -522,7 +530,7 @@ void BoundaryForOneDirection(const string dir,int max_iteration,
         string("_0_task.csv"), new_task);
     //run trajectory optimization and judge the solution
     TrajOptGivenModel(task, dir, traj_num, false);
-    CheckSolution(task,dir,traj_num);
+    CheckSolution(task,dir,traj_num,iter);
     double sample_cost =
         (readCSV(dir + to_string(traj_num) + string("_0_c.csv")))(0, 0);
     // without a good initial guess, the initial point is easily stuck in a local minimum
@@ -565,13 +573,13 @@ void BoundaryForOneDirection(const string dir,int max_iteration,
         TrajOptGivenModel(task, dir, traj_idx,
                           true, traj_idx-1);
         //make sure this sample success
-        CheckSolution(task,dir,traj_idx);
+        CheckSolution(task,dir,traj_idx,iter+1);
       }
       else{
         TrajOptGivenModel(task, dir, traj_idx,
                           true, traj_idx+1);
         //make sure this sample success
-        CheckSolution(task,dir,traj_idx);
+        CheckSolution(task,dir,traj_idx,iter+1);
       }
       //update cost list
       cost_list(iter,1) = readCSV(dir + to_string(traj_idx)

From b4a42c137dbba9a7945d4b5e66aaa7d4c35800e5 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshuhu98@gmail.com>
Date: Fri, 10 Jul 2020 19:21:38 -0400
Subject: [PATCH 64/82] add a flag used for continuing program from midpoint

---
 examples/goldilocks_models/find_boundary.cc | 67 ++++++++++++---------
 1 file changed, 38 insertions(+), 29 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index ef2321cd69..024bbd8a7b 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -84,6 +84,9 @@ DEFINE_bool(search_sl,true,"decide whether to search the stride length");
 DEFINE_bool(search_gi,true,"decide whether to search the ground incline");
 DEFINE_bool(search_v,false,"decide whether to search the velocity");
 DEFINE_bool(search_tr,false,"decide whether to search the turning rate");
+DEFINE_bool(continue_from_midpoint,false,"if the program was stopped by accident,"
+                                         "this can be used to accelerate rerunning"
+                                         " the program");
 
 //others
 DEFINE_string(
@@ -435,13 +438,11 @@ void SaveBoundaryPointInfor(const string dir,int boundary_point_index,
   cout<<endl;
 }
 
-//check the solution of trajectory optimization and rerun if necessary
-void CheckSolution(const Task& task, const string dir, int traj_num,
-    int iteration){
+//rerun unsuccessful trajectory optimization
+void RerunTrajOpt(const Task& task, const string dir, int traj_num){
   int rerun = 0;
   int max_rerun_num = 5;
   int is_success;
-  //check if snopt find a solution successfully. If not, rerun the Traj Opt
   for(rerun=0;rerun<max_rerun_num;rerun++){
     is_success = (readCSV(dir + to_string(traj_num) +
         string("_0_is_success.csv")))(0, 0);
@@ -452,36 +453,39 @@ void CheckSolution(const Task& task, const string dir, int traj_num,
       break;
     }
   }
+}
+
+//check the solution of trajectory optimization and rerun if necessary
+void CheckSolution(const Task& task, const string dir, int traj_num,
+    int iteration){
+  int is_success;
+
+  //check if snopt find a solution successfully. If not, rerun the Traj Opt
+  RerunTrajOpt(task,dir,traj_num);
+
   //if snopt still can't find a solution, try to use adjacent sample to help
-  //if this is iteration 1, we should use the central point to help
-  for(rerun=0;rerun<max_rerun_num;rerun++){
-    is_success = (readCSV(dir + to_string(traj_num) +
-        string("_0_is_success.csv")))(0, 0);
-    if(is_success==0){
-      if(iteration==1)
-      {
-        TrajOptGivenModel(task, dir, traj_num,true,0);
-      }
-      else{
-        TrajOptGivenModel(task, dir, traj_num,true,traj_num-1);
-      }
+  is_success = (readCSV(dir + to_string(traj_num) +
+      string("_0_is_success.csv")))(0, 0);
+  if(is_success==0){
+    if(iteration==1)
+    {
+      //if number of iteration is 1, we should use the central point to help
+      TrajOptGivenModel(task, dir, traj_num,true,0);
     }
     else{
-      break;
+      TrajOptGivenModel(task, dir, traj_num,true,traj_num-1);
     }
   }
-  //if snopt still failed to find a solution,turn off the scaling option
+  RerunTrajOpt(task,dir,traj_num);
+
+  // if snopt still failed to find a solution,turn off the scaling option
   // and try again
-  for(rerun=0;rerun<max_rerun_num;rerun++){
-    is_success = (readCSV(dir + to_string(traj_num) +
-        string("_0_is_success.csv")))(0, 0);
-    if(is_success==0){
-      TrajOptGivenModel(task, dir, traj_num,true,-1,false);
-    }
-    else{
-      break;
-    }
+  is_success = (readCSV(dir + to_string(traj_num) +
+      string("_0_is_success.csv")))(0, 0);
+  if(is_success==0){
+    TrajOptGivenModel(task, dir, traj_num,true,-1,false);
   }
+  RerunTrajOpt(task,dir,traj_num);
 }
 
 //search the boundary point along one direction
@@ -529,7 +533,10 @@ void BoundaryForOneDirection(const string dir,int max_iteration,
     writeCSV(dir + to_string(traj_num) +
         string("_0_task.csv"), new_task);
     //run trajectory optimization and judge the solution
-    TrajOptGivenModel(task, dir, traj_num, false);
+    if(! (FLAGS_continue_from_midpoint && file_exist(dir + to_string(traj_num) +
+        string("_0_w.csv")))){
+      TrajOptGivenModel(task, dir, traj_num, false);
+    }
     CheckSolution(task,dir,traj_num,iter);
     double sample_cost =
         (readCSV(dir + to_string(traj_num) + string("_0_c.csv")))(0, 0);
@@ -742,7 +749,9 @@ int find_boundary(int argc, char* argv[]){
   cout << "\nCalculate Central Point Cost:\n";
   cout << "sample# (rerun #) | stride | incline | velocity | turning rate | "
           "init_file | Status | Solve time | Cost (tau cost)\n";
-  TrajOptGivenModel(task, dir, traj_opt_num, false);
+  if(!FLAGS_continue_from_midpoint){
+    TrajOptGivenModel(task, dir, traj_opt_num, false);
+  }
   //make sure solution found for the initial point
   int init_is_success = (readCSV(dir + string("0_0_is_success.csv")))(0,0);
   while(!init_is_success){

From 2f4da02a70a5d9dc7784ae77f6a217641b00b972 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshuhu98@gmail.com>
Date: Sat, 11 Jul 2020 15:08:57 -0400
Subject: [PATCH 65/82] update functions used for plotting landscape

---
 .../find_boundary_utils/compare_boundary.py   | 207 +++++++-----------
 ...dscape_comparison.py => plot_landscape.py} |  55 ++---
 .../plot_normalized_landscape.py              |  90 ++++++++
 .../find_boundary_utils/poly_color_plot.py    |  77 +++++--
 4 files changed, 238 insertions(+), 191 deletions(-)
 rename examples/goldilocks_models/find_boundary_utils/{plot_landscape_comparison.py => plot_landscape.py} (66%)
 create mode 100644 examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py

diff --git a/examples/goldilocks_models/find_boundary_utils/compare_boundary.py b/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
index 91c21790e2..7c3dfdfd72 100644
--- a/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
+++ b/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
@@ -1,10 +1,12 @@
 """
 This function is used for comparing two cost landscape and plot the landscape with discrete color map.
-1.For every point in cost landscape 1 (C1), we check if this point exists in cost landscape 2 (C2):
-If it exists in C2, we compare the costs between them and set the corresponding value for this point.
-Else, we set the value zero for this point.
-2.For every point in C2, we do the same thing while we ignore the intersected point, and we set a large
-value for point existing in C2 rather than in C1.
+Considering we search along several directions, we process the data along those directions
+1.
+For each direction, we compare the cost at each point on the searching line both in cost landscape 1 (C1) and
+cost landscape. Set the value of the point according the cost.
+2.
+For those points which are not in two landscape, if it exists in C1, we set the value of this point -0.5,
+else we set the value of this point 2
 """
 import matplotlib.pyplot as plt
 import matplotlib
@@ -13,131 +15,86 @@
 
 robot_option = 1
 file_dir = '/Users/jason-hu/'
+if robot_option == 1:
+    robot = 'cassie/'
+    dir1 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
+           '_grid_iter50_sl_tr/'
+    dir2 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
+           '_nominal_sl_tr/'
 
-# optimization setting
-if robot_option == 0:
-    min_gi1 = -0.325
-    max_gi1 = 0.275
-    min_sl1 = 0.0925
-    max_sl1 = 0.3925
-else:
-    min_gi1 = -0.325
-    max_gi1 = 0.275
-    min_sl1 = 0.1425
-    max_sl1 = 0.4425
-plot_large_range = 1
+# number of searching directions
+n_direction = 16
 
-if robot_option == 0:
-    min_gi2 = -0.175
-    max_gi2 = 0.125
-    min_sl2 = 0.1675
-    max_sl2 = 0.3175
-else:
-    min_gi2 = -0.175
-    max_gi2 = 0.125
-    min_sl2 = 0.2175
-    max_sl2 = 0.3675
-plot_small_range = 1
 
-if robot_option == 0:
-    robot = 'five_link/'
-    dir1 = file_dir+'dairlib_data/find_boundary/' + robot + 'robot_' + str(robot_option) + \
-           '_nominal/'
-    dir2 = file_dir+'dairlib_data/find_boundary/' + robot + 'robot_' + str(robot_option) + \
-           '_initial_model/'
-    dir3 = file_dir+'dairlib_data/find_boundary/' + robot + 'large_task_space/' + 'robot_' + str(robot_option) + \
-           '_large_task_space_iter1000/'
-    dir4 = file_dir+'dairlib_data/find_boundary/' + robot + 'large_task_space/' + 'robot_' + str(robot_option) + \
-           '_large_task_space_iter2000/'
-    dir5 = file_dir+'dairlib_data/find_boundary/' + robot + 'large_task_space/' + 'robot_' + str(robot_option) + \
-           '_large_task_space_iter3000_with_scaling/'
-    dir6 = file_dir+'dairlib_data/find_boundary/' + robot + 'small_task_space/' + 'robot_' + str(robot_option) + \
-           '_small_task_space_iter500/'
-    dir7 = file_dir+'dairlib_data/find_boundary/' + robot + 'small_task_space/' + 'robot_' + str(robot_option) + \
-           '_small_task_space_iter1000/'
-    dir8 = file_dir+'dairlib_data/find_boundary/' + robot + 'small_task_space/' + 'robot_' + str(robot_option) + \
-           '_small_task_space_iter2000_with_scaling/'
-if robot_option == 1:
-    robot = 'cassie/'
-    dir1 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/'+'robot_' + str(robot_option) + \
-           '_2d_nominal/'
-    dir2 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-           '_2d_initial_model/'
-    dir3 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-           '_small_iter300/'
-    dir4 = file_dir + 'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-           '_large_iter150/'
-    dir5 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-           '_2d_initial_model/'
-    dir6 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-           '_small_iter200/'
-    dir7 = file_dir + 'dairlib_data/find_boundary/' + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-           '_large_iter100/'
+# Note:decide which column of the task to plot according to the task dimensions
+# Eg. column index 0 corresponds to stride length
+task_1_idx = 0
+task_2_idx = 3
 
 
-x = []
-y = []
-z = []
-dir = dir4
-dir_compare = dir3
-i = 0
-found_same_sample = False
-while os.path.isfile(dir+str(i)+'_'+str(0)+'_gamma.csv'):
-    gamma = np.genfromtxt(dir + str(i) + '_' + str(0) + '_gamma.csv', delimiter=",")
-    cost = float(np.genfromtxt(dir + str(i) + '_' + str(0) + '_c.csv', delimiter=","))
-    j = 0
-    # search if the intersected samples
-    while os.path.isfile(dir_compare+str(j)+'_'+str(0)+'_gamma.csv'):
-        gamma_compare = np.genfromtxt(dir_compare + str(j) + '_' + str(0) + '_gamma.csv', delimiter=",")
-        if np.array_equal(gamma, gamma_compare):
-            found_same_sample = True
-            # if np.genfromtxt(dir + str(i) + '_' + str(0) + '_is_success.csv', delimiter=",") == 1:
-            #     if np.genfromtxt(dir_compare + str(j) + '_' + str(0) + '_is_success.csv', delimiter=",") == 1:
-            compare_cost = float(np.genfromtxt(dir_compare + str(j) + '_' + str(0) + '_c.csv', delimiter=","))
-            x.append(gamma[0])
-            y.append(gamma[1])
-            if cost > compare_cost:
-                z.append(1.5)
-            else:
-                z.append(0.5)
-            break
-        j = j+1
-    if found_same_sample:
-        found_same_sample = False
-    else:
-        x.append(gamma[0])
-        y.append(gamma[1])
-        z.append(-1)
-    i = i+1
-# search another side
-i = 0
-while os.path.isfile(dir_compare+str(i)+'_'+str(0)+'_gamma.csv'):
-    gamma_compare = np.genfromtxt(dir_compare + str(i) + '_' + str(0) + '_gamma.csv', delimiter=",")
-    j = 0
-    # search if the intersected samples
-    while os.path.isfile(dir+str(j)+'_'+str(0)+'_gamma.csv'):
-        gamma = np.genfromtxt(dir + str(j) + '_' + str(0) + '_gamma.csv', delimiter=",")
-        if np.array_equal(gamma, gamma_compare):
-            found_same_sample = True
-            break
-        j = j+1
-    if found_same_sample:
-        found_same_sample = False
+def process_data_from_direction(dir1, dir_nominal):
+    # need to add central point on the points list
+    task0 = np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_task.csv', delimiter=",")
+    x0 = [task0[task_1_idx]]
+    y0 = [task0[task_2_idx]]
+    cost1 = np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_c.csv', delimiter=",")
+    cost2 = np.genfromtxt(dir_nominal + str(0) + '_' + str(0) + '_c.csv', delimiter=",")
+    if cost1 > cost2:
+        z0 = [1.5]
     else:
-        x.append(gamma_compare[0])
-        y.append(gamma_compare[1])
-        z.append(1.5)
-    i = i+1
+        z0 = [0.5]
+    for i in range(n_direction):
+        data_dir1 = np.genfromtxt(dir1 + str(i+1) + '_cost_list.csv', delimiter=",")
+        data_dir2 = np.genfromtxt(dir_nominal + str(i+1) + '_cost_list.csv', delimiter=",")
 
+        if data_dir1.shape[0] >= data_dir2.shape[0]:
+            num_small = data_dir2.shape[0]
+            num_large = data_dir1.shape[0]
+        else:
+            num_small = data_dir1.shape[0]
+            num_large = data_dir2.shape[0]
 
-print(np.shape(x))
-print(np.shape(y))
-print(np.shape(z))
+        # process the points on the line
+        x = []
+        y = []
+        z = []
 
-fig, ax = plt.subplots()
+        # set the value for intersected parts
+        for j in range(num_small):
+            cost1 = data_dir1[j, 1]
+            cost2 = data_dir2[j, 1]
+            # only consider reasonable point
+            if (cost1 < 35) & (cost2 < 35):
+                if cost1 > cost2:
+                    z.append(1.5)
+                else:
+                    z.append(0.5)
+                task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+                x.append(task[task_1_idx])
+                y.append(task[task_2_idx])
+        for j in range(num_small, num_large):
+            if data_dir1.shape[0] >= data_dir2.shape[0]:
+                # extended range
+                task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+                x.append(task[task_1_idx])
+                y.append(task[task_2_idx])
+                z.append(-0.5)
+            else:
+                # shrunk range
+                task = np.genfromtxt(dir_nominal + str(int(data_dir2[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+                x.append(task[task_1_idx])
+                y.append(task[task_2_idx])
+                z.append(2)
+        if len(x) > 10:
+            x0 = x0 + x
+            y0 = y0 + y
+            z0 = z0 + z
+    return np.array(x0), np.array(y0), np.array(z0)
 
-# discrete color map
 
+fig, ax = plt.subplots()
+x, y, z = process_data_from_direction(dir1, dir2)
+# discrete color map
 levels = [0, 1, 2]
 colors = ['green', 'blue']
 cmap, norm = matplotlib.colors.from_levels_and_colors(levels, colors)
@@ -151,16 +108,4 @@
 ax.set_ylabel('Ground incline')
 ax.set_title('Compare two cost landscapes')
 
-# optimization range
-# large range
-if plot_large_range:
-    x_line = np.array([min_sl1, max_sl1, max_sl1, min_sl1, min_sl1])
-    y_line = np.array([min_gi1, min_gi1, max_gi1, max_gi1, min_gi1])
-    ax.plot(x_line, y_line, 'black', linewidth=3)
-# small range
-if plot_small_range:
-    x_line2 = np.array([min_sl2, max_sl2, max_sl2, min_sl2, min_sl2])
-    y_line2 = np.array([min_gi2, min_gi2, max_gi2, max_gi2, min_gi2])
-    ax.plot(x_line2, y_line2, 'black', linewidth=3)
-
 plt.show()
\ No newline at end of file
diff --git a/examples/goldilocks_models/find_boundary_utils/plot_landscape_comparison.py b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
similarity index 66%
rename from examples/goldilocks_models/find_boundary_utils/plot_landscape_comparison.py
rename to examples/goldilocks_models/find_boundary_utils/plot_landscape.py
index 2c6ad3273d..208c0951a5 100644
--- a/examples/goldilocks_models/find_boundary_utils/plot_landscape_comparison.py
+++ b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
@@ -3,7 +3,6 @@
 import os
 
 robot_option = 1
-normalized_cost = True
 file_dir = '/Users/jason-hu/'
 
 # optimization setting
@@ -17,8 +16,8 @@
 max_gi2 = 0.125
 min_sl2 = 0.1675
 max_sl2 = 0.3175
-plot_large_range = 1
-plot_small_range = 1
+plot_large_range = 0
+plot_small_range = 0
 
 if robot_option == 0:
     robot = 'five_link/'
@@ -54,44 +53,25 @@
            '_small_iter200/'
     dir7 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
            '_large_iter100/'
+    dir8 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
+           '_grid_iter50_sl_tr/'
+    dir9 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
+           '_nominal_sl_tr/'
 
 
 i = 0
 x = []
 y = []
 z = []
-dir = dir4
-dir_nominal = dir3
-found_same_sample = False
-while os.path.isfile(dir+str(i)+'_'+str(0)+'_gamma.csv'):
-    gamma = np.genfromtxt(dir + str(i) + '_' + str(0) + '_gamma.csv', delimiter=",")
+dir = dir9
+while os.path.isfile(dir+str(i)+'_'+str(0)+'_task.csv'):
+    gamma = np.genfromtxt(dir + str(i) + '_' + str(0) + '_task.csv', delimiter=",")
     cost = float(np.genfromtxt(dir + str(i) + '_' + str(0) + '_c.csv', delimiter=","))
-    if np.genfromtxt(dir + str(i) + '_' + str(0) + '_is_success.csv', delimiter=",") == 1:
-        if normalized_cost:
-            j = 0
-            # search the nominal cost map
-            while os.path.isfile(dir_nominal+str(j)+'_'+str(0)+'_gamma.csv'):
-                gamma_nominal = np.genfromtxt(dir_nominal + str(j) + '_' + str(0) + '_gamma.csv', delimiter=",")
-                if np.array_equal(gamma, gamma_nominal):
-                    found_same_sample = True
-                    if np.genfromtxt(dir_nominal + str(j) + '_' + str(0) + '_is_success.csv', delimiter=",") == 1:
-                        nominal_cost = float(np.genfromtxt(dir_nominal + str(j) + '_' + str(0) + '_c.csv', delimiter=","))
-                        if cost/nominal_cost < 1.5:
-                            x.append(gamma[0])
-                            y.append(gamma[1])
-                            z.append(cost/nominal_cost)
-                    break
-                j = j+1
-            if found_same_sample:
-                found_same_sample = False
-            else:
-                x.append(gamma[0])
-                y.append(gamma[1])
-                z.append(0)
-        else:
-            x.append(gamma[0])
-            y.append(gamma[1])
-            z.append(cost)
+    # only consider reasonable point
+    if cost < 35:
+        x.append(gamma[0])
+        y.append(gamma[3])
+        z.append(cost)
     i = i+1
 
 fig, ax = plt.subplots()
@@ -104,11 +84,8 @@
 surf = ax.tricontourf(x, y, z)
 fig.colorbar(surf, shrink=0.5, aspect=6)
 ax.set_xlabel('Stride length')
-ax.set_ylabel('Ground incline')
-if normalized_cost:
-    ax.set_title('normalized cost landscape')
-else:
-    ax.set_title('cost landscape')
+ax.set_ylabel('Turning rate')
+ax.set_title('cost landscape')
 
 # optimization range
 # large range
diff --git a/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py b/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py
new file mode 100644
index 0000000000..b61a68dd5c
--- /dev/null
+++ b/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py
@@ -0,0 +1,90 @@
+"""
+This function is used for plotting normalized cost landscape.
+Considering we search along several directions, we process the data along those directions
+1.
+For each direction, we compare the cost at each point on the searching line both in cost landscape 1 (C1) and
+cost landscape. Set the value of reasonable point with the normalized cost.
+2.
+For those points which exist in the cost landscape we want to normalize but not in the nominal cost landscape,
+we set the value of this point 0.5.
+"""
+import matplotlib.pyplot as plt
+import numpy as np
+import os
+
+robot_option = 1
+file_dir = '/Users/jason-hu/'
+if robot_option == 1:
+    robot = 'cassie/'
+    dir1 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
+           '_grid_iter50_sl_tr/'
+    dir2 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
+           '_nominal_sl_tr/'
+
+# number of searching directions
+n_direction = 16
+
+
+# Note:decide which column of the task to plot according to the task dimensions
+# Eg. column index 0 corresponds to stride length
+task_1_idx = 0
+task_2_idx = 3
+def process_data_from_direction(dir1, dir_nominal):
+    # need to add central point on the points list
+    task0 = np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_task.csv', delimiter=",")
+    x0 = [task0[task_1_idx]]
+    y0 = [task0[task_2_idx]]
+    cost1 = np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_c.csv', delimiter=",")
+    cost2 = np.genfromtxt(dir_nominal + str(0) + '_' + str(0) + '_c.csv', delimiter=",")
+    z0 = [cost1 / cost2]
+    for i in range(n_direction):
+        data_dir1 = np.genfromtxt(dir1 + str(i+1) + '_cost_list.csv', delimiter=",")
+        data_dir2 = np.genfromtxt(dir_nominal + str(i+1) + '_cost_list.csv', delimiter=",")
+
+        if data_dir1.shape[0] >= data_dir2.shape[0]:
+            num_small = data_dir2.shape[0]
+            num_large = data_dir1.shape[0]
+        else:
+            num_small = data_dir1.shape[0]
+            num_large = data_dir2.shape[0]
+
+        # process the points on the line
+        x = []
+        y = []
+        z = []
+
+        # set the value for intersected parts
+        for j in range(num_small):
+            cost1 = data_dir1[j, 1]
+            cost2 = data_dir2[j, 1]
+            # we only append reasonable point
+            if (cost1/cost2 < 1.5) & (cost1/cost2 > 0.5):
+                task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+                x.append(task[task_1_idx])
+                y.append(task[task_2_idx])
+                z.append(cost1/cost2)
+        for j in range(num_small, num_large):
+            if data_dir1.shape[0] >= data_dir2.shape[0]:
+                # extended range
+                task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+                x.append(task[task_1_idx])
+                y.append(task[task_2_idx])
+                z.append(0.5)
+        if len(x) > 10:
+            x0 = x0+x
+            y0 = y0+y
+            z0 = z0+z
+    return np.array(x0), np.array(y0), np.array(z0)
+
+
+# continuous color map
+fig, ax = plt.subplots()
+x, y, z = process_data_from_direction(dir1, dir2)
+# ax.scatter(x,y)
+surf = ax.tricontourf(x, y, z)
+fig.colorbar(surf, shrink=0.5, aspect=6)
+ax.set_xlabel('Stride length')
+ax.set_ylabel('Turning rate')
+ax.set_title('normalized cost landscape')
+
+plt.show()
\ No newline at end of file
diff --git a/examples/goldilocks_models/find_boundary_utils/poly_color_plot.py b/examples/goldilocks_models/find_boundary_utils/poly_color_plot.py
index 5a0047a5e9..856ef7290e 100644
--- a/examples/goldilocks_models/find_boundary_utils/poly_color_plot.py
+++ b/examples/goldilocks_models/find_boundary_utils/poly_color_plot.py
@@ -10,10 +10,10 @@
 file_dir = '/Users/jason-hu/'
 if robot_option == 1:
     robot = 'cassie/'
-    dir1 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/'+'robot_' + str(robot_option) + \
-           '_2d_nominal/'
-    dir2 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-           '_2d_initial_model/'
+    dir1 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
+           '_grid_iter50_sl_tr/'
+    dir2 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
+           '_nominal_sl_tr/'
 
 # number of searching directions
 n_direction = 16
@@ -38,7 +38,7 @@ def tricolorplot(x1, y1, x2, y2, x3, y3, color):
     XY[:, 0] = X
     XY[:, 1] = Y
     t1 = plt.Polygon(XY, color=color)
-    plt.gca().add_patch(t1)
+    ax1.add_patch(t1)
     plt.plot(X, Y, color=color)
 
 
@@ -74,7 +74,6 @@ def polycolorplot(x1, y1, z1, x2, y2, z2):
             tricolorplot(x1[i-1], y1[i-1], x1[i], y1[i], x2[j-1], y2[j-1], color)
             i = i+1
 
-    plt.show()
 
 
 def extractadjacentline(dir):
@@ -82,10 +81,14 @@ def extractadjacentline(dir):
     for i in range(n_direction):
         min_sin = 1
         for j in range(n_direction):
-            line1 = np.genfromtxt(dir + str(i) + '_' + str(0) + '_searching_direction.csv', delimiter=",")
-            line2 = np.genfromtxt(dir + str(j) + '_' + str(0) + '_searching_direction.csv', delimiter=",")
-            sin = np.cross(line1, line2) / (np.linalg.norm(line1) * np.linalg.norm(line2))
-            cos = np.dot(line1, line2) / (np.linalg.norm(line1) * np.linalg.norm(line2))
+            # Note:decide which column of the searching direction to use
+            # Eg. column index 0 corresponds to stride length
+            line1 = np.genfromtxt(dir + str(i+1) + '_searching_direction.csv', delimiter=",")
+            vec1 = np.array([line1[0], line1[3]])
+            line2 = np.genfromtxt(dir + str(j+1) + '_searching_direction.csv', delimiter=",")
+            vec2 = np.array([line2[0], line2[3]])
+            sin = np.cross(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))
+            cos = np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))
             # restrict the direction
             if (cos > 0) & (sin > 0):
                 # find the adjacent pair
@@ -97,8 +100,8 @@ def extractadjacentline(dir):
 
 
 def process_data_from_direction(i, dir1, dir2):
-    data_dir1 = np.genfromtxt(dir1 + str(i) + '_' + str(0) + '_cost_list.csv', delimiter=",")
-    data_dir2 = np.genfromtxt(dir2 + str(i) + '_' + str(0) + '_cost_list.csv', delimiter=",")
+    data_dir1 = np.genfromtxt(dir1 + str(int(i+1)) + '_cost_list.csv', delimiter=",")
+    data_dir2 = np.genfromtxt(dir2 + str(int(i+1)) + '_cost_list.csv', delimiter=",")
     # choose the longer line
     if data_dir1.shape[0] >= data_dir2.shape[0]:
         num_large = data_dir1.shape[0]
@@ -109,7 +112,7 @@ def process_data_from_direction(i, dir1, dir2):
     # need to add central point on the points list
     task0 = np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_task.csv', delimiter=",")
     x0 = task0[0]
-    y0 = task0[1]
+    y0 = task0[3]
     cost1 = np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_c.csv', delimiter=",")
     cost2 = np.genfromtxt(dir2 + str(0) + '_' + str(0) + '_c.csv', delimiter=",")
     if cost1 > cost2:
@@ -124,9 +127,9 @@ def process_data_from_direction(i, dir1, dir2):
     # Note:decide which column of the task to plot according to the task dimensions
     # Eg. column index 0 corresponds to stride length
     for j in range(num_small):
-        task = np.genfromtxt(dir1 + str(data_dir1[j, 0]) + '_' + str(0) + '_task.csv', delimiter=",")
+        task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
         x[j] = task[0]
-        y[j] = task[1]
+        y[j] = task[3]
         cost1 = data_dir1[j,1]
         cost2 = data_dir2[j,1]
         if cost1 > cost2:
@@ -136,13 +139,20 @@ def process_data_from_direction(i, dir1, dir2):
     for j in range(num_small, num_large):
         if data_dir1.shape[0] >= data_dir2.shape[0]:
             # extended range
+            task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+            x[j] = task[0]
+            y[j] = task[3]
             z[j] = 0
         else:
             # shrunk range
+            task = np.genfromtxt(dir2 + str(int(data_dir2[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+            x[j] = task[0]
+            y[j] = task[3]
             z[j] = 2
-    x = np.concatenate([np.array([x0]), x])
-    y = np.concatenate([np.array([y0]), y])
-    z = np.concatenate([np.array([z0]), z])
+    if i != 0:
+        x = np.concatenate([np.array([x0]), x])
+        y = np.concatenate([np.array([y0]), y])
+        z = np.concatenate([np.array([z0]), z])
 
     return x,y,z
 
@@ -157,8 +167,33 @@ def generateplot(dir1, dir2, adj_index):
         polycolorplot(x1, y1, z1, x2, y2, z2)
 
 
-def main():
-    adjacent = extractadjacentline(dir1)
-    generateplot(dir1, dir2, adjacent)
+fig1 = plt.figure(num=1, figsize=(6.4, 4.8))
+ax1 = fig1.gca()
+adjacent = extractadjacentline(dir1)
+print(adjacent)
+generateplot(dir1, dir2, adjacent)
+plt.show()
+
+[0,-1,-0.5,0.5,1]
+[0,-1,-0.5,0.5,1]
+
+0,-1
+0,1
+-1,0
+-1,-1
+-1,-0.5
+-1,0.5
+-1,1
+-0.5,-1
+-0.5,1
+0.5,0
+0.5,-1
+0.5,-0.5
+0.5,0.5
+0.5,1
+1,-0.5
+1,0.5
+
+
 
 

From e290486465009f196ec6b12c696bcdf81868b145 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshuhu98@gmail.com>
Date: Sun, 12 Jul 2020 14:13:08 -0400
Subject: [PATCH 66/82] minor change

---
 examples/goldilocks_models/find_models/visualize_gait_MBP.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/goldilocks_models/find_models/visualize_gait_MBP.cc b/examples/goldilocks_models/find_models/visualize_gait_MBP.cc
index 9faeae781c..3373f60427 100644
--- a/examples/goldilocks_models/find_models/visualize_gait_MBP.cc
+++ b/examples/goldilocks_models/find_models/visualize_gait_MBP.cc
@@ -92,7 +92,7 @@ void visualizeGait(int argc, char* argv[]) {
     // Read in ground incline
     double ground_incline = goldilocks_models::readCSV(
         directory + to_string(iter) + string("_") + to_string(FLAGS_batch) +
-        string("_ground_incline.csv"))(0, 0);
+        string("_task.csv"))(1, 0);
 
     // Read in trajecotry
     VectorXd time_mat;

From fbd28d21742de6565888df7e7e709067437d5320 Mon Sep 17 00:00:00 2001
From: Jianshu Hu <58180987+Jianshu-Hu@users.noreply.github.com>
Date: Wed, 15 Jul 2020 17:09:30 -0400
Subject: [PATCH 67/82] Update initial_guess.cc

---
 examples/goldilocks_models/find_models/initial_guess.cc | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/examples/goldilocks_models/find_models/initial_guess.cc b/examples/goldilocks_models/find_models/initial_guess.cc
index 70c8bba8f3..d14051bce2 100644
--- a/examples/goldilocks_models/find_models/initial_guess.cc
+++ b/examples/goldilocks_models/find_models/initial_guess.cc
@@ -130,7 +130,8 @@ string SetInitialGuessByInterpolation(const string& directory, int iter,
     }
     initial_guess = CalculateInterpolation(weight_gamma, w_gamma);
     //    save initial guess and set init file
-    initial_file_name = prefix + string("_initial_guess.csv");
+    initial_file_name = to_string(iter) + "_" + to_string(sample) +
+                        string("_initial_guess.csv");
     writeCSV(directory + initial_file_name, initial_guess);
   } else {
     DRAKE_DEMAND(iter > 0);
@@ -213,4 +214,4 @@ string SetInitialGuessByInterpolation(const string& directory, int iter,
   return initial_file_name;
 }
 
-}  // namespace dairlib::goldilocks_models
\ No newline at end of file
+}  // namespace dairlib::goldilocks_models

From 8a003336272b23137f775515f7e65b8740eb0f76 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshuhu98@gmail.com>
Date: Sat, 18 Jul 2020 11:43:51 -0400
Subject: [PATCH 68/82] merge with goldilocks-model-dev

---
 examples/goldilocks_models/BUILD.bazel      |   7 +-
 examples/goldilocks_models/find_boundary.cc | 192 +++++---------------
 2 files changed, 49 insertions(+), 150 deletions(-)

diff --git a/examples/goldilocks_models/BUILD.bazel b/examples/goldilocks_models/BUILD.bazel
index e8370116b4..07f37a1a6c 100644
--- a/examples/goldilocks_models/BUILD.bazel
+++ b/examples/goldilocks_models/BUILD.bazel
@@ -22,18 +22,17 @@ cc_binary(
     name = "find_boundary",
     srcs = ["find_boundary.cc"],
     deps = [
-        ":dynamics_expression",
         ":goldilocks_utils",
-        ":kinematics_expression",
         ":task",
         "//common:eigen_utils",
-        "//examples/goldilocks_models/find_boundary_utils:search_setting",
+        "//common:file_utils",
+        "//examples/Cassie:cassie_utils",
         "//examples/goldilocks_models/find_models:initial_guess",
         "//examples/goldilocks_models/find_models:traj_opt_given_weigths",
-        "//systems/goldilocks_models",
         "//systems/trajectory_optimization:dircon",
         "@drake//:drake_shared_library",
         "@gflags",
+        "//examples/goldilocks_models/find_boundary_utils:search_setting",
     ],
 )
 
diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 024bbd8a7b..5644b5c3e6 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -1,14 +1,14 @@
-#include <gflags/gflags.h>
-#include <thread>  // multi-threading
 #include <chrono>
-#include <ctime>
-#include <queue>  // First in first out
-#include <deque>  // queue with feature of finding elements
-#include <utility>  // std::pair, std::make_pair
-#include <bits/stdc++.h>  // system call
 #include <cmath>
+#include <ctime>
+#include <deque>   // queue with feature of finding elements
+#include <queue>   // First in first out
+#include <thread>  // multi-threading
 #include <tuple>
-#include <Eigen/QR>  // CompleteOrthogonalDecomposition
+#include <utility>        // std::pair, std::make_pair
+#include <Eigen/QR>       // CompleteOrthogonalDecomposition
+#include <bits/stdc++.h>  // system call
+#include <gflags/gflags.h>
 
 #include "drake/multibody/parsing/parser.h"
 #include "drake/solvers/mathematical_program.h"
@@ -16,14 +16,14 @@
 #include "drake/solvers/solve.h"
 
 #include "common/eigen_utils.h"
+#include "common/file_utils.h"
 #include "common/find_resource.h"
-#include "examples/goldilocks_models/dynamics_expression.h"
+#include "examples/Cassie/cassie_utils.h"
+#include "examples/goldilocks_models/find_models/initial_guess.h"
 #include "examples/goldilocks_models/find_models/traj_opt_given_weigths.h"
 #include "examples/goldilocks_models/goldilocks_utils.h"
-#include "examples/goldilocks_models/find_models/initial_guess.h"
-#include "examples/goldilocks_models/kinematics_expression.h"
+#include "examples/goldilocks_models/reduced_order_models.h"
 #include "examples/goldilocks_models/task.h"
-#include "systems/goldilocks_models/file_utils.h"
 #include "examples/goldilocks_models/find_boundary_utils/search_setting.h"
 
 using std::cin;
@@ -93,32 +93,7 @@ DEFINE_string(
     program_name, "",
 "The name of the program (to keep a record for future references)");
 
-void createMBP(MultibodyPlant<double>* plant, int robot_option) {
-  if (robot_option == 0) {
-    Parser parser(plant);
-    std::string full_name = FindResourceOrThrow(
-        "examples/goldilocks_models/PlanarWalkerWithTorso.urdf");
-    parser.AddModelFromFile(full_name);
-    plant->mutable_gravity_field().set_gravity_vector(
-        -9.81 * Eigen::Vector3d::UnitZ());
-    plant->WeldFrames(
-        plant->world_frame(), plant->GetFrameByName("base"),
-        drake::math::RigidTransform<double>());
-    plant->Finalize();
-
-  } else if (robot_option == 1) {
-    Parser parser(plant);
-    string full_name =
-        FindResourceOrThrow("examples/Cassie/urdf/cassie_fixed_springs.urdf");
-    parser.AddModelFromFile(full_name);
-    plant->mutable_gravity_field().set_gravity_vector(
-        -9.81 * Eigen::Vector3d::UnitZ());
-    plant->Finalize();
-  } else {
-    throw std::runtime_error("Should not reach here");
-  }
-}
-void setCostWeight(double* Q, double* R, double* all_cost_scale,
+void setCostWeight(double *Q, double *R, double *all_cost_scale,
                    int robot_option) {
   if (robot_option == 0) {
     *Q = 1;
@@ -130,76 +105,6 @@ void setCostWeight(double* Q, double* R, double* all_cost_scale,
     *all_cost_scale = 0.2/* * 0.12*/;
   }
 }
-void setRomDim(int* n_s, int* n_tau, int rom_option) {
-  if (rom_option == 0) {
-    // 2D -- lipm
-    *n_s = 2;
-    *n_tau = 0;
-  } else if (rom_option == 1) {
-    // 4D -- lipm + swing foot
-    *n_s = 4;
-    *n_tau = 2;
-  } else if (rom_option == 2) {
-    // 1D -- fix com vertical acceleration
-    *n_s = 1;
-    *n_tau = 0;
-  } else if (rom_option == 3) {
-    // 3D -- fix com vertical acceleration + swing foot
-    *n_s = 3;
-    *n_tau = 2;
-  } else {
-    throw std::runtime_error("Should not reach here");
-  }
-}
-void setRomBMatrix(MatrixXd* B_tau, int rom_option) {
-  if ((rom_option == 0) || (rom_option == 2)) {
-    // passive rom, so we don't need B_tau
-  }
-  else if (rom_option == 1) {
-    DRAKE_DEMAND(B_tau->rows() == 4);
-    (*B_tau)(2, 0) = 1;
-    (*B_tau)(3, 1) = 1;
-  }
-  else if (rom_option == 3) {
-    DRAKE_DEMAND(B_tau->rows() == 3);
-    (*B_tau)(1, 0) = 1;
-    (*B_tau)(2, 1) = 1;
-  } else {
-    throw std::runtime_error("Should not reach here");
-  }
-}
-void setInitialTheta(VectorXd& theta_s, VectorXd& theta_sDDot,
-                     int n_feature_s, int rom_option) {
-  // // Testing intial theta
-  // theta_s = 0.25*VectorXd::Ones(n_theta_s);
-  // theta_sDDot = 0.5*VectorXd::Ones(n_theta_sDDot);
-  // theta_s = VectorXd::Random(n_theta_s);
-  // theta_sDDot = VectorXd::Random(n_theta_sDDot);
-
-  if (rom_option == 0) {
-    // 2D -- lipm
-    theta_s(0) = 1;
-    theta_s(1 + n_feature_s) = 1;
-    theta_sDDot(0) = 1;
-  } else if (rom_option == 1) {
-    // 4D -- lipm + swing foot
-    theta_s(0) = 1;
-    theta_s(1 + n_feature_s) = 1;
-    theta_s(2 + 2 * n_feature_s) = 1;
-    theta_s(3 + 3 * n_feature_s) = 1;
-    theta_sDDot(0) = 1;
-  } else if (rom_option == 2) {
-    // 1D -- fix com vertical acceleration
-    theta_s(1) = 1;
-  } else if (rom_option == 3) {
-    // 3D -- fix com vertical acceleration + swing foot
-    theta_s(1) = 1;
-    theta_s(2 + 1 * n_feature_s) = 1;
-    theta_s(3 + 2 * n_feature_s) = 1;
-  } else {
-    throw std::runtime_error("Should not reach here");
-  }
-}
 
 //read theta from files to use optimized model
 void ReadThetaFromFiles(const string dir,int theta_idx,
@@ -301,12 +206,16 @@ string getInitFileName(const string directory, int traj_opt_num,
 }
 
 // trajectory optimization for given task and model
-void TrajOptGivenModel(Task task, const string dir,int num,bool is_rerun,
-    int initial_guess_idx=-1,bool turn_on_scaling=true){
+void TrajOptGivenModel(Task task,
+                       const string dir,
+                       int num,
+                       bool is_rerun,
+                       int initial_guess_idx = -1,
+                       bool turn_on_scaling = true) {
   // Create MBP
   drake::logging::set_log_level("err");  // ignore warnings about joint limits
   MultibodyPlant<double> plant(0.0);
-  createMBP(&plant, FLAGS_robot_option);
+  CreateMBP(&plant, FLAGS_robot_option);
 
   // Create autoDiff version of the plant
   MultibodyPlant<AutoDiffXd> plant_autoDiff(plant);
@@ -334,55 +243,45 @@ void TrajOptGivenModel(Task task, const string dir,int num,bool is_rerun,
   inner_loop_setting.snopt_scaling = turn_on_scaling;
   inner_loop_setting.directory = dir;
 
-  // Reduced order model parameters
-  int n_y = 0;
-  int n_tau = 0;
-  setRomDim(&n_y, &n_tau, FLAGS_rom_option);
-  int n_yddot = n_y; // Assume that are the same (no quaternion)
-  MatrixXd B_tau = MatrixXd::Zero(n_yddot, n_tau);
-  setRomBMatrix(&B_tau, FLAGS_rom_option);
-  writeCSV(dir + string("B_tau.csv"), B_tau);
-
-  // Reduced order model setup
-  KinematicsExpression<double> kin_expression(n_y, 0, &plant, FLAGS_robot_option);
-  DynamicsExpression dyn_expression(n_yddot, 0, FLAGS_rom_option, FLAGS_robot_option);
-  VectorXd dummy_q = VectorXd::Ones(plant.num_positions());
-  VectorXd dummy_s = VectorXd::Ones(n_y);
-  int n_feature_y = kin_expression.getFeature(dummy_q).size();
-  int n_feature_yddot =
-      dyn_expression.getFeature(dummy_s, dummy_s).size();
-  int n_theta_y = n_y * n_feature_y;
-  int n_theta_yddot = n_yddot * n_feature_yddot;
+  // Construct reduced order model
+  std::unique_ptr<ReducedOrderModel> rom =
+      CreateRom(FLAGS_rom_option, FLAGS_robot_option, plant);
+  writeCSV(dir + string("rom_B.csv"), rom->B());
+  writeCSV(dir + string("rom_n_y.csv"), rom->n_y() * VectorXd::Ones(1));
+  writeCSV(dir + string("rom_n_tau.csv"), rom->n_tau() * VectorXd::Ones(1));
+  writeCSV(dir + string("rom_n_feature_y.csv"),
+           rom->n_feature_y() * VectorXd::Ones(1));
+  writeCSV(dir + string("rom_n_feature_yddot.csv"),
+           rom->n_feature_yddot() * VectorXd::Ones(1));
+
 
   // Initial guess of theta
-  VectorXd theta_y = VectorXd::Zero(n_theta_y);
-  VectorXd theta_yddot = VectorXd::Zero(n_theta_yddot);
-  if(FLAGS_use_optimized_model){
+  VectorXd theta_y = VectorXd::Zero(rom->n_theta_y());
+  VectorXd theta_yddot = VectorXd::Zero(rom->n_theta_yddot());
+  if (FLAGS_use_optimized_model) {
     //you have to specify the theta to use
-    DRAKE_DEMAND(FLAGS_theta_index>=0);
+    DRAKE_DEMAND(FLAGS_theta_index >= 0);
     int theta_idx = FLAGS_theta_index;
 
-    const string dir_find_models = "../dairlib_data/goldilocks_models/find_models/robot_" +
-        to_string(FLAGS_robot_option) + "/";
+    const string dir_find_models =
+        "../dairlib_data/goldilocks_models/find_models/robot_" +
+            to_string(FLAGS_robot_option) + "/";
     ReadThetaFromFiles(dir_find_models, theta_idx, theta_y, theta_yddot);
+    rom->SetThetaY(theta_y);
+    rom->SetThetaYddot(theta_yddot);
   }
-  else{
-    setInitialTheta(theta_y, theta_yddot, n_feature_y, FLAGS_rom_option);
-  }
-
-  RomData rom = RomData(n_y, n_tau, n_feature_y, n_feature_yddot, B_tau,
-                        theta_y, theta_yddot);
 
   bool is_get_nominal = FLAGS_is_get_nominal;
   int max_inner_iter_pass_in = is_get_nominal ? 200 : max_inner_iter;
 
 //  string init_file_pass_in = "";
   string init_file_pass_in = getInitFileName(dir, num, is_rerun,
-      initial_guess_idx);
+                                             initial_guess_idx);
   int sample_idx = 0;
-  string prefix = to_string(num) +  "_" + to_string(sample_idx) + "_";
+  string prefix = to_string(num) + "_" + to_string(sample_idx) + "_";
 
-  inner_loop_setting.n_node = (FLAGS_n_node>0)? FLAGS_n_node : 20;//fix number of nodes
+  inner_loop_setting.n_node =
+      (FLAGS_n_node > 0) ? FLAGS_n_node : 20;//fix number of nodes
   inner_loop_setting.max_iter = max_inner_iter_pass_in;
   inner_loop_setting.prefix = prefix;
   inner_loop_setting.init_file = init_file_pass_in;
@@ -408,7 +307,7 @@ void TrajOptGivenModel(Task task, const string dir,int num,bool is_rerun,
   trajOptGivenWeights(
       std::ref(plant),
       std::ref(plant_autoDiff),
-      std::ref(rom),
+      std::ref(*rom),
       inner_loop_setting,
       task,
       std::ref(QPs),
@@ -419,6 +318,7 @@ void TrajOptGivenModel(Task task, const string dir,int num,bool is_rerun,
       FLAGS_rom_option, FLAGS_robot_option);
 }
 
+
 //Save boundary point information
 void SaveBoundaryPointInfor(const string dir,int boundary_point_index,
     int traj_num,const VectorXd& boundary_point){

From 4e54329bf58690d611abe206ae0bb1733f618ae4 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshuhu98@gmail.com>
Date: Thu, 27 Aug 2020 16:50:10 -0400
Subject: [PATCH 69/82] modify the plotting functions for plotting cost
 landscape

---
 .../find_boundary_utils/plot_landscape.py     | 43 ++-------
 .../find_boundary_utils/plot_landscape_new.py | 87 +++++++++++++++++++
 .../find_boundary_utils/poly_color_plot.py    | 30 ++-----
 3 files changed, 102 insertions(+), 58 deletions(-)
 create mode 100644 examples/goldilocks_models/find_boundary_utils/plot_landscape_new.py

diff --git a/examples/goldilocks_models/find_boundary_utils/plot_landscape.py b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
index 208c0951a5..335ad892cb 100644
--- a/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
+++ b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
@@ -3,7 +3,8 @@
 import os
 
 robot_option = 1
-file_dir = '/Users/jason-hu/'
+file_dir = "/home/jianshu/workspace/dairlib_data/goldilocks_models/find_boundary/"
+# file_dir = "../dairlib_data/goldilocks_models/find_boundary/"
 
 # optimization setting
 # large range
@@ -21,49 +22,16 @@
 
 if robot_option == 0:
     robot = 'five_link/'
-    dir1 = file_dir+'dairlib_data/find_boundary/' + robot + 'robot_' + str(robot_option) + \
-           '_nominal/'
-    dir2 = file_dir+'dairlib_data/find_boundary/' + robot + 'robot_' + str(robot_option) + \
-           '_initial_model/'
-    dir3 = file_dir+'dairlib_data/find_boundary/' + robot + 'robot_' + str(robot_option) + \
-           '_large_task_space_iter1000/'
-    dir4 = file_dir+'dairlib_data/find_boundary/' + robot + 'robot_' + str(robot_option) + \
-           '_large_task_space_iter2000/'
-    dir5 = file_dir+'dairlib_data/find_boundary/' + robot + 'robot_' + str(robot_option) + \
-           '_large_task_space_iter3000_with_scaling/'
-    dir6 = file_dir+'dairlib_data/find_boundary/' + robot + 'robot_' + str(robot_option) + \
-           '_small_task_space_iter500/'
-    dir7 = file_dir+'dairlib_data/find_boundary/' + robot + 'robot_' + str(robot_option) + \
-           '_small_task_space_iter1000/'
-    dir8 = file_dir+'dairlib_data/find_boundary/' + robot + 'robot_' + str(robot_option) + \
-           '_small_task_space_iter2000_with_scaling/'
+    dir = file_dir + robot + 'robot_' + str(robot_option) + '_nominal/'
 if robot_option == 1:
     robot = 'cassie/'
-    dir1 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-           '_2d_nominal/'
-    dir2 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-           '_2d_initial_model/'
-    dir3 = file_dir+'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-           '_small_iter300/'
-    dir4 = file_dir + 'dairlib_data/find_boundary/' + robot + '1D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-           '_large_iter150/'
-    dir5 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-           '_2d_initial_model/'
-    dir6 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-           '_small_iter200/'
-    dir7 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-           '_large_iter100/'
-    dir8 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
-           '_grid_iter50_sl_tr/'
-    dir9 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
-           '_nominal_sl_tr/'
+    dir = file_dir + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + '_grid_iter50_sl_tr/'
 
 
 i = 0
 x = []
 y = []
 z = []
-dir = dir9
 while os.path.isfile(dir+str(i)+'_'+str(0)+'_task.csv'):
     gamma = np.genfromtxt(dir + str(i) + '_' + str(0) + '_task.csv', delimiter=",")
     cost = float(np.genfromtxt(dir + str(i) + '_' + str(0) + '_c.csv', delimiter=","))
@@ -80,6 +48,9 @@
 print(np.shape(y))
 print(np.shape(z))
 
+# scatter
+# surf = ax.scatter(x, y, z)
+
 # continuous color map
 surf = ax.tricontourf(x, y, z)
 fig.colorbar(surf, shrink=0.5, aspect=6)
diff --git a/examples/goldilocks_models/find_boundary_utils/plot_landscape_new.py b/examples/goldilocks_models/find_boundary_utils/plot_landscape_new.py
new file mode 100644
index 0000000000..08c2109dd2
--- /dev/null
+++ b/examples/goldilocks_models/find_boundary_utils/plot_landscape_new.py
@@ -0,0 +1,87 @@
+"""
+Given points on two adjacent lines, fill the region between two lines with the color corresponding to the cost.
+"""
+import matplotlib.pyplot as plt
+import numpy as np
+import os
+
+robot_option = 1
+file_dir = "/home/jianshu/workspace/dairlib_data/goldilocks_models/find_boundary/"
+# file_dir = "../dairlib_data/goldilocks_models/find_boundary/"
+if robot_option == 1:
+    robot = 'cassie/'
+    dir1 = file_dir + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + '_grid_iter50_sl_tr/'
+
+# number of searching directions
+n_direction = 16
+
+# Note: we need to decide which column of the searching direction to use
+# Eg. column index 0 corresponds to stride length
+task = ['Stride length', 'Ground incline', 'Velocity', 'Turning rate']
+dim1 = 0
+dim2 = 3
+
+def ExtractAdjacentLine(dir):
+    adj_direction = np.zeros([n_direction]).astype(int)
+    for i in range(n_direction):
+        min_sin = 1
+        line1 = np.genfromtxt(dir + str(i + 1) + '_searching_direction.csv', delimiter=",")
+        vec1 = np.array([line1[dim1], line1[dim2]])
+        for j in range(n_direction):
+            line2 = np.genfromtxt(dir + str(j+1) + '_searching_direction.csv', delimiter=",")
+            vec2 = np.array([line2[dim1], line2[dim2]])
+            sin = np.cross(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))
+            cos = np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))
+            # restrict the direction
+            if (cos > 0) & (sin > 0):
+                # find the adjacent pair
+                if sin < min_sin:
+                    adj_index = j
+                    min_sin = sin
+        adj_direction[i] = adj_index
+    return adj_direction
+
+
+def process_data_from_direction(i, dir1):
+    data_dir = np.genfromtxt(dir1 + str(int(i+1)) + '_cost_list.csv', delimiter=",")
+
+    # need to add central point on the points list
+    task0 = np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_task.csv', delimiter=",")
+    x = [task0[dim1]]
+    y = [task0[dim2]]
+    z = [float(np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_c.csv', delimiter=","))]
+
+    # process the points on the line
+    for i in range(data_dir.shape[0]):
+        gamma = np.genfromtxt(dir1 + str(int(data_dir[i, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+        cost = data_dir[i, 1]
+        if cost < 35:
+            x.append(gamma[dim1])
+            y.append(gamma[dim2])
+            z.append(cost)
+
+    return x,y,z
+
+
+def generateplot(dir1, adj_index):
+    for i in range(n_direction):
+    # process data on one line
+        x1, y1, z1 = process_data_from_direction(i, dir1)
+        # process data on adjacent line
+        x2, y2, z2 = process_data_from_direction(adj_index[i], dir1)
+        # plot
+        x = x1+x2
+        y = y1+y2
+        z = z1+z2
+        surf = ax.tricontourf(x, y, z)
+    fig.colorbar(surf, shrink=0.5, aspect=6)
+
+
+fig, ax = plt.subplots()
+ax.set_xlabel(task[dim1])
+ax.set_ylabel(task[dim2])
+ax.set_title('cost landscape')
+adjacent = ExtractAdjacentLine(dir1)
+print(adjacent)
+generateplot(dir1, adjacent)
+plt.show()
\ No newline at end of file
diff --git a/examples/goldilocks_models/find_boundary_utils/poly_color_plot.py b/examples/goldilocks_models/find_boundary_utils/poly_color_plot.py
index 856ef7290e..9b3823670a 100644
--- a/examples/goldilocks_models/find_boundary_utils/poly_color_plot.py
+++ b/examples/goldilocks_models/find_boundary_utils/poly_color_plot.py
@@ -75,7 +75,6 @@ def polycolorplot(x1, y1, z1, x2, y2, z2):
             i = i+1
 
 
-
 def extractadjacentline(dir):
     adj_direction = np.zeros([n_direction])
     for i in range(n_direction):
@@ -164,7 +163,14 @@ def generateplot(dir1, dir2, adj_index):
         # process data on adjacent line
         x2, y2, z2 = process_data_from_direction(adj_index[i], dir1, dir2)
         # plot
-        polycolorplot(x1, y1, z1, x2, y2, z2)
+        # continuous color map
+        fig, ax = plt.subplots()
+        surf = ax.tricontourf(x, y, z)
+        fig.colorbar(surf, shrink=0.5, aspect=6)
+        ax.set_xlabel('Stride length')
+        ax.set_ylabel('Turning rate')
+        ax.set_title('cost landscape')
+        # polycolorplot(x1, y1, z1, x2, y2, z2)
 
 
 fig1 = plt.figure(num=1, figsize=(6.4, 4.8))
@@ -174,26 +180,6 @@ def generateplot(dir1, dir2, adj_index):
 generateplot(dir1, dir2, adjacent)
 plt.show()
 
-[0,-1,-0.5,0.5,1]
-[0,-1,-0.5,0.5,1]
-
-0,-1
-0,1
--1,0
--1,-1
--1,-0.5
--1,0.5
--1,1
--0.5,-1
--0.5,1
-0.5,0
-0.5,-1
-0.5,-0.5
-0.5,0.5
-0.5,1
-1,-0.5
-1,0.5
-
 
 
 

From 713ea3d3d2fba584a1c60eee0aa4db465e04585b Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshuhu98@gmail.com>
Date: Fri, 28 Aug 2020 13:09:45 -0400
Subject: [PATCH 70/82] specify the color discretization for each segment

---
 .../find_boundary_utils/plot_landscape_new.py                 | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary_utils/plot_landscape_new.py b/examples/goldilocks_models/find_boundary_utils/plot_landscape_new.py
index 08c2109dd2..d9da10412b 100644
--- a/examples/goldilocks_models/find_boundary_utils/plot_landscape_new.py
+++ b/examples/goldilocks_models/find_boundary_utils/plot_landscape_new.py
@@ -65,7 +65,7 @@ def process_data_from_direction(i, dir1):
 
 def generateplot(dir1, adj_index):
     for i in range(n_direction):
-    # process data on one line
+        # process data on one line
         x1, y1, z1 = process_data_from_direction(i, dir1)
         # process data on adjacent line
         x2, y2, z2 = process_data_from_direction(adj_index[i], dir1)
@@ -73,7 +73,7 @@ def generateplot(dir1, adj_index):
         x = x1+x2
         y = y1+y2
         z = z1+z2
-        surf = ax.tricontourf(x, y, z)
+        surf = ax.tricontourf(x, y, z, levels=[0, 4, 8, 12, 16, 20, 24, 28, 32])
     fig.colorbar(surf, shrink=0.5, aspect=6)
 
 

From 6d1cdda1f94e5d8d7457975dc4f1efbba6a0c719 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshuhu98@gmail.com>
Date: Sat, 29 Aug 2020 16:00:55 -0400
Subject: [PATCH 71/82] update plotting functions

---
 .../find_boundary_utils/compare_boundary.py   | 97 +++++++++++--------
 .../find_boundary_utils/plot_landscape_new.py | 22 ++---
 .../plot_normalized_landscape.py              | 89 +++++++++++------
 3 files changed, 128 insertions(+), 80 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary_utils/compare_boundary.py b/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
index 7c3dfdfd72..b7585d7a37 100644
--- a/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
+++ b/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
@@ -17,17 +17,26 @@
 file_dir = '/Users/jason-hu/'
 if robot_option == 1:
     robot = 'cassie/'
-    dir1 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
-           '_grid_iter50_sl_tr/'
-    dir2 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
-           '_nominal_sl_tr/'
+else:
+    robot = 'five_link/'
+dir1 = file_dir + 'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
+        '_range1_iter300/'
+dir2 = file_dir + 'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
+       '_grid_iter50_sl_tr/'
 
 # number of searching directions
 n_direction = 16
 
+# optimization range
+min1 = 0.2775
+max1 = 0.3075
+min2 = -0.1875
+max2 = 0.0625
+plot_optimization_range = 0
 
 # Note:decide which column of the task to plot according to the task dimensions
 # Eg. column index 0 corresponds to stride length
+task_name = ['Stride length', 'Ground incline', 'Velocity', 'Turning rate']
 task_1_idx = 0
 task_2_idx = 3
 
@@ -39,10 +48,7 @@ def process_data_from_direction(dir1, dir_nominal):
     y0 = [task0[task_2_idx]]
     cost1 = np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_c.csv', delimiter=",")
     cost2 = np.genfromtxt(dir_nominal + str(0) + '_' + str(0) + '_c.csv', delimiter=",")
-    if cost1 > cost2:
-        z0 = [1.5]
-    else:
-        z0 = [0.5]
+    z0 = [cost1 / cost2]
     for i in range(n_direction):
         data_dir1 = np.genfromtxt(dir1 + str(i+1) + '_cost_list.csv', delimiter=",")
         data_dir2 = np.genfromtxt(dir_nominal + str(i+1) + '_cost_list.csv', delimiter=",")
@@ -59,53 +65,64 @@ def process_data_from_direction(dir1, dir_nominal):
         y = []
         z = []
 
-        # set the value for intersected parts
-        for j in range(num_small):
-            cost1 = data_dir1[j, 1]
-            cost2 = data_dir2[j, 1]
-            # only consider reasonable point
-            if (cost1 < 35) & (cost2 < 35):
-                if cost1 > cost2:
-                    z.append(1.5)
+        if num_small > 10:
+            # set the value for intersected parts
+            for j in range(num_small):
+                cost1 = data_dir1[j, 1]
+                cost2 = data_dir2[j, 1]
+                # only consider reasonable point
+                if (cost1 < 30) & (cost2 < 30):
+                    z.append(cost1 / cost2)
+                    task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+                    x.append(task[task_1_idx])
+                    y.append(task[task_2_idx])
+            for j in range(num_small, num_large):
+                if data_dir1.shape[0] >= data_dir2.shape[0]:
+                    # extended range
+                    task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+                    x.append(task[task_1_idx])
+                    y.append(task[task_2_idx])
+                    z.append(-0.5)
                 else:
-                    z.append(0.5)
-                task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
-                x.append(task[task_1_idx])
-                y.append(task[task_2_idx])
-        for j in range(num_small, num_large):
-            if data_dir1.shape[0] >= data_dir2.shape[0]:
-                # extended range
-                task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
-                x.append(task[task_1_idx])
-                y.append(task[task_2_idx])
-                z.append(-0.5)
-            else:
-                # shrunk range
-                task = np.genfromtxt(dir_nominal + str(int(data_dir2[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
-                x.append(task[task_1_idx])
-                y.append(task[task_2_idx])
-                z.append(2)
-        if len(x) > 10:
+                    # shrunk range
+                    task = np.genfromtxt(dir_nominal + str(int(data_dir2[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+                    x.append(task[task_1_idx])
+                    y.append(task[task_2_idx])
+                    z.append(2.5)
             x0 = x0 + x
             y0 = y0 + y
             z0 = z0 + z
     return np.array(x0), np.array(y0), np.array(z0)
 
 
+# plt.rcParams.update({'font.size': 28})
+# fig, ax = plt.subplots(figsize=(11,5.5))
 fig, ax = plt.subplots()
 x, y, z = process_data_from_direction(dir1, dir2)
+# ax.scatter(x, y)
 # discrete color map
-levels = [0, 1, 2]
-colors = ['green', 'blue']
+print('max', max(z))
+levels = [0, 0.25, 0.5, 0.75, 1, 2]
+colors = ['darkgreen', 'green', 'seagreen', 'mediumseagreen', 'blue']
 cmap, norm = matplotlib.colors.from_levels_and_colors(levels, colors)
 cmap.set_over('yellow')
 cmap.set_under('red')
 surf = ax.tricontourf(x, y, z, cmap=cmap, norm=norm, levels=levels, extend='both')
-cbar = fig.colorbar(surf, shrink=0.5, aspect=6, extend='both')
-cbar.ax.set_yticklabels(['0', '1', 'Infinity'])
+cbar = fig.colorbar(surf, shrink=0.9, aspect=10, extend='both')
+cbar.ax.set_yticklabels(['0', '0.25', '0.5', '0.75', '1', 'Inf'])
 
-ax.set_xlabel('Stride length')
-ax.set_ylabel('Ground incline')
+ax.set_xlabel(task_name[task_1_idx])
+ax.set_ylabel(task_name[task_2_idx])
 ax.set_title('Compare two cost landscapes')
 
+if plot_optimization_range == 1:
+    x_line = np.array([min1, max1, max1, min1, min1])
+    y_line = np.array([min2, min2, max2, max2, min2])
+    ax.plot(x_line, y_line, 'black', linewidth=3)
+
+# so that the label is not cut off by the window
+# plt.tight_layout()
+# plt.gcf().subplots_adjust(bottom=0.17)
+# plt.gcf().subplots_adjust(left=0.16)
+
 plt.show()
\ No newline at end of file
diff --git a/examples/goldilocks_models/find_boundary_utils/plot_landscape_new.py b/examples/goldilocks_models/find_boundary_utils/plot_landscape_new.py
index d9da10412b..1ee1d31be4 100644
--- a/examples/goldilocks_models/find_boundary_utils/plot_landscape_new.py
+++ b/examples/goldilocks_models/find_boundary_utils/plot_landscape_new.py
@@ -17,19 +17,19 @@
 
 # Note: we need to decide which column of the searching direction to use
 # Eg. column index 0 corresponds to stride length
-task = ['Stride length', 'Ground incline', 'Velocity', 'Turning rate']
-dim1 = 0
-dim2 = 3
+task_name = ['Stride length', 'Ground incline', 'Velocity', 'Turning rate']
+task_1_idx = 0
+task_2_idx = 3
 
 def ExtractAdjacentLine(dir):
     adj_direction = np.zeros([n_direction]).astype(int)
     for i in range(n_direction):
         min_sin = 1
         line1 = np.genfromtxt(dir + str(i + 1) + '_searching_direction.csv', delimiter=",")
-        vec1 = np.array([line1[dim1], line1[dim2]])
+        vec1 = np.array([line1[task_1_idx], line1[task_2_idx]])
         for j in range(n_direction):
             line2 = np.genfromtxt(dir + str(j+1) + '_searching_direction.csv', delimiter=",")
-            vec2 = np.array([line2[dim1], line2[dim2]])
+            vec2 = np.array([line2[task_1_idx], line2[task_2_idx]])
             sin = np.cross(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))
             cos = np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))
             # restrict the direction
@@ -47,8 +47,8 @@ def process_data_from_direction(i, dir1):
 
     # need to add central point on the points list
     task0 = np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_task.csv', delimiter=",")
-    x = [task0[dim1]]
-    y = [task0[dim2]]
+    x = [task0[task_1_idx]]
+    y = [task0[task_2_idx]]
     z = [float(np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_c.csv', delimiter=","))]
 
     # process the points on the line
@@ -56,8 +56,8 @@ def process_data_from_direction(i, dir1):
         gamma = np.genfromtxt(dir1 + str(int(data_dir[i, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
         cost = data_dir[i, 1]
         if cost < 35:
-            x.append(gamma[dim1])
-            y.append(gamma[dim2])
+            x.append(gamma[task_1_idx])
+            y.append(gamma[task_2_idx])
             z.append(cost)
 
     return x,y,z
@@ -78,8 +78,8 @@ def generateplot(dir1, adj_index):
 
 
 fig, ax = plt.subplots()
-ax.set_xlabel(task[dim1])
-ax.set_ylabel(task[dim2])
+ax.set_xlabel(task_name[task_1_idx])
+ax.set_ylabel(task_name[task_2_idx])
 ax.set_title('cost landscape')
 adjacent = ExtractAdjacentLine(dir1)
 print(adjacent)
diff --git a/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py b/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py
index b61a68dd5c..fc062043dd 100644
--- a/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py
+++ b/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py
@@ -11,24 +11,36 @@
 import matplotlib.pyplot as plt
 import numpy as np
 import os
+import math
 
 robot_option = 1
 file_dir = '/Users/jason-hu/'
 if robot_option == 1:
     robot = 'cassie/'
-    dir1 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
-           '_grid_iter50_sl_tr/'
-    dir2 = file_dir+'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
-           '_nominal_sl_tr/'
+else:
+    robot = 'five_link/'
+dir1 = file_dir + 'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
+        '_range1_iter300/'
+dir2 = file_dir + 'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
+       '_initial_sl_tr/'
 
 # number of searching directions
 n_direction = 16
 
+# optimization range
+min1 = 0.2775
+max1 = 0.3075
+min2 = -0.1875
+max2 = 0.0625
+plot_optimization_range = 0
 
 # Note:decide which column of the task to plot according to the task dimensions
 # Eg. column index 0 corresponds to stride length
+task_name = ['Stride length', 'Ground incline', 'Velocity', 'Turning rate']
 task_1_idx = 0
 task_2_idx = 3
+
+
 def process_data_from_direction(dir1, dir_nominal):
     # need to add central point on the points list
     task0 = np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_task.csv', delimiter=",")
@@ -54,37 +66,56 @@ def process_data_from_direction(dir1, dir_nominal):
         z = []
 
         # set the value for intersected parts
-        for j in range(num_small):
-            cost1 = data_dir1[j, 1]
-            cost2 = data_dir2[j, 1]
-            # we only append reasonable point
-            if (cost1/cost2 < 1.5) & (cost1/cost2 > 0.5):
-                task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
-                x.append(task[task_1_idx])
-                y.append(task[task_2_idx])
-                z.append(cost1/cost2)
-        for j in range(num_small, num_large):
-            if data_dir1.shape[0] >= data_dir2.shape[0]:
-                # extended range
-                task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
-                x.append(task[task_1_idx])
-                y.append(task[task_2_idx])
-                z.append(0.5)
-        if len(x) > 10:
-            x0 = x0+x
-            y0 = y0+y
-            z0 = z0+z
+        if num_small > 10:
+            for j in range(num_small):
+                cost1 = data_dir1[j, 1]
+                cost2 = data_dir2[j, 1]
+                # we only append reasonable point
+                if (cost1 < 30) & (cost2 < 30):
+                    task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+                    x.append(task[task_1_idx])
+                    y.append(task[task_2_idx])
+                    z.append(cost1/cost2)
+            for j in range(num_small, num_large):
+                if data_dir1.shape[0] >= data_dir2.shape[0]:
+                    # extended range
+                    task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+                    x.append(task[task_1_idx])
+                    y.append(task[task_2_idx])
+                    z.append(0)
+            x0 = x0 + x
+            y0 = y0 + y
+            z0 = z0 + z
     return np.array(x0), np.array(y0), np.array(z0)
 
 
 # continuous color map
+# plt.rcParams.update({'font.size': 28})
+# fig, ax = plt.subplots(figsize=(11,5.5))
 fig, ax = plt.subplots()
 x, y, z = process_data_from_direction(dir1, dir2)
 # ax.scatter(x,y)
-surf = ax.tricontourf(x, y, z)
-fig.colorbar(surf, shrink=0.5, aspect=6)
-ax.set_xlabel('Stride length')
-ax.set_ylabel('Turning rate')
-ax.set_title('normalized cost landscape')
+ceil = int(math.ceil(max(z)))
+print('max', max(z))
+print('ceil:', ceil)
+# levels = np.linspace(0,ceil,ceil+1).tolist()
+levels = [0.0, 0.5, 0.8, 1, 1.2] # manual specify level sets
+ticks = [0.0, 0.5, 0.8, 1, 1.2] # manual specify ticker values (it seems 0 is ignored)
+print(levels)
+surf = ax.tricontourf(x, y, z, levels=levels)
+fig.colorbar(surf, shrink=0.9, aspect=10, spacing='proportional', ticks=ticks)
+ax.set_xlabel(task_name[task_1_idx])
+ax.set_ylabel(task_name[task_2_idx])
+ax.set_title('Normalized cost landscape')
+
+if plot_optimization_range == 1:
+    x_line = np.array([min1, max1, max1, min1, min1])
+    y_line = np.array([min2, min2, max2, max2, min2])
+    ax.plot(x_line, y_line, 'black', linewidth=3)
+
+# so that the label is not cut off by the window
+# plt.tight_layout()
+# plt.gcf().subplots_adjust(bottom=0.18)
+# plt.gcf().subplots_adjust(left=0.15)
 
 plt.show()
\ No newline at end of file

From 0e69614d842bb326e835d6bbcd1ad1ab9fda2f0d Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshuhu98@gmail.com>
Date: Mon, 31 Aug 2020 15:09:41 -0400
Subject: [PATCH 72/82] update plotting functions

---
 .../find_boundary_utils/compare_boundary.py   | 138 +++++++++---------
 .../find_boundary_utils/plot_landscape.py     | 135 +++++++++--------
 .../plot_normalized_landscape.py              | 123 ++++++++--------
 3 files changed, 214 insertions(+), 182 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary_utils/compare_boundary.py b/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
index b7585d7a37..3940642b5f 100644
--- a/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
+++ b/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
@@ -14,15 +14,15 @@
 import os
 
 robot_option = 1
-file_dir = '/Users/jason-hu/'
+file_dir = "../dairlib_data/goldilocks_models/find_boundary/"
 if robot_option == 1:
     robot = 'cassie/'
 else:
     robot = 'five_link/'
-dir1 = file_dir + 'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
-        '_range1_iter300/'
-dir2 = file_dir + 'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
-       '_grid_iter50_sl_tr/'
+dir1 = file_dir + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+        '_large_iter100/'
+dir2 = file_dir + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+       '_small_iter200/'
 
 # number of searching directions
 n_direction = 16
@@ -38,79 +38,85 @@
 # Eg. column index 0 corresponds to stride length
 task_name = ['Stride length', 'Ground incline', 'Velocity', 'Turning rate']
 task_1_idx = 0
-task_2_idx = 3
+task_2_idx = 1
 
-
-def process_data_from_direction(dir1, dir_nominal):
+def process_data_from_direction(i, dir1, dir_nominal):
     # need to add central point on the points list
     task0 = np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_task.csv', delimiter=",")
-    x0 = [task0[task_1_idx]]
-    y0 = [task0[task_2_idx]]
-    cost1 = np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_c.csv', delimiter=",")
-    cost2 = np.genfromtxt(dir_nominal + str(0) + '_' + str(0) + '_c.csv', delimiter=",")
-    z0 = [cost1 / cost2]
-    for i in range(n_direction):
-        data_dir1 = np.genfromtxt(dir1 + str(i+1) + '_cost_list.csv', delimiter=",")
-        data_dir2 = np.genfromtxt(dir_nominal + str(i+1) + '_cost_list.csv', delimiter=",")
+    x = [task0[task_1_idx]]
+    y = [task0[task_2_idx]]
+    cost1 = float(np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_c.csv', delimiter=","))
+    cost2 = float(np.genfromtxt(dir_nominal + str(0) + '_' + str(0) + '_c.csv', delimiter=","))
+    z = [cost1 / cost2]
+
+    data_dir1 = np.genfromtxt(dir1 + str(int(i+1)) + '_cost_list.csv', delimiter=",")
+    data_dir2 = np.genfromtxt(dir_nominal + str(int(i+1)) + '_cost_list.csv', delimiter=",")
 
+    if data_dir1.shape[0] >= data_dir2.shape[0]:
+        num_small = data_dir2.shape[0]
+        num_large = data_dir1.shape[0]
+    else:
+        num_small = data_dir1.shape[0]
+        num_large = data_dir2.shape[0]
+
+    # process the points on the line
+    # set the value for intersected parts
+    for j in range(num_small):
+        cost1 = data_dir1[j, 1]
+        cost2 = data_dir2[j, 1]
+        # we only append reasonable point
+        if (cost1 < 35) & (cost2 < 35) & (cost1/cost2 < 4):
+            task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+            x.append(task[task_1_idx])
+            y.append(task[task_2_idx])
+            z.append(cost1/cost2)
+    for j in range(num_small, num_large):
         if data_dir1.shape[0] >= data_dir2.shape[0]:
-            num_small = data_dir2.shape[0]
-            num_large = data_dir1.shape[0]
+            # extended range
+            task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+            x.append(task[task_1_idx])
+            y.append(task[task_2_idx])
+            z.append(-1)
         else:
-            num_small = data_dir1.shape[0]
-            num_large = data_dir2.shape[0]
-
-        # process the points on the line
-        x = []
-        y = []
-        z = []
-
-        if num_small > 10:
-            # set the value for intersected parts
-            for j in range(num_small):
-                cost1 = data_dir1[j, 1]
-                cost2 = data_dir2[j, 1]
-                # only consider reasonable point
-                if (cost1 < 30) & (cost2 < 30):
-                    z.append(cost1 / cost2)
-                    task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
-                    x.append(task[task_1_idx])
-                    y.append(task[task_2_idx])
-            for j in range(num_small, num_large):
-                if data_dir1.shape[0] >= data_dir2.shape[0]:
-                    # extended range
-                    task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
-                    x.append(task[task_1_idx])
-                    y.append(task[task_2_idx])
-                    z.append(-0.5)
-                else:
-                    # shrunk range
-                    task = np.genfromtxt(dir_nominal + str(int(data_dir2[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
-                    x.append(task[task_1_idx])
-                    y.append(task[task_2_idx])
-                    z.append(2.5)
-            x0 = x0 + x
-            y0 = y0 + y
-            z0 = z0 + z
-    return np.array(x0), np.array(y0), np.array(z0)
+            task = np.genfromtxt(dir_nominal + str(int(data_dir2[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+            x.append(task[task_1_idx])
+            y.append(task[task_2_idx])
+            z.append(2.5)
+    return x, y, z
+
+
+def generateplot(dir1, dir_nominal, adj_index):
+    # discrete color map
+    levels = [0, 0.7, 0.8, 0.9, 1, 2]
+    colors = ['darkgreen', 'green', 'seagreen', 'mediumseagreen', 'blue']
+    cmap, norm = matplotlib.colors.from_levels_and_colors(levels, colors)
+    cmap.set_over('yellow')
+    cmap.set_under('red')
+
+    total_max = 0
+    for i in range(n_direction):
+        # process data on one line
+        x1, y1, z1 = process_data_from_direction(i, dir1, dir_nominal)
+        # process data on adjacent line
+        x2, y2, z2 = process_data_from_direction(adj_index[i], dir1,dir_nominal)
+        # plot
+        x = x1+x2
+        y = y1+y2
+        z = z1+z2
+        surf = ax.tricontourf(x, y, z, cmap=cmap, norm=norm, levels=levels, extend='both')
+        if max(z) > total_max:
+            total_max = max(z)
+    print('max', total_max)
+    cbar = fig.colorbar(surf, shrink=0.9, aspect=10, extend='both')
+    cbar.ax.set_yticklabels(['0', '0.7', '0.8', '0.9', '1', 'Inf'])
 
 
 # plt.rcParams.update({'font.size': 28})
 # fig, ax = plt.subplots(figsize=(11,5.5))
 fig, ax = plt.subplots()
-x, y, z = process_data_from_direction(dir1, dir2)
 # ax.scatter(x, y)
-# discrete color map
-print('max', max(z))
-levels = [0, 0.25, 0.5, 0.75, 1, 2]
-colors = ['darkgreen', 'green', 'seagreen', 'mediumseagreen', 'blue']
-cmap, norm = matplotlib.colors.from_levels_and_colors(levels, colors)
-cmap.set_over('yellow')
-cmap.set_under('red')
-surf = ax.tricontourf(x, y, z, cmap=cmap, norm=norm, levels=levels, extend='both')
-cbar = fig.colorbar(surf, shrink=0.9, aspect=10, extend='both')
-cbar.ax.set_yticklabels(['0', '0.25', '0.5', '0.75', '1', 'Inf'])
-
+adjacent = np.genfromtxt('adjacent.csv', delimiter=",")
+generateplot(dir1, dir2, adjacent)
 ax.set_xlabel(task_name[task_1_idx])
 ax.set_ylabel(task_name[task_2_idx])
 ax.set_title('Compare two cost landscapes')
diff --git a/examples/goldilocks_models/find_boundary_utils/plot_landscape.py b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
index 335ad892cb..801fa28ce9 100644
--- a/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
+++ b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
@@ -1,73 +1,94 @@
+"""
+Given points on two adjacent lines, fill the region between two lines with the color corresponding to the cost.
+"""
 import matplotlib.pyplot as plt
 import numpy as np
 import os
 
 robot_option = 1
-file_dir = "/home/jianshu/workspace/dairlib_data/goldilocks_models/find_boundary/"
-# file_dir = "../dairlib_data/goldilocks_models/find_boundary/"
-
-# optimization setting
-# large range
-min_gi1 = -0.325
-max_gi1 = 0.275
-min_sl1 = 0.0925
-max_sl1 = 0.3925
-# small range
-min_gi2 = -0.175
-max_gi2 = 0.125
-min_sl2 = 0.1675
-max_sl2 = 0.3175
-plot_large_range = 0
-plot_small_range = 0
-
-if robot_option == 0:
-    robot = 'five_link/'
-    dir = file_dir + robot + 'robot_' + str(robot_option) + '_nominal/'
+file_dir = "../dairlib_data/goldilocks_models/find_boundary/"
 if robot_option == 1:
     robot = 'cassie/'
-    dir = file_dir + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + '_grid_iter50_sl_tr/'
+    dir1 = file_dir + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
+           '_2d_nominal/'
+    dir2 = file_dir + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
+           '_2d_initial_model/'
+    dir3 = file_dir + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
+           '_large_iter100/'
+    dir4 = file_dir + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
+           '_small_iter200/'
 
+# number of searching directions
+n_direction = 16
 
-i = 0
-x = []
-y = []
-z = []
-while os.path.isfile(dir+str(i)+'_'+str(0)+'_task.csv'):
-    gamma = np.genfromtxt(dir + str(i) + '_' + str(0) + '_task.csv', delimiter=",")
-    cost = float(np.genfromtxt(dir + str(i) + '_' + str(0) + '_c.csv', delimiter=","))
-    # only consider reasonable point
-    if cost < 35:
-        x.append(gamma[0])
-        y.append(gamma[3])
-        z.append(cost)
-    i = i+1
+# Note: we need to decide which column of the searching direction to use
+# Eg. column index 0 corresponds to stride length
+task_name = ['Stride length', 'Ground incline', 'Velocity', 'Turning rate']
+task_1_idx = 0
+task_2_idx = 1
 
-fig, ax = plt.subplots()
+def ExtractAdjacentLine(dir):
+    adj_direction = np.zeros([n_direction]).astype(int)
+    for i in range(n_direction):
+        min_sin = 1
+        line1 = np.genfromtxt(dir + str(i + 1) + '_searching_direction.csv', delimiter=",")
+        vec1 = np.array([line1[task_1_idx], line1[task_2_idx]])
+        for j in range(n_direction):
+            line2 = np.genfromtxt(dir + str(j+1) + '_searching_direction.csv', delimiter=",")
+            vec2 = np.array([line2[task_1_idx], line2[task_2_idx]])
+            sin = np.cross(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))
+            cos = np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))
+            # restrict the direction
+            if (cos > 0) & (sin > 0):
+                # find the adjacent pair
+                if sin < min_sin:
+                    adj_index = j
+                    min_sin = sin
+        adj_direction[i] = adj_index
+    return adj_direction
 
-print(np.shape(x))
-print(np.shape(y))
-print(np.shape(z))
 
-# scatter
-# surf = ax.scatter(x, y, z)
+def process_data_from_direction(i, dir1):
+    data_dir = np.genfromtxt(dir1 + str(int(i+1)) + '_cost_list.csv', delimiter=",")
 
-# continuous color map
-surf = ax.tricontourf(x, y, z)
-fig.colorbar(surf, shrink=0.5, aspect=6)
-ax.set_xlabel('Stride length')
-ax.set_ylabel('Turning rate')
-ax.set_title('cost landscape')
+    # need to add central point on the points list
+    task0 = np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_task.csv', delimiter=",")
+    x = [task0[task_1_idx]]
+    y = [task0[task_2_idx]]
+    z = [float(np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_c.csv', delimiter=","))]
+
+    # process the points on the line
+    for j in range(data_dir.shape[0]):
+        task = np.genfromtxt(dir1 + str(int(data_dir[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+        cost = data_dir[j, 1]
+        if cost < 35:
+            x.append(task[task_1_idx])
+            y.append(task[task_2_idx])
+            z.append(cost)
 
-# optimization range
-# large range
-if plot_large_range == 1:
-    x_line = np.array([min_sl1, max_sl1, max_sl1, min_sl1, min_sl1])
-    y_line = np.array([min_gi1, min_gi1, max_gi1, max_gi1, min_gi1])
-    ax.plot(x_line, y_line, 'black', linewidth=3)
-# small range
-if plot_small_range == 1:
-    x_line2 = np.array([min_sl2, max_sl2, max_sl2, min_sl2, min_sl2])
-    y_line2 = np.array([min_gi2, min_gi2, max_gi2, max_gi2, min_gi2])
-    ax.plot(x_line2, y_line2, 'black', linewidth=3)
+    return x, y, z
 
+
+def generateplot(dir1, adj_index):
+    for i in range(n_direction):
+        # process data on one line
+        x1, y1, z1 = process_data_from_direction(i, dir1)
+        # process data on adjacent line
+        x2, y2, z2 = process_data_from_direction(adj_index[i], dir1)
+        # plot
+        x = x1+x2
+        y = y1+y2
+        z = z1+z2
+        surf = ax.tricontourf(x, y, z, levels=[0, 4, 8, 12, 16, 20, 24, 28, 32])
+    fig.colorbar(surf, shrink=0.5, aspect=6)
+
+# adjacent = ExtractAdjacentLine(dir1)
+# np.savetxt('adjacent.csv', adjacent, delimiter=',')
+
+fig, ax = plt.subplots()
+ax.set_xlabel(task_name[task_1_idx])
+ax.set_ylabel(task_name[task_2_idx])
+ax.set_title('cost landscape')
+adjacent = np.genfromtxt('adjacent.csv', delimiter=",")
+generateplot(dir3, adjacent)
 plt.show()
\ No newline at end of file
diff --git a/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py b/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py
index fc062043dd..0f7baa89dc 100644
--- a/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py
+++ b/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py
@@ -2,8 +2,8 @@
 This function is used for plotting normalized cost landscape.
 Considering we search along several directions, we process the data along those directions
 1.
-For each direction, we compare the cost at each point on the searching line both in cost landscape 1 (C1) and
-cost landscape. Set the value of reasonable point with the normalized cost.
+For each direction, we compare the cost at each point on the searching line both in cost landscape 1 and
+cost landscape and set the value of reasonable point with the normalized cost.
 2.
 For those points which exist in the cost landscape we want to normalize but not in the nominal cost landscape,
 we set the value of this point 0.5.
@@ -14,15 +14,15 @@
 import math
 
 robot_option = 1
-file_dir = '/Users/jason-hu/'
+file_dir = "../dairlib_data/goldilocks_models/find_boundary/"
 if robot_option == 1:
     robot = 'cassie/'
 else:
     robot = 'five_link/'
-dir1 = file_dir + 'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
-        '_range1_iter300/'
-dir2 = file_dir + 'dairlib_data/find_boundary/' + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
-       '_initial_sl_tr/'
+dir1 = file_dir + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+        '_large_iter100/'
+dir2 = file_dir + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+       '_small_iter200/'
 
 # number of searching directions
 n_direction = 16
@@ -38,72 +38,77 @@
 # Eg. column index 0 corresponds to stride length
 task_name = ['Stride length', 'Ground incline', 'Velocity', 'Turning rate']
 task_1_idx = 0
-task_2_idx = 3
+task_2_idx = 1
 
 
-def process_data_from_direction(dir1, dir_nominal):
+def process_data_from_direction(i, dir1, dir_nominal):
     # need to add central point on the points list
     task0 = np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_task.csv', delimiter=",")
-    x0 = [task0[task_1_idx]]
-    y0 = [task0[task_2_idx]]
-    cost1 = np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_c.csv', delimiter=",")
-    cost2 = np.genfromtxt(dir_nominal + str(0) + '_' + str(0) + '_c.csv', delimiter=",")
-    z0 = [cost1 / cost2]
-    for i in range(n_direction):
-        data_dir1 = np.genfromtxt(dir1 + str(i+1) + '_cost_list.csv', delimiter=",")
-        data_dir2 = np.genfromtxt(dir_nominal + str(i+1) + '_cost_list.csv', delimiter=",")
+    x = [task0[task_1_idx]]
+    y = [task0[task_2_idx]]
+    cost1 = float(np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_c.csv', delimiter=","))
+    cost2 = float(np.genfromtxt(dir_nominal + str(0) + '_' + str(0) + '_c.csv', delimiter=","))
+    z = [cost1 / cost2]
 
-        if data_dir1.shape[0] >= data_dir2.shape[0]:
-            num_small = data_dir2.shape[0]
-            num_large = data_dir1.shape[0]
-        else:
-            num_small = data_dir1.shape[0]
-            num_large = data_dir2.shape[0]
+    data_dir1 = np.genfromtxt(dir1 + str(int(i+1)) + '_cost_list.csv', delimiter=",")
+    data_dir2 = np.genfromtxt(dir_nominal + str(int(i+1)) + '_cost_list.csv', delimiter=",")
 
-        # process the points on the line
-        x = []
-        y = []
-        z = []
+    if data_dir1.shape[0] >= data_dir2.shape[0]:
+        num_small = data_dir2.shape[0]
+        num_large = data_dir1.shape[0]
+    else:
+        num_small = data_dir1.shape[0]
+        num_large = data_dir2.shape[0]
 
-        # set the value for intersected parts
-        if num_small > 10:
-            for j in range(num_small):
-                cost1 = data_dir1[j, 1]
-                cost2 = data_dir2[j, 1]
-                # we only append reasonable point
-                if (cost1 < 30) & (cost2 < 30):
-                    task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
-                    x.append(task[task_1_idx])
-                    y.append(task[task_2_idx])
-                    z.append(cost1/cost2)
-            for j in range(num_small, num_large):
-                if data_dir1.shape[0] >= data_dir2.shape[0]:
-                    # extended range
-                    task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
-                    x.append(task[task_1_idx])
-                    y.append(task[task_2_idx])
-                    z.append(0)
-            x0 = x0 + x
-            y0 = y0 + y
-            z0 = z0 + z
-    return np.array(x0), np.array(y0), np.array(z0)
+    # process the points on the line
+    # set the value for intersected parts
+    for j in range(num_small):
+        cost1 = data_dir1[j, 1]
+        cost2 = data_dir2[j, 1]
+        # we only append reasonable point
+        if (cost1 < 35) & (cost2 < 35) & (cost1/cost2<2):
+            task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+            x.append(task[task_1_idx])
+            y.append(task[task_2_idx])
+            z.append(cost1/cost2)
+    for j in range(num_small, num_large):
+        if data_dir1.shape[0] >= data_dir2.shape[0]:
+            # extended range
+            task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+            x.append(task[task_1_idx])
+            y.append(task[task_2_idx])
+            z.append(0)
+    return x, y, z
 
 
+def generateplot(dir1, dir_nominal, adj_index, levels, ticks):
+    total_max = 0
+    for i in range(n_direction):
+        # process data on one line
+        x1, y1, z1 = process_data_from_direction(i, dir1, dir_nominal)
+        # process data on adjacent line
+        x2, y2, z2 = process_data_from_direction(adj_index[i], dir1,dir_nominal)
+        # plot
+        x = x1+x2
+        y = y1+y2
+        z = z1+z2
+        surf = ax.tricontourf(x, y, z, levels=levels)
+        if max(z) > total_max:
+            total_max = max(z)
+    print('max', total_max)
+    fig.colorbar(surf, shrink=0.9, aspect=10, spacing='proportional', ticks=ticks)
+
 # continuous color map
 # plt.rcParams.update({'font.size': 28})
 # fig, ax = plt.subplots(figsize=(11,5.5))
 fig, ax = plt.subplots()
-x, y, z = process_data_from_direction(dir1, dir2)
-# ax.scatter(x,y)
-ceil = int(math.ceil(max(z)))
-print('max', max(z))
-print('ceil:', ceil)
-# levels = np.linspace(0,ceil,ceil+1).tolist()
-levels = [0.0, 0.5, 0.8, 1, 1.2] # manual specify level sets
-ticks = [0.0, 0.5, 0.8, 1, 1.2] # manual specify ticker values (it seems 0 is ignored)
+# ceil = int(math.ceil(max(z)))
+# print('ceil:', ceil)
+levels = [0.0, 0.5, 1, 1.5, 2] # manual specify level sets
+ticks = [0.0, 0.5, 1, 1.5, 2] # manual specify ticker values (it seems 0 is ignored)
 print(levels)
-surf = ax.tricontourf(x, y, z, levels=levels)
-fig.colorbar(surf, shrink=0.9, aspect=10, spacing='proportional', ticks=ticks)
+adjacent = np.genfromtxt('adjacent.csv', delimiter=",")
+generateplot(dir1,dir2, adjacent, levels, ticks)
 ax.set_xlabel(task_name[task_1_idx])
 ax.set_ylabel(task_name[task_2_idx])
 ax.set_title('Normalized cost landscape')

From 772ffd09b33bc716404562a93f70322d040f6608 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshuhu98@gmail.com>
Date: Mon, 31 Aug 2020 18:33:29 -0400
Subject: [PATCH 73/82] small update

---
 examples/goldilocks_models/find_boundary.cc | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 8b807963e9..fbf664c195 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -387,6 +387,8 @@ void CheckSolution(const Task& task, const string dir, int traj_num,
       string("_0_is_success.csv")))(0, 0);
   if(is_success!=1){
     TrajOptGivenModel(task, dir, traj_num,true,-1,false);
+    //must rerun with snopt scaling again
+    TrajOptGivenModel(task, dir, traj_num,true,-1,true);
   }
   RerunTrajOpt(task,dir,traj_num);
 

From 13ee0c3cb027a05957589573025e75e9e0c1ae05 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshuhu98@gmail.com>
Date: Mon, 31 Aug 2020 20:52:27 -0400
Subject: [PATCH 74/82] add a function to try one intermediate sample to help

---
 examples/goldilocks_models/find_boundary.cc | 47 ++++++++++++++++++---
 1 file changed, 40 insertions(+), 7 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index fbf664c195..3190290865 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -357,8 +357,31 @@ void RerunTrajOpt(const Task& task, const string dir, int traj_num,
     }
   }
 }
+// check the cost increase rate compared to adjacent sample to avoid being
+// stuck in the local minimum
+void CheckCost(const Task& task,const string dir,int traj_num,int iteration){
+  int adjacent_sample_idx = (iteration==1) ? 0:traj_num-1;
+  double adjacent_sample_cost =
+      (readCSV(dir + to_string(adjacent_sample_idx) + string("_0_c.csv")))(0, 0);
+  double sample_cost =
+      (readCSV(dir + to_string(traj_num) + string("_0_c.csv")))(0, 0);
+  if(sample_cost>1.5*adjacent_sample_cost){
+    // run intermediate sample
+    Task task_mediate;
+    VectorXd current_task_vectorxd = Eigen::Map<const VectorXd>(
+        task.get().data(), task.get().size());
+    VectorXd adjacent_task_vectorxd = readCSV(
+        dir + to_string(adjacent_sample_idx) + string("_0_task.csv"));
+    task_mediate.set(CopyVectorXdToStdVector(
+        (current_task_vectorxd+adjacent_task_vectorxd)/2 ));
+    // run intermediate sample with adjacent sample solution as the initial guess
+    TrajOptGivenModel(task_mediate, dir, traj_num,true,adjacent_sample_idx);
+    // run current task with intermediate sample solution as the initial guess
+    TrajOptGivenModel(task, dir, traj_num,true);
+  }
+}
 
-//check the solution of trajectory optimization and rerun if necessary
+// check the solution of trajectory optimization and rerun if necessary
 void CheckSolution(const Task& task, const string dir, int traj_num,
     int iteration){
   int is_success;
@@ -407,6 +430,11 @@ void CheckSolution(const Task& task, const string dir, int traj_num,
     }
   }
   RerunTrajOpt(task,dir,traj_num,true);
+
+  // TODO: successful solution might be local minimum sometimes,
+  //  we need to check the cost increase again to make sure it
+  //  was a reasonable sample
+  //  CheckCost(task,dir,traj_num,iteration);
 }
 
 //search the boundary point along one direction
@@ -474,12 +502,17 @@ void BoundaryForOneDirection(const string dir,int max_iteration,
     cost_list.conservativeResize(cost_list.rows()+1, 2);
     cost_list.row(cost_list.rows()-1)<<traj_num,sample_cost;
 
-    if(sample_cost>max_cost){
-      boundary_point_idx += 1;
-      boundary_point = new_task-step;
-      SaveBoundaryPointInfor(dir,boundary_point_idx,
-                             traj_num,boundary_point);
-      break;
+    // to break the loop, we must confirm that we have found the boundary
+    // instead of easily passing the condition by local minimum solution
+    if(cost_list.rows()!=1){
+      if( (sample_cost > max_cost) &&
+      (sample_cost < 1.5*cost_list(cost_list.rows()-2,1)) ){
+        boundary_point_idx += 1;
+        boundary_point = new_task-step;
+        SaveBoundaryPointInfor(dir,boundary_point_idx,
+                               traj_num,boundary_point);
+        break;
+      }
     }
   }
 

From 6fa1066ac63167ebdcb81a05793b0c0aa1c0f5a5 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshuhu98@gmail.com>
Date: Wed, 2 Sep 2020 13:34:39 -0400
Subject: [PATCH 75/82] update plotting functions and the main program

---
 examples/goldilocks_models/find_boundary.cc   | 27 +++++-----
 .../find_boundary_utils/adjacent.csv          | 16 ++++++
 .../find_boundary_utils/plot_landscape.py     | 54 +++++++++++--------
 3 files changed, 62 insertions(+), 35 deletions(-)
 create mode 100644 examples/goldilocks_models/find_boundary_utils/adjacent.csv

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 3190290865..5c511379ce 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -431,10 +431,10 @@ void CheckSolution(const Task& task, const string dir, int traj_num,
   }
   RerunTrajOpt(task,dir,traj_num,true);
 
-  // TODO: successful solution might be local minimum sometimes,
-  //  we need to check the cost increase again to make sure it
-  //  was a reasonable sample
-  //  CheckCost(task,dir,traj_num,iteration);
+  // successful solution might be local minimum sometimes,
+  // we need to check the cost increase again to make sure it
+  // was a reasonable sample
+  CheckCost(task,dir,traj_num,iteration);
 }
 
 //search the boundary point along one direction
@@ -625,10 +625,10 @@ int find_boundary(int argc, char* argv[]){
       FLAGS_search_sl ? search_elements : non_search_elements,
       FLAGS_search_gi ? search_elements : non_search_elements,
       FLAGS_search_v ? search_elements : non_search_elements};
-    task = Task({"stride length", "ground incline",
-                 "velocity"});
-    search_setting = SearchSetting(3,{"stride length", "ground incline",
-                                      "velocity"},{0.25,0,0.4},
+    vector<string> task_names{"stride length", "ground incline","velocity"};
+    SaveStringVecToCsv(task_names, dir + string("task_names.csv"));
+    task = Task(task_names);
+    search_setting = SearchSetting(3,task_names,{0.25,0,0.4},
                                           {0.01,0.01,0.02},elements);
   }
   else if(robot_option==1){
@@ -637,11 +637,12 @@ int find_boundary(int argc, char* argv[]){
       FLAGS_search_gi ? search_elements : non_search_elements,
       FLAGS_search_v ? search_elements : non_search_elements,
       FLAGS_search_tr ? search_elements : non_search_elements};
-    task = Task({"stride length", "ground incline",
-                 "velocity", "turning rate"});
-    search_setting = SearchSetting(4,{"stride length", "ground incline",
-                                      "velocity","turning rate"},
-                                   {0.3,0,0.5,0},{0.01,0.01,0.01,0.02},elements);
+    vector<string> task_names{"stride length", "ground incline",
+                 "velocity", "turning rate"};
+    SaveStringVecToCsv(task_names, dir + string("task_names.csv"));
+    task = Task(task_names);
+    search_setting = SearchSetting(4,task_names,{0.3,0,0.5,0},
+        {0.01,0.01,0.01,0.02},elements);
   }
   //cout initial point information
   int dim = 0;
diff --git a/examples/goldilocks_models/find_boundary_utils/adjacent.csv b/examples/goldilocks_models/find_boundary_utils/adjacent.csv
new file mode 100644
index 0000000000..428f8751fb
--- /dev/null
+++ b/examples/goldilocks_models/find_boundary_utils/adjacent.csv
@@ -0,0 +1,16 @@
+9.000000000000000000e+00
+8.000000000000000000e+00
+4.000000000000000000e+00
+7.000000000000000000e+00
+3.000000000000000000e+00
+2.000000000000000000e+00
+5.000000000000000000e+00
+0.000000000000000000e+00
+6.000000000000000000e+00
+1.200000000000000000e+01
+1.000000000000000000e+00
+1.400000000000000000e+01
+1.300000000000000000e+01
+1.100000000000000000e+01
+1.500000000000000000e+01
+1.000000000000000000e+01
diff --git a/examples/goldilocks_models/find_boundary_utils/plot_landscape.py b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
index 801fa28ce9..9059f5f16a 100644
--- a/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
+++ b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
@@ -6,27 +6,33 @@
 import os
 
 robot_option = 1
-file_dir = "../dairlib_data/goldilocks_models/find_boundary/"
+file_dir = '../dairlib_data/goldilocks_models/find_boundary/'
+
 if robot_option == 1:
     robot = 'cassie/'
-    dir1 = file_dir + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
-           '_2d_nominal/'
-    dir2 = file_dir + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
-           '_2d_initial_model/'
-    dir3 = file_dir + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
+    dir1 = file_dir + robot + '2D_LIP/4D_task_space/' + 'robot_' + str(robot_option) + \
+           '_grid_iter50_sl_tr/'
+    dir2 = file_dir + robot + '2D_LIP/4D_task_space/' + 'robot_' + str(robot_option) + \
+           '_initial_sl_tr/'
+    dir3 = file_dir + robot + '2D_LIP/4D_task_space/' + 'robot_' + str(robot_option) + \
            '_large_iter100/'
-    dir4 = file_dir + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + \
+    dir4 = file_dir + robot + '2D_LIP/4D_task_space/' + 'robot_' + str(robot_option) + \
            '_small_iter200/'
+    dir5 = file_dir+'robot_1/'
 
 # number of searching directions
 n_direction = 16
 
 # Note: we need to decide which column of the searching direction to use
 # Eg. column index 0 corresponds to stride length
+
+# f = open(dir + "task_names.csv", "r")
+# task_name = f.read().splitlines()
 task_name = ['Stride length', 'Ground incline', 'Velocity', 'Turning rate']
 task_1_idx = 0
 task_2_idx = 1
 
+
 def ExtractAdjacentLine(dir):
     adj_direction = np.zeros([n_direction]).astype(int)
     for i in range(n_direction):
@@ -58,13 +64,14 @@ def process_data_from_direction(i, dir1):
     z = [float(np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_c.csv', delimiter=","))]
 
     # process the points on the line
-    for j in range(data_dir.shape[0]):
-        task = np.genfromtxt(dir1 + str(int(data_dir[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
-        cost = data_dir[j, 1]
-        if cost < 35:
-            x.append(task[task_1_idx])
-            y.append(task[task_2_idx])
-            z.append(cost)
+    if data_dir.ndim > 1:
+        for j in range(data_dir.shape[0]):
+            task = np.genfromtxt(dir1 + str(int(data_dir[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+            cost = data_dir[j, 1]
+            if cost < 35:
+                x.append(task[task_1_idx])
+                y.append(task[task_2_idx])
+                z.append(cost)
 
     return x, y, z
 
@@ -76,19 +83,22 @@ def generateplot(dir1, adj_index):
         # process data on adjacent line
         x2, y2, z2 = process_data_from_direction(adj_index[i], dir1)
         # plot
-        x = x1+x2
-        y = y1+y2
-        z = z1+z2
-        surf = ax.tricontourf(x, y, z, levels=[0, 4, 8, 12, 16, 20, 24, 28, 32])
-    fig.colorbar(surf, shrink=0.5, aspect=6)
+        if (len(x1) > 1) & (len(x2) > 1):
+            x = x1+x2
+            y = y1+y2
+            z = z1+z2
+            # surf = ax.scatter(x, y, z)
+            surf = ax.tricontourf(x, y, z, levels=[0, 4, 8, 12, 16, 20, 24, 28, 32])
+    # fig.colorbar(surf, shrink=0.5, aspect=6)
+
 
-# adjacent = ExtractAdjacentLine(dir1)
+adjacent = ExtractAdjacentLine(dir5)
 # np.savetxt('adjacent.csv', adjacent, delimiter=',')
 
 fig, ax = plt.subplots()
 ax.set_xlabel(task_name[task_1_idx])
 ax.set_ylabel(task_name[task_2_idx])
 ax.set_title('cost landscape')
-adjacent = np.genfromtxt('adjacent.csv', delimiter=",")
-generateplot(dir3, adjacent)
+# adjacent = np.genfromtxt('adjacent.csv', delimiter=",").astype(int)
+generateplot(dir5, adjacent)
 plt.show()
\ No newline at end of file

From fe5b0bf6cb4afe3390be65f1e2cddbeaf895ff41 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshuhu98@gmail.com>
Date: Wed, 2 Sep 2020 14:20:36 -0400
Subject: [PATCH 76/82] fix a bug related to creating intermediate sample

---
 examples/goldilocks_models/find_boundary.cc        |  2 +-
 .../find_boundary_utils/plot_landscape.py          | 14 +++++++-------
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index 5c511379ce..daa54517c4 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -367,7 +367,7 @@ void CheckCost(const Task& task,const string dir,int traj_num,int iteration){
       (readCSV(dir + to_string(traj_num) + string("_0_c.csv")))(0, 0);
   if(sample_cost>1.5*adjacent_sample_cost){
     // run intermediate sample
-    Task task_mediate;
+    Task task_mediate = task;
     VectorXd current_task_vectorxd = Eigen::Map<const VectorXd>(
         task.get().data(), task.get().size());
     VectorXd adjacent_task_vectorxd = readCSV(
diff --git a/examples/goldilocks_models/find_boundary_utils/plot_landscape.py b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
index 9059f5f16a..eca5f95f4e 100644
--- a/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
+++ b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
@@ -11,13 +11,13 @@
 if robot_option == 1:
     robot = 'cassie/'
     dir1 = file_dir + robot + '2D_LIP/4D_task_space/' + 'robot_' + str(robot_option) + \
-           '_grid_iter50_sl_tr/'
+           '_nominal_sl_tr/'
     dir2 = file_dir + robot + '2D_LIP/4D_task_space/' + 'robot_' + str(robot_option) + \
            '_initial_sl_tr/'
     dir3 = file_dir + robot + '2D_LIP/4D_task_space/' + 'robot_' + str(robot_option) + \
-           '_large_iter100/'
+           '_range1_iter300/'
     dir4 = file_dir + robot + '2D_LIP/4D_task_space/' + 'robot_' + str(robot_option) + \
-           '_small_iter200/'
+           '_range2_iter200/'
     dir5 = file_dir+'robot_1/'
 
 # number of searching directions
@@ -30,7 +30,7 @@
 # task_name = f.read().splitlines()
 task_name = ['Stride length', 'Ground incline', 'Velocity', 'Turning rate']
 task_1_idx = 0
-task_2_idx = 1
+task_2_idx = 3
 
 
 def ExtractAdjacentLine(dir):
@@ -89,10 +89,10 @@ def generateplot(dir1, adj_index):
             z = z1+z2
             # surf = ax.scatter(x, y, z)
             surf = ax.tricontourf(x, y, z, levels=[0, 4, 8, 12, 16, 20, 24, 28, 32])
-    # fig.colorbar(surf, shrink=0.5, aspect=6)
+    fig.colorbar(surf, shrink=0.5, aspect=6)
 
 
-adjacent = ExtractAdjacentLine(dir5)
+adjacent = ExtractAdjacentLine(dir3)
 # np.savetxt('adjacent.csv', adjacent, delimiter=',')
 
 fig, ax = plt.subplots()
@@ -100,5 +100,5 @@ def generateplot(dir1, adj_index):
 ax.set_ylabel(task_name[task_2_idx])
 ax.set_title('cost landscape')
 # adjacent = np.genfromtxt('adjacent.csv', delimiter=",").astype(int)
-generateplot(dir5, adjacent)
+generateplot(dir1, adjacent)
 plt.show()
\ No newline at end of file

From 41baac50485d8dc1eaef67bac11678270e342763 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshuhu98@gmail.com>
Date: Thu, 3 Sep 2020 13:16:19 -0400
Subject: [PATCH 77/82] updating plotting function to fit the incomplete data

---
 .../find_boundary_utils/plot_landscape.py     | 24 +++--
 .../find_boundary_utils/plot_landscape_new.py | 87 -------------------
 2 files changed, 17 insertions(+), 94 deletions(-)
 delete mode 100644 examples/goldilocks_models/find_boundary_utils/plot_landscape_new.py

diff --git a/examples/goldilocks_models/find_boundary_utils/plot_landscape.py b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
index eca5f95f4e..214fc3c5a4 100644
--- a/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
+++ b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
@@ -76,14 +76,24 @@ def process_data_from_direction(i, dir1):
     return x, y, z
 
 
+def find_adjacent_line(dir1, adj_index, i):
+    # process data on adjacent line
+    x2, y2, z2 = process_data_from_direction(adj_index[i], dir1)
+    if len(x2) > 10:
+        return x2, y2, z2
+    else:
+        x2, y2, z2 = find_adjacent_line(dir1, adj_index, adj_index[i])
+        return x2, y2, z2
+
+
 def generateplot(dir1, adj_index):
     for i in range(n_direction):
         # process data on one line
         x1, y1, z1 = process_data_from_direction(i, dir1)
         # process data on adjacent line
-        x2, y2, z2 = process_data_from_direction(adj_index[i], dir1)
-        # plot
-        if (len(x1) > 1) & (len(x2) > 1):
+        if len(x1) > 10:
+            x2, y2, z2 = find_adjacent_line(dir1, adj_index, i)
+            # plot
             x = x1+x2
             y = y1+y2
             z = z1+z2
@@ -92,13 +102,13 @@ def generateplot(dir1, adj_index):
     fig.colorbar(surf, shrink=0.5, aspect=6)
 
 
-adjacent = ExtractAdjacentLine(dir3)
-# np.savetxt('adjacent.csv', adjacent, delimiter=',')
+# adjacent = ExtractAdjacentLine(dir3)
+# np.savetxt('examples/goldilocks_models/find_boundary_utils/adjacent.csv', adjacent, delimiter=',')
 
 fig, ax = plt.subplots()
 ax.set_xlabel(task_name[task_1_idx])
 ax.set_ylabel(task_name[task_2_idx])
 ax.set_title('cost landscape')
-# adjacent = np.genfromtxt('adjacent.csv', delimiter=",").astype(int)
-generateplot(dir1, adjacent)
+adjacent = np.genfromtxt('examples/goldilocks_models/find_boundary_utils/adjacent.csv', delimiter=",").astype(int)
+generateplot(dir2, adjacent)
 plt.show()
\ No newline at end of file
diff --git a/examples/goldilocks_models/find_boundary_utils/plot_landscape_new.py b/examples/goldilocks_models/find_boundary_utils/plot_landscape_new.py
deleted file mode 100644
index 1ee1d31be4..0000000000
--- a/examples/goldilocks_models/find_boundary_utils/plot_landscape_new.py
+++ /dev/null
@@ -1,87 +0,0 @@
-"""
-Given points on two adjacent lines, fill the region between two lines with the color corresponding to the cost.
-"""
-import matplotlib.pyplot as plt
-import numpy as np
-import os
-
-robot_option = 1
-file_dir = "/home/jianshu/workspace/dairlib_data/goldilocks_models/find_boundary/"
-# file_dir = "../dairlib_data/goldilocks_models/find_boundary/"
-if robot_option == 1:
-    robot = 'cassie/'
-    dir1 = file_dir + robot + '2D_rom/4D_task_space/' + 'robot_' + str(robot_option) + '_grid_iter50_sl_tr/'
-
-# number of searching directions
-n_direction = 16
-
-# Note: we need to decide which column of the searching direction to use
-# Eg. column index 0 corresponds to stride length
-task_name = ['Stride length', 'Ground incline', 'Velocity', 'Turning rate']
-task_1_idx = 0
-task_2_idx = 3
-
-def ExtractAdjacentLine(dir):
-    adj_direction = np.zeros([n_direction]).astype(int)
-    for i in range(n_direction):
-        min_sin = 1
-        line1 = np.genfromtxt(dir + str(i + 1) + '_searching_direction.csv', delimiter=",")
-        vec1 = np.array([line1[task_1_idx], line1[task_2_idx]])
-        for j in range(n_direction):
-            line2 = np.genfromtxt(dir + str(j+1) + '_searching_direction.csv', delimiter=",")
-            vec2 = np.array([line2[task_1_idx], line2[task_2_idx]])
-            sin = np.cross(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))
-            cos = np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))
-            # restrict the direction
-            if (cos > 0) & (sin > 0):
-                # find the adjacent pair
-                if sin < min_sin:
-                    adj_index = j
-                    min_sin = sin
-        adj_direction[i] = adj_index
-    return adj_direction
-
-
-def process_data_from_direction(i, dir1):
-    data_dir = np.genfromtxt(dir1 + str(int(i+1)) + '_cost_list.csv', delimiter=",")
-
-    # need to add central point on the points list
-    task0 = np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_task.csv', delimiter=",")
-    x = [task0[task_1_idx]]
-    y = [task0[task_2_idx]]
-    z = [float(np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_c.csv', delimiter=","))]
-
-    # process the points on the line
-    for i in range(data_dir.shape[0]):
-        gamma = np.genfromtxt(dir1 + str(int(data_dir[i, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
-        cost = data_dir[i, 1]
-        if cost < 35:
-            x.append(gamma[task_1_idx])
-            y.append(gamma[task_2_idx])
-            z.append(cost)
-
-    return x,y,z
-
-
-def generateplot(dir1, adj_index):
-    for i in range(n_direction):
-        # process data on one line
-        x1, y1, z1 = process_data_from_direction(i, dir1)
-        # process data on adjacent line
-        x2, y2, z2 = process_data_from_direction(adj_index[i], dir1)
-        # plot
-        x = x1+x2
-        y = y1+y2
-        z = z1+z2
-        surf = ax.tricontourf(x, y, z, levels=[0, 4, 8, 12, 16, 20, 24, 28, 32])
-    fig.colorbar(surf, shrink=0.5, aspect=6)
-
-
-fig, ax = plt.subplots()
-ax.set_xlabel(task_name[task_1_idx])
-ax.set_ylabel(task_name[task_2_idx])
-ax.set_title('cost landscape')
-adjacent = ExtractAdjacentLine(dir1)
-print(adjacent)
-generateplot(dir1, adjacent)
-plt.show()
\ No newline at end of file

From 1b03c0b8ee65e2c22a05f9cf2844c6e491c409fa Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshuhu98@gmail.com>
Date: Thu, 3 Sep 2020 17:20:36 -0400
Subject: [PATCH 78/82] update plotting functions and find_boundary

---
 examples/goldilocks_models/find_boundary.cc   | 33 +++++++++--------
 .../find_boundary_utils/compare_boundary.py   | 36 ++++++++++++-------
 .../plot_normalized_landscape.py              | 31 ++++++++++------
 3 files changed, 61 insertions(+), 39 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
index daa54517c4..100fe772ab 100644
--- a/examples/goldilocks_models/find_boundary.cc
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -68,9 +68,7 @@ DEFINE_int32(
 "resolve again.");
 DEFINE_int32(n_node, -1, "# of nodes for traj opt");
 DEFINE_double(eps_regularization, 1e-8, "Weight of regularization term"); //1e-4
-DEFINE_bool(is_get_nominal,false,"is calculating the cost without ROM constraints");
-DEFINE_bool(use_optimized_model,false,"read theta from files to apply optimized model");
-DEFINE_int32(theta_index,-1,"# of optimized model to use");
+DEFINE_int32(theta_index,-1,"# of model to use");
 
 //tasks
 DEFINE_bool(is_zero_touchdown_impact, false,
@@ -159,9 +157,9 @@ string getInitFileName(const string directory, int traj_opt_num,
     string prefix;
     // initial guess for initial point
     if(traj_opt_num==0){
-      //specify initial guess if using optimized model
+      //specify initial guess if using data from optimizing models
       //use solutions during ROM optimization process to calculate the initial guess
-      if(FLAGS_use_optimized_model){
+      if(FLAGS_theta_index>=0){
         const string dir_find_models = "../dairlib_data/goldilocks_models/find_models/robot_" +
             to_string(FLAGS_robot_option) + "/";
         //calculate the weighted sum of solutions
@@ -260,9 +258,8 @@ void TrajOptGivenModel(Task task,
   // Initial guess of theta
   VectorXd theta_y = VectorXd::Zero(rom->n_theta_y());
   VectorXd theta_yddot = VectorXd::Zero(rom->n_theta_yddot());
-  if (FLAGS_use_optimized_model) {
+  if (FLAGS_theta_index >= 0) {
     //you have to specify the theta to use
-    DRAKE_DEMAND(FLAGS_theta_index >= 0);
     int theta_idx = FLAGS_theta_index;
 
     const string dir_find_models =
@@ -273,7 +270,7 @@ void TrajOptGivenModel(Task task,
     rom->SetThetaYddot(theta_yddot);
   }
 
-  bool is_get_nominal = FLAGS_is_get_nominal;
+  bool is_get_nominal = (FLAGS_theta_index==0);
   int max_inner_iter_pass_in = is_get_nominal ? 200 : max_inner_iter;
 
 //  string init_file_pass_in = "";
@@ -386,6 +383,9 @@ void CheckSolution(const Task& task, const string dir, int traj_num,
     int iteration){
   int is_success;
 
+  // check the cost increase first
+  CheckCost(task,dir,traj_num,iteration);
+
   //check if snopt find a solution successfully. If not, rerun the Traj Opt
   RerunTrajOpt(task,dir,traj_num);
 
@@ -431,10 +431,6 @@ void CheckSolution(const Task& task, const string dir, int traj_num,
   }
   RerunTrajOpt(task,dir,traj_num,true);
 
-  // successful solution might be local minimum sometimes,
-  // we need to check the cost increase again to make sure it
-  // was a reasonable sample
-  CheckCost(task,dir,traj_num,iteration);
 }
 
 //search the boundary point along one direction
@@ -656,15 +652,18 @@ int find_boundary(int argc, char* argv[]){
    * Iteration setting
    */
   cout << "\nIteration setting:\n";
-  cout<<"get nominal cost: "<<FLAGS_is_get_nominal<<endl;
-  cout<<"use optimized model: "<<FLAGS_use_optimized_model<<endl;
-  cout<<"optimized model index: "<<FLAGS_theta_index<<endl;
+  DRAKE_DEMAND(FLAGS_theta_index>=0);
+  bool get_nominal = (FLAGS_theta_index==0);
+  bool use_optimized_model = (FLAGS_theta_index>1);
+  cout<<"model index: "<<FLAGS_theta_index<<endl;
+  cout<<"get nominal cost: "<<get_nominal<<endl;
+  cout<<"use optimized model: "<<use_optimized_model<<endl;
   int max_iter = FLAGS_max_outer_iter;
   //TODO:decide the threshold under different situation
   double cost_threshold = 30;
   if(robot_option==0)
   {
-    if(FLAGS_is_get_nominal){
+    if(get_nominal){
       cost_threshold = 30;
     }
     else{
@@ -672,7 +671,7 @@ int find_boundary(int argc, char* argv[]){
     }
   }
   else if(robot_option==1){
-    if(FLAGS_is_get_nominal){
+    if(get_nominal){
       cost_threshold = 30;
     }
     else{
diff --git a/examples/goldilocks_models/find_boundary_utils/compare_boundary.py b/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
index 3940642b5f..1ca189531a 100644
--- a/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
+++ b/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
@@ -19,9 +19,9 @@
     robot = 'cassie/'
 else:
     robot = 'five_link/'
-dir1 = file_dir + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+dir1 = file_dir + robot + '2D_LIP/2D_task_space/' + 'robot_' + str(robot_option) + \
         '_large_iter100/'
-dir2 = file_dir + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
+dir2 = file_dir + robot + '2D_LIP/2D_task_space/' + 'robot_' + str(robot_option) + \
        '_small_iter200/'
 
 # number of searching directions
@@ -40,6 +40,7 @@
 task_1_idx = 0
 task_2_idx = 1
 
+
 def process_data_from_direction(i, dir1, dir_nominal):
     # need to add central point on the points list
     task0 = np.genfromtxt(dir1 + str(0) + '_' + str(0) + '_task.csv', delimiter=",")
@@ -85,6 +86,16 @@ def process_data_from_direction(i, dir1, dir_nominal):
     return x, y, z
 
 
+def find_adjacent_line(dir1, dir2, adj_index, i):
+    # process data on adjacent line
+    x2, y2, z2 = process_data_from_direction(adj_index[i], dir1, dir2)
+    if len(x2) > 10:
+        return x2, y2, z2
+    else:
+        x2, y2, z2 = find_adjacent_line(dir1, dir2, adj_index, adj_index[i])
+        return x2, y2, z2
+
+
 def generateplot(dir1, dir_nominal, adj_index):
     # discrete color map
     levels = [0, 0.7, 0.8, 0.9, 1, 2]
@@ -97,15 +108,16 @@ def generateplot(dir1, dir_nominal, adj_index):
     for i in range(n_direction):
         # process data on one line
         x1, y1, z1 = process_data_from_direction(i, dir1, dir_nominal)
-        # process data on adjacent line
-        x2, y2, z2 = process_data_from_direction(adj_index[i], dir1,dir_nominal)
-        # plot
-        x = x1+x2
-        y = y1+y2
-        z = z1+z2
-        surf = ax.tricontourf(x, y, z, cmap=cmap, norm=norm, levels=levels, extend='both')
-        if max(z) > total_max:
-            total_max = max(z)
+        if len(x1) > 10:
+            # process data on adjacent line
+            x2, y2, z2 = find_adjacent_line(dir1, dir_nominal, adj_index, i)
+            # plot
+            x = x1+x2
+            y = y1+y2
+            z = z1+z2
+            surf = ax.tricontourf(x, y, z, cmap=cmap, norm=norm, levels=levels, extend='both')
+            if max(z) > total_max:
+                total_max = max(z)
     print('max', total_max)
     cbar = fig.colorbar(surf, shrink=0.9, aspect=10, extend='both')
     cbar.ax.set_yticklabels(['0', '0.7', '0.8', '0.9', '1', 'Inf'])
@@ -115,7 +127,7 @@ def generateplot(dir1, dir_nominal, adj_index):
 # fig, ax = plt.subplots(figsize=(11,5.5))
 fig, ax = plt.subplots()
 # ax.scatter(x, y)
-adjacent = np.genfromtxt('adjacent.csv', delimiter=",")
+adjacent = np.genfromtxt('examples/goldilocks_models/find_boundary_utils/adjacent.csv', delimiter=",").astype(int)
 generateplot(dir1, dir2, adjacent)
 ax.set_xlabel(task_name[task_1_idx])
 ax.set_ylabel(task_name[task_2_idx])
diff --git a/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py b/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py
index 0f7baa89dc..a978787a27 100644
--- a/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py
+++ b/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py
@@ -81,20 +81,31 @@ def process_data_from_direction(i, dir1, dir_nominal):
     return x, y, z
 
 
+def find_adjacent_line(dir1, dir2, adj_index, i):
+    # process data on adjacent line
+    x2, y2, z2 = process_data_from_direction(adj_index[i], dir1, dir2)
+    if len(x2) > 10:
+        return x2, y2, z2
+    else:
+        x2, y2, z2 = find_adjacent_line(dir1, dir2, adj_index, adj_index[i])
+        return x2, y2, z2
+
+
 def generateplot(dir1, dir_nominal, adj_index, levels, ticks):
     total_max = 0
     for i in range(n_direction):
         # process data on one line
         x1, y1, z1 = process_data_from_direction(i, dir1, dir_nominal)
-        # process data on adjacent line
-        x2, y2, z2 = process_data_from_direction(adj_index[i], dir1,dir_nominal)
-        # plot
-        x = x1+x2
-        y = y1+y2
-        z = z1+z2
-        surf = ax.tricontourf(x, y, z, levels=levels)
-        if max(z) > total_max:
-            total_max = max(z)
+        if len(x1) > 10:
+            # process data on adjacent line
+            x2, y2, z2 = find_adjacent_line(dir1, dir_nominal, adj_index, i)
+            # plot
+            x = x1+x2
+            y = y1+y2
+            z = z1+z2
+            surf = ax.tricontourf(x, y, z, levels=levels)
+            if max(z) > total_max:
+                total_max = max(z)
     print('max', total_max)
     fig.colorbar(surf, shrink=0.9, aspect=10, spacing='proportional', ticks=ticks)
 
@@ -107,7 +118,7 @@ def generateplot(dir1, dir_nominal, adj_index, levels, ticks):
 levels = [0.0, 0.5, 1, 1.5, 2] # manual specify level sets
 ticks = [0.0, 0.5, 1, 1.5, 2] # manual specify ticker values (it seems 0 is ignored)
 print(levels)
-adjacent = np.genfromtxt('adjacent.csv', delimiter=",")
+adjacent = np.genfromtxt('examples/goldilocks_models/find_boundary_utils/adjacent.csv', delimiter=",").astype(int)
 generateplot(dir1,dir2, adjacent, levels, ticks)
 ax.set_xlabel(task_name[task_1_idx])
 ax.set_ylabel(task_name[task_2_idx])

From 90a0bc4ecdd3b43f42c53d21cbabd05ad45f080f Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshuhu98@gmail.com>
Date: Thu, 3 Sep 2020 19:30:30 -0400
Subject: [PATCH 79/82] updating plotting functions

---
 .../find_boundary_utils/compare_boundary.py   | 14 ++++++++-----
 .../find_boundary_utils/plot_landscape.py     |  2 +-
 .../plot_normalized_landscape.py              | 21 +++++++++++++------
 3 files changed, 25 insertions(+), 12 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary_utils/compare_boundary.py b/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
index 1ca189531a..98c7a34e83 100644
--- a/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
+++ b/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
@@ -19,10 +19,10 @@
     robot = 'cassie/'
 else:
     robot = 'five_link/'
-dir1 = file_dir + robot + '2D_LIP/2D_task_space/' + 'robot_' + str(robot_option) + \
-        '_large_iter100/'
-dir2 = file_dir + robot + '2D_LIP/2D_task_space/' + 'robot_' + str(robot_option) + \
-       '_small_iter200/'
+dir1 = file_dir + robot + '2D_LIP/4D_task_space/' + 'robot_' + str(robot_option) + \
+        '_range2_iter200/'
+dir2 = file_dir + robot + '2D_LIP/4D_task_space/' + 'robot_' + str(robot_option) + \
+       '_nominal_sl_tr/'
 
 # number of searching directions
 n_direction = 16
@@ -38,7 +38,7 @@
 # Eg. column index 0 corresponds to stride length
 task_name = ['Stride length', 'Ground incline', 'Velocity', 'Turning rate']
 task_1_idx = 0
-task_2_idx = 1
+task_2_idx = 3
 
 
 def process_data_from_direction(i, dir1, dir_nominal):
@@ -60,6 +60,10 @@ def process_data_from_direction(i, dir1, dir_nominal):
         num_small = data_dir1.shape[0]
         num_large = data_dir2.shape[0]
 
+    # discard this line if the data is not reasonable
+    if num_small < 10:
+        return x, y, z
+
     # process the points on the line
     # set the value for intersected parts
     for j in range(num_small):
diff --git a/examples/goldilocks_models/find_boundary_utils/plot_landscape.py b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
index 214fc3c5a4..9ad2ce3fb3 100644
--- a/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
+++ b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
@@ -110,5 +110,5 @@ def generateplot(dir1, adj_index):
 ax.set_ylabel(task_name[task_2_idx])
 ax.set_title('cost landscape')
 adjacent = np.genfromtxt('examples/goldilocks_models/find_boundary_utils/adjacent.csv', delimiter=",").astype(int)
-generateplot(dir2, adjacent)
+generateplot(dir4, adjacent)
 plt.show()
\ No newline at end of file
diff --git a/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py b/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py
index a978787a27..34c18f8259 100644
--- a/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py
+++ b/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py
@@ -19,10 +19,10 @@
     robot = 'cassie/'
 else:
     robot = 'five_link/'
-dir1 = file_dir + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-        '_large_iter100/'
-dir2 = file_dir + robot + '2D_rom/2D_task_space/' + 'robot_' + str(robot_option) + \
-       '_small_iter200/'
+dir1 = file_dir + robot + '2D_LIP/4D_task_space/' + 'robot_' + str(robot_option) + \
+        '_range2_iter200/'
+dir2 = file_dir + robot + '2D_LIP/4D_task_space/' + 'robot_' + str(robot_option) + \
+       '_nominal_sl_tr/'
 
 # number of searching directions
 n_direction = 16
@@ -38,7 +38,7 @@
 # Eg. column index 0 corresponds to stride length
 task_name = ['Stride length', 'Ground incline', 'Velocity', 'Turning rate']
 task_1_idx = 0
-task_2_idx = 1
+task_2_idx = 3
 
 
 def process_data_from_direction(i, dir1, dir_nominal):
@@ -60,13 +60,16 @@ def process_data_from_direction(i, dir1, dir_nominal):
         num_small = data_dir1.shape[0]
         num_large = data_dir2.shape[0]
 
+    # discard this line if the data is not reasonable
+    if num_small < 10:
+        return x, y, z
     # process the points on the line
     # set the value for intersected parts
     for j in range(num_small):
         cost1 = data_dir1[j, 1]
         cost2 = data_dir2[j, 1]
         # we only append reasonable point
-        if (cost1 < 35) & (cost2 < 35) & (cost1/cost2<2):
+        if (cost1 < 35) & (cost2 < 35) & (cost1/cost2 < 2):
             task = np.genfromtxt(dir1 + str(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
             x.append(task[task_1_idx])
             y.append(task[task_2_idx])
@@ -78,6 +81,12 @@ def process_data_from_direction(i, dir1, dir_nominal):
             x.append(task[task_1_idx])
             y.append(task[task_2_idx])
             z.append(0)
+        else:
+            # extended range
+            task = np.genfromtxt(dir_nominal + str(int(data_dir2[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+            x.append(task[task_1_idx])
+            y.append(task[task_2_idx])
+            z.append(2)
     return x, y, z
 
 

From 8098f58bcea3b0b99226b5fca280076f44657b6c Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshuhu98@gmail.com>
Date: Fri, 4 Sep 2020 13:11:46 -0400
Subject: [PATCH 80/82] add comments to plotting functions and finalize the
 version of plotting functions

---
 .../find_boundary_utils/compare_boundary.py   |  8 +--
 .../find_boundary_utils/plot_landscape.py     | 61 +++++++++++--------
 .../plot_normalized_landscape.py              | 11 +---
 3 files changed, 41 insertions(+), 39 deletions(-)

diff --git a/examples/goldilocks_models/find_boundary_utils/compare_boundary.py b/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
index 98c7a34e83..800a0c29a0 100644
--- a/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
+++ b/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
@@ -1,12 +1,6 @@
 """
 This function is used for comparing two cost landscape and plot the landscape with discrete color map.
-Considering we search along several directions, we process the data along those directions
-1.
-For each direction, we compare the cost at each point on the searching line both in cost landscape 1 (C1) and
-cost landscape. Set the value of the point according the cost.
-2.
-For those points which are not in two landscape, if it exists in C1, we set the value of this point -0.5,
-else we set the value of this point 2
+The algorithm is similar to plot_landscape while we process the data on a ray from two data sets.
 """
 import matplotlib.pyplot as plt
 import matplotlib
diff --git a/examples/goldilocks_models/find_boundary_utils/plot_landscape.py b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
index 9ad2ce3fb3..64fe97f52b 100644
--- a/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
+++ b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
@@ -1,47 +1,49 @@
 """
-Given points on two adjacent lines, fill the region between two lines with the color corresponding to the cost.
+Considering that we search along different directions to get the boundary, we can use the data point on the lines to
+generate landscape.
+Currently the best method is plotting the region within two adjacent rays using the function traicontourf with
+continuous color map.
 """
+
 import matplotlib.pyplot as plt
 import numpy as np
-import os
 
 robot_option = 1
 file_dir = '../dairlib_data/goldilocks_models/find_boundary/'
 
 if robot_option == 1:
     robot = 'cassie/'
-    dir1 = file_dir + robot + '2D_LIP/4D_task_space/' + 'robot_' + str(robot_option) + \
-           '_nominal_sl_tr/'
-    dir2 = file_dir + robot + '2D_LIP/4D_task_space/' + 'robot_' + str(robot_option) + \
-           '_initial_sl_tr/'
-    dir3 = file_dir + robot + '2D_LIP/4D_task_space/' + 'robot_' + str(robot_option) + \
-           '_range1_iter300/'
-    dir4 = file_dir + robot + '2D_LIP/4D_task_space/' + 'robot_' + str(robot_option) + \
-           '_range2_iter200/'
-    dir5 = file_dir+'robot_1/'
+    dir1 = file_dir+'robot_1/'
 
 # number of searching directions
 n_direction = 16
 
-# Note: we need to decide which column of the searching direction to use
+# Note: we need to decide which column of the task to use
 # Eg. column index 0 corresponds to stride length
-
+# Currently we manually specify the task names, we will save the task name when generating the data in the future.
 # f = open(dir + "task_names.csv", "r")
 # task_name = f.read().splitlines()
 task_name = ['Stride length', 'Ground incline', 'Velocity', 'Turning rate']
 task_1_idx = 0
-task_2_idx = 3
+task_2_idx = 1
+
+# define a flag used to decide the plotting function: use tricontourf or scatter
+# scatter can used for debugging and checking the feasibility of the data
+is_contour_plot = True
 
 
-def ExtractAdjacentLine(dir):
+# function used to find the index of adjacent ray
+def extract_adjacent_line(dir):
+    # array used to save the index of adjacent ray for each ray
     adj_direction = np.zeros([n_direction]).astype(int)
     for i in range(n_direction):
         min_sin = 1
         line1 = np.genfromtxt(dir + str(i + 1) + '_searching_direction.csv', delimiter=",")
         vec1 = np.array([line1[task_1_idx], line1[task_2_idx]])
         for j in range(n_direction):
-            line2 = np.genfromtxt(dir + str(j+1) + '_searching_direction.csv', delimiter=",")
+            line2 = np.genfromtxt(dir + str(j + 1) + '_searching_direction.csv', delimiter=",")
             vec2 = np.array([line2[task_1_idx], line2[task_2_idx]])
+            # find the adjacent rays according to the angle between two rays
             sin = np.cross(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))
             cos = np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))
             # restrict the direction
@@ -54,7 +56,10 @@ def ExtractAdjacentLine(dir):
     return adj_direction
 
 
+# function to process the data on one ray
 def process_data_from_direction(i, dir1):
+    # cost list is used to store the cost for each sample
+    # the first column stores the sample index and the second column stores the corresponding cost
     data_dir = np.genfromtxt(dir1 + str(int(i+1)) + '_cost_list.csv', delimiter=",")
 
     # need to add central point on the points list
@@ -65,10 +70,12 @@ def process_data_from_direction(i, dir1):
 
     # process the points on the line
     if data_dir.ndim > 1:
+        # avoid error when there is only one sample on the ray
         for j in range(data_dir.shape[0]):
             task = np.genfromtxt(dir1 + str(int(data_dir[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
             cost = data_dir[j, 1]
             if cost < 35:
+                # only include reasonable samples
                 x.append(task[task_1_idx])
                 y.append(task[task_2_idx])
                 z.append(cost)
@@ -76,6 +83,7 @@ def process_data_from_direction(i, dir1):
     return x, y, z
 
 
+# recursively find the reasonable adjacent ray and return the processed data
 def find_adjacent_line(dir1, adj_index, i):
     # process data on adjacent line
     x2, y2, z2 = process_data_from_direction(adj_index[i], dir1)
@@ -86,7 +94,8 @@ def find_adjacent_line(dir1, adj_index, i):
         return x2, y2, z2
 
 
-def generateplot(dir1, adj_index):
+# plotting each segment between two rays
+def generate_plot(dir1, adj_index):
     for i in range(n_direction):
         # process data on one line
         x1, y1, z1 = process_data_from_direction(i, dir1)
@@ -97,18 +106,22 @@ def generateplot(dir1, adj_index):
             x = x1+x2
             y = y1+y2
             z = z1+z2
-            # surf = ax.scatter(x, y, z)
-            surf = ax.tricontourf(x, y, z, levels=[0, 4, 8, 12, 16, 20, 24, 28, 32])
-    fig.colorbar(surf, shrink=0.5, aspect=6)
+            if is_contour_plot:
+                # we can manually set the color discretization if needed
+                surf = ax.tricontourf(x, y, z, levels=[0, 4, 8, 12, 16, 20, 24, 28, 32])
+            else:
+                surf = ax.scatter(x, y, z)
+    if is_contour_plot:
+        fig.colorbar(surf, shrink=0.5, aspect=6)
 
 
-# adjacent = ExtractAdjacentLine(dir3)
+adjacent = extract_adjacent_line(dir1)
 # np.savetxt('examples/goldilocks_models/find_boundary_utils/adjacent.csv', adjacent, delimiter=',')
+# adjacent = np.genfromtxt('examples/goldilocks_models/find_boundary_utils/adjacent.csv', delimiter=",").astype(int)
 
 fig, ax = plt.subplots()
+generate_plot(dir1, adjacent)
 ax.set_xlabel(task_name[task_1_idx])
 ax.set_ylabel(task_name[task_2_idx])
 ax.set_title('cost landscape')
-adjacent = np.genfromtxt('examples/goldilocks_models/find_boundary_utils/adjacent.csv', delimiter=",").astype(int)
-generateplot(dir4, adjacent)
-plt.show()
\ No newline at end of file
+plt.show()
diff --git a/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py b/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py
index 34c18f8259..02f7554435 100644
--- a/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py
+++ b/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py
@@ -1,12 +1,7 @@
 """
-This function is used for plotting normalized cost landscape.
-Considering we search along several directions, we process the data along those directions
-1.
-For each direction, we compare the cost at each point on the searching line both in cost landscape 1 and
-cost landscape and set the value of reasonable point with the normalized cost.
-2.
-For those points which exist in the cost landscape we want to normalize but not in the nominal cost landscape,
-we set the value of this point 0.5.
+This function is used for normalized cost landscape with another cost landscape and plot the landscape with
+continuous color map.
+The algorithm is similar to plot_landscape while we process the data on a ray from two data sets.
 """
 import matplotlib.pyplot as plt
 import numpy as np

From 1aff54ebd62016180f188ca1926fe9b0b6a77ad5 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshuhu98@gmail.com>
Date: Fri, 4 Sep 2020 13:49:13 -0400
Subject: [PATCH 81/82] add the plotting for optimization range

---
 .../find_boundary_utils/plot_landscape.py             | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/examples/goldilocks_models/find_boundary_utils/plot_landscape.py b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
index 64fe97f52b..84a00fe8d4 100644
--- a/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
+++ b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
@@ -18,6 +18,13 @@
 # number of searching directions
 n_direction = 16
 
+# optimization range
+min1 = 0.2775
+max1 = 0.3075
+min2 = -0.1875
+max2 = 0.0625
+plot_optimization_range = 0
+
 # Note: we need to decide which column of the task to use
 # Eg. column index 0 corresponds to stride length
 # Currently we manually specify the task names, we will save the task name when generating the data in the future.
@@ -121,6 +128,10 @@ def generate_plot(dir1, adj_index):
 
 fig, ax = plt.subplots()
 generate_plot(dir1, adjacent)
+if plot_optimization_range == 1:
+    x_line = np.array([min1, max1, max1, min1, min1])
+    y_line = np.array([min2, min2, max2, max2, min2])
+    ax.plot(x_line, y_line, 'black', linewidth=3)
 ax.set_xlabel(task_name[task_1_idx])
 ax.set_ylabel(task_name[task_2_idx])
 ax.set_title('cost landscape')

From 826c4648c00daa1c78af4bd2f884985e7fa21c20 Mon Sep 17 00:00:00 2001
From: Jianshu-Hu <jianshuhu98@gmail.com>
Date: Sun, 6 Sep 2020 13:59:00 -0400
Subject: [PATCH 82/82] add a flag so the data generated from find_boundary can
 be visualized

---
 .../find_models/visualize_gait_MBP.cc               | 13 ++++++++++---
 1 file changed, 10 insertions(+), 3 deletions(-)

diff --git a/examples/goldilocks_models/find_models/visualize_gait_MBP.cc b/examples/goldilocks_models/find_models/visualize_gait_MBP.cc
index 766d0d5b36..2de656c456 100644
--- a/examples/goldilocks_models/find_models/visualize_gait_MBP.cc
+++ b/examples/goldilocks_models/find_models/visualize_gait_MBP.cc
@@ -43,6 +43,7 @@ DEFINE_double(realtime_factor, 1, "Rate of which the traj is played back");
 DEFINE_int32(n_step, 3, "# of foot steps");
 
 DEFINE_int32(robot_option, 1, "0: plannar robot. 1: cassie_fixed_spring");
+DEFINE_int32(data_dir,0,"0: data from find_models. 1: data from find_boundary");
 
 DEFINE_bool(construct_cubic, false,
             "True if you want to construct cubic spline. Old files (before "
@@ -68,9 +69,15 @@ void visualizeGait(int argc, char* argv[]) {
   /*const string directory =
      "examples/goldilocks_models/find_models/data/robot_"
                            + to_string(FLAGS_robot_option) + "/";*/
-  const string directory =
-      "../dairlib_data/goldilocks_models/find_models/robot_" +
-      to_string(FLAGS_robot_option) + "/";
+  string folder_name;
+  if(FLAGS_data_dir==0){
+    folder_name = "find_models";
+  }
+  else if(FLAGS_data_dir==1){
+    folder_name = "find_boundary";
+  }
+  const string directory = "../dairlib_data/goldilocks_models/"+folder_name+"/robot_" +
+  to_string(FLAGS_robot_option) + "/";
 
   // Read in task name
   vector<string> task_name = ParseCsvToStringVec(directory + "task_names.csv");