Init

world/commons/field.py

@@ -0,0 +1,50 @@
+from abc import ABC, abstractmethod
+from typing_extensions import override
+from world.commons.field_landmarks import FieldLandmarks
+
+
+class Field(ABC):
+    def __init__(self, world):
+        from world.world import World  # type hinting
+        self.world: World = world
+        self.field_landmarks: FieldLandmarks = FieldLandmarks(world=self.world)
+
+    def get_our_goal_position(self):
+        return (-self.get_length() / 2, 0)
+
+    def get_their_goal_position(self):
+        return (self.get_length() / 2, 0)
+
+    @abstractmethod
+    def get_width(self):
+        raise NotImplementedError()
+
+    @abstractmethod
+    def get_length(self):
+        raise NotImplementedError()
+
+
+class FIFAField(Field):
+    def __init__(self, world):
+        super().__init__(world)
+
+    @override
+    def get_width(self):
+        return 68
+
+    @override
+    def get_length(self):
+        return 105
+
+
+class HLAdultField(Field):
+    def __init__(self, world):
+        super().__init__(world)
+
+    @override
+    def get_width(self):
+        return 9
+
+    @override
+    def get_length(self):
+        return 14

world/commons/field_landmarks.py

@@ -0,0 +1,34 @@
+import numpy as np
+from utils.math_ops import MathOps
+
+
+class FieldLandmarks:
+    def __init__(self, world):
+        from world.world import World  # type hinting
+
+        self.world: World = world
+
+        self.landmarks: dict = {}
+
+    def update_from_perception(self, landmark_id: str, landmark_pos: np.ndarray) -> None:
+        """
+        Calculates the global position of all currently visible landmarks.
+        """
+
+        world = self.world
+        local_cart_3d = MathOps.deg_sph2cart(landmark_pos)
+
+        global_pos_3d = MathOps.rel_to_global_3d(
+            local_pos_3d=local_cart_3d,
+            global_pos_3d=world.global_position,
+            global_orientation_quat=world.agent.robot.global_orientation_quat
+        )
+
+        self.landmarks[landmark_id] = global_pos_3d
+
+    def get_landmark_position(self, landmark_id: str) -> np.ndarray | None:
+        """
+        Returns the calculated 2d global position for a given landmark ID.
+        Returns None if the landmark is not currently visible or processed.
+        """
+        return self.global_positions.get(landmark_id)

world/commons/other_robot.py

@@ -0,0 +1,7 @@
+import numpy as np
+
+class OtherRobot:
+    def __init__(self, is_teammate: bool=True):
+        self.is_teammate = is_teammate
+        self.position = np.zeros(3)
+        self.last_seen_time = None

world/commons/play_mode.py

