Merge branch 'updated-place-grid' of...

Merge branch 'updated-place-grid' of gitlab.engr.illinois.edu:rmoan2/db-guided-mrmp into updated-place-grid

experiments/environments/empty.yaml

+circle_obstacles: []
+rectangle_obstacles: []
+x_range: [0, 10]
+y_range: [0, 10]
\ No newline at end of file

experiments/settings_files/10robotscircle.yaml

+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
+  
+
+

experiments/settings_files/4robotscircle.yaml

+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
+  
+
+

experiments/settings_files/8robotscircle.yaml

+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
+  
+
+

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