diff --git a/guided_mrmp/main.py b/guided_mrmp/main.py index 544c1713f2401e9a6445a1f0fe25814d5e961e00..6c0673484ca97cd3ee7cc9e1118fdbc955dc1219 100644 --- a/guided_mrmp/main.py +++ b/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) diff --git a/settings.yaml b/settings.yaml index 07d7bb598a9d4374071b55f9778234923f54fcea..42242de9d23314d2e62b6611bb1f2fb6169c42c8 100644 --- a/settings.yaml +++ b/settings.yaml @@ -1,13 +1,13 @@ 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