@@ -0,0 +1,232 @@
+from enum import Enum, auto
+
+
+class PlayModeEnum(Enum):
+    NOT_INITIALIZED = auto()
+
+    """Enum specifying possible play modes."""
+
+    BEFORE_KICK_OFF = auto()
+    """The game hasn't started yet."""
+
+    OUR_KICK_OFF = auto()
+    """Our team has kick off."""
+
+    THEIR_KICK_OFF = auto()
+    """Their team has kick off."""
+
+    PLAY_ON = auto()
+    """The game is running normal."""
+
+    OUR_THROW_IN = auto()
+    """The ball left the field and our team has throw in."""
+
+    THEIR_THROW_IN = auto()
+    """The ball left the field and their team has throw in."""
+
+    OUR_CORNER_KICK = auto()
+    """Our team has corner kick."""
+
+    THEIR_CORNER_KICK = auto()
+    """Their team has corner kick."""
+
+    OUR_GOAL_KICK = auto()
+    """Our team has goal kick."""
+
+    THEIR_GOAL_KICK = auto()
+    """Their team has goal kick."""
+
+    OUR_OFFSIDE = auto()
+    """Their team violated the offside rule."""
+
+    THEIR_OFFSIDE = auto()
+    """Our team violated the offside rule."""
+
+    GAME_OVER = auto()
+    """The game has ended."""
+
+    OUR_GOAL = auto()
+    """Our team scored a goal."""
+
+    THEIR_GOAL = auto()
+    """Their team scored a goal."""
+
+    OUR_FREE_KICK = auto()
+    """Our team has a free kick."""
+
+    THEIR_FREE_KICK = auto()
+    """Their team has a free kick."""
+
+    OUR_DIRECT_FREE_KICK = auto()
+    """Our team has a direct free kick."""
+
+    THEIR_DIRECT_FREE_KICK = auto()
+    """Their team has a direct free kick."""
+
+    OUR_PENALTY_KICK = auto()
+    """Our team has a penalty kick (from the penalty spot)."""
+
+    THEIR_PENALTY_KICK = auto()
+    """Their team has a penalty kick (from the penalty spot)."""
+
+    OUR_PENALTY_SHOOT = auto()
+    """Our team has a penalty shoot (starting from somewhere on the field, allowed to touch the ball more than once)."""
+
+    THEIR_PENALTY_SHOOT = auto()
+    """Their team has a penalty shoot (starting from somewhere on the field, allowed to touch the ball more than once)."""
+
+    @classmethod
+    def get_playmode_from_string(
+        cls, playmode: str, is_left_team: bool
+    ) -> "PlayModeEnum":
+        assert isinstance(is_left_team, bool)
+
+        playmode_ids = {
+            "BeforeKickOff": (PlayModeEnum.BEFORE_KICK_OFF,),
+            "KickOff_Left": (
+                PlayModeEnum.OUR_KICK_OFF,
+                PlayModeEnum.THEIR_KICK_OFF,
+            ),
+            "KickOff_Right": (
+                PlayModeEnum.THEIR_KICK_OFF,
+                PlayModeEnum.OUR_KICK_OFF,
+            ),
+            "PlayOn": (PlayModeEnum.PLAY_ON,),
+            "KickIn_Left": (
+                PlayModeEnum.OUR_THROW_IN,
+                PlayModeEnum.THEIR_THROW_IN,
+            ),
+            "KickIn_Right": (
+                PlayModeEnum.THEIR_THROW_IN,
+                PlayModeEnum.OUR_THROW_IN,
+            ),
+            "corner_kick_left": (
+                PlayModeEnum.OUR_CORNER_KICK,
+                PlayModeEnum.THEIR_CORNER_KICK,
+            ),
+            "corner_kick_right": (
+                PlayModeEnum.THEIR_CORNER_KICK,
+                PlayModeEnum.OUR_CORNER_KICK,
+            ),
+            "goal_kick_left": (
+                PlayModeEnum.OUR_GOAL_KICK,
+                PlayModeEnum.THEIR_GOAL_KICK,
+            ),
+            "goal_kick_right": (
+                PlayModeEnum.THEIR_GOAL_KICK,
+                PlayModeEnum.OUR_GOAL_KICK,
+            ),
+            "offside_left": (
+                PlayModeEnum.OUR_OFFSIDE,
+                PlayModeEnum.THEIR_OFFSIDE,
+            ),
+            "offside_right": (
+                PlayModeEnum.THEIR_OFFSIDE,
+                PlayModeEnum.OUR_OFFSIDE,
+            ),
+            "GameOver": (PlayModeEnum.GAME_OVER,),
+            "Goal_Left": (
+                PlayModeEnum.OUR_GOAL,
+                PlayModeEnum.THEIR_GOAL,
+            ),
+            "Goal_Right": (
+                PlayModeEnum.THEIR_GOAL,
+                PlayModeEnum.OUR_GOAL,
+            ),
+            "free_kick_left": (
+                PlayModeEnum.OUR_FREE_KICK,
+                PlayModeEnum.THEIR_FREE_KICK,
+            ),
+            "free_kick_right": (
+                PlayModeEnum.THEIR_FREE_KICK,
+                PlayModeEnum.OUR_FREE_KICK,
+            ),
+            "direct_free_kick_left": (
+                PlayModeEnum.OUR_DIRECT_FREE_KICK,
+                PlayModeEnum.THEIR_DIRECT_FREE_KICK,
+            ),
+            "direct_free_kick_right": (
+                PlayModeEnum.THEIR_DIRECT_FREE_KICK,
+                PlayModeEnum.OUR_DIRECT_FREE_KICK,
+            ),
+            "penalty_kick_left": (
+                PlayModeEnum.OUR_PENALTY_KICK,
+                PlayModeEnum.THEIR_PENALTY_KICK,
+            ),
+            "penalty_kick_right": (
+                PlayModeEnum.THEIR_PENALTY_KICK,
+                PlayModeEnum.OUR_PENALTY_KICK,
+            ),
+            "penalty_shoot_left": (
+                PlayModeEnum.OUR_PENALTY_SHOOT,
+                PlayModeEnum.THEIR_PENALTY_SHOOT,
+            ),
+            "penalty_shoot_right": (
+                PlayModeEnum.THEIR_PENALTY_SHOOT,
+                PlayModeEnum.OUR_PENALTY_SHOOT,
+            ),
+        }[playmode]
+
+        playmode = None
+        if len(playmode_ids) > 1:
+            playmode = playmode_ids[0 if is_left_team else 1]
+        else:
+            playmode = playmode_ids[0]
+
+        return playmode
+
+
+class PlayModeGroupEnum(Enum):
+    NOT_INITIALIZED = auto()
+    OTHER = auto()
+    OUR_KICK = auto()
+    THEIR_KICK = auto()
+    ACTIVE_BEAM = auto()
+    PASSIVE_BEAM = auto()
+
+    @classmethod
+    def get_group_from_playmode(
+        cls, playmode: PlayModeEnum, is_left_team: bool
+    ) -> "PlayModeGroupEnum":
+        playmode_group: PlayModeGroupEnum = None
+
+        if playmode in (PlayModeEnum.PLAY_ON, PlayModeEnum.GAME_OVER):
+            playmode_group = cls.OTHER
+        elif playmode in (
+            PlayModeEnum.OUR_CORNER_KICK,
+            PlayModeEnum.OUR_DIRECT_FREE_KICK,
+            PlayModeEnum.OUR_FREE_KICK,
+            PlayModeEnum.OUR_GOAL_KICK,
+            PlayModeEnum.OUR_KICK_OFF,
+            PlayModeEnum.OUR_OFFSIDE,
+            PlayModeEnum.OUR_PENALTY_KICK,
+            PlayModeEnum.OUR_PENALTY_SHOOT,
+            PlayModeEnum.OUR_THROW_IN,
+        ):
+            playmode_group = cls.OUR_KICK
+        elif playmode in (
+            PlayModeEnum.THEIR_CORNER_KICK,
+            PlayModeEnum.THEIR_DIRECT_FREE_KICK,
+            PlayModeEnum.THEIR_FREE_KICK,
+            PlayModeEnum.THEIR_GOAL_KICK,
+            PlayModeEnum.THEIR_KICK_OFF,
+            PlayModeEnum.THEIR_OFFSIDE,
+            PlayModeEnum.THEIR_PENALTY_KICK,
+            PlayModeEnum.THEIR_PENALTY_SHOOT,
+            PlayModeEnum.THEIR_THROW_IN,
+        ):
+            playmode_group = cls.THEIR_KICK
+        elif (playmode is PlayModeEnum.THEIR_GOAL) or (
+            is_left_team and playmode is PlayModeEnum.BEFORE_KICK_OFF
+        ):
+            playmode_group = cls.ACTIVE_BEAM
+        elif (playmode is PlayModeEnum.OUR_GOAL) or (
+            not is_left_team and playmode is PlayModeEnum.BEFORE_KICK_OFF
+        ):
+            playmode_group = cls.PASSIVE_BEAM
+        else:
+            raise NotImplementedError(
+                f"Not implemented playmode group for playmode {playmode}"
+            )
+
+        return playmode_group

