diff --git a/experiments/environments/empty.yaml b/experiments/environments/empty.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..52b7b428084ad9318b6979177bfa2824624c97b4
--- /dev/null
+++ b/experiments/environments/empty.yaml
@@ -0,0 +1,4 @@
+circle_obstacles: []
+rectangle_obstacles: []
+x_range: [0, 10]
+y_range: [0, 10]
\ No newline at end of file
diff --git a/experiments/settings_files/10robotscircle.yaml b/experiments/settings_files/10robotscircle.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..079b6ef9447435eff977b31c51501ba620893031
--- /dev/null
+++ b/experiments/settings_files/10robotscircle.yaml
@@ -0,0 +1,91 @@
+single_agent_planner: "RRTStar"
+
+prediction_horizon: 2 # seconds
+discretization_step: .2 # seconds
+
+model_predictive_controller:
+ Q: [45, 45, 0] # state error cost for a differential drive robot
+ Qf: [25, 25, 0] # state final error cost for a differential drive robot
+ R: [10, 10] # input cost for a differential drive robot
+ P: [10, 10] # input rate of change cost for a differential drive robot
+ robot_robot_collision_weight: .5
+ obstacle_collision_weight: 0
+ print_level: 0
+ print_time: 0
+ acceptable_tol: 1.0e-4
+ acceptable_iter: 100
+ parallelize: 0
+
+multi_robot_traj_opt:
+ rob_dist_weight: 50
+ obstacle_weight: 10
+ time_weight: 5
+ control_weight: 1
+ goal_weight: 5
+
+simulator:
+ dt: .1
+ show_plots: 1
+ show_collision_resolution: 1
+
+robot_starts:
+ - [9.5, 5.0, 0]
+ - [8.64, 7.65, 0]
+ - [6.39, 9.28, 0]
+ - [3.61, 9.28, 0]
+ - [1.36, 7.65, 0]
+ - [0.5, 5.0, 0]
+ - [1.36, 2.35, 0]
+ - [3.61, 0.72, 0]
+ - [6.39, 0.72, 0]
+ - [8.64, 2.35, 0]
+
+robot_goals:
+ - [8.64, 2.35, 0]
+ - [6.39, 0.72, 0]
+ - [3.61, 0.72, 0]
+ - [1.36, 2.35, 0]
+ - [0.5, 5.0, 0]
+ - [1.36, 7.65, 0]
+ - [3.61, 9.28, 0]
+ - [6.39, 9.28, 0]
+ - [8.64, 7.65, 0]
+ - [9.5, 5.0, 0]
+
+
+robot_radii:
+ - .5
+ - .5
+ - .5
+ - .5
+ - .5
+ - .5
+ - .5
+ - .5
+ - .5
+ - .5
+
+Roomba:
+ max_speed: 50.0
+ max_acc: 30.0
+ max_d_acc: 10.0
+ max_steer: 360
+ max_d_steer: 180
+ radius: .5
+
+target_v: 1.0
+
+dynamics_models:
+ - Roomba
+ - Roomba
+ - Roomba
+ - Roomba
+ - Roomba
+ - Roomba
+ - Roomba
+ - Roomba
+ - Roomba
+ - Roomba
+
+
+
diff --git a/experiments/settings_files/4robotscircle.yaml b/experiments/settings_files/4robotscircle.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..920d3d21fb7ca63389ae46c21e9838cb8c32d49b
--- /dev/null
+++ b/experiments/settings_files/4robotscircle.yaml
@@ -0,0 +1,66 @@
+single_agent_planner: "RRTStar"
+
+prediction_horizon: 2 # seconds
+discretization_step: .2 # seconds
+
+model_predictive_controller:
+ Q: [45, 45, 0] # state error cost for a differential drive robot
+ Qf: [25, 25, 0] # state final error cost for a differential drive robot
+ R: [10, 10] # input cost for a differential drive robot
+ P: [10, 10] # input rate of change cost for a differential drive robot
+ robot_robot_collision_weight: .5
+ obstacle_collision_weight: 0
+ print_level: 0
+ print_time: 0
+ acceptable_tol: 1.0e-4
+ acceptable_iter: 100
+ parallelize: 0
+
+multi_robot_traj_opt:
+ rob_dist_weight: 50
+ obstacle_weight: 10
+ time_weight: 5
+ control_weight: 1
+ goal_weight: 5
+
+simulator:
+ dt: .1
+ show_plots: 1
+ show_collision_resolution: 1
+
+robot_starts:
+ - [9.5, 5.0, 0]
+ - [5.0, 9.5, 0]
+ - [0.5, 5.0, 0]
+ - [5.0, 0.5, 0]
+
+robot_goals:
+ - [5.0, 0.5, 0]
+ - [0.5, 5.0, 0]
+ - [5.0, 9.5, 0]
+ - [9.5, 5.0, 0]
+
+robot_radii:
+ - .5
+ - .5
+ - .5
+ - .5
+
+Roomba:
+ max_speed: 50.0
+ max_acc: 30.0
+ max_d_acc: 10.0
+ max_steer: 360
+ max_d_steer: 180
+ radius: .5
+
+target_v: 1.0
+
+dynamics_models:
+ - Roomba
+ - Roomba
+ - Roomba
+ - Roomba
+
+
+
diff --git a/experiments/settings_files/8robotscircle.yaml b/experiments/settings_files/8robotscircle.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6e357a9588e550c1f8a2f8a3886f3476f5902258
--- /dev/null
+++ b/experiments/settings_files/8robotscircle.yaml
@@ -0,0 +1,82 @@
+single_agent_planner: "RRTStar"
+
+prediction_horizon: 2 # seconds
+discretization_step: .2 # seconds
+
+model_predictive_controller:
+ Q: [45, 45, 0] # state error cost for a differential drive robot
+ Qf: [25, 25, 0] # state final error cost for a differential drive robot
+ R: [10, 10] # input cost for a differential drive robot
+ P: [10, 10] # input rate of change cost for a differential drive robot
+ robot_robot_collision_weight: .5
+ obstacle_collision_weight: 0
+ print_level: 0
+ print_time: 0
+ acceptable_tol: 1.0e-4
+ acceptable_iter: 100
+ parallelize: 0
+
+multi_robot_traj_opt:
+ rob_dist_weight: 50
+ obstacle_weight: 10
+ time_weight: 5
+ control_weight: 1
+ goal_weight: 5
+
+simulator:
+ dt: .1
+ show_plots: 1
+ show_collision_resolution: 1
+
+robot_starts:
+ - [9.5, 5.0, 0]
+ - [8.18, 8.18, 0]
+ - [5.0, 9.5, 0]
+ - [1.82, 8.18, 0]
+ - [0.5, 5.0, 0]
+ - [1.82, 1.82, 0]
+ - [5.0, 0.5, 0]
+ - [8.18, 1.82, 0]
+
+robot_goals:
+ - [8.18, 1.82, 0]
+ - [5.0, 0.5, 0]
+ - [1.82, 1.82, 0]
+ - [0.5, 5.0, 0]
+ - [1.82, 8.18, 0]
+ - [5.0, 9.5, 0]
+ - [8.18, 8.18, 0]
+ - [9.5, 5.0, 0]
+
+robot_radii:
+ - .5
+ - .5
+ - .5
+ - .5
+ - .5
+ - .5
+ - .5
+ - .5
+
+Roomba:
+ max_speed: 50.0
+ max_acc: 30.0
+ max_d_acc: 10.0
+ max_steer: 360
+ max_d_steer: 180
+ radius: .5
+
+target_v: 1.0
+
+dynamics_models:
+ - Roomba
+ - Roomba
+ - Roomba
+ - Roomba
+ - Roomba
+ - Roomba
+ - Roomba
+ - Roomba
+
+
+
diff --git a/guided_mrmp/controllers/multi_path_tracking_db.py b/guided_mrmp/controllers/multi_path_tracking_db.py
index 59ac3f746a846ef5be7a2f7dcccd5282c3715ae1..6c71670020ef172483c1fe4d5c6552d56f6d9bd8 100644
--- a/guided_mrmp/controllers/multi_path_tracking_db.py
+++ b/guided_mrmp/controllers/multi_path_tracking_db.py
@@ -463,7 +463,7 @@ class MultiPathTrackerDB(MultiPathTracker):
self.target_v,
self.T,
self.DT,
- self.visited_points_on_guide_paths[i])
+ [])
self.visited_points_on_guide_paths[i] = visited_guide_points