diff --git a/examples/goldilocks_models/BUILD.bazel b/examples/goldilocks_models/BUILD.bazel
index f194a13a8e..5097e06c64 100644
--- a/examples/goldilocks_models/BUILD.bazel
+++ b/examples/goldilocks_models/BUILD.bazel
@@ -18,6 +18,24 @@ cc_binary(
     ],
 )
 
+cc_binary(
+    name = "find_boundary",
+    srcs = ["find_boundary.cc"],
+    deps = [
+        ":goldilocks_utils",
+        ":task",
+        "//common:eigen_utils",
+        "//common:file_utils",
+        "//examples/Cassie:cassie_utils",
+        "//examples/goldilocks_models/find_models:initial_guess",
+        "//examples/goldilocks_models/find_models:traj_opt_given_weigths",
+        "//systems/trajectory_optimization:dircon",
+        "@drake//:drake_shared_library",
+        "@gflags",
+        "//examples/goldilocks_models/find_boundary_utils:search_setting",
+    ],
+)
+
 cc_binary(
     name = "plan_with_rom_fom_five_link",
     srcs = ["plan_with_rom_fom_five_link.cc"],
diff --git a/examples/goldilocks_models/find_boundary.cc b/examples/goldilocks_models/find_boundary.cc
new file mode 100644
index 0000000000..100fe772ab
--- /dev/null
+++ b/examples/goldilocks_models/find_boundary.cc
@@ -0,0 +1,730 @@
+#include <chrono>
+#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 <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"
+#include "drake/solvers/snopt_solver.h"
+#include "drake/solvers/solve.h"
+
+#include "common/eigen_utils.h"
+#include "common/file_utils.h"
+#include "common/find_resource.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/reduced_order_models.h"
+#include "examples/goldilocks_models/task.h"
+#include "examples/goldilocks_models/find_boundary_utils/search_setting.h"
+
+using std::cin;
+using std::cout;
+using std::endl;
+using std::vector;
+using std::pair;
+using std::string;
+using std::to_string;
+using Eigen::Vector3d;
+using Eigen::VectorXd;
+using Eigen::VectorXcd;
+using Eigen::MatrixXd;
+using Eigen::MatrixXi;
+using drake::solvers::MathematicalProgram;
+using drake::solvers::MathematicalProgramResult;
+
+using drake::geometry::SceneGraph;
+using drake::multibody::MultibodyPlant;
+using drake::multibody::Body;
+using drake::multibody::Parser;
+using drake::AutoDiffXd;
+using dairlib::FindResourceOrThrow;
+
+
+namespace dairlib::goldilocks_models {
+// 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_optimality_tol, 1e-4,
+"tolerance for optimality condition (complementarity gap)");
+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
+DEFINE_int32(theta_index,-1,"# of model to use");
+
+//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");
+
+//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");
+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(
+    program_name, "",
+"The name of the program (to keep a record for future references)");
+
+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*/;
+  }
+}
+
+//read theta from files to use optimized model
+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"));
+}
+
+
+//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_task.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.02;
+    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 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)
+          + string("_0_w.csv");
+    }
+    else{
+      initial_file_name = to_string(rerun_traj_idx)
+          + 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 data from optimizing models
+      //use solutions during ROM optimization process to calculate the initial guess
+      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
+        int theta_idx = FLAGS_theta_index;
+        while(file_exist(dir_find_models + to_string(theta_idx)+ '_' +
+        to_string(sample_num)+ string("_is_success.csv"))){
+          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;
+        }
+        //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");
+        writeCSV(directory + initial_file_name, initial_guess);
+      }
+      else{
+        initial_file_name = "";
+      }
+    }else{
+      //use past solutions to calculate interpolated initial guess
+      //calculate the weighted sum of past solutions
+      for (sample_num = 0; sample_num < traj_opt_num; sample_num++) {
+        prefix = to_string(sample_num)+"_"+to_string(0);
+        InterpolateAmongDifferentTasks(directory, prefix,
+            current_gamma,gamma_scale,weight_gamma,w_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");
+      writeCSV(directory + initial_file_name, initial_guess);
+    }
+  }
+
+  return initial_file_name;
+}
+
+// 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,
+                       bool use_ipopt = false) {
+  // Create MBP
+  drake::logging::set_log_level("err");  // ignore warnings about joint limits
+  MultibodyPlant<double> plant(0.0);
+  CreateMBP(&plant, FLAGS_robot_option);
+
+  // Create autoDiff version of the plant
+  MultibodyPlant<AutoDiffXd> plant_autoDiff(plant);
+
+  // 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);
+  // Inner loop setup
+  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 = turn_on_scaling;
+  inner_loop_setting.use_ipopt = use_ipopt;
+  inner_loop_setting.directory = dir;
+
+  // 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(rom->n_theta_y());
+  VectorXd theta_yddot = VectorXd::Zero(rom->n_theta_yddot());
+  if (FLAGS_theta_index >= 0) {
+    //you have to specify the theta to use
+    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_y, theta_yddot);
+    rom->SetThetaY(theta_y);
+    rom->SetThetaYddot(theta_yddot);
+  }
+
+  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 = "";
+  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) + "_";
+
+  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;
+
+  // 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++) {
+    thread_finished_vec[i] = std::make_shared<int>(0);
+  }
+
+  bool extend_model_this_iter = false;
+  int n_rerun = 0;
+  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),
+      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,
+      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){
+  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];
+  }
+  cout<<endl;
+}
+
+//rerun unsuccessful trajectory optimization
+void RerunTrajOpt(const Task& task, const string dir, int traj_num,
+    bool use_ipopt=false){
+  int rerun = 0;
+  int max_rerun_num = 4;
+  int is_success;
+  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!=1){
+      TrajOptGivenModel(task, dir, traj_num,true,-1,true,use_ipopt);
+    }
+    else{
+      break;
+    }
+  }
+}
+// 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 = task;
+    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
+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);
+
+  //if snopt still can't find a solution, try to use adjacent sample to help
+  is_success = (readCSV(dir + to_string(traj_num) +
+      string("_0_is_success.csv")))(0, 0);
+  if(is_success!=1){
+    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{
+      TrajOptGivenModel(task, dir, traj_num,true,traj_num-1);
+    }
+  }
+  RerunTrajOpt(task,dir,traj_num);
+
+  // if snopt still failed to find a solution,turn off the scaling option
+  // and try again
+  is_success = (readCSV(dir + to_string(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);
+
+  // if snopt still failed to find a solution,try ipopt and use adjacent sample
+  // as initial guess
+  is_success = (readCSV(dir + to_string(traj_num) +
+      string("_0_is_success.csv")))(0, 0);
+  if(is_success!=1){
+    if(iteration==1)
+    {
+      //if number of iteration is 1, we should use the central point to help
+      TrajOptGivenModel(task, dir, traj_num,true,0,true,true);
+    }
+    else{
+      TrajOptGivenModel(task, dir, traj_num,true,traj_num-1,true,true);
+    }
+  }
+  RerunTrajOpt(task,dir,traj_num,true);
+
+}
+
+//search the boundary point along one direction
+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;
+  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 | 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_task = last_task+decay_factor*step;
+      last_task = new_task;
+    }
+    else{
+      new_task = last_task+step;
+      last_task = new_task;
+    }
+    //if stride length is negative or zero,stop searching
+    if(new_task[0]<=0){
+      boundary_point_idx += 1;
+      boundary_point = new_task-step;
+      SaveBoundaryPointInfor(dir,boundary_point_idx,
+          traj_num,boundary_point);
+      break;
+    }
+    //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;
+    writeCSV(dir + to_string(traj_num) +
+        string("_0_task.csv"), new_task);
+    //run trajectory optimization and judge the solution
+    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);
+    // 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(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;
+
+    // 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;
+      }
+    }
+  }
+
+  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 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(task, dir, traj_idx,
+                          true, traj_idx-1);
+        //make sure this sample success
+        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,iter+1);
+      }
+      //update cost list
+      cost_list(iter,1) = readCSV(dir + to_string(traj_idx)
+                                      + 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";
+}
+
+//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);
+
+  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;
+
+  /*
+   * cout basic program information
+   */
+  cout << "\nBasic information:\n";
+  cout << FLAGS_program_name << endl;
+  int robot_option = FLAGS_robot_option;
+  int rom_option = FLAGS_rom_option;
+  cout << "robot_option: " << robot_option << endl;
+  cout << "rom_option: " << rom_option << endl;
+
+  /*
+   * initialize task space
+   */
+  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
+  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{
+      FLAGS_search_sl ? search_elements : non_search_elements,
+      FLAGS_search_gi ? search_elements : non_search_elements,
+      FLAGS_search_v ? search_elements : non_search_elements};
+    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){
+    vector<vector<double>> elements{
+      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};
+    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;
+  for(dim = 0;dim<search_setting.task_dim();dim++)
+  {
+    cout<<"initial "<<search_setting.names()[dim]<<
+    ": "+to_string(search_setting.task_0()[dim])<<endl;
+  }
+
+  /*
+   * Iteration setting
+   */
+  cout << "\nIteration setting:\n";
+  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(get_nominal){
+      cost_threshold = 30;
+    }
+    else{
+      cost_threshold = 30;
+    }
+  }
+  else if(robot_option==1){
+    if(get_nominal){
+      cost_threshold = 30;
+    }
+    else{
+      cost_threshold = 30;
+    }
+  }
+  cout<<"cost_threshold "<<cost_threshold<<endl;
+
+  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
+   */
+  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<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);
+  cout << "\nCalculate Central Point Cost:\n";
+  cout << "sample# (rerun #) | stride | incline | velocity | turning rate | "
+          "init_file | Status | Solve time | Cost (tau cost)\n";
+  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){
+    TrajOptGivenModel(task, dir, traj_opt_num, true);
+    init_is_success = (readCSV(dir + string("0_0_is_success.csv")))(0,0);
+  }
+
+  // search the boundary
+  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;
+}
+}
+
+
+int main(int argc, char* argv[]) {
+    return dairlib::goldilocks_models::find_boundary(argc, argv);
+}
\ No newline at end of file
diff --git a/examples/goldilocks_models/find_boundary_utils/BUILD.bazel b/examples/goldilocks_models/find_boundary_utils/BUILD.bazel
new file mode 100644
index 0000000000..5aa98f3ea6
--- /dev/null
+++ b/examples/goldilocks_models/find_boundary_utils/BUILD.bazel
@@ -0,0 +1,15 @@
+package(default_visibility = ["//visibility:public"])
+
+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_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/compare_boundary.py b/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
new file mode 100644
index 0000000000..800a0c29a0
--- /dev/null
+++ b/examples/goldilocks_models/find_boundary_utils/compare_boundary.py
@@ -0,0 +1,144 @@
+"""
+This function is used for comparing two cost landscape and plot the landscape with discrete 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 matplotlib
+import numpy as np
+import os
+
+robot_option = 1
+file_dir = "../dairlib_data/goldilocks_models/find_boundary/"
+if robot_option == 1:
+    robot = 'cassie/'
+else:
+    robot = 'five_link/'
+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
+
+# 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(i, dir1, dir_nominal):
+    # 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]]
+    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]
+
+    # 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 < 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]:
+            # 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:
+            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 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]
+    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)
+        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'])
+
+
+# plt.rcParams.update({'font.size': 28})
+# fig, ax = plt.subplots(figsize=(11,5.5))
+fig, ax = plt.subplots()
+# ax.scatter(x, y)
+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])
+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.py b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
new file mode 100644
index 0000000000..84a00fe8d4
--- /dev/null
+++ b/examples/goldilocks_models/find_boundary_utils/plot_landscape.py
@@ -0,0 +1,138 @@
+"""
+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
+
+robot_option = 1
+file_dir = '../dairlib_data/goldilocks_models/find_boundary/'
+
+if robot_option == 1:
+    robot = 'cassie/'
+    dir1 = file_dir+'robot_1/'
+
+# 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.
+# 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
+
+# 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
+
+
+# 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=",")
+            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
+            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
+
+
+# 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
+    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
+    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)
+
+    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)
+    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
+
+
+# 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)
+        # process data on adjacent line
+        if len(x1) > 10:
+            x2, y2, z2 = find_adjacent_line(dir1, adj_index, i)
+            # plot
+            x = x1+x2
+            y = y1+y2
+            z = z1+z2
+            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 = 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)
+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')
+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
new file mode 100644
index 0000000000..02f7554435
--- /dev/null
+++ b/examples/goldilocks_models/find_boundary_utils/plot_normalized_landscape.py
@@ -0,0 +1,141 @@
+"""
+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
+import os
+import math
+
+robot_option = 1
+file_dir = "../dairlib_data/goldilocks_models/find_boundary/"
+if robot_option == 1:
+    robot = 'cassie/'
+else:
+    robot = 'five_link/'
+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
+
+# 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(i, dir1, dir_nominal):
+    # 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]]
+    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]
+
+    # 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):
+            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)
+        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
+
+
+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)
+        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)
+
+# continuous color map
+# plt.rcParams.update({'font.size': 28})
+# fig, ax = plt.subplots(figsize=(11,5.5))
+fig, ax = plt.subplots()
+# 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)
+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])
+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
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..9b3823670a
--- /dev/null
+++ b/examples/goldilocks_models/find_boundary_utils/poly_color_plot.py
@@ -0,0 +1,185 @@
+"""
+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 + '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
+
+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)
+    ax1.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
+
+
+def extractadjacentline(dir):
+    adj_direction = np.zeros([n_direction])
+    for i in range(n_direction):
+        min_sin = 1
+        for j in range(n_direction):
+            # 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
+                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(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]
+        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[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:
+        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(int(data_dir1[j, 0])) + '_' + str(0) + '_task.csv', delimiter=",")
+        x[j] = task[0]
+        y[j] = task[3]
+        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
+            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
+    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
+
+
+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
+        # 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))
+ax1 = fig1.gca()
+adjacent = extractadjacentline(dir1)
+print(adjacent)
+generateplot(dir1, dir2, adjacent)
+plt.show()
+
+
+
+
diff --git a/examples/goldilocks_models/find_boundary_utils/search_setting.cc b/examples/goldilocks_models/find_boundary_utils/search_setting.cc
new file mode 100644
index 0000000000..4c0855ff65
--- /dev/null
+++ b/examples/goldilocks_models/find_boundary_utils/search_setting.cc
@@ -0,0 +1,24 @@
+#include "examples/goldilocks_models/find_boundary_utils/search_setting.h"
+
+
+namespace dairlib {
+namespace goldilocks_models {
+
+SearchSetting::SearchSetting(int task_dim,
+                             std::vector <string> names,
+                             std::vector<double> task_0,
+                             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),
+      elements_(elements){
+
+  for (unsigned int i = 0; i < names.size(); i++) {
+    name_to_index_map_[names[i]] = i;
+  }
+}
+
+}
+}
\ 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
new file mode 100644
index 0000000000..e464f3abf8
--- /dev/null
+++ b/examples/goldilocks_models/find_boundary_utils/search_setting.h
@@ -0,0 +1,42 @@
+#include <iostream>
+#include <string>
+#include <vector>
+#include <unordered_map>
+#include "drake/common/drake_assert.h"
+
+using std::vector;
+using std::string;
+
+namespace dairlib {
+namespace goldilocks_models {
+
+class SearchSetting {
+ public:
+  // Default constructor
+  SearchSetting(){};
+  SearchSetting(int task_dim, vector<string> names,
+      vector<double> task_0,vector<double> task_delta,
+      vector<vector<double>> elements);
+
+  //getters
+  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_index) const {return elements_[task_index].size();}
+  double get_element(int task_index,int element_index) const {
+    return elements_[task_index][element_index];
+  }
+
+ 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<vector<double>> elements_;
+};
+
+}
+}
diff --git a/examples/goldilocks_models/find_goldilocks_models.cc b/examples/goldilocks_models/find_goldilocks_models.cc
index 6b439b9f34..94367b7c7f 100644
--- a/examples/goldilocks_models/find_goldilocks_models.cc
+++ b/examples/goldilocks_models/find_goldilocks_models.cc
@@ -1401,7 +1401,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();
diff --git a/examples/goldilocks_models/find_models/initial_guess.cc b/examples/goldilocks_models/find_models/initial_guess.cc
index 55db6b937a..fff13efc54 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;
@@ -74,8 +71,8 @@ void InterpolateAmongDifferentTasks(const string& dir, string prefix,
 VectorXd CalculateInterpolation(const VectorXd& weight_vector,
                                 const MatrixXd& solution_matrix) {
   DRAKE_DEMAND(weight_vector.rows() > 0);
-  // interpolation
-  VectorXd interpolated_solution = solution_matrix * weight_vector.normalized();
+  // normalize the weight vector by L1 norm and interpolate
+  VectorXd interpolated_solution = solution_matrix * weight_vector/weight_vector.sum();
   return interpolated_solution;
 }
 
@@ -120,15 +117,18 @@ 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;
     }
     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);