world/robot.py

@@ -0,0 +1,273 @@
+from abc import ABC, abstractmethod
+from typing_extensions import override
+import numpy as np
+from communication.server import Server
+
+
+class Robot(ABC):
+    """
+    Base class for all robot models.
+
+    This class defines the main structure and common data used by any robot,
+    such as motor positions, sensors, and control messages.
+    """
+    def __init__(self, agent):
+        """
+        Creates a new robot linked to the given agent.
+
+        Args:
+            agent: The main agent that owns this robot.
+        """
+        from agent.base_agent import Base_Agent  # type hinting
+
+        self.agent: Base_Agent = agent
+        self.server: Server = self.agent.server
+
+        self.motor_targets: dict = {
+            motor: {"target_position": 0.0, "kp": 0.0, "kd": 0.0}
+            for motor in self.ROBOT_MOTORS
+        }
+
+        self.motor_positions: dict = {motor: 0.0 for motor in self.ROBOT_MOTORS}  # degrees
+
+        self.motor_speeds: dict = {motor: 0.0 for motor in self.ROBOT_MOTORS}  # degrees/s
+
+        self._global_cheat_orientation = np.array([0, 0, 0, 1])  # quaternion [x, y, z, w]
+
+        self.global_orientation_quat = np.array([0, 0, 0, 1]) # quaternion [x, y, z, w]
+
+        self.global_orientation_euler = np.zeros(3) # euler [roll, pitch, yaw]
+
+        self.gyroscope = np.zeros(3)  # angular velocity [roll, pitch, yaw] (degrees/s)
+
+        self.accelerometer = np.zeros(3)  # linear acceleration [x, y, z] (m/s²)
+
+    @property
+    @abstractmethod
+    def name(self) -> str:
+        """
+        Returns the robot model name.
+        """
+        raise NotImplementedError()
+
+    @property
+    @abstractmethod
+    def ROBOT_MOTORS(self) -> tuple[str, ...]:
+        """
+        Returns the list of motor names used by this robot.
+        """
+        raise NotImplementedError()
+
+    def set_motor_target_position(
+        self, motor_name: str, target_position: float, kp: float = 10, kd: float = 0.1
+    ) -> None:
+        """
+        Sets the desired position and PD gains for a given motor.
+
+        For now, directly sets positions, as the simulator is doing the control
+        Args:
+            motor_name: Name of the motor.
+            target_position: Desired position in radians.
+            kp: Proportional gain.
+            kd: Derivative gain.
+        """
+        self.motor_targets[motor_name] = {
+            "target_position": target_position,
+            "kp": kp,
+            "kd": kd,
+        }
+
+    def commit_motor_targets_pd(self) -> None:
+        """
+        Sends all motor target commands to the simulator.
+        """
+        for motor_name, target_description in self.motor_targets.items():
+            motor_msg = f'({motor_name} {target_description["target_position"]:.2f} 0.0 {target_description["kp"]:.2f} {target_description["kd"]:.2f} 0.0)'
+            self.server.commit(motor_msg)
+
+
+class T1(Robot):
+    """
+    Booster T1
+    """
+    @override
+    def __init__(self, agent):
+        super().__init__(agent)
+
+        self.joint_nominal_position = np.array(
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+            ]
+        )
+
+    @property
+    @override
+    def name(self) -> str:
+        return "T1"
+
+    @property
+    @override
+    def ROBOT_MOTORS(self) -> tuple[str, ...]:
+        return (
+            "he1",
+            "he2",
+            "lae1",
+            "lae2",
+            "lae3",
+            "lae4",
+            "rae1",
+            "rae2",
+            "rae3",
+            "rae4",
+            "te1",
+            "lle1",
+            "lle2",
+            "lle3",
+            "lle4",
+            "lle5",
+            "lle6",
+            "rle1",
+            "rle2",
+            "rle3",
+            "rle4",
+            "rle5",
+            "rle6",
+        )
+
+    @property
+    def MOTOR_FROM_READABLE_TO_SERVER(self) -> dict:
+        """
+        Maps readable joint names to their simulator motor codes.
+        """
+        return {
+            "Head_yaw": "he1",
+            "Head_pitch": "he2",
+            "Left_Shoulder_Pitch": "lae1",
+            "Left_Shoulder_Roll": "lae2",
+            "Left_Elbow_Pitch": "lae3",
+            "Left_Elbow_Yaw": "lae4",
+            "Right_Shoulder_Pitch": "rae1",
+            "Right_Shoulder_Roll": "rae2",
+            "Right_Elbow_Pitch": "rae3",
+            "Right_Elbow_Yaw": "rae4",
+            "Waist": "te1",
+            "Left_Hip_Pitch": "lle1",
+            "Left_Hip_Roll": "lle2",
+            "Left_Hip_Yaw": "lle3",
+            "Left_Knee_Pitch": "lle4",
+            "Left_Ankle_Pitch": "lle5",
+            "Left_Ankle_Roll": "lle6",
+            "Right_Hip_Pitch": "rle1",
+            "Right_Hip_Roll": "rle2",
+            "Right_Hip_Yaw": "rle3",
+            "Right_Knee_Pitch": "rle4",
+            "Right_Ankle_Pitch": "rle5",
+            "Right_Ankle_Roll": "rle6",
+        }
+
+    @property
+    def MOTOR_SYMMETRY(self) -> list[str, bool]:
+        """
+        Defines pairs of symmetric motors and whether their direction is inverted.
+
+        Returns:
+            A dictionary where each key is a logical joint group name,
+            and the value is a tuple (motor_names, inverted).
+        """
+        return {
+            "Head_yaw": (("Head_yaw",), False),
+            "Head_pitch": (("Head_pitch",), False),
+            "Shoulder_Pitch": (
+                (
+                    "Left_Shoulder_Pitch",
+                    "Right_Shoulder_Pitch",
+                ),
+                False,
+            ),
+            "Shoulder_Roll": (
+                (
+                    "Left_Shoulder_Roll",
+                    "Right_Shoulder_Roll",
+                ),
+                True,
+            ),
+            "Elbow_Pitch": (
+                (
+                    "Left_Elbow_Pitch",
+                    "Right_Elbow_Pitch",
+                ),
+                False,
+            ),
+            "Elbow_Yaw": (
+                (
+                    "Left_Elbow_Yaw",
+                    "Right_Elbow_Yaw",
+                ),
+                True,
+            ),
+            "Waist": (("Waist",), False),
+            "Hip_Pitch": (
+                (
+                    "Left_Hip_Pitch",
+                    "Right_Hip_Pitch",
+                ),
+                False,
+            ),
+            "Hip_Roll": (
+                (
+                    "Left_Hip_Roll",
+                    "Right_Hip_Roll",
+                ),
+                True,
+            ),
+            "Hip_Yaw": (
+                (
+                    "Left_Hip_Yaw",
+                    "Right_Hip_Yaw",
+                ),
+                True,
+            ),
+            "Knee_Pitch": (
+                (
+                    "Left_Knee_Pitch",
+                    "Right_Knee_Pitch",
+                ),
+                False,
+            ),
+            "Ankle_Pitch": (
+                (
+                    "Left_Ankle_Pitch",
+                    "Right_Ankle_Pitch",
+                ),
+                False,
+            ),
+            "Ankle_Roll": (
+                (
+                    "Left_Ankle_Roll",
+                    "Right_Ankle_Roll",
+                ),
+                True,
+            ),
+        }

