Add fixes from 26 Nov

main.py

@@ -1,6 +1,11 @@
 from utama_core.replay import ReplayWriterConfig
 from utama_core.run import StrategyRunner
-from utama_core.strategy import DefenceStrategy, RobotPlacementStrategy, StartupStrategy
+from utama_core.strategy.examples import (
+    DefenceStrategy,
+    GoToBallExampleStrategy,
+    RobotPlacementStrategy,
+    StartupStrategy,
+)
 
 
 def main():

utama_core/config/robot_params.py

@@ -34,10 +34,10 @@ class RobotParams:
 )
 
 REAL_PARAMS = RobotParams(
-    MAX_VEL=0.2,
-    MAX_ANGULAR_VEL=0.5,
-    MAX_ACCELERATION=2,
-    MAX_ANGULAR_ACCELERATION=0.2,
+    MAX_VEL=1,
+    MAX_ANGULAR_VEL=2,
+    MAX_ACCELERATION=4,
+    MAX_ANGULAR_ACCELERATION=50,
     KICK_SPD=5,
     DRIBBLE_SPD=3,
     CHIP_ANGLE=pi / 4,

utama_core/config/settings.py

@@ -24,7 +24,7 @@
 
 ### REAL CONTROLLER SETTINGS ###
 BAUD_RATE = 115200
-PORT = "/dev/ttyACM0"
+PORT = "/dev/ttyUSB0"
 TIMEOUT = 0.1
 
 MAX_GAME_HISTORY = 20

utama_core/rsoccer_simulator/src/ssl/ssl_gym_base.py

@@ -268,8 +268,8 @@ def _render(self):
             overlay = RenderOverlay(self.overlay, self.field_renderer.scale)
             overlay.draw(self.window_surface)
 
-        for i in range(self.n_robots_blue):
-            robot = self.frame.robots_blue[i]
+        for blue_rb in self.frame.robots_blue.values():
+            robot = blue_rb
             x, y = self._pos_transform(robot.x, robot.y)
             rbt = RenderSSLRobot(
                 x,
@@ -281,8 +281,8 @@ def _render(self):
             )
             rbt.draw(self.window_surface)
 
-        for i in range(self.n_robots_yellow):
-            robot = self.frame.robots_yellow[i]
+        for yellow_rb in self.frame.robots_yellow.values():
+            robot = yellow_rb
             x, y = self._pos_transform(robot.x, robot.y)
             rbt = RenderSSLRobot(
                 x,

utama_core/run/refiners/position.py

@@ -128,11 +128,10 @@ def _get_most_confident_ball(balls: List[VisionBallData]) -> Ball:
 
     def _combine_single_team_positions(
         self,
-        game_robots: Dict[int, Robot],
+        new_game_robots: Dict[int, Robot],
         vision_robots: List[VisionRobotData],
         friendly: bool,
     ) -> Dict[int, Robot]:
-        new_game_robots = game_robots.copy()
         for robot in vision_robots:
             new_x, new_y = robot.x, robot.y
 

utama_core/strategy/__init__.py

@@ -1,9 +0,0 @@
-from .examples.strategies.defense_strategy import DefenceStrategy
-from .examples.strategies.one_robot_placement_strategy import RobotPlacementStrategy
-from .examples.strategies.startup_strategy import StartupStrategy
-
-__all__ = [
-    "StartupStrategy",
-    "DefenceStrategy",
-    "RobotPlacementStrategy",
-]

utama_core/strategy/examples/__init__.py

@@ -0,0 +1,6 @@
+from utama_core.strategy.examples.defense_strategy import DefenceStrategy
+from utama_core.strategy.examples.go_to_ball_ex import GoToBallExampleStrategy
+from utama_core.strategy.examples.one_robot_placement_strategy import (
+    RobotPlacementStrategy,
+)
+from utama_core.strategy.examples.startup_strategy import StartupStrategy

utama_core/strategy/examples/strategies/defense_strategy.py → utama_core/strategy/examples/defense_strategy.py

@@ -38,14 +38,19 @@ class BlockAttackerStep(AbstractBehaviour):
 
     **Blackboard Interaction:**
         Reads:
-            - `rd_defender_id` (int): The ID of the robot to check for ball possession. Typically from the `SetBlackboardVariable` node.
+            - `rd_defender_id` (int): The ID of the defending robot that will block the attacker. Typically from the `SetBlackboardVariable` node.
             - `rd_blocking_target` (int): The ID of the enemy robot to block. Typically from the `FindBlockingTarget` node.
 
     **Returns:**
         - `py_trees.common.Status.RUNNING`: The behaviour is actively commanding the robot to block the attacker.
     """
 
-    def __init__(self, rd_defender_id: str, rd_locking_target: str, name: Optional[str] = "BlockAttackerStep"):
+    def __init__(
+        self,
+        rd_defender_id: str,
+        rd_locking_target: str,
+        name: Optional[str] = "BlockAttackerStep",
+    ):
         super().__init__(name=name)
         self.defender_id_key = rd_defender_id
         self.locking_target_key = rd_locking_target

utama_core/strategy/examples/go_to_ball_ex.py

@@ -0,0 +1,189 @@
+from typing import Any, Optional
+
+import py_trees
+from py_trees.composites import Selector, Sequence
+
+from utama_core.config.physical_constants import ROBOT_RADIUS
+from utama_core.config.settings import TIMESTEP
+from utama_core.entities.data.vector import Vector2D
+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
+
+
+class HasBall(AbstractBehaviour):
+    """
+    Checks whether the configured robot currently owns the ball.
+    **Args:**
+        - visual (bool): If True, uses distance-based possession check; otherwise uses game state.
+    **Blackboard Interaction:**
+        - Reads:
+            - `robot_id` (int): The ID of the robot to check for ball possession.
+    **Returns:**
+        - `py_trees.common.Status.SUCCESS`: The robot has possession of the ball.
+        - `py_trees.common.Status.FAILURE`: The robot does not have possession of the ball.
+    """
+
+    def __init__(self, rd_robot_id: str, visual: bool = True, name: Optional[str] = None):
+        super().__init__(name)
+        self.robot_id_key = rd_robot_id
+
+        self.visual = visual
+        DISTANCE_BUFFER = 0.04  # meters
+        self.ball_capture_dist = DISTANCE_BUFFER + ROBOT_RADIUS
+
+    def setup_(self):
+        self.blackboard.register_key(key=self.robot_id_key, access=py_trees.common.Access.READ)
+
+    def update(self):
+        # print(f"Checking if robot {self.blackboard.get(self.robot_id_key)} has the ball")
+        game = self.blackboard.game
+        robot_id = self.blackboard.get(self.robot_id_key)
+
+        if self.visual:
+            return self._has_ball_visual(game, robot_id)
+        else:
+            return self._has_ball_from_state(game, robot_id)
+
+    def _has_ball_visual(self, game: Game, robot_id: int) -> py_trees.common.Status:
+        """
+        Visual possession: success if the robot is within `ball_capture_radius` of the ball.
+        Success if the robot is within `ball_capture_radius` of the ball (uses Euclidean distance).
+        """
+        robot = game.friendly_robots[robot_id]
+        ball = game.ball
+
+        r_pos = Vector2D(robot.p.x, robot.p.y)
+        b_pos = Vector2D(ball.p.x, ball.p.y)
+
+        dist = r_pos.distance_to(b_pos)
+        return py_trees.common.Status.SUCCESS if dist < self.ball_capture_dist else py_trees.common.Status.FAILURE
+
+    def _has_ball_from_state(self, game: Game, robot_id: int) -> py_trees.common.Status:
+        """
+        State possession: success if the game state's `has_ball` flag is true.
+        """
+        has_ball = game.current.friendly_robots[robot_id].has_ball
+
+        # print(f"Robot {robot_id} has_ball: {has_ball}")
+        return py_trees.common.Status.SUCCESS if has_ball else py_trees.common.Status.FAILURE
+
+
+class GoToBallStep(AbstractBehaviour):
+    """
+    Continuously moves the robot towards the ball using the core go_to_ball skill.
+    **Blackboard Interaction:**
+        - Reads:
+            - `robot_id` (int): The ID of the robot to move towards the ball.
+    **Returns:**
+        - `py_trees.common.Status.RUNNING`: The behaviour is actively commanding the robot to go to the ball.
+    """
+
+    def __init__(self, rd_robot_id: str, name: str = "GoToBallStep"):
+        super().__init__(name)
+        self.robot_id_key = rd_robot_id
+
+    def setup_(self):
+        self.blackboard.register_key(key=self.robot_id_key, access=py_trees.common.Access.READ)
+
+    def update(self) -> py_trees.common.Status:
+        game = self.blackboard.game
+        env = self.blackboard.rsim_env
+        robot_id = self.blackboard.get(self.robot_id_key)
+        if env:
+            v = game.friendly_robots[robot_id].v
+            p = game.friendly_robots[robot_id].p
+            env.draw_point(p.x + v.x * TIMESTEP * 5, p.y + v.y * TIMESTEP * 5, color="green")
+
+        command = go_to_ball(game, self.blackboard.motion_controller, robot_id)
+        self.blackboard.cmd_map[robot_id] = command
+        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.
+    **Args:**
+        - rd_robot_id: Blackboard key that holds the robot ID to control. **Required.**
+    **Status:**
+        - py_trees.common.Status.SUCCESS: The robot already has the ball.
+        - py_trees.common.Status.RUNNING: The robot is being commanded to go to the ball.
+    **Returns:**
+        - py_trees.behaviour.Behaviour: The root node of the Go-To-Ball subtree.
+    """
+    root = Selector(
+        name="GoToBallSubtree",
+        memory=False,
+    )
+
+    ### Assemble the tree ###
+
+    root.add_children(
+        [
+            HasBall(name="HasBall?", rd_robot_id=rd_robot_id),
+            GoToBallStep(name="GoToBallStep", rd_robot_id=rd_robot_id),
+        ]
+    )
+
+    return root
+
+
+class GoToBallExampleStrategy(AbstractStrategy):
+    def __init__(self, robot_id: int):
+        """Initializes the GoToBallStrategy with a specific robot ID.
+        :param robot_id: The ID of the robot this strategy will control.
+        """
+        self.robot_id = robot_id
+        self.robot_id_key = "robot_id"
+        super().__init__()
+
+    def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int):
+        if 1 <= n_runtime_friendly <= 3:
+            return True
+        return False
+
+    def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: bool):
+        return True
+
+    def get_min_bounding_zone(self):
+        return None
+
+    def create_behaviour_tree(self) -> py_trees.behaviour.Behaviour:
+        """Factory function to create a complete go_to_ball behaviour tree."""
+
+        root = Sequence(name="GoToBallRoot", memory=True)
+
+        set_robot_id = SetBlackboardVariable(
+            name="SetRobotID",
+            variable_name=self.robot_id_key,
+            value=self.robot_id,
+        )
+
+        ### Assemble the tree ###
+
+        root.add_children([set_robot_id, go_to_ball_subtree(rd_robot_id=self.robot_id_key)])
+
+        return root

