From c3fae6d53197d0cb5b2f339102e6eb3610940834 Mon Sep 17 00:00:00 2001
From: rachelmoan <moanrachel516@gmail.com>
Date: Mon, 9 Sep 2024 21:04:57 -0500
Subject: [PATCH] Add ability to use randm starts and goals
---
guided_mrmp/main.py | 8 ++++----
settings.yaml | 27 +++++++++++++++++++++------
2 files changed, 25 insertions(+), 10 deletions(-)
diff --git a/guided_mrmp/main.py b/guided_mrmp/main.py
index 544c171..6c06734 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 07d7bb5..42242de 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
--
GitLab