Merge remote-tracking branch 'origin/main'

overcooked_simulator/counters.py

@@ -116,12 +116,20 @@ class Counter:
         """What is on top of the counter, e.g., `Item`s."""
         self.hook = hook
         """Reference to the hook manager."""
+        self.orientation: npt.NDArray[float] = np.array([0, 1], dtype=float)
+        """In what direction the counter is facing."""
 
     @property
     def occupied(self) -> bool:
         """Is something on top of the counter."""
         return self.occupied_by is not None
 
+    def set_orientation(self, orientation: npt.NDArray[float]) -> None:
+        if not np.isclose(np.linalg.norm(orientation), 1):
+            self.orientation = orientation / np.linalg.norm(orientation)
+        else:
+            self.orientation = orientation
+
     def pick_up(self, on_hands: bool = True) -> Item | None:
         """Gets called upon a player performing the pickup action. If the counter can give something to
         the player, it does so. In the standard counter this is when an item is on the counter.
@@ -210,6 +218,7 @@ class Counter:
             "category": COUNTER_CATEGORY,
             "type": self.__class__.__name__,
             "pos": self.pos.tolist(),
+            "orientation": self.orientation.tolist(),
             "occupied_by": None
             if self.occupied_by is None
             else (

overcooked_simulator/game_content/layouts/rot_test.layout

+##S+#
+S___#
++___S
+#___+
+#+SP#
\ No newline at end of file

overcooked_simulator/gui_2d_vis/drawing.py

 import argparse
 import colorsys
 import json
-import math
 from datetime import datetime, timedelta
 from pathlib import Path
 
@@ -24,6 +23,12 @@ SHOW_INTERACTION_RANGE = False
 SHOW_COUNTER_CENTERS = False
 
 
+def calc_angle(vec_a: list[float], vec_b: list[float]) -> float:
+    a = pygame.math.Vector2(vec_a)
+    b = pygame.math.Vector2(vec_b)
+    return a.angle_to(b)
+
+
 def create_polygon(n, length):
     if n == 1:
         return np.array([0, 0])
@@ -44,12 +49,26 @@ def create_polygon(n, length):
 
 
 class Visualizer:
+    """Class for visualizing the game state retrieved from the gameserver.
+    2D game screen is drawn with pygame shapes and images.
+
+    Args:
+        config: Visualization configuration (loaded from yaml file) given as a dict.
+
+    """
+
     def __init__(self, config):
         self.image_cache_dict = {}
         self.player_colors = []
         self.config = config
 
     def create_player_colors(self, n) -> None:
+        """Create different colors for the players. The color hues are sampled uniformly in HSV-Space,
+        then the corresponding colors from the defined colors list are looked up.
+
+        Args:
+            n: Number of players to create colors for.
+        """
         hue_values = np.linspace(0, 1, n + 1)
 
         colors_vec = np.array([col for col in colors.values()])
@@ -67,10 +86,18 @@ class Visualizer:
 
     def draw_gamescreen(
         self,
-        screen,
-        state,
-        grid_size,
+        screen: pygame.Surface,
+        state: dict,
+        grid_size: int,
     ):
+        """Draws the game state on the given surface.
+
+        Args:
+            screen: The pygame surface to draw the game on.
+            state: The gamestate retrieved from the environment.
+            grid_size: The gridsize to base every object size in the game on.
+        """
+
         width = int(np.ceil(state["kitchen"]["width"] * grid_size))
         height = int(np.ceil(state["kitchen"]["height"] * grid_size))
         self.draw_background(
@@ -91,8 +118,18 @@ class Visualizer:
             grid_size,
         )
 
-    def draw_background(self, surface, width, height, grid_size):
-        """Visualizes a game background."""
+    def draw_background(
+        self, surface: pygame.Surface, width: int, height: int, grid_size: int
+    ):
+        """Visualizes a game background.
+
+        Args:
+            surface: The pygame surface to draw the background on.
+            width: The kitchen width.
+            height: The kitchen height.
+            grid_size: The gridsize to base the background shapes on.
+        """
+
         block_size = grid_size // 2  # Set the size of the grid block
         surface.fill(colors[self.config["Kitchen"]["ground_tiles_color"]])
         for x in range(0, width, block_size):
@@ -113,6 +150,15 @@ class Visualizer:
         pos: npt.NDArray,
         rot_angle=0,
     ):
+        """Draws an image on the given screen.
+
+        Args:
+            screen: The pygame surface to draw the image on.
+            img_path: The path to the image file, given relative to the gui_2d_vis directory.
+            size: The size of the image, given in pixels.
+            pos: The position of the center of the image, given in pixels.
+            rot_angle: Optional angle to rotate the image around.
+        """
         cache_entry = f"{img_path}"
         if cache_entry in self.image_cache_dict.keys():
             image = self.image_cache_dict[cache_entry]
@@ -138,14 +184,18 @@ class Visualizer:
     ):
         """Visualizes the players as circles with a triangle for the facing direction.
         If the player holds something in their hands, it is displayed
