Add ability to use randm starts and goals

guided_mrmp/main.py

@@ -7,7 +7,7 @@ from guided_mrmp.utils import Env, Roomba, Robot, create_random_starts_and_goals
 from guided_mrmp.planners import RRT
 from guided_mrmp.planners import RRTStar
 
-from guided_mrmp.planners.multirobot.db_guided_mrmp import GuidedMRMP
+from guided_mrmp.planners.db_guided_mrmp import GuidedMRMP
 from guided_mrmp.simulator import Simulator
 
 class_names_dist = {
@@ -65,7 +65,6 @@ def initialize_robots(starts, goals, dynamics_models, radii, target_v, env):
     print("initializing robots")
 
     robots = []
-    RADIUS = 10
 
     colors = [list(np.random.choice(range(256), size=3)) for i in range(len(starts))]
 
@@ -113,8 +112,9 @@ if __name__ == "__main__":
     robot_goals = settings['robot_goals']
     robot_radii = settings['robot_radii']
     target_v = settings['target_v']
-    if robot_starts is None:
-        starts, goals = create_random_starts_and_goals(env, 4)
+    if robot_starts == []:
+        robot_starts, robot_goals = create_random_starts_and_goals(env, len(robot_radii))
+    print(f"starts = {len(robot_starts)}")
 
     robots = initialize_robots(robot_starts, robot_goals, dynamics_models, robot_radii, target_v, env)
 

settings.yaml

 single_agent_planner: "RRTStar"
 
-prediction_horizon: 1   # seconds
-discretization_step: .2 # seconds 
+prediction_horizon: 8   # seconds
+discretization_step: 1 # seconds 
 
 model_predictive_controller:
   Q: [20, 20, 20]         # state error cost for a differntial drive robot
   Qf: [30, 30, 30]        # state final error cost for a differntial drive robot
   R: [10, 10]             # input cost for a differntial drive robot
-  P: [10, 10]             # input rate of change cost for a differntial drive robot
+  P: [10, 10]             # input rate of change cost for a differential drive robot
 
 multi_robot_traj_opt:
   rob_dist_weight: 10
@@ -23,23 +23,38 @@ environment:
   x_range: [0, 640]
   y_range: [0, 480]
 
-robot_starts:
+robot_starts: 
   - [10, 10, 0]
   - [600, 300, 0]
 
-robot_goals:
+robot_goals: 
   - [600, 300, 0]
   - [10, 10, 0]
 
 robot_radii:
   - 10
   - 10
+  - 10
+  - 10
+  - 10
+  - 10
+  - 10
+  - 10
+  - 10
+  - 10
 
 target_v: 3.0
 
 dynamics_models: 
   - Roomba
   - Roomba
-
+  - Roomba
+  - Roomba
+  - Roomba
+  - Roomba
+  - Roomba
+  - Roomba
+  - Roomba
+  - Roomba