Rezas expert

expert/NavigationExpert.py

@@ -34,10 +34,10 @@ def load_map(map_file, device):
     for i, row_element in enumerate(row_elements):
         row_data = row_element.text.split()
         for j, value in enumerate(row_data):
-            grid[i, j] = int(value)
-    return grid
+            grid[height - 1 - i, j] = int(value)
+    return grid, width, height
 
-def load_episodes(log_dir, device):
+def load_episodes(log_dir, device, width, height):
     f = os.path.dirname(__file__)
     episodes = {}
     for file in os.listdir(f'{f}/{log_dir}/'):
@@ -49,7 +49,7 @@ def load_episodes(log_dir, device):
         episode_name = int(file[:-4].split('_')[-2])
 
         agent_paths = {}
-        log = ET.parse(f'{f}/logs/{file}')
+        log = ET.parse(f'{f}/{log_dir}/{file}')
         log_root = log.getroot()
         agents = log_root.findall(".//agent[@number]")
         for agent in agents:
@@ -60,13 +60,13 @@ def load_episodes(log_dir, device):
             first = True
             for waypoint in sections:
                 if first:
-                    x = int(waypoint.get('start_i'))
-                    y = int(waypoint.get('start_j'))
+                    y = height - 1 - int(waypoint.get('start_i')) #i is row, j is column...
+                    x = int(waypoint.get('start_j'))
                     duration = 0.0
                     waypoints.append(torch.tensor([duration, x, y], device=device))
                     first=False
-                x = int(waypoint.get("goal_i"))
-                y = int(waypoint.get('goal_j'))
+                y = height - 1 - int(waypoint.get("goal_i"))
+                x = int(waypoint.get('goal_j'))
                 duration = float(waypoint.get('duration')) + waypoints[-1][0]
                 waypoints.append(torch.tensor([duration, x, y], device=device))
             agent_paths[number] = waypoints
@@ -142,11 +142,11 @@ def main(args):
     config_root = config.getroot()
     agent_size = float(config_root.find('.//agent_size').text)
 
+    #load map. Currently assumes every task takes place on the same map
+    grid_map, width, height = load_map(args.map, device)
+
     #Load in all of the pre generated episodes
-    episodes = load_episodes(args.log_dir, device=device)
-
-    #load map
-    grid_map = load_map(args.map, device)
+    episodes = load_episodes(args.log_dir, device, width, height)
 
     scenario_name = 'navigation2' #Scenario name
 
@@ -155,7 +155,7 @@ def main(args):
 
     num_envs = args.num_envs  # Number of vectorized environments
     continuous_actions = True