-        Args:            state: The game state returned by the environment.
+
+        Args:
+            screen: The pygame surface to draw the players on.
+            players: The state of the players returned by the environment.
+            grid_size: The gridsize to rescale the drawn players to.
         """
         for p_idx, player_dict in enumerate(players):
             player_dict: PlayerState
             pos = np.array(player_dict["pos"]) * grid_size
             pos += grid_size / 2  # correct for grid offset
 
-            facing = np.array(player_dict["facing_direction"])
+            facing = np.array(player_dict["facing_direction"], dtype=float)
 
             if USE_PLAYER_COOK_SPRITES:
                 pygame.draw.circle(
@@ -156,8 +206,7 @@ class Visualizer:
                 )
 
                 img_path = self.config["Cook"]["parts"][0]["path"]
-                rel_x, rel_y = facing
-                angle = -np.rad2deg(math.atan2(rel_y, rel_x)) + 90
+                angle = calc_angle(facing.tolist(), [0, 1])
                 size = self.config["Cook"]["parts"][0]["size"] * grid_size
                 self.draw_image(screen, img_path, size, pos, angle)
 
@@ -230,6 +279,7 @@ class Visualizer:
         grid_size: float,
         parts: list[dict[str]],
         scale: float = 1.0,
+        orientation: list[float] | None = None,
     ):
         """Draws an item, based on its visual parts specified in the visualization config.
 
@@ -239,23 +289,41 @@ class Visualizer:
             pos: Where to draw the item parts.
             parts: The visual parts to draw.
             scale: Rescale the item by this factor.
+            orientation: Rotate the item to face this direction.
         """
         for part in parts:
             part_type = part["type"]
+            angle, angle_offset = 0, 0
 
             draw_pos = pos.copy()
-            if "center_offset" in part:
-                draw_pos += np.array(part["center_offset"]) * grid_size
+
+            if orientation is not None:
+                angle_offset = calc_angle(orientation, [0, 1])
+                if "rotate_image" in part.keys():
+                    if part["rotate_image"]:
+                        angle = calc_angle(orientation, [0, 1])
+                else:
+                    angle = angle_offset
 
             match part_type:
                 case "image":
+                    if "center_offset" in part:
+                        d = pygame.math.Vector2(part["center_offset"]) * grid_size
+                        d.rotate_ip(angle_offset)
+                        draw_pos += np.array(d)
                     self.draw_image(
                         screen,
                         part["path"],
                         part["size"] * scale * grid_size,
                         draw_pos,
+                        rot_angle=angle,
                     )
+
                 case "rect":
+                    if "center_offset" in part:
+                        d = pygame.math.Vector2(part["center_offset"]) * grid_size
+                        d.rotate_ip(angle_offset)
+                        draw_pos += np.array(d)
                     height = part["height"] * grid_size
                     width = part["width"] * grid_size
                     color = part["color"]
@@ -266,9 +334,15 @@ class Visualizer:
                         width,
                     )
                     pygame.draw.rect(screen, color, rect)
+
                 case "circle":
+                    if "center_offset" in part:
+                        d = pygame.math.Vector2(part["center_offset"]) * grid_size
+                        d.rotate_ip(-angle_offset)
+                        draw_pos += np.array(d)
                     radius = part["radius"] * grid_size
                     color = colors[part["color"]]
+
                     pygame.draw.circle(screen, color, draw_pos, radius)
 
     def draw_item(
@@ -305,7 +379,7 @@ class Visualizer:
                     screen=screen,
                     grid_size=grid_size,
                 )
-                #
+
         if "progress_percentage" in item and item["progress_percentage"] > 0.0:
             self.draw_progress_bar(
                 screen, pos, item["progress_percentage"], grid_size=grid_size
@@ -342,7 +416,14 @@ class Visualizer:
         percent: float,
         grid_size: float,
     ):
-        """Visualize progress of progressing item as a green bar under the item."""
+        """Visualize progress of progressing item as a green bar under the item.
+
+        Args:
+            screen: The pygame surface to draw the progress bar on.
+            pos: The center position of a tile to draw the progress bar under.
+            percent: Progressed percent of the progress bar.
+            grid_size: Scaling of the progress bar given in pixels.
+        """
         bar_pos = pos - (grid_size / 2)
 
         bar_height = grid_size * 0.2
@@ -361,16 +442,31 @@ class Visualizer:
         """Visualization of a counter at its position. If it is occupied by an item, it is also shown.
         The visual composition of the counter is read in from visualization.yaml file, where it is specified as
         different parts to be drawn.