@@ -162,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)) {
+      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
@@ -211,4 +211,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
diff --git a/examples/goldilocks_models/find_models/initial_guess.h b/examples/goldilocks_models/find_models/initial_guess.h
index e701fc3f51..1f0f8d9a8f 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/find_models/initial_guess_test.cc b/examples/goldilocks_models/find_models/initial_guess_test.cc
index cf3ec84d83..b6cfad21b2 100644
--- a/examples/goldilocks_models/find_models/initial_guess_test.cc
+++ b/examples/goldilocks_models/find_models/initial_guess_test.cc
@@ -62,7 +62,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) {
@@ -94,7 +94,17 @@ 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;
@@ -126,7 +136,6 @@ int test_initial_guess(int iter, int sample, int robot) {
       writeCSV(dir + prefix + string("w.csv"), w);
     }
   }
-
   string initial_file = SetInitialGuessByInterpolation(
       dir, iter, sample, task_gen, task, *rom, use_database, robot);
   return 1;
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");
diff --git a/examples/goldilocks_models/task.h b/examples/goldilocks_models/task.h
index fe24418a09..eafb377a35 100644
--- a/examples/goldilocks_models/task.h
+++ b/examples/goldilocks_models/task.h
@@ -34,11 +34,14 @@ class Task {
       name_to_index_map_[names[i]] = i;
     }
   }
+  //Default constructor;
+  Task(){};
 
   // Getters and setters for task values
   double get(const string& name) const {
     return task_.at(name_to_index_map_.at(name));
   }
+  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_);