Jan17 robot placement

.github/copilot-instructions.md

@@ -3,4 +3,6 @@
 - Do not comment on docstring grammar or punctuation.
 - Focus on logic, correctness, and API design
 - Assume Black and Ruff enforce formatting
-- Ignore trivial, non functional changes, namely unused imports and formatting changes.
+- Ignore trivial, non functional changes, namely unused imports and formatting 
+changes.
+- Ignore unused imports.

main.py

@@ -1,22 +1,27 @@
+from utama_core.entities.game.field import FieldBounds
 from utama_core.replay import ReplayWriterConfig
 from utama_core.run import StrategyRunner
 from utama_core.strategy.examples import (
     DefenceStrategy,
     GoToBallExampleStrategy,
     RobotPlacementStrategy,
     StartupStrategy,
+    TwoRobotPlacementStrategy,
 )
 
 
 def main():
+    # Setup for real testing
+    # Custom field size based setup in real
+    custom_bounds = FieldBounds(top_left=(2.25, 1.5), bottom_right=(4.5, -1.5))
+
     runner = StrategyRunner(
-        strategy=StartupStrategy(),
+        strategy=TwoRobotPlacementStrategy(first_robot_id=0, second_robot_id=1, field_bounds=custom_bounds),
         my_team_is_yellow=True,
         my_team_is_right=True,
         mode="rsim",
-        exp_friendly=6,
-        exp_enemy=3,
-        control_scheme="dwa",
+        exp_friendly=2,
+        exp_enemy=0,
         replay_writer_config=ReplayWriterConfig(replay_name="test_replay", overwrite_existing=True),
         print_real_fps=True,
         profiler_name=None,

utama_core/entities/game/field.py

@@ -16,6 +16,13 @@ class FieldBounds:
     top_left: tuple[float, float]
     bottom_right: tuple[float, float]
 
+    @property
+    def center(self) -> tuple[float, float]:
+        """Calculates the geometric center of the field bounds."""
+        cx = (self.top_left[0] + self.bottom_right[0]) / 2.0
+        cy = (self.top_left[1] + self.bottom_right[1]) / 2.0
+        return (cx, cy)
+
 
 class Field:
     """Field class that contains all the information about the field.
@@ -163,6 +170,10 @@ def half_width(self) -> float:
     def field_bounds(self) -> FieldBounds:
         return self._field_bounds
 
+    @property
+    def center(self) -> tuple[float, float]:
+        return self._field_bounds.center
+
     ### Class Properties for standard field dimensions ###
 
     @ClassProperty

utama_core/strategy/examples/__init__.py

@@ -4,3 +4,4 @@
     RobotPlacementStrategy,
 )
 from utama_core.strategy.examples.startup_strategy import StartupStrategy
+from utama_core.strategy.examples.two_robot_placement import TwoRobotPlacementStrategy

utama_core/strategy/examples/go_to_ball_ex.py

@@ -9,6 +9,7 @@
 from utama_core.entities.game import Game
 from utama_core.skills.src.go_to_ball import go_to_ball
 from utama_core.strategy.common import AbstractBehaviour, AbstractStrategy
+from utama_core.strategy.examples.utils import SetBlackboardVariable
 
 
 class HasBall(AbstractBehaviour):
@@ -100,29 +101,6 @@ def update(self) -> py_trees.common.Status:
         return py_trees.common.Status.RUNNING
 
 
-class SetBlackboardVariable(AbstractBehaviour):
-    """
-    Writes a constant `value` onto the blackboard with the key `variable_name`.
-    **Blackboard Interaction:**
-        - Writes:
-            - `variable_name` (Any): The name of the blackboard variable to be set.
-    **Returns:**
-        - `py_trees.common.Status.SUCCESS`: The variable has been set.
-    """
-
-    def __init__(self, name: str, variable_name: str, value: Any):
-        super().__init__(name=name)
-        self.variable_name = variable_name
-        self.value = value
-
-    def setup_(self):
-        self.blackboard.register_key(key=self.variable_name, access=py_trees.common.Access.WRITE)
-
-    def update(self) -> py_trees.common.Status:
-        self.blackboard.set(self.variable_name, self.value, overwrite=True)
-        return py_trees.common.Status.SUCCESS
-
-
 def go_to_ball_subtree(rd_robot_id: str) -> py_trees.behaviour.Behaviour:
     """
     Builds a selector that drives the robot to the ball until it gains possession.

utama_core/strategy/examples/one_robot_placement_strategy.py

@@ -7,13 +7,17 @@
 from py_trees.composites import Sequence
 
 from utama_core.config.settings import TIMESTEP
-from utama_core.entities.game.field import FieldBounds
+from utama_core.entities.game.field import Field, FieldBounds
 from utama_core.global_utils.math_utils import Vector2D
 from utama_core.skills.src.utils.move_utils import move
 from utama_core.strategy.common.abstract_behaviour import AbstractBehaviour
 
 # from robot_control.src.tests.utils import one_robot_placement
 from utama_core.strategy.common.abstract_strategy import AbstractStrategy
+from utama_core.strategy.examples.utils import (
+    CalculateFieldCenter,
+    SetBlackboardVariable,
+)
 
 
 class RobotPlacementStep(AbstractBehaviour):
@@ -30,18 +34,39 @@ class RobotPlacementStep(AbstractBehaviour):
         - `py_trees.common.Status.RUNNING`: The behaviour is actively commanding the robot to move.
     """
 
-    def __init__(self, rd_robot_id: str, invert: bool = False):
+    def __init__(self, rd_robot_id: str, field_center_key: str = "FieldCenter"):
         super().__init__()
+        self.field_center_key = field_center_key
         self.robot_id_key = rd_robot_id
-
-        self.ty = -1
-        self.tx = -1 if invert else 1
+        self.initialized = False
+        self.center_x = 0.0
+        self.center_y = 0.0
+        self.tx = 0.0
+        self.ty = 0.0
 
     def setup_(self):
         self.blackboard.register_key(key=self.robot_id_key, access=py_trees.common.Access.READ)
+        self.blackboard.register_key(key=self.field_center_key, access=py_trees.common.Access.READ)
 
     def update(self) -> py_trees.common.Status:
         """Closure which advances the simulation by one step."""
+
+        # Initialize targets if not ready
+        if not self.initialized:
+            try:
+                center = self.blackboard.get(self.field_center_key)
+                if center:
+                    self.center_x, self.center_y = center
+                    self.tx = self.center_x
+                    self.ty = self.center_y + 0.5
+                    self.initialized = True
+            except KeyError:
+                # Center not yet available
+                return py_trees.common.Status.FAILURE
+
+        if not self.initialized:
+            return py_trees.common.Status.FAILURE
+
         game = self.blackboard.game
         rsim_env = self.blackboard.rsim_env
         id: int = self.blackboard.get(self.robot_id_key)
@@ -52,15 +77,25 @@ def update(self) -> py_trees.common.Status:
         cx, cy = rp.x, rp.y
         error = math.dist((self.tx, self.ty), (cx, cy))
 
-        switch = error < 0.05
-        if switch:
-            if self.ty == -1:
-                self.ty = 1
-                self.tx = random.choice([0, 1])
-            else:
-                self.ty = -1
-                self.tx = 1
-                # self.tx = random.choice([0, 1])
+        if game.friendly_robots and game.ball is not None:
+            friendly_robots = game.friendly_robots
+            bx, by = game.ball.p.x, game.ball.p.y
+            rp = friendly_robots[id].p
+            cx, cy, _ = rp.x, rp.y, friendly_robots[id].orientation
+            error = math.dist((self.tx, self.ty), (cx, cy))
+
+            # Ensure target x is always the center x
+            self.tx = self.center_x
+
+            switch = error < 0.1
+            if switch:
+                upper_target = self.center_y + 0.5
+                lower_target = self.center_y - 0.5
+
+                if math.isclose(self.ty, lower_target, abs_tol=0.1):
+                    self.ty = upper_target
+                else:
+                    self.ty = lower_target
 
             # changed so the robot tracks the ball while moving
             oren = np.atan2(by - cy, bx - cx)
@@ -81,31 +116,16 @@ def update(self) -> py_trees.common.Status:
         return py_trees.common.Status.RUNNING
 
 
-class SetBlackboardVariable(AbstractBehaviour):
-    """A generic behaviour to set a variable on the blackboard."""
-
-    def __init__(self, name: str, variable_name: str, value: Any):
-        super().__init__(name=name)
-        self.variable_name = variable_name
-        self.value = value
-
-    def setup_(self):
-        self.blackboard.register_key(key=self.variable_name, access=py_trees.common.Access.WRITE)
-
-    def update(self) -> py_trees.common.Status:
-        # print(f"Setting {self.variable_name} to {self.value} on the blackboard.")
-        self.blackboard.set(self.variable_name, self.value, overwrite=True)
-        return py_trees.common.Status.SUCCESS
-
-
 class RobotPlacementStrategy(AbstractStrategy):
-    def __init__(self, robot_id: int):
+    def __init__(self, robot_id: int, field_bounds: Optional[FieldBounds] = None):
         """
         Initializes the RobotPlacementStrategy with a specific robot ID.
 
         :param robot_id: The ID of the robot this strategy will control.
+        :param field_bounds: The bounds of the field to operate within.
         """
         self.robot_id = robot_id
+        self.field_bounds = field_bounds if field_bounds else Field.FULL_FIELD_BOUNDS
         super().__init__()
 
     def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int):
@@ -124,6 +144,7 @@ def create_behaviour_tree(self) -> py_trees.behaviour.Behaviour:
         """Factory function to create a complete behaviour tree."""
 
         robot_id_key = "target_robot_id"
+        field_center_key = "FieldCenter"
 
         coach_root = Sequence(name="CoachRoot", memory=False)
 
@@ -135,6 +156,15 @@ def create_behaviour_tree(self) -> py_trees.behaviour.Behaviour:
 
         ### Assemble the tree ###
 
-        coach_root.add_children([set_rbt_id, RobotPlacementStep(rd_robot_id=robot_id_key)])
+        # Calculate Field Center from custom field_bounds
+        calc_center = CalculateFieldCenter(field_bounds=self.field_bounds, output_key=field_center_key)
+
+        coach_root.add_children(
+            [
+                set_rbt_id,
+                calc_center,
+                RobotPlacementStep(rd_robot_id=robot_id_key, field_center_key=field_center_key),
+            ]
+        )
 
         return coach_root