world/world.py

@@ -0,0 +1,66 @@
+from dataclasses import Field
+import numpy as np
+from world.commons.other_robot import OtherRobot
+from world.commons.field import FIFAField, HLAdultField
+from world.commons.play_mode import PlayModeEnum, PlayModeGroupEnum
+
+
+class World:
+    """
+    Represents the current simulation world, containing all relevant
+    information about the environment, the ball, and the robots.
+    """
+
+    MAX_PLAYERS_PER_TEAM = 11
+
+    def __init__(self, agent, team_name: str, number: int, field_name: str):
+        """
+        Initializes the world state.
+
+        Args:
+            agent: Reference to the agent that owns this world.
+            team_name (str): The name of the agent's team.
+            number (int): The player's number within the team.
+            field_name (str): The name of the field to initialize
+                              (e.g., 'fifa' or 'hl_adult').
+        """
+
+        from agent.base_agent import Agent  # type hinting
+
+        self.agent: Agent = agent
+        self.team_name: str = team_name
+        self.number: int = number
+        self.playmode: PlayModeEnum = PlayModeEnum.NOT_INITIALIZED
+        self.playmode_group: PlayModeGroupEnum = PlayModeGroupEnum.NOT_INITIALIZED
+        self.is_left_team: bool = None
+        self.game_time: float = None
+        self.server_time: float = None
+        self.score_left: int = None
+        self.score_right: int = None
+        self.their_team_name: str = None
+        self.last_server_time: str = None
+        self._global_cheat_position: np.ndarray = np.zeros(3)
+        self.global_position: np.ndarray = np.zeros(3)
+        self.ball_pos: np.ndarray = np.zeros(3)
+        self.is_ball_pos_updated: bool = False
+        self.our_team_players: list[OtherRobot] = [OtherRobot() for _ in range(self.MAX_PLAYERS_PER_TEAM)]
+        self.their_team_players: list[OtherRobot] = [OtherRobot(is_teammate=False) for _ in
+                                                     range(self.MAX_PLAYERS_PER_TEAM)]
+        self.field: Field = self.__initialize_field(field_name=field_name)
+
+    def update(self) -> None:
+        """
+        Updates the world state
+        """
+        self.playmode_group = PlayModeGroupEnum.get_group_from_playmode(
+            playmode=self.playmode, is_left_team=self.is_left_team
+        )
+
+    def is_fallen(self) -> bool:
+        return self.global_position[2] < 0.3
+
+    def __initialize_field(self, field_name: str) -> Field:
+        if field_name in ('hl_adult', 'hl_adult_2020', 'hl_adult_2019',):
+            return HLAdultField(world=self)
+        else:
+            return FIFAField(world=self)