{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "## Import libraries" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import sys\n", "import os\n", "import time\n", "import re\n", "import math\n", "from pathlib import Path\n", "sys.path.append(str(Path().resolve().parent))\n", "from simworld.config import Config\n", "from simworld.communicator.communicator import Communicator\n", "from simworld.communicator.unrealcv import UnrealCV\n", "from simworld.llm.base_llm import BaseLLM\n", "from simworld.map.map import Map\n", "from simworld.agent.humanoid import Humanoid\n", "from simworld.utils.vector import Vector" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Init" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "communicator = Communicator(UnrealCV())" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import os \n", "os.environ['OPENAI_API_KEY'] = ''" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Agent and Environment Classes\n", "\n", "**Agent**: Uses LLM to decide actions for navigation\n", "- Takes observation and target position as input\n", "- Calculates angle deviation to target (positive = turn left, negative = turn right)\n", "- Outputs simple commands: \"forward `duration`\", \"rotate `angle` `direction`\", or \"wait\"\n", "- Observation is a dict with 'position', 'direction', and 'ego_view'\n", "\n", "**Environment**: Gym-like interface that manages humanoid and executes actions\n", "- Defines target position at Vector(1700, -1700)\n", "- `reset()`: Spawns/resets humanoid at origin, returns initial observation dict\n", "- `step(action)`: Parses action string and calls humanoid movement functions\n", "- Returns: `(observation, reward, success)` where:\n", " - `observation` = {'position': Vector, 'direction': Vector (direction unit vector), 'ego_view': RGB image}\n", " - `reward` = negative distance to target (closer is better)\n", " - `success` = Boolean indicating if action was successfully parsed and executed" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class Agent():\n", " def __init__(self):\n", " self.llm = BaseLLM(\"gpt-4o\")\n", " self.system_prompt = \"\"\"You are an intelligent agent in a 3D simulation world.\n", "Your task is to navigate to the target position by choosing appropriate actions. Do not keep rotating in circles.\n", "\n", "You can only output ONE of the following actions:\n", "- \"forward \": Move forward for seconds (e.g., \"forward 2\")\n", "- \"rotate \": Rotate degrees in (left/right) (e.g., \"rotate 45 left\")\n", "- \"wait\": Do nothing for 1 second\n", "\n", "Output ONLY the action command, nothing else.\"\"\"\n", "\n", " def action(self, obs, target):\n", " \"\"\"Decide next action based on observation and target position.\n", " \n", " Args:\n", " obs: Dictionary containing:\n", " - 'position' (Vector): Current 2D position\n", " - 'direction' (Vector): Current direction unit vector\n", " - 'ego_view' (ndarray): First-person camera view\n", " target: Target position (Vector) to navigate to\n", " \n", " Returns:\n", " str: Action command\n", " \"\"\"\n", " position = obs['position']\n", " direction = obs['direction'] # Direction is now a Vector\n", " ego_view = obs['ego_view']\n", " \n", " # Convert direction vector to yaw angle\n", " current_yaw = math.degrees(math.atan2(direction.y, direction.x))\n", " \n", " # Calculate the angle to target\n", " delta_x = target.x - position.x\n", " delta_y = target.y - position.y\n", " target_angle = math.degrees(math.atan2(delta_y, delta_x))\n", " \n", " # Calculate angle difference (normalized to [-180, 180])\n", " angle_diff = target_angle - current_yaw\n", " # Normalize to [-180, 180]\n", " while angle_diff > 180:\n", " angle_diff -= 360\n", " while angle_diff < -180:\n", " angle_diff += 360\n", " \n", " prompt = f\"\"\"Current position: {position}\n", "Current direction: {direction} (direction vector, yaw: {current_yaw:.1f}°)\n", "Target position: {target}\n", "Distance to target: {position.distance(target):.1f}\n", "Angle to target: {angle_diff:.1f}° (positive = turn left, negative = turn right)\n", "\n", "Choose your next action to move closer to the target.\"\"\"\n", " action, time = self.llm.generate_text(system_prompt=self.system_prompt, user_prompt=prompt)\n", "\n", " # print(f\"Raw response: {action}\")\n", " return action" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class Environment:\n", " def __init__(self, communicator, config=Config()):\n", " self.communicator = communicator\n", " self.agent = None\n", " self.agent_name = None\n", " self.agent_spawned = False # Track if agent has been spawned\n", " self.target = None\n", " self.config = config\n", " self.map = Map(config)\n", " self.map.initialize_map_from_file(roads_file=os.path.join('../data/example_city/demo_city_1/roads.json'))\n", "\n", " def reset(self):\n", " \"\"\"Reset the humanoid to initial position and orientation.\n", " \n", " If agent hasn't been spawned yet, spawn it. Otherwise, reset its position and orientation.\n", " \"\"\"\n", " # Blueprint path for the humanoid agent to spawn in the UE level\n", " agent_bp = \"/Game/TrafficSystem/Pedestrian/Base_User_Agent.Base_User_Agent_C\"\n", "\n", " # Initial spawn position and facing direction for the humanoid (2D)\n", " spawn_location = Vector(0, 0)\n", " spawn_forward = Vector(1, 0)\n", " \n", " if not self.agent_spawned:\n", " # First time: Create and spawn the agent\n", " self.agent = Humanoid(\n", " communicator=self.communicator,\n", " position=spawn_location,\n", " direction=spawn_forward,\n", " config=self.config,\n", " map=self.map\n", " )\n", "\n", " # Spawn the humanoid agent in the Unreal world\n", " self.communicator.spawn_agent(self.agent, name=None, model_path=agent_bp, type=\"humanoid\")\n", " self.communicator.humanoid_set_speed(self.agent.id, 200) # Set walking speed\n", "\n", " # Cache the generated UE actor name\n", " self.agent_name = self.communicator.get_humanoid_name(self.agent.id)\n", " self.agent_spawned = True\n", " else:\n", " # Agent already exists: Reset position and orientation\n", " # Convert 2D position to 3D (x, y, z)\n", " location_3d = [\n", " spawn_location.x,\n", " spawn_location.y,\n", " 600 # Z coordinate (ground level for humanoid)\n", " ]\n", " \n", " # Convert 2D direction to 3D orientation (pitch, yaw, roll)\n", " orientation_3d = [\n", " 0, # Pitch\n", " math.degrees(math.atan2(spawn_forward.y, spawn_forward.x)), # Yaw\n", " 0 # Roll\n", " ]\n", " \n", " # Reset position and orientation\n", " self.communicator.unrealcv.set_location(location_3d, self.agent_name)\n", " self.communicator.unrealcv.set_orientation(orientation_3d, self.agent_name)\n", " \n", " # Update agent's internal state (direction setter expects yaw angle)\n", " spawn_yaw = math.degrees(math.atan2(spawn_forward.y, spawn_forward.x))\n", " self.agent.position = spawn_location\n", " self.agent.direction = spawn_yaw # Pass yaw angle, not Vector\n", "\n", " # Define a target position the agent is encouraged to move toward\n", " self.target = Vector(1700, -1700)\n", "\n", " time.sleep(5) # Wait for everything to spawn\n", "\n", " # Get initial observation (position, direction, and ego-view camera)\n", " loc_3d = self.communicator.unrealcv.get_location(self.agent_name) # Returns [x, y, z]\n", " position = Vector(loc_3d[0], loc_3d[1])\n", " \n", " orientation = self.communicator.unrealcv.get_orientation(self.agent_name) # Returns [pitch, yaw, roll]\n", " yaw = orientation[1] # Yaw angle in degrees\n", " # Convert yaw to direction vector\n", " direction = Vector(math.cos(math.radians(yaw)), math.sin(math.radians(yaw)))\n", " \n", " ego_view = self.communicator.get_camera_observation(self.agent.camera_id, \"lit\")\n", " \n", " observation = {\n", " 'position': position,\n", " 'direction': direction, # Direction vector (normalized)\n", " 'ego_view': ego_view\n", " }\n", " return observation\n", "\n", " def step(self, action):\n", " \"\"\"Parse and execute the action using humanoid movement functions.\n", " \n", " Returns:\n", " observation: Dict with 'position', 'direction', 'ego_view'\n", " reward: Negative distance to target\n", " success: Boolean indicating if action was successfully parsed and executed\n", " \"\"\"\n", " # Parse the action string and execute corresponding humanoid command\n", " # Remove surrounding quotes if present (LLM sometimes returns with quotes)\n", " action_cleaned = action.strip().strip('\"').strip(\"'\")\n", " action_lower = action_cleaned.lower().strip()\n", " success = False\n", " \n", " if action_lower.startswith(\"forward\"):\n", " # Extract duration: \"forward 2\" -> duration=2\n", " match = re.search(r'forward\\s+(\\d+\\.?\\d*)', action_lower)\n", " if match:\n", " duration = float(match.group(1))\n", " # Set move forward for duration\n", " self.communicator.humanoid_step_forward(self.agent.id, duration, direction=0)\n", " success = True\n", " else:\n", " print(f\"[Warning] Failed to parse forward action: '{action_cleaned}'. Expected format: 'forward '\")\n", " \n", " elif action_lower.startswith(\"rotate\"):\n", " # Extract angle and direction: \"rotate 45 left\" -> angle=45, direction=\"left\"\n", " match = re.search(r'rotate\\s+(\\d+\\.?\\d*)\\s+(left|right)', action_lower)\n", " if match:\n", " angle = float(match.group(1))\n", " direction = match.group(2)\n", " self.communicator.humanoid_rotate(self.agent.id, angle, direction)\n", " success = True\n", " else:\n", " print(f\"[Warning] Failed to parse rotate action: '{action_cleaned}'. Expected format: 'rotate '\")\n", " \n", " elif action_lower == \"wait\":\n", " time.sleep(1)\n", " success = True\n", " \n", " else:\n", " print(f\"[Warning] Unknown action: '{action_cleaned}'. Valid actions: 'forward ', 'rotate ', 'wait'\")\n", " time.sleep(0.5) # Wait a bit even if action is invalid\n", "\n", " # Get current position and direction from UE\n", " loc_3d = self.communicator.unrealcv.get_location(self.agent_name) # Returns [x, y, z]\n", " position = Vector(loc_3d[0], loc_3d[1])\n", " \n", " orientation = self.communicator.unrealcv.get_orientation(self.agent_name) # Returns [pitch, yaw, roll]\n", " yaw = orientation[1] # Yaw angle in degrees\n", " # Convert yaw to direction vector\n", " direction = Vector(math.cos(math.radians(yaw)), math.sin(math.radians(yaw)))\n", "\n", " # Update agent state (direction setter expects yaw angle, not Vector)\n", " self.agent.position = position\n", " self.agent.direction = yaw # This will automatically convert yaw to direction vector\n", "\n", " # Get ego-view camera observation\n", " ego_view = self.communicator.get_camera_observation(self.agent.camera_id, \"lit\")\n", "\n", " # Observation includes position, direction, and camera view\n", " observation = {\n", " 'position': position,\n", " 'direction': direction, # Direction vector (normalized)\n", " 'ego_view': ego_view\n", " }\n", "\n", " # Reward: negative Euclidean distance to target\n", " reward = -position.distance(self.target)\n", "\n", " return observation, reward, success" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Implementation Details\n", "\n", "**Observation Format:**\n", "- Dictionary with three keys:\n", " - `'position'`: 2D Vector (x, y) coordinates in the simulation world\n", " - `'direction'`: 2D Vector representing the direction the humanoid is facing (normalized unit vector)\n", " - `'ego_view'`: RGB image from humanoid's first-person camera\n", "\n", "**Action Parsing:**\n", "- Valid action formats:\n", " - `\"forward \"` - e.g., \"forward 2\"\n", " - `\"rotate \"` - e.g., \"rotate 45 left\"\n", " - `\"wait\"` - pause for 1 second\n", "- Action cleaning: Surrounding quotes are automatically removed (LLM may return `\"rotate 45 left\"` with quotes)\n", "- If action parsing fails:\n", " - A warning message is printed\n", " - The agent waits 0.5 seconds\n", " - `step()` returns `success=False` and step details are not displayed\n", "\n", "**Available Humanoid Movement Functions:**\n", "- `humanoid_set_speed(humanoid_id, speed)`: Set movement speed (e.g., 200)\n", "- `humanoid_step_forward(humanoid_id, duration, direction)`: Move forward for duration seconds\n", "- `humanoid_rotate(humanoid_id, angle, direction)`: Rotate by angle degrees (\"left\" or \"right\")" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Main Loop\n", "\n", "The agent navigates from origin (0, 0) to the target position (1700, -1700) by:\n", "1. Observing current position, direction (unit vector), and ego-view camera\n", "2. Computing angle deviation to target from direction vector (helps agent decide to turn left or right)\n", "3. Deciding next action using LLM based on observation, distance, and angle deviation\n", "4. Executing action via humanoid movement functions\n", "5. Receiving reward based on distance to target (negative distance)" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "agent = Agent()\n", "env = Environment(communicator)\n", "\n", "obs = env.reset()\n", "\n", "print(f\"Task: Navigate to target position {env.target}\")\n", "print(f\"Starting position: {obs['position']}\\n\")\n", "\n", "# Roll out a short trajectory\n", "for i in range(100):\n", " # Agent decides next action based on current observation and target\n", " action = agent.action(obs, env.target)\n", " print(f\"Step {i+1}: Action = '{action}'\")\n", " \n", " # Execute action and get new observation\n", " obs, reward, success = env.step(action)\n", " \n", " # Only print details if action was successfully executed\n", " if success:\n", " position = obs['position']\n", " direction = obs['direction'] # Direction is now a Vector\n", " \n", " # Convert direction vector to yaw angle\n", " current_yaw = math.degrees(math.atan2(direction.y, direction.x))\n", " \n", " # Calculate angle to target for display\n", " delta_x = env.target.x - position.x\n", " delta_y = env.target.y - position.y\n", " target_angle = math.degrees(math.atan2(delta_y, delta_x))\n", " angle_diff = target_angle - current_yaw\n", " # Normalize to [-180, 180]\n", " while angle_diff > 180:\n", " angle_diff -= 360\n", " while angle_diff < -180:\n", " angle_diff += 360\n", " \n", " print(f\" Position: {position}, Direction: {direction} (yaw: {current_yaw:.1f}°)\")\n", " print(f\" Reward: {reward:.2f}, Distance to target: {position.distance(env.target):.1f}\")\n", " print(f\" Angle to target: {angle_diff:+.1f}° ({'turn left' if angle_diff > 0 else 'turn right'})\\n\")\n", " communicator.show_img(obs['ego_view'])\n", " \n", " # Check if reached target (within 100 units)\n", " if position.distance(env.target) < 200:\n", " print(f\"Target reached! Final position: {position}, Direction: {direction} (yaw: {current_yaw:.1f}°)\")\n", " break\n", " else:\n", " print(f\" [Action failed - skipping step]\\n\")" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "communicator.disconnect()" ] } ], "metadata": { "kernelspec": { "display_name": "simworld", "language": "python", "name": "python3" }, "language_info": { "codemirror_mode": { "name": "ipython", "version": 3 }, "file_extension": ".py", "mimetype": "text/x-python", "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", "version": "3.10.18" } }, "nbformat": 4, "nbformat_minor": 2 }