-    n_steps = 500 # Number of steps before returning done
+    n_steps = args.max_steps # Number of steps before returning done
     dict_spaces = True  # Weather to return obs, rewards, and infos as dictionaries with agent names (by default they are lists of len # of agents)
 
     start_action = (
@@ -225,12 +225,13 @@ def main(args):
 
 
 def create_parser():
-    parser = argparse.ArgumentParser(description='Generate a bunch of random scenarios based on the given map')
+    parser = argparse.ArgumentParser(description='Use the expert to solve the navigation2 scenario')
     parser.add_argument('--map', '-m', default='grid_map.xml', type=str, help='The map to generate tasks for. Only supports grid type maps')
     parser.add_argument('--config', '-c', default='config.xml', help='The config file for the task')
     parser.add_argument('--log_dir', '-l', default='logs', help='local path to logs directory')
     parser.add_argument('--device', '-d', default = 'cpu', help='device to run on')
     parser.add_argument('--num_envs', type=int, default = 10, help='number of environments to run at once')
+    parser.add_argument('--max_steps', type=int, default = 500, help='Max number of steps to run for')
     return parser
 
 if __name__ == "__main__":

expert/grid_test.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0" ?>
+<root>
+    <map>
+        <width>10</width>
+        <height>10</height>
+        <grid>
+			<row>0 1 0 0 0 0 0 0 0 0</row>
+			<row>0 1 0 0 0 0 0 0 0 0</row>
+			<row>0 1 0 0 0 0 0 0 0 0</row>
+			<row>0 1 0 0 0 0 0 0 0 0</row>
+			<row>0 1 0 0 0 0 0 0 0 0</row>
+			<row>0 1 0 0 0 0 0 0 0 0</row>
+			<row>0 1 0 0 0 0 0 0 0 0</row>
+			<row>0 1 0 0 0 0 0 0 0 0</row>
+			<row>0 1 0 0 0 0 0 0 1 1</row>
+			<row>0 0 0 0 0 0 0 0 0 0</row>
+		</grid>
+    </map>
+</root>

expert/readme.md

@@ -0,0 +1,17 @@
+## Using the expert
+
+This program will load in the task logs in `log_dir` and use them to generate actions in the `navigation2` task.
+
+Information about the arguments can be found by running `python3 NavigationExpert.py -h`
+
+The grid_map.xml, config.xml, and task log files should all be taken from the CCBS fork. More information about these can be found there.
+
+Make sure that the task log files are all named in the following manner: 
+
+`<anything you want>_<a unique number>_log.xml`
+
+Current assumptions: 
+- Every file in `log_dir` that ends in `.xml` should be used
+- All tasks use the same map
+- The agents should definitely not collide. This leads to the agents driving relatively slowly. 
+  - The `select_action` method can be modified to fix this but CCBS assumes all agents move at the same speed which is a big limiting factor. db-CBS may be able to help

expert/test_log/grid_task_test_1_log.xml

@@ -0,0 +1,47 @@
+<?xml version="1.0" ?>
+<root>
+    <agent start_i="0" start_j="0" goal_i="0" goal_j="2"/>
+    <agent start_i="9" start_j="9" goal_i="0" goal_j="9"/>
+    <log>
+        <summary time="6.4941e-05" flowtime="31.828427124746192" makespan="20"/>
+        <agent number="0">
+            <path duration="20">
+                <section number="0" start_i="0" start_j="0" goal_i="1" goal_j="0" duration="1"/>
+                <section number="1" start_i="1" start_j="0" goal_i="2" goal_j="0" duration="1"/>
+                <section number="2" start_i="2" start_j="0" goal_i="3" goal_j="0" duration="1"/>
+                <section number="3" start_i="3" start_j="0" goal_i="4" goal_j="0" duration="1"/>
+                <section number="4" start_i="4" start_j="0" goal_i="5" goal_j="0" duration="1"/>
+                <section number="5" start_i="5" start_j="0" goal_i="6" goal_j="0" duration="1"/>
+                <section number="6" start_i="6" start_j="0" goal_i="7" goal_j="0" duration="1"/>
+                <section number="7" start_i="7" start_j="0" goal_i="8" goal_j="0" duration="1"/>
+                <section number="8" start_i="8" start_j="0" goal_i="9" goal_j="0" duration="1"/>
+                <section number="9" start_i="9" start_j="0" goal_i="9" goal_j="1" duration="1"/>
+                <section number="10" start_i="9" start_j="1" goal_i="9" goal_j="2" duration="1"/>
+                <section number="11" start_i="9" start_j="2" goal_i="8" goal_j="2" duration="1"/>
+                <section number="12" start_i="8" start_j="2" goal_i="7" goal_j="2" duration="1"/>
+                <section number="13" start_i="7" start_j="2" goal_i="6" goal_j="2" duration="1"/>
+                <section number="14" start_i="6" start_j="2" goal_i="5" goal_j="2" duration="1"/>
+                <section number="15" start_i="5" start_j="2" goal_i="4" goal_j="2" duration="1"/>
+                <section number="16" start_i="4" start_j="2" goal_i="3" goal_j="2" duration="1"/>
+                <section number="17" start_i="3" start_j="2" goal_i="2" goal_j="2" duration="1"/>
+                <section number="18" start_i="2" start_j="2" goal_i="1" goal_j="2" duration="1"/>
+                <section number="19" start_i="1" start_j="2" goal_i="0" goal_j="2" duration="1"/>
+            </path>
+        </agent>
+        <agent number="1">
+            <path duration="11.82842712474619">
+                <section number="0" start_i="9" start_j="9" goal_i="9" goal_j="8" duration="1"/>
+                <section number="1" start_i="9" start_j="8" goal_i="9" goal_j="7" duration="1"/>
+                <section number="2" start_i="9" start_j="7" goal_i="8" goal_j="7" duration="1"/>
+                <section number="3" start_i="8" start_j="7" goal_i="7" goal_j="7" duration="1"/>
+                <section number="4" start_i="7" start_j="7" goal_i="6" goal_j="8" duration="1.4142135623730949"/>
+                <section number="5" start_i="6" start_j="8" goal_i="5" goal_j="9" duration="1.4142135623730949"/>
+                <section number="6" start_i="5" start_j="9" goal_i="4" goal_j="9" duration="1"/>
+                <section number="7" start_i="4" start_j="9" goal_i="3" goal_j="9" duration="1"/>
+                <section number="8" start_i="3" start_j="9" goal_i="2" goal_j="9" duration="1"/>
+                <section number="9" start_i="2" start_j="9" goal_i="1" goal_j="9" duration="1"/>
+                <section number="10" start_i="1" start_j="9" goal_i="0" goal_j="9" duration="1"/>
+            </path>
+        </agent>
+    </log>
+</root>

vmas/scenarios/navigation2.py

@@ -14,7 +14,7 @@
 import torch
 from torch import Tensor
 from vmas import render_interactively
-from vmas.simulator.core import Agent, Landmark, World, Sphere, Entity
+from vmas.simulator.core import Agent, Box, Landmark, World, Sphere, Entity
 from vmas.simulator.scenario import BaseScenario
 from vmas.simulator.sensors import Lidar
 from vmas.simulator.utils import Color, ScenarioUtils
@@ -45,10 +45,7 @@ def make_world(self, batch_dim: int, device: torch.device, **kwargs):
 
         self.episodes = kwargs.get('episodes')
         self.map = kwargs.get('map')
-
-        #TODO delete next two lines
-        self.min_distance_between_entities = self.agent_radius * 2 + 0.05
-        self.world_semidim = 1
+        self.first = True
 
         self.min_collision_distance = 0.005
 
@@ -113,6 +110,16 @@ def make_world(self, batch_dim: int, device: torch.device, **kwargs):
             world.add_landmark(goal)
             agent.goal = goal
 
+        #Add in the map obstacles
+        indices = torch.nonzero(self.map == 1)
+        for coord in indices:
+            obstacle = Landmark(
+                name = f"Obstacle at {coord}",
+                collide = True,
+                shape = Box(1,1)
+            )
+            world.add_landmark(obstacle)            
+
         self.pos_rew = torch.zeros(batch_dim, device=device)
         self.final_rew = self.pos_rew.clone()
 
@@ -121,6 +128,21 @@ def make_world(self, batch_dim: int, device: torch.device, **kwargs):
     def reset_world_at(self, env_index: int = None):
         episode_name, episode_agents = random.choice(list(self.episodes.items()))
 
+        if self.first:
+            #TODO: is there a way to do this in the constructor since it only needs to happen once?
+            counter = 0
+            indices = torch.nonzero(self.map == 1)
+            for i,coord in enumerate(indices):
+                self.world.landmarks[i+self.n_agents].set_pos(
+                    torch.tensor(
+                        [coord[1], coord[0]],
+                        dtype=torch.float32,
+                        device=self.world.device
+                    ),
+                    batch_index = None
+                )
+            self.first=False
+
         for i, agent in enumerate(self.world.agents):
             agent.set_pos(
                 torch.tensor(