-        Args:            counter: The counter to visualize.
+        Args:
+            screen: The pygame surface to draw the counter on.
+            counter_dict: The counter to visualize, given as a dict from the game state.
+            grid_size: Scaling of the counter given in pixels.
         """
         pos = np.array(counter_dict["pos"], dtype=float) * grid_size
         counter_type = counter_dict["type"]
 
         pos += grid_size // 2  # correct for grid offset
 
-        self.draw_thing(screen, pos, grid_size, self.config["Counter"]["parts"])
+        self.draw_thing(
+            screen,
+            pos,
+            grid_size,
+            self.config["Counter"]["parts"],
+            orientation=counter_dict["orientation"],
+        )
         if counter_type in self.config:
-            self.draw_thing(screen, pos, grid_size, self.config[counter_type]["parts"])
+            self.draw_thing(
+                screen,
+                pos,
+                grid_size,
+                self.config[counter_type]["parts"],
+                orientation=counter_dict["orientation"],
+            )
         else:
             if counter_type in self.config:
                 parts = self.config[counter_type]["parts"]
@@ -383,6 +479,7 @@ class Visualizer:
                 pos=pos,
                 parts=parts,
                 grid_size=grid_size,
+                orientation=counter_dict["orientation"],
             )
 
     def draw_counter_occupier(
@@ -391,7 +488,16 @@ class Visualizer:
         occupied_by: dict | list,
         grid_size,
         pos: npt.NDArray[float],
+        item_scale: float,
     ):
+        """Visualization of a thing lying on a counter.
+        Args:
+            screen: The pygame surface to draw the item on the counter on.
+            occupied_by: The thing that occupies the counter.
+            grid_size: Scaling of the object given in pixels.
+            pos: The position of the counter which the thing lies on.
+            item_scale: Relative scaling of the item.
+        """
         # Multiple plates on plate return:
         if isinstance(occupied_by, list):
             for i, o in enumerate(occupied_by):
@@ -400,6 +506,7 @@ class Visualizer:
                     pos=np.abs([pos[0], pos[1] - (i * 3)]),
                     grid_size=grid_size,
                     item=o,
+                    scale=item_scale,
                 )
         # All other items:
         else:
@@ -408,35 +515,93 @@ class Visualizer:
                 grid_size=grid_size,
                 item=occupied_by,
                 screen=screen,
+                scale=item_scale,
             )
 
-    def draw_counters(self, screen: pygame, counters, grid_size):
+    def draw_counters(self, screen: pygame, counters: dict, grid_size: int):
         """Visualizes the counters in the environment.
 