utama_core/strategy/examples/strategies/one_robot_placement_strategy.py → utama_core/strategy/examples/one_robot_placement_strategy.py

@@ -6,6 +6,7 @@
 import py_trees
 from py_trees.composites import Sequence
 
+from utama_core.config.settings import TIMESTEP
 from utama_core.entities.game.field import FieldBounds
 from utama_core.global_utils.math_utils import Vector2D
 from utama_core.skills.src.utils.move_utils import move
@@ -46,23 +47,20 @@ def update(self) -> py_trees.common.Status:
         id: int = self.blackboard.get(self.robot_id_key)
 
         friendly_robots = game.friendly_robots
-
-        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))
-
-            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])
+        bx, by = game.ball.p.x, game.ball.p.y
+        rp = friendly_robots[id].p
+        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])
 
             # changed so the robot tracks the ball while moving
             oren = np.atan2(by - cy, bx - cx)
@@ -77,10 +75,10 @@ def update(self) -> py_trees.common.Status:
                 rsim_env.draw_point(self.tx, self.ty, color="red")
                 v = game.friendly_robots[id].v
                 p = game.friendly_robots[id].p
-                rsim_env.draw_point(p.x + v.x * 0.167 * 5, p.y + v.y * 0.167 * 5, color="green")
+                rsim_env.draw_point(p.x + v.x * TIMESTEP * 5, p.y + v.y * TIMESTEP * 5, color="green")
 
-            self.blackboard.cmd_map[id] = cmd
-            return py_trees.common.Status.RUNNING
+        self.blackboard.cmd_map[id] = cmd
+        return py_trees.common.Status.RUNNING
 
 
 class SetBlackboardVariable(AbstractBehaviour):