-        Args:            state: The game state returned by the environment.
+        Args:
+            screen: The pygame surface to draw the counters on.
+            counters: The counter state returned by the environment.
+            grid_size: Scaling of the object given in pixels.
         """
         for counter in counters:
             self.draw_counter(screen, counter, grid_size)
 
         for counter in counters:
             if counter["occupied_by"]:
+                item_pos = np.array(counter["pos"])
+                item_scale = 1.0
+
+                counter_type = counter["type"]
+
+                if counter_type.endswith("Dispenser") and "Plate" not in counter_type:
+                    if "item_offset" in self.config["Dispenser"].keys():
+                        offset_vec = pygame.math.Vector2(
+                            self.config["Dispenser"]["item_offset"]
+                        )
+                        offset_vec.rotate_ip(
+                            offset_vec.angle_to(
+                                pygame.math.Vector2(counter["orientation"])
+                            )
+                            + 180
+                        )
+                        item_pos += offset_vec
+                    if "item_scale" in self.config["Dispenser"].keys():
+                        item_scale = self.config["Dispenser"]["item_scale"]
+
                 self.draw_counter_occupier(
-                    screen,
-                    counter["occupied_by"],
-                    grid_size,
-                    np.array(counter["pos"]) * grid_size + (grid_size / 2),
+                    screen=screen,
+                    occupied_by=counter["occupied_by"],
+                    grid_size=grid_size,
+                    pos=item_pos * grid_size + (grid_size / 2),
+                    item_scale=item_scale,
                 )
+
             if SHOW_COUNTER_CENTERS:
-                pygame.draw.circle(
+                pos = np.array(counter["pos"]) * grid_size
+                pygame.draw.circle(screen, colors["green1"], pos, 3)
+                pygame.draw.circle(screen, colors["green1"], pos, 3)
+                facing = np.array(counter["orientation"])
+                pygame.draw.polygon(
                     screen,
-                    colors["green1"],
-                    np.array(counter["pos"]) * grid_size + (grid_size / 2),
-                    3,
+                    colors["red"],
+                    (
+                        (
+                            pos[0] + (facing[1] * 0.1 * grid_size),
+                            pos[1] - (facing[0] * 0.1 * grid_size),
+                        ),
+                        (
+                            pos[0] - (facing[1] * 0.1 * grid_size),
+                            pos[1] + (facing[0] * 0.1 * grid_size),
+                        ),
+                        pos + (facing * 0.5 * grid_size),
+                    ),
                 )
 
     def draw_orders(
-        self, screen, state, grid_size, width, height, screen_margin, config
+        self,
+        screen: pygame.surface,
+        state: dict,
+        grid_size: int,
+        width: int,
+        height: int,
+        screen_margin: int,
+        config: dict,
     ):
+        """Visualization of the current orders.
+
+        Args:
+            screen: pygame surface to draw the orders on, probably not the game screen itself.
+            state: The game state returned by the environment.
+            grid_size: Scaling of the drawn orders, given in pixels.
+            width: Width of the pygame window
+            height: Height of the pygame window.
+            screen_margin: Size of the space around the game screen, for buttons, ... .
+            config: Visualization configuration (loaded from yaml file) given as a dict.
+
+        """
         orders_width = width - 100
         orders_height = screen_margin
         order_screen = pygame.Surface(
@@ -503,6 +668,14 @@ class Visualizer:
     def save_state_image(
         self, grid_size: int, state: dict, filename: str | Path
     ) -> None:
+        """Saves a screenshot of the visualization of the given state.
+
+        Args:
+            grid_size: Scaling of the world elements given in pixels.
+            state: Game state returned by the environment.
+            filename: Filename to save the screenshot to.
+
+        """
         width = int(np.ceil(state["kitchen"]["width"] * grid_size))
         height = int(np.ceil(state["kitchen"]["height"] * grid_size))
 
@@ -514,6 +687,15 @@ class Visualizer:
 
 
 def save_screenshot(state: dict, config: dict, filename: str | Path) -> None:
+    """Standalone function to save a screenshot. Creates a visualizer from the config and visualizes
+    the game state, saves it to the given filename.
+
+    Args:
+        state: The gamestate to visualize.
+        config: Visualization config for the visualizer.
+        filename: Filename to save the image to.
+
+    """
     viz = Visualizer(config)
     viz.create_player_colors(len(state["players"]))
     pygame.init()

overcooked_simulator/gui_2d_vis/visualization.yaml

@@ -22,84 +22,97 @@ Kitchen:
 
 Counter:
   parts:
-    - type: rect
-      height: 1
-      width: 1
-      color: whitesmoke
+    #    - type: rect
+    #      height: 1
+    #      width: 1
+    #      color: whitesmoke
+    - type: image
+      path: images/counter5.png
+      size: 1
 
 CuttingBoard:
   parts:
     - type: image
       path: images/cutting_board_large.png
-      size: 0.9
+      size: 0.75
+      center_offset: [ 0, 0.05 ]
 
 
 PlateDispenser:
-  parts:
-    - type: rect
-      height: 0.95
-      width: 0.95
-      color: cadetblue1
+  parts: [ ]
+#    - type: rect
+#      height: 0.95
+#      width: 0.95
+#      color: cadetblue1
 
 Trashcan:
   parts:
     - type: image
       path: images/trash3.png
-      size: 0.9
-      center_offset: [ 0, 0 ]
-
-TomatoDispenser:
-  parts:
-    - color: orangered1
-      type: rect
-      height: 0.8
-      width: 0.8
-
-LettuceDispenser:
-  parts:
-    - color: palegreen3
-      type: rect
-      height: 0.8
-      width: 0.8
+      size: 0.88
+      center_offset: [ 0, -0.05 ]
 
-OnionDispenser:
-  parts:
-    - color: deeppink3
-      type: rect
-      height: 0.8
-      width: 0.8
+#TomatoDispenser:
+#  parts:
+#    - color: orangered1
+#      type: rect
+#      height: 0.8
+#      width: 0.8
+#
+#LettuceDispenser:
+#  parts:
+#    - color: palegreen3
+#      type: rect
+#      height: 0.8
+#      width: 0.8
+#
+#OnionDispenser:
+#  parts:
+#    - color: deeppink3
+#      type: rect
+#      height: 0.8
+#      width: 0.8
+#
+#MeatDispenser:
+#  parts:
+#    - color: indianred1
+#      type: rect
+#      height: 0.8
+#      width: 0.8
+#
+#BunDispenser:
+#  parts:
+#    - color: sandybrown
+#      type: rect
+#      height: 0.8
+#      width: 0.8
 
-MeatDispenser:
+Dispenser:
   parts:
-    - color: indianred1
-      type: rect
-      height: 0.8
-      width: 0.8
+    - type: circle
+      color: black
+      radius: 0.35
+      center_offset: [ 0, -0.05 ]
+    - type: circle
+      color: gray83
+      radius: 0.33
+      center_offset: [ 0, -0.05 ]
 
-BunDispenser:
-  parts:
-    - color: sandybrown
-      type: rect
-      height: 0.8
-      width: 0.8
 
-Dispenser:
-  parts:
-    - color: gray83
-      type: rect
-      height: 0.8
-      width: 0.8
+  item_offset: [ 0, -0.05 ]
+  item_scale: 0.9
 
 ServingWindow:
   parts:
-    - type: image
-      path: images/arrow_right.png
-      size: 1
-      center_offset: [ 0, 0 ]
+    #    - type: image
+    #      path: images/arrow_right.png
+    #      size: 1
+    #      center_offset: [ 0, 0 ]
     - type: image
       path: images/bell_gold.png
       size: 0.5
-      center_offset: [ 0.1, -0.4 ]
+      center_offset: [ -0.4, 0.1 ]
+      rotate_image: False
 
 Stove:
   parts:
@@ -115,15 +128,15 @@ Sink:
   parts:
     - type: image
       path: images/sink1.png
-      size: 1
-      center_offset: [ 0, -0.05 ]
+      size: 0.85
+      center_offset: [ 0, -0.12 ]
 
 SinkAddon:
   parts:
     - type: image
       path: images/drip2.png
-      size: 0.85
-      center_offset: [ 0, 0.03 ]
+      size: 0.75
+      center_offset: [ 0, -0.05 ]
 
 # Items
 Tomato:
@@ -275,6 +288,7 @@ Oven:
       color: black
       height: 0.8
       width: 0.3
+      center_offset: [ 0, -0.1 ]
 
 Basket:
   parts:

overcooked_simulator/overcooked_environment.py

@@ -344,12 +344,16 @@ class Environment:
         else:
             lines = self.layout_config.split("\n")
 
+        grid = []
+
         for line in lines:
             line = line.replace("\n", "").replace(" ", "")  # remove newline char
             current_x: float = starting_at
+            grid_line = []
             for character in line:
                 character = character.capitalize()
                 pos = np.array([current_x, current_y])
+
                 assert self.counter_factory.can_map(
                     character
                 ), f"{character=} in layout file can not be mapped"
@@ -357,7 +361,9 @@ class Environment:
                     counters.append(
                         self.counter_factory.get_counter_object(character, pos)
                     )
+                    grid_line.append(1)
                 else:
+                    grid_line.append(0)
                     match self.counter_factory.map_not_counter(character):
                         case "Agent":
                             designated_player_positions.append(pos)
@@ -365,14 +371,79 @@ class Environment:
                             free_positions.append(np.array([current_x, current_y]))
 
                 current_x += 1
+
+            grid.append(grid_line)
             current_y += 1
 
         self.kitchen_width: float = len(lines[0]) + starting_at
         self.kitchen_height = len(lines) + starting_at
+
+        self.determine_counter_orientations(
+            counters, grid, np.array([self.kitchen_width / 2, self.kitchen_height / 2])
+        )
+
         self.counter_factory.post_counter_setup(counters)
 
         return counters, designated_player_positions, free_positions
 
+    def determine_counter_orientations(self, counters, grid, kitchen_center):
+        for l in grid:
+            print(l)
+
+        grid = np.array(grid).T
+
+        grid_width = grid.shape[0]
+        grid_height = grid.shape[1]
+
+        last_counter = None
+        fst_counter_in_row = None
+        for c in counters:
+            grid_idx = np.floor(c.pos).astype(int)
+            neighbour_offsets = np.array([[0, 1], [0, -1], [1, 0], [-1, 0]], dtype=int)
+
+            neighbours_free = []
+            for offset in neighbour_offsets:
+                neighbour_pos = grid_idx + offset
+                if (
+                    neighbour_pos[0] > (grid_width - 1)
+                    or neighbour_pos[0] < 0
+                    or neighbour_pos[1] > (grid_height - 1)
+                    or neighbour_pos[1] < 0
+                ):
+                    pass
+                else:
+                    if grid[neighbour_pos[0]][neighbour_pos[1]] == 0:
+                        neighbours_free.append(offset)
+            if len(neighbours_free) > 0:
+                vector_to_center = c.pos - kitchen_center
+                vector_to_center /= np.linalg.norm(vector_to_center)
+                n_idx = np.argmin(
+                    np.linalg.norm(vector_to_center - n) for n in neighbours_free
+                )
+                nearest_vec = neighbours_free[n_idx]
+                # print(nearest_vec, type(nearest_vec))
+                c.set_orientation(nearest_vec)
+
+            elif grid_idx[0] == 0:
+                if grid_idx[1] == 0:
+                    # counter top left
+                    c.set_orientation(np.array([1, 0]))
+                else:
+                    c.set_orientation(fst_counter_in_row.orientation)
+                fst_counter_in_row = c
+            else:
+                c.set_orientation(last_counter.orientation)
+
+            last_counter = c
+
+        # for c in counters:
+        #     near_counters = [
+        #         other
+        #         for other in counters
+        #         if np.isclose(np.linalg.norm(c.pos - other.pos), 1)
+        #     ]
+        #     # print(c.pos, len(near_counters))
+
     def perform_action(self, action: Action):
         """Performs an action of a player in the environment. Maps different types of action inputs to the
         correct execution of the players.

overcooked_simulator/state_representation.py

@@ -33,6 +33,7 @@ class CounterState(TypedDict):
     category: Literal["Counter"]
     type: str
     pos: list[float]
+    orientation: list[float]
     occupied_by: None | list[
         ItemState | CookingEquipmentState
     ] | ItemState | CookingEquipmentState