diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..09f2d87 --- /dev/null +++ b/.gitignore @@ -0,0 +1,49 @@ +# Python +__pycache__/ +*.py[cod] +*.pyo +*.pyd +.Python +*.egg +*.egg-info/ +dist/ +build/ +eggs/ +parts/ +var/ +sdist/ +develop-eggs/ +.installed.cfg +lib/ +lib64/ + +# Virtual environments +.env +.venv +env/ +venv/ + +# Testing +.pytest_cache/ +.coverage +htmlcov/ +.tox/ + +# IDEs +.vscode/ +.idea/ +*.swp +*.swo + +# Models (large binary files) +models/*.pt +models/*.pth +models/*.onnx + +# Logs +*.log +logs/ + +# OS +.DS_Store +Thumbs.db diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..8fcd0b7 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,56 @@ +# Dockerfile – rag7 AGI Robotics Framework +# Multi-stage build: install deps then copy source. + +# --------------------------------------------------------------------------- +# Stage 1: Builder – install Python dependencies +# --------------------------------------------------------------------------- +FROM python:3.10-slim AS builder + +WORKDIR /build + +# Install system build tools +RUN apt-get update && apt-get install -y --no-install-recommends \ + gcc \ + g++ \ + cmake \ + git \ + && rm -rf /var/lib/apt/lists/* + +# Install Python dependencies +COPY requirements.txt . +RUN pip install --no-cache-dir --user \ + torch>=2.0.0 \ + torchvision>=0.15.0 \ + numpy>=1.24.0 \ + scipy>=1.10.0 \ + opencv-python-headless>=4.7.0 \ + pyyaml>=6.0 \ + pytest>=7.0.0 \ + pytest-cov>=4.0.0 + +# --------------------------------------------------------------------------- +# Stage 2: Runtime – lean image with the application +# --------------------------------------------------------------------------- +FROM python:3.10-slim AS runtime + +WORKDIR /app + +# Copy installed Python packages from the builder stage +COPY --from=builder /root/.local /root/.local + +# Install minimal runtime system libraries (OpenCV needs libGL) +RUN apt-get update && apt-get install -y --no-install-recommends \ + libglib2.0-0 \ + libsm6 \ + libxext6 \ + && rm -rf /var/lib/apt/lists/* + +# Copy source code +COPY . . + +# Ensure user-installed packages are on the path +ENV PATH="/root/.local/bin:$PATH" +ENV PYTHONPATH="/app" + +# Default command: run the test suite +CMD ["python", "-m", "pytest", "tests/", "-v", "--tb=short"] diff --git a/README.md b/README.md index f5a8ce3..7845570 100644 --- a/README.md +++ b/README.md @@ -1 +1,206 @@ -# rag7 \ No newline at end of file +# rag7 – Agentic AGI Robotics Framework + +![Python 3.10+](https://img.shields.io/badge/python-3.10%2B-blue) +![License MIT](https://img.shields.io/badge/license-MIT-green) +![Tests](https://img.shields.io/badge/tests-pytest-orange) +![ROS2 Optional](https://img.shields.io/badge/ROS2-optional-lightgrey) +![PyTorch](https://img.shields.io/badge/PyTorch-2.0%2B-red) + +A production-quality, multi-agent AGI robotics framework that integrates +perception, planning, natural language control, reinforcement learning, and +ROS2 communication into a unified, extensible system. + +--- + +## Features + +- **Multi-agent architecture** – PerceptionAgent, PlanningAgent, ControlAgent, + CommunicationAgent, CoordinationAgent, all built on a common `BaseAgent` + abstract class with perceive → reason → act loops. +- **Natural language control** – Parse free-text operator commands into + structured robot actions via intent classification and LLM integration + (LangChain / GPT-4, with a rule-based fallback). +- **Modular perception** – RGB-D object detection, semantic segmentation, + IoU multi-object tracking, occupancy-grid SLAM, and sensor fusion. +- **Intelligent planning** – A* path planning, joint-space motion planning + with IK/FK, LLM-powered task decomposition, and safety-aware decision making. +- **Reinforcement learning** – DQN, PPO, experience replay, and behavioural + cloning trainers built on PyTorch. +- **ROS2 integration** – Optional rclpy node wrappers for every agent; + graceful fallback to mock mode when ROS2 is not installed. +- **Simulation** – Gazebo-compatible mock environment for policy development. + +--- + +## Quick Start + +```python +from agents.robotics_agi import RoboticsAGI + +agi = RoboticsAGI(config_path="config") + +# Natural language control +result = agi.execute_command("navigate to the charging station") +print(result["response"]) # "Understood. I will navigate to the specified location." + +# Task API +task = agi.create_task("grasp", target_object="red_cube") +result = agi.execute_task(task) +print(result) # {"success": True, "details": "Grasped 'red_cube'"} + +# System status +print(agi.get_status()["system"]) # "online" +``` + +--- + +## Installation + +```bash +git clone https://github.com/your-org/rag7.git +cd rag7 +pip install -e . # core only +pip install -e ".[llm,dev]" # + LangChain + testing tools +``` + +See [docs/installation.md](docs/installation.md) for full instructions +including ROS2 setup and Docker. + +--- + +## Project Structure + +``` +rag7/ +├── agents/ # Core agent classes +│ ├── base_agent.py +│ ├── perception_agent.py +│ ├── planning_agent.py +│ ├── control_agent.py +│ ├── communication_agent.py +│ ├── coordination_agent.py +│ └── robotics_agi.py # Top-level orchestrator +├── perception/ # Sensor processing +│ ├── vision/ # Detection, segmentation, tracking +│ ├── slam/ # Mapping & localisation +│ └── sensor_fusion.py +├── planning/ # Task, path, motion planning +├── nlp/ # NLP pipeline +├── learning/ # RL & imitation learning +│ ├── rl/ # DQN, PPO, ReplayBuffer +│ └── imitation/ # Behavioural cloning +├── ros2_interface/ # ROS2 node wrappers +│ ├── ros2_nodes/ +│ └── launch/ +├── simulation/ # Gazebo environment +├── config/ # YAML configuration files +├── examples/ # Runnable example scripts +├── tests/ # pytest test suite +├── docs/ # Documentation +├── Dockerfile +├── requirements.txt +└── setup.py +``` + +--- + +## Architecture Overview + +The system follows a hierarchical multi-agent architecture: + +``` +Operator NL input + │ + ▼ +CommunicationAgent ──► parse intent & entities + │ + ▼ +PlanningAgent ──► decompose goal into steps + │ + ▼ +ControlAgent ──► execute steps (navigate / grasp / stop) + │ + ▼ +ROS2Interface (optional) ──► robot hardware +``` + +See [docs/architecture.md](docs/architecture.md) for the full diagram. + +--- + +## Usage Examples + +### Navigation + +```python +agi = RoboticsAGI(config_path="config") +agi.execute_command("go to the storage room") +``` + +### Object Manipulation + +```python +agi.execute_command("pick up the blue box and place it on shelf B") +``` + +### Multi-Robot Coordination + +```python +from agents.control_agent import ControlAgent + +robot_a = ControlAgent(config={}) +robot_b = ControlAgent(config={}) +task = agi.create_task("navigate", location={"x": 5.0, "y": 5.0}) +agi.coordinate_robots([robot_a, robot_b], task) +``` + +### Reinforcement Learning + +```python +from learning.rl.dqn_agent import DQNAgent +from learning.rl.replay_buffer import ReplayBuffer + +agent = DQNAgent(state_dim=8, action_dim=4) +buffer = ReplayBuffer(capacity=10000) +buffer.push(state, action, reward, next_state, done) +batch = buffer.sample(32) +loss = agent.update(batch) +``` + +--- + +## Configuration + +All runtime parameters live in `config/`: + +| File | Description | +|------|-------------| +| `robot_config.yaml` | Sensor topics, dimensions, safety thresholds | +| `agent_config.yaml` | Per-agent update rates, tolerances, memory | +| `llm_config.yaml` | LLM provider, model, temperature, API key env var | + +--- + +## Running Tests + +```bash +python -m pytest tests/ -v +python -m pytest tests/ -v --cov=. --cov-report=html +``` + +--- + +## Contributing + +1. Fork the repository and create a feature branch. +2. Follow PEP 8 and add docstrings to all public classes and functions. +3. Add or update tests for any changed behaviour. +4. Open a pull request with a clear description. + +--- + +## License + +MIT License – see [LICENSE](LICENSE) for details. + +© 2024 Stacey Williams \ No newline at end of file diff --git a/agents/__init__.py b/agents/__init__.py new file mode 100644 index 0000000..0d7941e --- /dev/null +++ b/agents/__init__.py @@ -0,0 +1,25 @@ +""" +Agents package for the RAG7 AGI Robotics Framework. + +This package provides the core agent classes that implement +perceive-reason-act loops for autonomous robot control. +""" + +from agents.base_agent import BaseAgent, AgentState +from agents.perception_agent import PerceptionAgent +from agents.planning_agent import PlanningAgent +from agents.control_agent import ControlAgent +from agents.communication_agent import CommunicationAgent +from agents.coordination_agent import CoordinationAgent +from agents.robotics_agi import RoboticsAGI + +__all__ = [ + "BaseAgent", + "AgentState", + "PerceptionAgent", + "PlanningAgent", + "ControlAgent", + "CommunicationAgent", + "CoordinationAgent", + "RoboticsAGI", +] diff --git a/agents/base_agent.py b/agents/base_agent.py new file mode 100644 index 0000000..2dc3be2 --- /dev/null +++ b/agents/base_agent.py @@ -0,0 +1,180 @@ +""" +Base agent module for the RAG7 AGI Robotics Framework. + +Provides the abstract base class and supporting data structures +for all agents in the system. +""" + +import logging +import time +from abc import ABC, abstractmethod +from dataclasses import dataclass, field +from typing import Any, Dict, List, Optional + + +@dataclass +class AgentState: + """Dataclass representing the runtime state of an agent. + + Attributes: + agent_id: Unique identifier for the agent. + status: Current status ('idle', 'running', 'error'). + current_task: Description of the current task being executed. + last_update: Unix timestamp of the last state update. + """ + + agent_id: str + status: str = "idle" + current_task: Optional[str] = None + last_update: float = field(default_factory=time.time) + + +class BaseAgent(ABC): + """Abstract base class for all AGI robotic agents. + + All agents in the RAG7 framework inherit from this class and must + implement the perceive-reason-act loop methods. + + Args: + name: Human-readable name for this agent instance. + config: Configuration dictionary for agent parameters. + logger: Optional logger instance; creates a default logger if not provided. + """ + + def __init__( + self, + name: str, + config: Dict[str, Any], + logger: Optional[logging.Logger] = None, + ) -> None: + """Initialize the base agent with name, config, and optional logger.""" + self._name = name + self._config = config + self._logger = logger or logging.getLogger(f"rag7.{name}") + self._state: Dict[str, Any] = {} + self._memory: List[Any] = [] + self._tools: List[Any] = [] + self._agent_state = AgentState(agent_id=name) + + # ------------------------------------------------------------------ + # Properties + # ------------------------------------------------------------------ + + @property + def name(self) -> str: + """Return the agent's name.""" + return self._name + + @property + def state(self) -> Dict[str, Any]: + """Return the agent's current state dictionary.""" + return self._state + + @property + def memory(self) -> List[Any]: + """Return the agent's memory list.""" + return self._memory + + @property + def tools(self) -> List[Any]: + """Return the list of registered tools.""" + return self._tools + + # ------------------------------------------------------------------ + # Abstract methods – must be implemented by subclasses + # ------------------------------------------------------------------ + + @abstractmethod + def perceive(self, observation: Any) -> Any: + """Process an observation from the environment. + + Args: + observation: Raw sensor data or environment observation. + + Returns: + Processed perception output. + """ + + @abstractmethod + def reason(self, context: Any) -> Any: + """Reason over perception data to produce a plan or decision. + + Args: + context: Contextual information for reasoning. + + Returns: + Reasoning output (plan, decision, etc.). + """ + + @abstractmethod + def act(self, action: Any) -> Any: + """Execute an action in the environment. + + Args: + action: Action specification to execute. + + Returns: + Result of the action. + """ + + # ------------------------------------------------------------------ + # Concrete utility methods + # ------------------------------------------------------------------ + + def update_state(self, key: str, value: Any) -> None: + """Update a single key in the agent's state dictionary. + + Args: + key: State key to update. + value: New value for the key. + """ + self._state[key] = value + self._agent_state.last_update = time.time() + self._logger.debug("State updated: %s = %s", key, value) + + def add_to_memory(self, item: Any) -> None: + """Append an item to the agent's memory. + + Respects the capacity limit defined in config key 'memory_capacity' + (defaults to 100). Oldest items are evicted when capacity is exceeded. + + Args: + item: Item to store in memory. + """ + capacity = self._config.get("memory_capacity", 100) + self._memory.append(item) + if len(self._memory) > capacity: + self._memory.pop(0) + + def get_memory(self, n: int = 10) -> List[Any]: + """Return the most recent *n* items from memory. + + Args: + n: Number of recent items to retrieve (default 10). + + Returns: + List of the most recent memory items. + """ + return self._memory[-n:] + + def clear_memory(self) -> None: + """Clear all items from the agent's memory.""" + self._memory.clear() + self._logger.debug("Memory cleared for agent '%s'.", self._name) + + def register_tool(self, tool: Any) -> None: + """Register a callable tool for use by this agent. + + Args: + tool: Tool object or callable to register. + """ + self._tools.append(tool) + self._logger.debug("Tool registered: %s", tool) + + def get_tools(self) -> List[Any]: + """Return all registered tools. + + Returns: + List of registered tool objects. + """ + return list(self._tools) diff --git a/agents/communication_agent.py b/agents/communication_agent.py new file mode 100644 index 0000000..0bee1b2 --- /dev/null +++ b/agents/communication_agent.py @@ -0,0 +1,288 @@ +""" +Communication agent module for the RAG7 AGI Robotics Framework. + +Handles natural language understanding and generation to enable +human-robot interaction through text-based commands. +""" + +import logging +from typing import Any, Dict, List, Optional + +from agents.base_agent import BaseAgent + +try: + from langchain_openai import ChatOpenAI # noqa: F401 + + LANGCHAIN_AVAILABLE = True +except ImportError: + LANGCHAIN_AVAILABLE = False + + +class CommunicationAgent(BaseAgent): + """Agent responsible for natural language communication. + + Parses incoming text commands into structured intents and generates + natural language responses. Uses LangChain with an LLM backend when + available; falls back to keyword-based parsing otherwise. + + Args: + config: Communication agent configuration dictionary. + llm_config: Optional LLM configuration dictionary. + """ + + # Supported intent labels + INTENTS = ["navigate", "grasp", "place", "inspect", "stop", "query", "unknown"] + + def __init__( + self, + config: Dict[str, Any], + llm_config: Optional[Dict[str, Any]] = None, + ) -> None: + """Initialize the communication agent.""" + super().__init__( + name="communication_agent", + config=config, + logger=logging.getLogger("rag7.communication"), + ) + self._llm_config = llm_config or {} + self._llm = None + self._dialog_history: List[Dict[str, str]] = [] + self._max_history = config.get("max_dialog_history", 10) + + if LANGCHAIN_AVAILABLE and self._llm_config.get("provider") == "openai": + self._try_init_llm() + + # ------------------------------------------------------------------ + # Abstract method implementations + # ------------------------------------------------------------------ + + def perceive(self, observation: Dict[str, Any]) -> Dict[str, Any]: + """Process an incoming message observation. + + Args: + observation: Dictionary with optional ``message`` key + containing the user text. + + Returns: + Parsed command dict produced by :meth:`parse_command`. + """ + message = observation.get("message", "") + parsed = self.parse_command(message) if message else {} + self.update_state("last_parsed_command", parsed) + return parsed + + def reason(self, context: Any) -> Dict[str, Any]: + """Generate an appropriate text response for the given context. + + Args: + context: Dictionary with ``intent`` and ``robot_state`` keys, + or a plain string. + + Returns: + Dictionary with ``response`` string. + """ + if isinstance(context, str): + response = self.generate_response({"intent": self.classify_intent(context)}) + else: + response = self.generate_response(context) + return {"response": response} + + def act(self, action: Dict[str, Any]) -> Dict[str, Any]: + """Send or receive a message. + + Args: + action: Dictionary with ``type`` key (``send`` or ``receive``) + and ``message`` payload. + + Returns: + Result dictionary. + """ + action_type = action.get("type", "") + message = action.get("message", "") + + if action_type == "send": + self._dialog_history.append({"role": "robot", "content": message}) + if len(self._dialog_history) > self._max_history: + self._dialog_history.pop(0) + return {"status": "sent", "message": message} + + if action_type == "receive": + parsed = self.parse_command(message) + self._dialog_history.append({"role": "human", "content": message}) + if len(self._dialog_history) > self._max_history: + self._dialog_history.pop(0) + return {"status": "received", "parsed": parsed} + + return {"status": "unknown_action"} + + # ------------------------------------------------------------------ + # Public NLP API + # ------------------------------------------------------------------ + + def parse_command(self, text: str) -> Dict[str, Any]: + """Parse a natural language command into a structured representation. + + Args: + text: Raw natural language input from the operator. + + Returns: + Dictionary with keys: + - ``intent``: Detected intent string. + - ``action``: Primary action verb. + - ``target_object``: Target object (if detected). + - ``location``: Target location (if detected). + - ``confidence``: Confidence score between 0 and 1. + """ + if self._llm is not None: + try: + return self._llm_parse(text) + except Exception as exc: # noqa: BLE001 + self._logger.warning("LLM parsing failed: %s", exc) + + return self._rule_based_parse(text) + + def generate_response(self, context: Dict[str, Any]) -> str: + """Generate a natural language response for the given context. + + Args: + context: Dictionary with ``intent`` and optional + ``robot_state`` keys. + + Returns: + Response string. + """ + intent = context.get("intent", "unknown") + robot_state = context.get("robot_state", {}) + + if self._llm is not None: + try: + prompt = ( + f"Robot status: {robot_state}\n" + f"Intent: {intent}\n" + "Generate a concise robot status response:" + ) + resp = self._llm.invoke(prompt) + return resp.content.strip() + except Exception as exc: # noqa: BLE001 + self._logger.warning("LLM response generation failed: %s", exc) + + return self._template_response(intent, robot_state) + + def classify_intent(self, text: str) -> str: + """Classify the primary intent of a text string. + + Args: + text: Natural language input. + + Returns: + Intent string from :attr:`INTENTS`. + """ + text_lower = text.lower() + + keyword_map = { + "navigate": ["go", "move", "navigate", "drive", "travel", "head"], + "grasp": ["grasp", "pick", "grab", "take", "hold", "lift"], + "place": ["place", "put", "drop", "set", "release"], + "inspect": ["inspect", "look", "examine", "scan", "check", "observe"], + "stop": ["stop", "halt", "freeze", "pause", "emergency"], + "query": ["what", "where", "how", "status", "tell", "show", "report"], + } + + for intent, keywords in keyword_map.items(): + if any(kw in text_lower for kw in keywords): + return intent + + return "unknown" + + # ------------------------------------------------------------------ + # Private helpers + # ------------------------------------------------------------------ + + def _rule_based_parse(self, text: str) -> Dict[str, Any]: + """Fallback rule-based command parser. + + Args: + text: Raw natural language input. + + Returns: + Structured command dictionary. + """ + intent = self.classify_intent(text) + words = text.lower().split() + + # Extract simple numeric location tokens + location = None + for i, word in enumerate(words): + if word in ("to", "at", "towards") and i + 1 < len(words): + location = words[i + 1] + break + + return { + "intent": intent, + "action": intent, + "target_object": None, + "location": location, + "confidence": 0.6, + } + + def _llm_parse(self, text: str) -> Dict[str, Any]: + """Parse a command using the LLM backend. + + Args: + text: Raw natural language input. + + Returns: + Structured command dictionary. + """ + prompt = ( + f"Parse this robot command: '{text}'\n" + "Return JSON with keys: intent, action, target_object, location, confidence." + ) + import json + + response = self._llm.invoke(prompt) + try: + return json.loads(response.content) + except json.JSONDecodeError: + return self._rule_based_parse(text) + + def _template_response(self, intent: str, robot_state: Dict[str, Any]) -> str: + """Generate a templated response string. + + Args: + intent: Detected intent. + robot_state: Current robot state dictionary. + + Returns: + Human-readable response string. + """ + templates = { + "navigate": "Understood. Navigating to the specified location.", + "grasp": "Acknowledged. Attempting to grasp the object.", + "place": "Understood. Placing the object at the target location.", + "inspect": "Initiating inspection of the specified area.", + "stop": "Emergency stop acknowledged. All motion halted.", + "query": f"Current robot state: {robot_state}", + "unknown": "Command not understood. Please rephrase.", + } + return templates.get(intent, templates["unknown"]) + + def _try_init_llm(self) -> None: + """Attempt to initialise the LangChain LLM client.""" + try: + import os + + from langchain_openai import ChatOpenAI + + api_key = os.environ.get( + self._llm_config.get("api_key_env", "OPENAI_API_KEY"), "" + ) + if not api_key: + return + self._llm = ChatOpenAI( + model=self._llm_config.get("model", "gpt-4"), + temperature=self._llm_config.get("temperature", 0.1), + openai_api_key=api_key, + ) + except Exception as exc: # noqa: BLE001 + self._logger.warning("Failed to initialise LLM for communication: %s", exc) diff --git a/agents/control_agent.py b/agents/control_agent.py new file mode 100644 index 0000000..e3b3994 --- /dev/null +++ b/agents/control_agent.py @@ -0,0 +1,226 @@ +""" +Control agent module for the RAG7 AGI Robotics Framework. + +Handles low-level robot control including navigation, manipulation, +and emergency stop functionality. +""" + +import logging +from typing import Any, Dict, Optional + +from agents.base_agent import BaseAgent + +try: + import rclpy # noqa: F401 + + ROS2_AVAILABLE = True +except ImportError: + ROS2_AVAILABLE = False + + +class ControlAgent(BaseAgent): + """Agent responsible for low-level robot control execution. + + Translates high-level action commands into robot actuator commands, + optionally publishing to a ROS2 system. + + Args: + config: Control agent configuration dictionary. + ros2_enabled: If True and rclpy is available, use ROS2 communication. + """ + + def __init__( + self, + config: Dict[str, Any], + ros2_enabled: bool = False, + ) -> None: + """Initialize the control agent.""" + super().__init__( + name="control_agent", + config=config, + logger=logging.getLogger("rag7.control"), + ) + self._ros2_enabled = ros2_enabled and ROS2_AVAILABLE + self._is_stopped: bool = False + self._position: Dict[str, float] = {"x": 0.0, "y": 0.0, "theta": 0.0} + self._velocity: Dict[str, float] = {"linear": 0.0, "angular": 0.0} + self._control_frequency = config.get("control_frequency", 50) + self._position_tolerance = config.get("position_tolerance", 0.05) + self._orientation_tolerance = config.get("orientation_tolerance", 0.1) + + if self._ros2_enabled: + self._init_ros2() + + # ------------------------------------------------------------------ + # Abstract method implementations + # ------------------------------------------------------------------ + + def perceive(self, observation: Dict[str, Any]) -> Dict[str, Any]: + """Update the internal robot state from sensor observations. + + Args: + observation: Dictionary containing ``position`` and/or + ``velocity`` sub-dicts. + + Returns: + Updated robot state. + """ + if "position" in observation: + self._position.update(observation["position"]) + self.update_state("position", dict(self._position)) + if "velocity" in observation: + self._velocity.update(observation["velocity"]) + self.update_state("velocity", dict(self._velocity)) + return {"position": self._position, "velocity": self._velocity} + + def reason(self, context: Any) -> Dict[str, Any]: + """Determine required control actions from current context. + + Args: + context: Dictionary with ``target_position`` and optional + ``target_orientation`` keys. + + Returns: + Recommended control action dictionary. + """ + if self._is_stopped: + return {"action": "stop", "reason": "emergency_stop_active"} + + target = context.get("target_position") if isinstance(context, dict) else None + if target: + dx = target.get("x", 0.0) - self._position["x"] + dy = target.get("y", 0.0) - self._position["y"] + distance = (dx**2 + dy**2) ** 0.5 + if distance < self._position_tolerance: + return {"action": "stop", "reason": "goal_reached"} + return {"action": "navigate", "target": target, "distance": distance} + + return {"action": "idle"} + + def act(self, action: Dict[str, Any]) -> Dict[str, Any]: + """Execute a control action on the robot. + + Args: + action: Dictionary with a ``type`` key specifying the action. + Supported types: ``navigate``, ``grasp``, ``stop``. + + Returns: + Result dictionary with ``success`` and ``message`` keys. + """ + action_type = action.get("type", "") + + if self._is_stopped and action_type != "reset_stop": + return {"success": False, "message": "Emergency stop is active."} + + if action_type == "navigate": + x = action.get("x", self._position["x"]) + y = action.get("y", self._position["y"]) + theta = action.get("theta", self._position["theta"]) + success = self.navigate_to(x, y, theta) + return {"success": success, "message": f"Navigate to ({x}, {y})"} + + if action_type == "grasp": + object_id = action.get("object_id", "unknown") + success = self.execute_grasp(object_id) + return {"success": success, "message": f"Grasp object '{object_id}'"} + + if action_type == "stop": + self.emergency_stop() + return {"success": True, "message": "Emergency stop executed."} + + if action_type == "reset_stop": + self._is_stopped = False + return {"success": True, "message": "Emergency stop cleared."} + + self._logger.warning("Unknown control action type: %s", action_type) + return {"success": False, "message": f"Unknown action: {action_type}"} + + # ------------------------------------------------------------------ + # Public control API + # ------------------------------------------------------------------ + + def navigate_to(self, x: float, y: float, theta: float) -> bool: + """Command the robot to navigate to a target pose. + + In simulation / no-ROS mode this immediately updates the internal + pose; with ROS2 enabled, the goal is published to the navigation + stack. + + Args: + x: Target X position in metres. + y: Target Y position in metres. + theta: Target heading in radians. + + Returns: + True if the navigation command was accepted successfully. + """ + if self._is_stopped: + self._logger.warning("Cannot navigate – emergency stop is active.") + return False + + self._logger.info("Navigating to (%.3f, %.3f, %.3f).", x, y, theta) + + if self._ros2_enabled: + return self._ros2_navigate(x, y, theta) + + # Simulate movement by updating internal state + self._position = {"x": x, "y": y, "theta": theta} + self.update_state("position", dict(self._position)) + return True + + def execute_grasp(self, object_id: str) -> bool: + """Command the arm to grasp a detected object. + + Args: + object_id: Identifier of the object to grasp. + + Returns: + True if the grasp was executed successfully. + """ + if self._is_stopped: + self._logger.warning("Cannot grasp – emergency stop is active.") + return False + + self._logger.info("Executing grasp on object '%s'.", object_id) + self.update_state("grasped_object", object_id) + return True + + def emergency_stop(self) -> None: + """Immediately halt all robot motion. + + Sets the internal stopped flag and zeros all velocity commands. + When ROS2 is enabled, also publishes to the emergency stop topic. + """ + self._is_stopped = True + self._velocity = {"linear": 0.0, "angular": 0.0} + self.update_state("is_stopped", True) + self._logger.warning("EMERGENCY STOP activated.") + + if self._ros2_enabled: + self._ros2_emergency_stop() + + # ------------------------------------------------------------------ + # Private ROS2 helpers + # ------------------------------------------------------------------ + + def _init_ros2(self) -> None: + """Initialise ROS2 publishers and subscribers.""" + try: + import rclpy + + if not rclpy.ok(): + rclpy.init() + self._logger.info("ROS2 control interface initialised.") + except Exception as exc: # noqa: BLE001 + self._logger.warning("ROS2 init failed: %s", exc) + self._ros2_enabled = False + + def _ros2_navigate(self, x: float, y: float, theta: float) -> bool: + """Publish navigation goal via ROS2.""" + self._logger.debug("ROS2 navigate_to (%.3f, %.3f, %.3f)", x, y, theta) + # Stub: real implementation would publish geometry_msgs/PoseStamped + return True + + def _ros2_emergency_stop(self) -> None: + """Publish emergency stop signal via ROS2.""" + self._logger.debug("ROS2 emergency stop published.") diff --git a/agents/coordination_agent.py b/agents/coordination_agent.py new file mode 100644 index 0000000..85ab457 --- /dev/null +++ b/agents/coordination_agent.py @@ -0,0 +1,209 @@ +""" +Coordination agent module for the RAG7 AGI Robotics Framework. + +Manages multi-agent task allocation and coordination across a fleet +of robotic agents. +""" + +import logging +from typing import Any, Dict, List, Optional + +from agents.base_agent import BaseAgent + + +class CoordinationAgent(BaseAgent): + """Agent responsible for multi-robot task coordination. + + Maintains a registry of managed agents and allocates tasks to them + using a capability-based strategy. + + Args: + config: Coordination agent configuration dictionary. + """ + + def __init__(self, config: Dict[str, Any]) -> None: + """Initialize the coordination agent.""" + super().__init__( + name="coordination_agent", + config=config, + logger=logging.getLogger("rag7.coordination"), + ) + self._agents: List[BaseAgent] = [] + self._task_allocations: Dict[str, str] = {} # task_id -> agent_name + self._strategy = config.get("task_allocation_strategy", "capability_based") + self._max_robots = config.get("max_robots", 10) + + # ------------------------------------------------------------------ + # Abstract method implementations + # ------------------------------------------------------------------ + + def perceive(self, observation: Dict[str, Any]) -> Dict[str, Any]: + """Update multi-agent state from an observation. + + Args: + observation: Dictionary containing agent status updates. + + Returns: + Current aggregated multi-agent state. + """ + if "agent_updates" in observation: + for agent_name, status in observation["agent_updates"].items(): + self.update_state(f"agent_{agent_name}_status", status) + return self.get_agent_status() + + def reason(self, context: Any) -> Dict[str, Any]: + """Determine optimal task allocation across managed agents. + + Args: + context: Dictionary with a ``tasks`` key listing task dicts, + or a single task dict. + + Returns: + Allocation plan dictionary. + """ + tasks = [] + if isinstance(context, dict): + tasks = context.get("tasks", [context] if "task_type" in context else []) + elif isinstance(context, list): + tasks = context + + if not tasks: + return {"status": "no_tasks", "allocations": {}} + + allocations = self.coordinate(tasks) + return {"status": "allocated", "allocations": allocations} + + def act(self, action: Dict[str, Any]) -> Dict[str, Any]: + """Coordinate agents according to the given action. + + Args: + action: Dictionary with ``type`` key. + Supported: ``allocate``, ``status``, ``broadcast``. + + Returns: + Result dictionary. + """ + action_type = action.get("type", "") + + if action_type == "allocate": + task = action.get("task", {}) + agent = self.allocate_task(task, self._agents) + return {"status": "allocated", "agent": agent.name if agent else None} + + if action_type == "status": + return {"status": "ok", "agent_status": self.get_agent_status()} + + if action_type == "broadcast": + message = action.get("message", {}) + results = [] + for agent in self._agents: + try: + result = agent.perceive(message) + results.append({"agent": agent.name, "result": result}) + except Exception as exc: # noqa: BLE001 + results.append({"agent": agent.name, "error": str(exc)}) + return {"status": "broadcasted", "results": results} + + return {"status": "unknown_action"} + + # ------------------------------------------------------------------ + # Public coordination API + # ------------------------------------------------------------------ + + def register_agent(self, agent: BaseAgent) -> None: + """Add an agent to the managed pool. + + Args: + agent: Agent instance to register. + + Raises: + ValueError: If the pool is already at maximum capacity. + """ + if len(self._agents) >= self._max_robots: + raise ValueError( + f"Maximum agent capacity ({self._max_robots}) reached." + ) + self._agents.append(agent) + self._logger.info("Agent '%s' registered.", agent.name) + + def allocate_task( + self, + task: Dict[str, Any], + agents: Optional[List[BaseAgent]] = None, + ) -> Optional[BaseAgent]: + """Allocate a task to the most suitable agent. + + Uses capability-based matching when the strategy is + ``capability_based``; otherwise assigns round-robin. + + Args: + task: Task specification dictionary with optional ``requires`` + key listing required capability strings. + agents: Optional list of candidate agents. Defaults to the + full registered pool. + + Returns: + The selected agent, or None if no suitable agent is found. + """ + candidate_pool = agents if agents is not None else self._agents + if not candidate_pool: + self._logger.warning("No agents available for task allocation.") + return None + + required = task.get("requires", []) + + if self._strategy == "capability_based" and required: + for agent in candidate_pool: + agent_state = agent.state + capabilities = agent_state.get("capabilities", []) + if all(cap in capabilities for cap in required): + self._record_allocation(task, agent) + return agent + + # Default: least loaded agent (smallest memory size as proxy) + selected = min(candidate_pool, key=lambda a: len(a.memory)) + self._record_allocation(task, selected) + return selected + + def coordinate( + self, task_list: List[Dict[str, Any]] + ) -> Dict[str, Optional[str]]: + """Allocate each task in the list to a managed agent. + + Args: + task_list: List of task specification dictionaries. + + Returns: + Dictionary mapping task ID strings to agent name strings (or + None if no agent was available). + """ + allocations: Dict[str, Optional[str]] = {} + for task in task_list: + task_id = str(task.get("task_id", id(task))) + agent = self.allocate_task(task) + allocations[task_id] = agent.name if agent else None + self._logger.info("Task coordination complete: %s", allocations) + return allocations + + def get_agent_status(self) -> Dict[str, Any]: + """Return a status summary for all registered agents. + + Returns: + Dictionary mapping agent names to their state dicts. + """ + return {agent.name: dict(agent.state) for agent in self._agents} + + # ------------------------------------------------------------------ + # Private helpers + # ------------------------------------------------------------------ + + def _record_allocation(self, task: Dict[str, Any], agent: BaseAgent) -> None: + """Record a task-to-agent allocation. + + Args: + task: Task specification. + agent: Assigned agent. + """ + task_id = str(task.get("task_id", id(task))) + self._task_allocations[task_id] = agent.name + self._logger.debug("Task '%s' allocated to agent '%s'.", task_id, agent.name) diff --git a/agents/perception_agent.py b/agents/perception_agent.py new file mode 100644 index 0000000..418fa88 --- /dev/null +++ b/agents/perception_agent.py @@ -0,0 +1,247 @@ +""" +Perception agent module for the RAG7 AGI Robotics Framework. + +Handles processing of multi-modal sensor data including RGB-D camera, +2-D LiDAR, and IMU readings. +""" + +import logging +from typing import Any, Dict, List, Optional + +import numpy as np + +from agents.base_agent import BaseAgent + +try: + import torch # noqa: F401 + + TORCH_AVAILABLE = True +except ImportError: + TORCH_AVAILABLE = False + + +class PerceptionAgent(BaseAgent): + """Agent responsible for processing raw sensor observations. + + Processes camera images, LiDAR scans, and IMU data into structured + perception outputs that higher-level agents can consume. + + Args: + config: Configuration dictionary (see config/agent_config.yaml). + device: Compute device for neural network inference ('cpu' or 'cuda'). + """ + + def __init__( + self, + config: Dict[str, Any], + device: str = "cpu", + ) -> None: + """Initialize the perception agent.""" + super().__init__( + name="perception_agent", + config=config, + logger=logging.getLogger("rag7.perception"), + ) + self._device = device + self._model = None + self._confidence_threshold = config.get("confidence_threshold", 0.7) + model_path = config.get("model_path") + if model_path and TORCH_AVAILABLE: + self._try_load_model(model_path) + + # ------------------------------------------------------------------ + # Abstract method implementations + # ------------------------------------------------------------------ + + def perceive(self, observation: Dict[str, Any]) -> Dict[str, Any]: + """Process a multi-modal sensor observation. + + Args: + observation: Dictionary that may contain: + - ``image``: numpy array (H x W x C) from RGB-D camera. + - ``lidar``: list or array of range measurements. + - ``imu``: dict with ``acceleration`` and ``angular_velocity``. + + Returns: + Dictionary with keys: + - ``detections``: list of detected objects. + - ``position``: estimated robot position (if available). + - ``orientation``: estimated robot orientation. + - ``nearest_obstacle``: closest obstacle distance in metres. + """ + result: Dict[str, Any] = { + "detections": [], + "position": self._state.get("position", {"x": 0.0, "y": 0.0, "z": 0.0}), + "orientation": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "nearest_obstacle": float("inf"), + } + + if "image" in observation and observation["image"] is not None: + result["detections"] = self._process_image(observation["image"]) + + if "lidar" in observation and observation["lidar"] is not None: + result["nearest_obstacle"] = self._process_lidar(observation["lidar"]) + + if "imu" in observation and observation["imu"] is not None: + result["orientation"] = self._process_imu(observation["imu"]) + + self.update_state("last_perception", result) + self.add_to_memory(result) + return result + + def reason(self, context: Any) -> Dict[str, Any]: + """Analyse recent perception data and describe the current scene. + + Args: + context: Optional additional context (currently unused). + + Returns: + Dictionary with ``scene_description``, ``num_objects``, + ``obstacle_detected``, and ``safe_to_proceed`` fields. + """ + last = self._state.get("last_perception", {}) + detections = last.get("detections", []) + nearest = last.get("nearest_obstacle", float("inf")) + obstacle_detected = nearest < self._config.get("collision_threshold", 0.3) + + scene: Dict[str, Any] = { + "scene_description": f"Detected {len(detections)} object(s).", + "num_objects": len(detections), + "obstacle_detected": obstacle_detected, + "safe_to_proceed": not obstacle_detected, + } + self._logger.debug("Scene analysis: %s", scene) + return scene + + def act(self, action: Dict[str, Any]) -> Dict[str, Any]: + """Update internal model parameters based on an action directive. + + Args: + action: Dictionary specifying the action, e.g. + ``{"type": "update_threshold", "value": 0.8}``. + + Returns: + Acknowledgement dictionary with ``status`` key. + """ + action_type = action.get("type", "") + if action_type == "update_threshold": + self._confidence_threshold = float(action.get("value", self._confidence_threshold)) + self._logger.info("Confidence threshold updated to %.2f", self._confidence_threshold) + else: + self._logger.warning("Unknown action type for PerceptionAgent: %s", action_type) + return {"status": "ok", "action": action_type} + + # ------------------------------------------------------------------ + # Private helpers + # ------------------------------------------------------------------ + + def _process_image(self, image: np.ndarray) -> List[Dict[str, Any]]: + """Run object detection on a single image frame. + + Uses a loaded PyTorch model when available; otherwise returns + mock detections for testing and fallback purposes. + + Args: + image: RGB image as a numpy array of shape (H, W, C). + + Returns: + List of detection dictionaries, each with ``label``, + ``confidence``, and ``bbox`` keys. + """ + if self._model is not None and TORCH_AVAILABLE: + try: + import torch + + tensor = self._preprocess(image) + with torch.no_grad(): + outputs = self._model(tensor) + return self._parse_model_output(outputs) + except Exception as exc: # noqa: BLE001 + self._logger.warning("Model inference failed: %s", exc) + + # Fallback: mock detections based on image statistics + mock_detections = [ + { + "label": "unknown_object", + "confidence": 0.75, + "bbox": [10, 10, 50, 50], + } + ] + return mock_detections + + def _process_lidar(self, data: Any) -> float: + """Determine the nearest obstacle distance from a LiDAR scan. + + Args: + data: Array-like of range measurements in metres. + + Returns: + Minimum valid range reading (nearest obstacle distance). + """ + try: + ranges = np.asarray(data, dtype=float) + valid = ranges[np.isfinite(ranges) & (ranges > 0)] + return float(np.min(valid)) if len(valid) > 0 else float("inf") + except Exception as exc: # noqa: BLE001 + self._logger.warning("LiDAR processing failed: %s", exc) + return float("inf") + + def _process_imu(self, data: Dict[str, Any]) -> Dict[str, float]: + """Extract orientation estimates from IMU data. + + Args: + data: Dictionary with optional ``roll``, ``pitch``, ``yaw`` keys + or ``angular_velocity`` sub-dict. + + Returns: + Orientation dictionary with ``roll``, ``pitch``, ``yaw`` in radians. + """ + orientation = { + "roll": float(data.get("roll", 0.0)), + "pitch": float(data.get("pitch", 0.0)), + "yaw": float(data.get("yaw", 0.0)), + } + return orientation + + def _preprocess(self, image: np.ndarray) -> Any: + """Preprocess a numpy image for model inference. + + Args: + image: Raw numpy image array. + + Returns: + Preprocessed torch Tensor on the configured device. + """ + import torch + + if image.dtype != np.float32: + image = image.astype(np.float32) / 255.0 + tensor = torch.from_numpy(image).permute(2, 0, 1).unsqueeze(0) + return tensor.to(self._device) + + def _parse_model_output(self, outputs: Any) -> List[Dict[str, Any]]: + """Convert raw model output to detection dictionaries. + + Args: + outputs: Raw model output tensors. + + Returns: + List of detection dictionaries. + """ + # Placeholder – real implementation would parse model-specific format + return [] + + def _try_load_model(self, path: str) -> None: + """Attempt to load a detection model from disk. + + Args: + path: Filesystem path to the serialised model file. + """ + try: + import torch + + self._model = torch.load(path, map_location=self._device) + self._model.eval() + self._logger.info("Detection model loaded from '%s'.", path) + except Exception as exc: # noqa: BLE001 + self._logger.warning("Could not load model from '%s': %s", path, exc) diff --git a/agents/planning_agent.py b/agents/planning_agent.py new file mode 100644 index 0000000..b976650 --- /dev/null +++ b/agents/planning_agent.py @@ -0,0 +1,217 @@ +""" +Planning agent module for the RAG7 AGI Robotics Framework. + +Provides high-level task planning, decomposition, and replanning using +LangChain-based LLMs with a rule-based fallback. +""" + +import logging +from typing import Any, Dict, List, Optional + +from agents.base_agent import BaseAgent + +try: + from langchain.agents import AgentExecutor # noqa: F401 + from langchain_openai import ChatOpenAI # noqa: F401 + + LANGCHAIN_AVAILABLE = True +except ImportError: + LANGCHAIN_AVAILABLE = False + + +class PlanningAgent(BaseAgent): + """Agent responsible for high-level task planning and goal decomposition. + + Uses LangChain with an LLM backend when available; falls back to a + deterministic rule-based planner otherwise. + + Args: + config: Agent configuration dictionary. + llm_config: Optional LLM configuration dictionary. + """ + + def __init__( + self, + config: Dict[str, Any], + llm_config: Optional[Dict[str, Any]] = None, + ) -> None: + """Initialize the planning agent.""" + super().__init__( + name="planning_agent", + config=config, + logger=logging.getLogger("rag7.planning"), + ) + self._llm_config = llm_config or {} + self._llm = None + self._current_plan: List[Dict[str, Any]] = [] + self._plan_index: int = 0 + self._world_state: Dict[str, Any] = {} + + if LANGCHAIN_AVAILABLE and self._llm_config.get("provider") == "openai": + self._try_init_llm() + + # ------------------------------------------------------------------ + # Abstract method implementations + # ------------------------------------------------------------------ + + def perceive(self, observation: Dict[str, Any]) -> Dict[str, Any]: + """Update the internal world model with new observations. + + Args: + observation: Dictionary containing world state updates. + + Returns: + Updated world state. + """ + self._world_state.update(observation) + self.update_state("world_state", self._world_state) + return self._world_state + + def reason(self, context: Any) -> Dict[str, Any]: + """Create or refine a task plan based on current context. + + Args: + context: Goal description string or context dictionary. + + Returns: + Dictionary with ``plan`` (list of step dicts) and + ``status`` keys. + """ + goal = context if isinstance(context, str) else context.get("goal", "idle") + plan = self.create_plan(goal) + self._current_plan = plan + self._plan_index = 0 + return {"plan": plan, "status": "planned", "num_steps": len(plan)} + + def act(self, action: Dict[str, Any]) -> Dict[str, Any]: + """Execute the next step in the current plan. + + Args: + action: Action override dict; if ``{"type": "next"}`` is + provided, advances to the next plan step. + + Returns: + Dictionary describing which step was executed. + """ + if not self._current_plan: + return {"status": "no_plan"} + + if self._plan_index >= len(self._current_plan): + return {"status": "plan_complete"} + + step = self._current_plan[self._plan_index] + self._plan_index += 1 + self._logger.info("Executing plan step %d: %s", self._plan_index, step) + return {"status": "executing", "step": step} + + # ------------------------------------------------------------------ + # Public planning API + # ------------------------------------------------------------------ + + def create_plan(self, goal: str) -> List[Dict[str, Any]]: + """Create a task plan to achieve the given goal. + + Attempts LLM-based planning if available; falls back to + rule-based decomposition. + + Args: + goal: High-level goal description. + + Returns: + Ordered list of step dictionaries, each containing at least + ``step_id``, ``action``, and ``description`` keys. + """ + if self._llm is not None: + try: + return self._llm_plan(goal) + except Exception as exc: # noqa: BLE001 + self._logger.warning("LLM planning failed (%s); using rule-based.", exc) + + return self._decompose_task(goal) + + def _decompose_task(self, task: str) -> List[Dict[str, Any]]: + """Decompose a task string into a sequence of primitive steps. + + Args: + task: Task description string. + + Returns: + List of step dictionaries. + """ + task_lower = task.lower() + + if "navigate" in task_lower or "go to" in task_lower or "move" in task_lower: + return [ + {"step_id": 1, "action": "perceive_environment", "description": "Scan surroundings"}, + {"step_id": 2, "action": "plan_path", "description": "Compute collision-free path"}, + {"step_id": 3, "action": "navigate", "description": "Execute navigation"}, + {"step_id": 4, "action": "verify_position", "description": "Confirm arrival"}, + ] + + if "grasp" in task_lower or "pick" in task_lower or "grab" in task_lower: + return [ + {"step_id": 1, "action": "detect_object", "description": "Locate target object"}, + {"step_id": 2, "action": "approach", "description": "Move arm above object"}, + {"step_id": 3, "action": "grasp", "description": "Close gripper"}, + {"step_id": 4, "action": "lift", "description": "Lift object"}, + ] + + if "place" in task_lower or "put" in task_lower: + return [ + {"step_id": 1, "action": "move_to_target", "description": "Move to placement location"}, + {"step_id": 2, "action": "lower", "description": "Lower arm"}, + {"step_id": 3, "action": "release", "description": "Open gripper"}, + ] + + # Generic default plan + return [ + {"step_id": 1, "action": "assess", "description": f"Assess task: {task}"}, + {"step_id": 2, "action": "execute", "description": "Execute task"}, + {"step_id": 3, "action": "verify", "description": "Verify completion"}, + ] + + # ------------------------------------------------------------------ + # Private helpers + # ------------------------------------------------------------------ + + def _llm_plan(self, goal: str) -> List[Dict[str, Any]]: + """Generate a plan using the LLM backend. + + Args: + goal: Goal description. + + Returns: + List of step dictionaries from LLM output. + """ + prompt = ( + f"Create a step-by-step robot task plan for: {goal}\n" + "Return steps as a numbered list." + ) + response = self._llm.invoke(prompt) + lines = [l.strip() for l in response.content.split("\n") if l.strip()] + steps = [] + for i, line in enumerate(lines, 1): + steps.append({"step_id": i, "action": "llm_step", "description": line}) + return steps + + def _try_init_llm(self) -> None: + """Attempt to initialise the LangChain LLM client.""" + try: + import os + + from langchain_openai import ChatOpenAI + + api_key = os.environ.get( + self._llm_config.get("api_key_env", "OPENAI_API_KEY"), "" + ) + if not api_key: + self._logger.info("No OpenAI API key found; using rule-based planning.") + return + self._llm = ChatOpenAI( + model=self._llm_config.get("model", "gpt-4"), + temperature=self._llm_config.get("temperature", 0.1), + openai_api_key=api_key, + ) + self._logger.info("LLM planning backend initialised.") + except Exception as exc: # noqa: BLE001 + self._logger.warning("Failed to initialise LLM: %s", exc) diff --git a/agents/robotics_agi.py b/agents/robotics_agi.py new file mode 100644 index 0000000..9c281f9 --- /dev/null +++ b/agents/robotics_agi.py @@ -0,0 +1,247 @@ +""" +Main RoboticsAGI orchestrator for the RAG7 AGI Robotics Framework. + +Integrates all agents into a unified system capable of executing +natural language commands and complex multi-step robotic tasks. +""" + +import logging +import os +from typing import Any, Dict, List, Optional + +import yaml + +from agents.base_agent import BaseAgent +from agents.communication_agent import CommunicationAgent +from agents.control_agent import ControlAgent +from agents.coordination_agent import CoordinationAgent +from agents.perception_agent import PerceptionAgent +from agents.planning_agent import PlanningAgent + + +class RoboticsAGI: + """Top-level orchestrator for the RAG7 AGI robotics system. + + Composes all specialised agents and exposes a high-level API for + natural language command execution and task management. + + Args: + ros2_interface: Optional ROS2 interface instance. + llm_provider: LLM provider string (e.g. ``"openai"``). + enable_learning: Enable reinforcement-learning components. + config_path: Path to the directory containing YAML config files. + """ + + def __init__( + self, + ros2_interface: Optional[Any] = None, + llm_provider: str = "openai", + enable_learning: bool = False, + config_path: str = "config", + ) -> None: + """Initialize the RoboticsAGI system.""" + self._logger = logging.getLogger("rag7.agi") + self._ros2_interface = ros2_interface + self._enable_learning = enable_learning + + # Load configurations + self._agent_config = self._load_yaml( + os.path.join(config_path, "agent_config.yaml") + ) + self._llm_config = self._load_yaml( + os.path.join(config_path, "llm_config.yaml") + ) + self._robot_config = self._load_yaml( + os.path.join(config_path, "robot_config.yaml") + ) + + # Override provider if specified + if llm_provider: + self._llm_config["provider"] = llm_provider + + # Initialise agents + self._perception = PerceptionAgent( + config=self._agent_config.get("perception_agent", {}) + ) + self._planning = PlanningAgent( + config=self._agent_config.get("planning_agent", {}), + llm_config=self._llm_config, + ) + self._control = ControlAgent( + config=self._agent_config.get("control_agent", {}), + ros2_enabled=(ros2_interface is not None), + ) + self._communication = CommunicationAgent( + config=self._agent_config.get("communication_agent", {}), + llm_config=self._llm_config, + ) + self._coordination = CoordinationAgent( + config=self._agent_config.get("coordination_agent", {}) + ) + + # Register all agents with the coordinator + for agent in [ + self._perception, + self._planning, + self._control, + self._communication, + ]: + self._coordination.register_agent(agent) + + self._logger.info("RoboticsAGI system initialised.") + + # ------------------------------------------------------------------ + # Public API + # ------------------------------------------------------------------ + + def execute_command(self, text: str) -> Dict[str, Any]: + """Parse and execute a natural language command. + + Args: + text: Natural language command string from the operator. + + Returns: + Execution result dictionary with ``success``, ``intent``, + and ``result`` keys. + """ + parsed = self._communication.parse_command(text) + intent = parsed.get("intent", "unknown") + self._logger.info("Executing command: '%s' (intent=%s)", text, intent) + + task = self.create_task( + task_type=intent, + location=parsed.get("location"), + target_object=parsed.get("target_object"), + ) + result = self.execute_task(task) + response = self._communication.generate_response( + {"intent": intent, "robot_state": self.get_status()} + ) + return { + "success": result.get("success", False), + "intent": intent, + "result": result, + "response": response, + } + + def create_task(self, task_type: str, **kwargs: Any) -> Dict[str, Any]: + """Create a structured task dictionary. + + Args: + task_type: Type of task (e.g. ``"navigate"``, ``"grasp"``). + **kwargs: Additional task parameters (e.g. ``location``, + ``target_object``). + + Returns: + Task specification dictionary. + """ + import time + + return { + "task_id": f"{task_type}_{int(time.time())}", + "task_type": task_type, + **kwargs, + } + + def execute_task(self, task: Dict[str, Any]) -> Dict[str, Any]: + """Execute a structured task using the appropriate agents. + + Args: + task: Task specification dictionary. + + Returns: + Result dictionary with ``success`` and ``details`` keys. + """ + task_type = task.get("task_type", "unknown") + + if task_type == "navigate": + location = task.get("location") or {} + x = float(location.get("x", 0.0)) if isinstance(location, dict) else 0.0 + y = float(location.get("y", 0.0)) if isinstance(location, dict) else 0.0 + success = self._control.navigate_to(x, y, 0.0) + return {"success": success, "details": f"Navigated to ({x}, {y})"} + + if task_type == "grasp": + obj = task.get("target_object", "unknown") + success = self._control.execute_grasp(obj) + return {"success": success, "details": f"Grasped '{obj}'"} + + if task_type == "stop": + self._control.emergency_stop() + return {"success": True, "details": "Emergency stop executed"} + + if task_type in ("query", "unknown"): + status = self.get_status() + return {"success": True, "details": status} + + # For complex tasks, use planning agent + plan_result = self._planning.reason({"goal": str(task)}) + return { + "success": True, + "details": f"Plan created with {plan_result.get('num_steps', 0)} steps", + "plan": plan_result.get("plan", []), + } + + def coordinate_robots( + self, robots: List[Any], task: Dict[str, Any] + ) -> Dict[str, Any]: + """Coordinate multiple robots to execute a task. + + Args: + robots: List of robot agent instances. + task: Task specification dictionary. + + Returns: + Coordination result dictionary. + """ + for robot in robots: + if isinstance(robot, BaseAgent): + try: + self._coordination.register_agent(robot) + except ValueError: + pass # Already at capacity or already registered + return self._coordination.coordinate([task]) + + def get_status(self) -> Dict[str, Any]: + """Return a comprehensive system status snapshot. + + Returns: + Dictionary with per-agent status and system-level flags. + """ + return { + "system": "online", + "perception": self._perception.state, + "planning": self._planning.state, + "control": self._control.state, + "communication": self._communication.state, + "coordination": self._coordination.get_agent_status(), + "robot_config": self._robot_config.get("robot_name", "unknown"), + } + + # ------------------------------------------------------------------ + # Private helpers + # ------------------------------------------------------------------ + + @staticmethod + def _load_yaml(path: str) -> Dict[str, Any]: + """Load a YAML configuration file. + + Args: + path: Path to the YAML file. + + Returns: + Parsed configuration dictionary, or empty dict on failure. + """ + try: + with open(path, "r", encoding="utf-8") as fh: + return yaml.safe_load(fh) or {} + except FileNotFoundError: + logging.getLogger("rag7.agi").warning( + "Config file not found: %s", path + ) + return {} + except yaml.YAMLError as exc: + logging.getLogger("rag7.agi").error( + "Failed to parse config file '%s': %s", path, exc + ) + return {} diff --git a/config/agent_config.yaml b/config/agent_config.yaml new file mode 100644 index 0000000..769a453 --- /dev/null +++ b/config/agent_config.yaml @@ -0,0 +1,27 @@ +perception_agent: + update_rate: 10 + confidence_threshold: 0.7 + model_path: models/detection_model.pt + +planning_agent: + planning_horizon: 10 + max_replanning_attempts: 3 + timeout: 30.0 + +control_agent: + control_frequency: 50 + position_tolerance: 0.05 + orientation_tolerance: 0.1 + +communication_agent: + max_dialog_history: 10 + response_timeout: 5.0 + +coordination_agent: + task_allocation_strategy: capability_based + max_robots: 10 + +memory: + short_term_capacity: 100 + long_term_enabled: true + vector_store_type: faiss diff --git a/config/llm_config.yaml b/config/llm_config.yaml new file mode 100644 index 0000000..f2889c7 --- /dev/null +++ b/config/llm_config.yaml @@ -0,0 +1,19 @@ +provider: openai + +model: gpt-4 +temperature: 0.1 +max_tokens: 2000 +api_key_env: OPENAI_API_KEY + +local_model: + enabled: false + model_path: models/local_llm + device: cuda + +embeddings: + model: text-embedding-3-small + dimensions: 1536 + +agent_type: react +max_iterations: 10 +verbose: false diff --git a/config/robot_config.yaml b/config/robot_config.yaml new file mode 100644 index 0000000..c34a06f --- /dev/null +++ b/config/robot_config.yaml @@ -0,0 +1,43 @@ +robot_name: rag7_robot +robot_type: mobile_manipulator + +dimensions: + width: 0.5 + length: 0.7 + height: 1.2 + +sensors: + camera: + type: rgb_depth + topic: /camera/image_raw + resolution: [640, 480] + fps: 30 + lidar: + type: 2d + topic: /scan + range_max: 10.0 + imu: + topic: /imu/data + +actuators: + wheels: + type: differential_drive + max_velocity: 1.0 + wheel_radius: 0.1 + wheel_base: 0.5 + arm: + type: 6dof + max_payload: 5.0 + +workspace: + max_x: 10.0 + max_y: 10.0 + max_z: 10.0 + min_x: -10.0 + min_y: -10.0 + min_z: 0.0 + +safety: + emergency_stop_topic: /emergency_stop + collision_threshold: 0.3 + human_safety_distance: 1.0 diff --git a/docs/api_reference.md b/docs/api_reference.md new file mode 100644 index 0000000..303953f --- /dev/null +++ b/docs/api_reference.md @@ -0,0 +1,284 @@ +# API Reference + +## agents + +### `BaseAgent` + +Abstract base class for all rag7 agents. + +```python +class BaseAgent(ABC): + def __init__(self, name: str, config: dict, logger=None) + def update_state(self, key: str, value: Any) -> None + def add_to_memory(self, item: Any) -> None + def get_memory(self, n: int = 10) -> list + def clear_memory(self) -> None + def register_tool(self, tool: Any) -> None + def get_tools(self) -> list + + # Abstract + def perceive(self, observation: Any) -> Any + def reason(self, context: Any) -> Any + def act(self, action: Any) -> Any +``` + +### `PerceptionAgent` + +```python +class PerceptionAgent(BaseAgent): + def __init__(self, config: dict, device: str = "cpu") + def perceive(self, observation: dict) -> dict + # observation keys: image (ndarray), lidar (array), imu (dict) + # returns: detections, position, orientation, nearest_obstacle + def reason(self, context: Any) -> dict + def act(self, action: dict) -> dict +``` + +### `PlanningAgent` + +```python +class PlanningAgent(BaseAgent): + def __init__(self, config: dict, llm_config: dict = None) + def perceive(self, observation: dict) -> dict + def reason(self, context: Any) -> dict # returns plan dict + def act(self, action: dict) -> dict + def create_plan(self, goal: str) -> list[dict] +``` + +### `ControlAgent` + +```python +class ControlAgent(BaseAgent): + def __init__(self, config: dict, ros2_enabled: bool = False) + def perceive(self, observation: dict) -> dict + def reason(self, context: Any) -> dict + def act(self, action: dict) -> dict # type: navigate | grasp | stop + def navigate_to(self, x: float, y: float, theta: float) -> bool + def execute_grasp(self, object_id: str) -> bool + def emergency_stop(self) -> None +``` + +### `CommunicationAgent` + +```python +class CommunicationAgent(BaseAgent): + def __init__(self, config: dict, llm_config: dict = None) + def perceive(self, observation: dict) -> dict + def reason(self, context: Any) -> dict + def act(self, action: dict) -> dict # type: send | receive + def parse_command(self, text: str) -> dict + def generate_response(self, context: dict) -> str + def classify_intent(self, text: str) -> str +``` + +### `CoordinationAgent` + +```python +class CoordinationAgent(BaseAgent): + def __init__(self, config: dict) + def register_agent(self, agent: BaseAgent) -> None + def allocate_task(self, task: dict, agents: list = None) -> BaseAgent + def coordinate(self, task_list: list) -> dict + def get_agent_status(self) -> dict +``` + +### `RoboticsAGI` + +```python +class RoboticsAGI: + def __init__( + self, + ros2_interface=None, + llm_provider: str = "openai", + enable_learning: bool = False, + config_path: str = "config", + ) + def execute_command(self, text: str) -> dict + def create_task(self, task_type: str, **kwargs) -> dict + def execute_task(self, task: dict) -> dict + def coordinate_robots(self, robots: list, task: dict) -> dict + def get_status(self) -> dict +``` + +--- + +## perception + +### `ObjectDetector` + +```python +class ObjectDetector: + def __init__(self, model_path=None, device="cpu", confidence_threshold=0.5) + def detect(self, image: ndarray) -> list[Detection] + def load_model(self, path: str) -> bool +``` + +### `Segmenter` + +```python +class Segmenter: + def __init__(self, model_path=None, device="cpu") + def segment(self, image: ndarray) -> dict # masks, labels, scores +``` + +### `ObjectTracker` + +```python +class ObjectTracker: + def __init__(self, max_age=30, min_hits=3) + def update(self, detections: list) -> list[dict] # id, bbox, label +``` + +### `SLAMMapper` + +```python +class SLAMMapper: + def __init__(self, map_resolution=0.05, map_size=100) + def update(self, lidar_scan, pose: dict) -> None + def get_map(self) -> ndarray + def localize(self, scan) -> dict # x, y, theta, confidence +``` + +### `SensorFusion` + +```python +class SensorFusion: + def fuse(self, camera_data, lidar_data, imu_data) -> dict +``` + +--- + +## planning + +### `TaskPlanner` + +```python +class TaskPlanner: + def __init__(self, llm_config: dict = None) + def plan(self, goal: str, context: dict = None) -> list[dict] + def decompose(self, task: str) -> list[dict] + def replan(self, failed_step: dict, context: dict = None) -> list[dict] +``` + +### `PathPlanner` + +```python +class PathPlanner: + def __init__(self, grid_resolution=0.1) + def plan(self, start: tuple, goal: tuple, obstacles: list = None) -> list[tuple] +``` + +### `MotionPlanner` + +```python +class MotionPlanner: + def __init__(self, dof=6) + def plan_trajectory(self, start_config, goal_config, duration=1.0) -> dict + def compute_ik(self, pose: dict) -> list[float] + def compute_fk(self, joint_angles: list) -> dict +``` + +### `DecisionMaker` + +```python +class DecisionMaker: + def __init__(self, config: dict = None) + def decide(self, state: dict, options: list) -> Any + def evaluate(self, state: dict, action: Any) -> float +``` + +--- + +## nlp + +### `CommandParser` + +```python +class CommandParser: + def __init__(self, llm_config: dict = None) + def parse(self, text: str) -> dict # intent, action, target_object, location, confidence + def extract_entities(self, text: str) -> dict +``` + +### `IntentClassifier` + +```python +class IntentClassifier: + INTENTS = ["navigate", "grasp", "place", "inspect", "stop", "query", "unknown"] + def classify(self, text: str) -> str + def get_confidence(self, text: str, intent: str) -> float +``` + +### `DialogManager` + +```python +class DialogManager: + def __init__(self, max_history=10) + def process(self, user_input: str, robot_state: dict = None) -> str + def add_turn(self, role: str, content: str) -> None + def get_history(self) -> list[dict] + def clear_history(self) -> None +``` + +--- + +## learning + +### `DQNAgent` + +```python +class DQNAgent: + def __init__(self, state_dim, action_dim, lr=1e-3, gamma=0.99, ...) + def select_action(self, state) -> int + def update(self, batch: dict) -> float + def save(self, path: str) -> None + def load(self, path: str) -> None +``` + +### `PPOAgent` + +```python +class PPOAgent: + def __init__(self, state_dim, action_dim, lr=3e-4, gamma=0.99, ...) + def select_action(self, state) -> tuple[int, tensor] + def update(self, memory: dict) -> float + def save(self, path: str) -> None + def load(self, path: str) -> None +``` + +### `ReplayBuffer` + +```python +class ReplayBuffer: + def __init__(self, capacity=10000) + def push(self, state, action, reward, next_state, done) -> None + def sample(self, batch_size: int) -> dict + def __len__(self) -> int +``` + +### `BCTrainer` + +```python +class BCTrainer: + def __init__(self, state_dim, action_dim, lr=1e-3) + def add_demonstration(self, state, action) -> None + def train(self, epochs=10, batch_size=32) -> list[float] + def predict(self, state) -> ndarray + def save(self, path: str) -> None + def load(self, path: str) -> None +``` + +--- + +## simulation + +### `GazeboEnv` + +```python +class GazeboEnv: + def __init__(self, world_name="empty", robot_name="robot", headless=True) + def reset(self) -> dict + def step(self, action: dict) -> tuple[dict, float, bool, dict] + def render(self) -> ndarray | None + def close(self) -> None +``` diff --git a/docs/architecture.md b/docs/architecture.md new file mode 100644 index 0000000..359775b --- /dev/null +++ b/docs/architecture.md @@ -0,0 +1,140 @@ +# rag7 AGI Robotics Framework – Architecture + +## Overview + +The rag7 framework is a modular, multi-agent AGI robotics platform built +around the **perceive → reason → act** loop. Each subsystem is an +independent Python package that can be used standalone or composed into +the full `RoboticsAGI` orchestrator. + +--- + +## System Architecture Diagram + +``` +┌──────────────────────────────────────────────────────────────────────┐ +│ RoboticsAGI │ +│ ┌────────────┐ ┌──────────────┐ ┌─────────────┐ ┌───────────────┐ │ +│ │ Perception │ │ Planning │ │ Control │ │ Communication │ │ +│ │ Agent │ │ Agent │ │ Agent │ │ Agent │ │ +│ └─────┬──────┘ └──────┬───────┘ └──────┬──────┘ └───────┬───────┘ │ +│ │ │ │ │ │ +│ ┌─────▼───────────────▼────────────────▼────────────────▼───────┐ │ +│ │ CoordinationAgent │ │ +│ └────────────────────────────────────────────────────────────────┘ │ +└──────────────────────────────────────────────────────────────────────┘ + │ │ │ │ + ┌────▼────┐ ┌──────▼──────┐ ┌─────▼─────┐ ┌─────▼─────┐ + │Perception│ │ Planning │ │ NLP │ │ Learning │ + │ Module │ │ Module │ │ Module │ │ Module │ + └─────────┘ └─────────────┘ └───────────┘ └───────────┘ + │ │ + ┌────▼────┐ ┌──────▼──────┐ + │ SLAM │ │ Path/Motion │ + │ Mapping │ │ Planner │ + └─────────┘ └─────────────┘ + │ + ┌────▼────────────┐ + │ ROS2 Interface │ + │ (optional) │ + └─────────────────┘ +``` + +--- + +## Agent Layer + +### BaseAgent +Abstract class that all agents inherit from. Enforces the +perceive/reason/act interface and provides shared memory and tool +registration. + +### PerceptionAgent +Processes raw sensor data (RGB-D camera, 2-D LiDAR, IMU) into +structured scene representations. + +### PlanningAgent +Decomposes high-level goals into ordered action sequences. Uses +LangChain + GPT-4 when available; falls back to rule-based planning. + +### ControlAgent +Translates planned actions into robot actuator commands. Publishes +navigation goals and arm trajectories to ROS2 topics when available. + +### CommunicationAgent +Handles natural language I/O. Parses operator commands and generates +contextual responses using the NLP module. + +### CoordinationAgent +Manages multi-robot task allocation using capability-based matching. + +--- + +## Perception Module + +| Component | Description | +|-----------|-------------| +| `ObjectDetector` | PyTorch-based 2-D bounding-box detector | +| `Segmenter` | Semantic segmentation | +| `ObjectTracker` | IoU-based multi-object tracker | +| `SLAMMapper` | Occupancy-grid SLAM with ray-casting | +| `SensorFusion` | Complementary-filter fusion of camera/LiDAR/IMU | + +--- + +## Planning Module + +| Component | Description | +|-----------|-------------| +| `TaskPlanner` | LLM / rule-based goal decomposition | +| `PathPlanner` | A* grid-based path planning | +| `MotionPlanner` | Joint-space trajectory interpolation + simplified IK/FK | +| `DecisionMaker` | Rule-based action selection with safety gating | + +--- + +## NLP Module + +| Component | Description | +|-----------|-------------| +| `CommandParser` | Maps utterances to structured command dicts | +| `IntentClassifier` | Weighted keyword intent classification | +| `DialogManager` | Multi-turn conversation management | + +--- + +## Learning Module + +| Component | Description | +|-----------|-------------| +| `DQNAgent` | Deep Q-Network with experience replay | +| `PPOAgent` | Proximal Policy Optimisation actor-critic | +| `ReplayBuffer` | Circular experience replay buffer | +| `BCTrainer` | Behavioural cloning from expert demos | + +--- + +## ROS2 Interface + +Optional bridge layer that wraps each agent in a `rclpy` node. +When `rclpy` is not installed all nodes degrade to mock mode. + +--- + +## Data Flow + +``` +Operator text ──► CommunicationAgent ──► CommandParser ──► intent/action + │ + ▼ +Sensor data ──► PerceptionAgent ──► SensorFusion ──► scene state + │ + ▼ + PlanningAgent ──► task plan + │ + ▼ + ControlAgent ──► actuator cmds + │ + ▼ + ROS2Interface ──► robot hardware +``` diff --git a/docs/installation.md b/docs/installation.md new file mode 100644 index 0000000..647a913 --- /dev/null +++ b/docs/installation.md @@ -0,0 +1,92 @@ +# Installation Guide + +## Prerequisites + +- Python 3.10 or higher +- pip 23+ +- (Optional) CUDA 11.8+ for GPU inference +- (Optional) ROS2 Humble or later for hardware integration + +--- + +## Quick Install + +```bash +git clone https://github.com/your-org/rag7.git +cd rag7 +pip install -e . +``` + +For full LLM and ML support: + +```bash +pip install -e ".[llm]" +``` + +For development (includes testing tools): + +```bash +pip install -e ".[llm,dev]" +``` + +--- + +## Installing All Requirements + +```bash +pip install -r requirements.txt +``` + +> **Note:** `rclpy`, `geometry_msgs`, `sensor_msgs`, `nav_msgs`, and +> `action_msgs` are ROS2 packages installed via the ROS2 toolchain, not pip. +> All other packages install normally. + +--- + +## ROS2 Setup (Optional) + +1. Install ROS2 Humble following the [official guide](https://docs.ros.org/en/humble/Installation.html). +2. Source the ROS2 environment: + + ```bash + source /opt/ros/humble/setup.bash + ``` + +3. Build the rag7 ROS2 package: + + ```bash + colcon build --packages-select rag7_agi + source install/setup.bash + ``` + +--- + +## OpenAI API Key (Optional) + +Set the environment variable to enable LLM-based planning and NLP: + +```bash +export OPENAI_API_KEY="sk-..." +``` + +Without a key the system falls back to rule-based planning and parsing. + +--- + +## Docker + +```bash +docker build -t rag7-agi . +docker run --rm rag7-agi +``` + +--- + +## Verifying the Installation + +```bash +cd rag7 +python -m pytest tests/ -v +``` + +All tests should pass without ROS2 or a GPU. diff --git a/examples/__init__.py b/examples/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/examples/nlp_control.py b/examples/nlp_control.py new file mode 100644 index 0000000..3091c6b --- /dev/null +++ b/examples/nlp_control.py @@ -0,0 +1,61 @@ +""" +Natural language control example for the RAG7 AGI Robotics Framework. + +Demonstrates the NLP pipeline: command parsing, intent classification, +dialog management, and NL-driven robot control. +""" + +import logging +import os +import sys + +sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + +from agents.robotics_agi import RoboticsAGI +from nlp.command_parser import CommandParser +from nlp.intent_classifier import IntentClassifier +from nlp.dialog_manager import DialogManager + +logging.basicConfig(level=logging.INFO, format="%(name)s - %(levelname)s - %(message)s") + + +def main() -> None: + """Run an NLP control demo.""" + config_path = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), "config") + agi = RoboticsAGI(config_path=config_path) + dialog = DialogManager(max_history=5) + classifier = IntentClassifier() + parser = CommandParser() + + print("\n=== RAG7 AGI – NLP Control Demo ===\n") + + commands = [ + "What is the current system status?", + "Navigate to the loading bay", + "Pick up the package on conveyor belt A", + "Emergency stop!", + "Go to the charging dock", + ] + + robot_state = agi.get_status() + + for cmd in commands: + print(f"Operator: {cmd}") + intent = classifier.classify(cmd) + parsed = parser.parse(cmd) + response = dialog.process(cmd, robot_state) + + result = agi.execute_command(cmd) + robot_state = agi.get_status() + + print(f" Intent: {intent} (confidence: {parsed.get('confidence', '?')})") + print(f" Robot: {response}") + print(f" Success: {result['success']}\n") + + print("Dialog history:") + for turn in dialog.get_history(): + print(f" [{turn['role']}]: {turn['content']}") + + +if __name__ == "__main__": + main() diff --git a/examples/object_manipulation.py b/examples/object_manipulation.py new file mode 100644 index 0000000..3df28b6 --- /dev/null +++ b/examples/object_manipulation.py @@ -0,0 +1,48 @@ +""" +Object manipulation example for the RAG7 AGI Robotics Framework. + +Demonstrates how to use the RoboticsAGI system to pick up and place +an object using task-level commands. +""" + +import logging +import os +import sys + +sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + +from agents.robotics_agi import RoboticsAGI +from agents.control_agent import ControlAgent + +logging.basicConfig(level=logging.INFO, format="%(name)s - %(levelname)s - %(message)s") + + +def main() -> None: + """Run an object manipulation demo.""" + config_path = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), "config") + agi = RoboticsAGI(config_path=config_path) + + print("\n=== RAG7 AGI – Object Manipulation Demo ===\n") + + # Step 1: Navigate near the object + nav_task = agi.create_task("navigate", location={"x": 2.0, "y": 1.5}) + nav_result = agi.execute_task(nav_task) + print(f"Navigation: {nav_result}") + + # Step 2: Grasp the object + grasp_task = agi.create_task("grasp", target_object="red_cube") + grasp_result = agi.execute_task(grasp_task) + print(f"Grasp: {grasp_result}") + + # Step 3: Navigate to placement location + place_nav = agi.create_task("navigate", location={"x": 5.0, "y": 5.0}) + agi.execute_task(place_nav) + + # Step 4: Demonstrate planning for a complex task + cmd = agi.execute_command("pick up the blue box and place it on the shelf") + print(f"\nComplex command intent: {cmd['intent']}") + print(f"Response: {cmd['response']}") + + +if __name__ == "__main__": + main() diff --git a/examples/simple_navigation.py b/examples/simple_navigation.py new file mode 100644 index 0000000..5cc4f74 --- /dev/null +++ b/examples/simple_navigation.py @@ -0,0 +1,40 @@ +""" +Simple navigation example for the RAG7 AGI Robotics Framework. + +Demonstrates how to use the RoboticsAGI system to navigate a robot +to a target location using a natural language command. +""" + +import logging +import os +import sys + +# Ensure the project root is on the Python path +sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + +from agents.robotics_agi import RoboticsAGI + +logging.basicConfig(level=logging.INFO, format="%(name)s - %(levelname)s - %(message)s") + + +def main() -> None: + """Run a simple navigation demo.""" + config_path = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), "config") + agi = RoboticsAGI(config_path=config_path) + + print("\n=== RAG7 AGI – Simple Navigation Demo ===\n") + print("System status:", agi.get_status()["system"]) + + # Navigate via task API + task = agi.create_task("navigate", location={"x": 5.0, "y": 3.0}) + result = agi.execute_task(task) + print(f"Task result: {result}") + + # Navigate via natural language command + cmd_result = agi.execute_command("go to the charging station") + print(f"NL command result: intent={cmd_result['intent']}, success={cmd_result['success']}") + print(f"Robot response: {cmd_result['response']}") + + +if __name__ == "__main__": + main() diff --git a/learning/__init__.py b/learning/__init__.py new file mode 100644 index 0000000..c1fad92 --- /dev/null +++ b/learning/__init__.py @@ -0,0 +1,10 @@ +""" +Learning package for the RAG7 AGI Robotics Framework. +""" + +from learning.rl.dqn_agent import DQNAgent +from learning.rl.ppo_agent import PPOAgent +from learning.rl.replay_buffer import ReplayBuffer +from learning.imitation.bc_trainer import BCTrainer + +__all__ = ["DQNAgent", "PPOAgent", "ReplayBuffer", "BCTrainer"] diff --git a/learning/imitation/__init__.py b/learning/imitation/__init__.py new file mode 100644 index 0000000..d05b1dc --- /dev/null +++ b/learning/imitation/__init__.py @@ -0,0 +1,7 @@ +""" +Imitation learning sub-package for the RAG7 learning module. +""" + +from learning.imitation.bc_trainer import BCTrainer + +__all__ = ["BCTrainer"] diff --git a/learning/imitation/bc_trainer.py b/learning/imitation/bc_trainer.py new file mode 100644 index 0000000..2beb71e --- /dev/null +++ b/learning/imitation/bc_trainer.py @@ -0,0 +1,195 @@ +""" +Behavioural Cloning trainer for the RAG7 learning module. + +Trains a policy network to imitate expert demonstrations via +supervised learning on state-action pairs. +""" + +import logging +import random +from typing import Any, List, Optional, Tuple + +import numpy as np + +try: + import torch + import torch.nn as nn + import torch.optim as optim + + TORCH_AVAILABLE = True +except ImportError: + TORCH_AVAILABLE = False + + +if TORCH_AVAILABLE: + + class PolicyNetwork(nn.Module): + """Feedforward policy network for behavioural cloning. + + Args: + state_dim: Input state dimensionality. + action_dim: Output action dimensionality. + """ + + def __init__(self, state_dim: int, action_dim: int) -> None: + """Initialize the policy network.""" + super().__init__() + self.net = nn.Sequential( + nn.Linear(state_dim, 256), + nn.ReLU(), + nn.Linear(256, 256), + nn.ReLU(), + nn.Linear(256, action_dim), + ) + + def forward(self, x: "torch.Tensor") -> "torch.Tensor": + """Forward pass. + + Args: + x: Input state tensor. + + Returns: + Action logit tensor. + """ + return self.net(x) + +else: + PolicyNetwork = None # type: ignore[assignment,misc] + + +class BCTrainer: + """Behavioural Cloning (BC) policy trainer. + + Trains a policy network to mimic expert demonstrations using + supervised imitation learning. + + Args: + state_dim: Dimensionality of the state input. + action_dim: Dimensionality of the action output. + lr: Learning rate for the Adam optimiser. + """ + + def __init__( + self, + state_dim: int, + action_dim: int, + lr: float = 1e-3, + ) -> None: + """Initialize the BC trainer.""" + self._logger = logging.getLogger("rag7.learning.bc") + self.state_dim = state_dim + self.action_dim = action_dim + self._demonstrations: List[Tuple[Any, Any]] = [] + + if TORCH_AVAILABLE: + self.policy = PolicyNetwork(state_dim, action_dim) + self.optimizer = optim.Adam(self.policy.parameters(), lr=lr) + self.criterion = nn.MSELoss() + else: + self._logger.warning("PyTorch not available; BCTrainer in mock mode.") + self.policy = None + + # ------------------------------------------------------------------ + # Public API + # ------------------------------------------------------------------ + + def add_demonstration(self, state: Any, action: Any) -> None: + """Add a single expert demonstration to the dataset. + + Args: + state: Expert state observation. + action: Expert action corresponding to the state. + """ + self._demonstrations.append((state, action)) + + def train( + self, epochs: int = 10, batch_size: int = 32 + ) -> List[float]: + """Train the policy network on stored demonstrations. + + Args: + epochs: Number of training epochs. + batch_size: Mini-batch size. + + Returns: + List of per-epoch mean loss values. + """ + if not self._demonstrations: + self._logger.warning("No demonstrations available for training.") + return [] + + if not TORCH_AVAILABLE or self.policy is None: + return [random.random() * 0.1 for _ in range(epochs)] + + import torch + + states = torch.FloatTensor( + np.asarray([d[0] for d in self._demonstrations], dtype=np.float32) + ) + actions = torch.FloatTensor( + np.asarray([d[1] for d in self._demonstrations], dtype=np.float32) + ) + n = len(self._demonstrations) + epoch_losses: List[float] = [] + + for epoch in range(epochs): + indices = list(range(n)) + random.shuffle(indices) + epoch_loss = 0.0 + num_batches = 0 + + for start in range(0, n, batch_size): + batch_idx = indices[start : start + batch_size] + s_batch = states[batch_idx] + a_batch = actions[batch_idx] + + pred = self.policy(s_batch) + loss = self.criterion(pred, a_batch) + self.optimizer.zero_grad() + loss.backward() + self.optimizer.step() + epoch_loss += float(loss.item()) + num_batches += 1 + + mean_loss = epoch_loss / max(num_batches, 1) + epoch_losses.append(mean_loss) + self._logger.debug("Epoch %d/%d – loss: %.6f", epoch + 1, epochs, mean_loss) + + return epoch_losses + + def predict(self, state: Any) -> Any: + """Predict an action for a given state. + + Args: + state: Current state observation (array-like). + + Returns: + Predicted action as a numpy array. + """ + if not TORCH_AVAILABLE or self.policy is None: + return np.zeros(self.action_dim, dtype=np.float32) + + import torch + + state_t = torch.FloatTensor( + np.asarray(state, dtype=np.float32) + ).unsqueeze(0) + with torch.no_grad(): + pred = self.policy(state_t) + return pred.squeeze().numpy() + + def save(self, path: str) -> None: + """Save policy weights to disk.""" + if TORCH_AVAILABLE and self.policy is not None: + import torch + + torch.save(self.policy.state_dict(), path) + self._logger.info("BC policy saved to '%s'.", path) + + def load(self, path: str) -> None: + """Load policy weights from disk.""" + if TORCH_AVAILABLE and self.policy is not None: + import torch + + self.policy.load_state_dict(torch.load(path)) + self._logger.info("BC policy loaded from '%s'.", path) diff --git a/learning/rl/__init__.py b/learning/rl/__init__.py new file mode 100644 index 0000000..c01c62d --- /dev/null +++ b/learning/rl/__init__.py @@ -0,0 +1,9 @@ +""" +RL sub-package for the RAG7 learning module. +""" + +from learning.rl.dqn_agent import DQNAgent +from learning.rl.ppo_agent import PPOAgent +from learning.rl.replay_buffer import ReplayBuffer + +__all__ = ["DQNAgent", "PPOAgent", "ReplayBuffer"] diff --git a/learning/rl/dqn_agent.py b/learning/rl/dqn_agent.py new file mode 100644 index 0000000..61f1465 --- /dev/null +++ b/learning/rl/dqn_agent.py @@ -0,0 +1,189 @@ +""" +DQN (Deep Q-Network) agent for the RAG7 learning module. + +Implements experience-replay DQN with epsilon-greedy exploration. +""" + +import logging +import random +from typing import Any, Dict, List, Optional + +import numpy as np + +try: + import torch + import torch.nn as nn + import torch.optim as optim + + TORCH_AVAILABLE = True +except ImportError: + TORCH_AVAILABLE = False + + +if TORCH_AVAILABLE: + + class QNetwork(nn.Module): + """Fully-connected Q-value network. + + Args: + state_dim: Dimensionality of the state input vector. + action_dim: Number of discrete actions. + """ + + def __init__(self, state_dim: int, action_dim: int) -> None: + """Initialize the Q-network.""" + super().__init__() + self.net = nn.Sequential( + nn.Linear(state_dim, 128), + nn.ReLU(), + nn.Linear(128, 128), + nn.ReLU(), + nn.Linear(128, action_dim), + ) + + def forward(self, x: "torch.Tensor") -> "torch.Tensor": + """Forward pass through the network. + + Args: + x: Input state tensor of shape (batch, state_dim). + + Returns: + Q-value tensor of shape (batch, action_dim). + """ + return self.net(x) + +else: + QNetwork = None # type: ignore[assignment,misc] + + +class DQNAgent: + """Deep Q-Network reinforcement learning agent. + + Implements the DQN algorithm (Mnih et al., 2015) with + epsilon-greedy exploration and experience replay. + + Args: + state_dim: Dimensionality of the observation space. + action_dim: Number of discrete actions. + lr: Learning rate for the Adam optimiser. + gamma: Discount factor for future rewards. + epsilon: Initial exploration probability. + epsilon_min: Minimum exploration probability. + epsilon_decay: Multiplicative decay applied each update step. + """ + + def __init__( + self, + state_dim: int, + action_dim: int, + lr: float = 1e-3, + gamma: float = 0.99, + epsilon: float = 1.0, + epsilon_min: float = 0.01, + epsilon_decay: float = 0.995, + ) -> None: + """Initialize the DQN agent.""" + self._logger = logging.getLogger("rag7.learning.dqn") + self.state_dim = state_dim + self.action_dim = action_dim + self.gamma = gamma + self.epsilon = epsilon + self.epsilon_min = epsilon_min + self.epsilon_decay = epsilon_decay + + if TORCH_AVAILABLE: + self.q_network = QNetwork(state_dim, action_dim) + self.target_network = QNetwork(state_dim, action_dim) + self.target_network.load_state_dict(self.q_network.state_dict()) + self.optimizer = optim.Adam(self.q_network.parameters(), lr=lr) + self.criterion = nn.MSELoss() + else: + self._logger.warning("PyTorch not available; DQN agent in mock mode.") + self.q_network = None + self.target_network = None + + # ------------------------------------------------------------------ + # Public API + # ------------------------------------------------------------------ + + def select_action(self, state: Any) -> int: + """Select an action using epsilon-greedy policy. + + Args: + state: Current environment state (array-like). + + Returns: + Integer action index. + """ + if random.random() < self.epsilon: + return random.randint(0, self.action_dim - 1) + + if not TORCH_AVAILABLE or self.q_network is None: + return random.randint(0, self.action_dim - 1) + + import torch + + state_t = torch.FloatTensor(np.asarray(state, dtype=np.float32)).unsqueeze(0) + with torch.no_grad(): + q_values = self.q_network(state_t) + return int(q_values.argmax().item()) + + def update(self, batch: Dict[str, Any]) -> float: + """Update the Q-network from a sampled experience batch. + + Args: + batch: Dictionary with tensor keys: ``states``, ``actions``, + ``rewards``, ``next_states``, ``dones``. + + Returns: + Scalar loss value. + """ + if not TORCH_AVAILABLE or self.q_network is None: + return 0.0 + + import torch + + states = batch["states"] + actions = batch["actions"] + rewards = batch["rewards"] + next_states = batch["next_states"] + dones = batch["dones"] + + q_current = self.q_network(states).gather(1, actions.unsqueeze(1)).squeeze() + with torch.no_grad(): + q_next = self.target_network(next_states).max(1)[0] + q_target = rewards + self.gamma * q_next * (1 - dones) + + loss = self.criterion(q_current, q_target) + self.optimizer.zero_grad() + loss.backward() + self.optimizer.step() + + # Decay epsilon + self.epsilon = max(self.epsilon_min, self.epsilon * self.epsilon_decay) + return float(loss.item()) + + def save(self, path: str) -> None: + """Save the Q-network weights to disk. + + Args: + path: Destination file path. + """ + if TORCH_AVAILABLE and self.q_network is not None: + import torch + + torch.save(self.q_network.state_dict(), path) + self._logger.info("DQN model saved to '%s'.", path) + + def load(self, path: str) -> None: + """Load Q-network weights from disk. + + Args: + path: Source file path. + """ + if TORCH_AVAILABLE and self.q_network is not None: + import torch + + self.q_network.load_state_dict(torch.load(path)) + self.target_network.load_state_dict(self.q_network.state_dict()) + self._logger.info("DQN model loaded from '%s'.", path) diff --git a/learning/rl/ppo_agent.py b/learning/rl/ppo_agent.py new file mode 100644 index 0000000..688bb57 --- /dev/null +++ b/learning/rl/ppo_agent.py @@ -0,0 +1,204 @@ +""" +PPO (Proximal Policy Optimisation) agent for the RAG7 learning module. + +Implements the PPO-Clip algorithm for continuous and discrete action +spaces using actor-critic networks. +""" + +import logging +from typing import Any, Dict, List, NamedTuple, Optional, Tuple + +import numpy as np + +try: + import torch + import torch.nn as nn + import torch.optim as optim + from torch.distributions import Categorical + + TORCH_AVAILABLE = True +except ImportError: + TORCH_AVAILABLE = False + + +if TORCH_AVAILABLE: + + class ActorCritic(nn.Module): + """Combined actor-critic network for PPO. + + Args: + state_dim: Input state dimensionality. + action_dim: Number of discrete actions. + """ + + def __init__(self, state_dim: int, action_dim: int) -> None: + """Initialize the ActorCritic network.""" + super().__init__() + self.shared = nn.Sequential( + nn.Linear(state_dim, 128), + nn.Tanh(), + nn.Linear(128, 64), + nn.Tanh(), + ) + self.actor_head = nn.Linear(64, action_dim) + self.critic_head = nn.Linear(64, 1) + + def forward( + self, x: "torch.Tensor" + ) -> Tuple["torch.Tensor", "torch.Tensor"]: + """Compute action logits and state value. + + Args: + x: Input state tensor. + + Returns: + Tuple of (action_logits, state_value). + """ + features = self.shared(x) + return self.actor_head(features), self.critic_head(features) + + def act( + self, state: "torch.Tensor" + ) -> Tuple[int, "torch.Tensor"]: + """Sample an action and return its log probability. + + Args: + state: State tensor. + + Returns: + Tuple of (action integer, log_prob tensor). + """ + logits, _ = self.forward(state) + dist = Categorical(logits=logits) + action = dist.sample() + return action.item(), dist.log_prob(action) + +else: + ActorCritic = None # type: ignore[assignment,misc] + + +class PPOAgent: + """Proximal Policy Optimisation reinforcement learning agent. + + Args: + state_dim: Dimensionality of the observation space. + action_dim: Number of discrete actions. + lr: Learning rate. + gamma: Discount factor. + epsilon_clip: PPO clipping parameter. + k_epochs: Number of gradient update epochs per batch. + """ + + def __init__( + self, + state_dim: int, + action_dim: int, + lr: float = 3e-4, + gamma: float = 0.99, + epsilon_clip: float = 0.2, + k_epochs: int = 4, + ) -> None: + """Initialize the PPO agent.""" + self._logger = logging.getLogger("rag7.learning.ppo") + self.state_dim = state_dim + self.action_dim = action_dim + self.gamma = gamma + self.epsilon_clip = epsilon_clip + self.k_epochs = k_epochs + + if TORCH_AVAILABLE: + self.policy = ActorCritic(state_dim, action_dim) + self.optimizer = optim.Adam(self.policy.parameters(), lr=lr) + else: + self._logger.warning("PyTorch not available; PPO agent in mock mode.") + self.policy = None + + # ------------------------------------------------------------------ + # Public API + # ------------------------------------------------------------------ + + def select_action(self, state: Any) -> Tuple[int, Any]: + """Select an action using the current policy. + + Args: + state: Current environment state (array-like). + + Returns: + Tuple of (action integer, log_prob tensor or float). + """ + if not TORCH_AVAILABLE or self.policy is None: + import random + + return random.randint(0, self.action_dim - 1), 0.0 + + import torch + + state_t = torch.FloatTensor(np.asarray(state, dtype=np.float32)).unsqueeze(0) + with torch.no_grad(): + action, log_prob = self.policy.act(state_t) + return action, log_prob + + def update(self, memory: Dict[str, Any]) -> float: + """Update actor-critic networks using collected experience. + + Args: + memory: Dictionary with ``states``, ``actions``, + ``log_probs``, ``rewards``, ``dones`` keys (tensors). + + Returns: + Mean policy loss over update epochs. + """ + if not TORCH_AVAILABLE or self.policy is None: + return 0.0 + + import torch + import torch.nn.functional as F + + states = memory["states"] + actions = memory["actions"] + old_log_probs = memory["log_probs"] + rewards = memory["rewards"] + + # Compute discounted returns + returns = [] + discounted = 0.0 + for r in reversed(rewards.tolist()): + discounted = r + self.gamma * discounted + returns.insert(0, discounted) + returns_t = torch.FloatTensor(returns) + returns_t = (returns_t - returns_t.mean()) / (returns_t.std() + 1e-8) + + total_loss = 0.0 + for _ in range(self.k_epochs): + logits, values = self.policy(states) + dist = torch.distributions.Categorical(logits=logits) + log_probs = dist.log_prob(actions) + ratio = (log_probs - old_log_probs).exp() + advantages = returns_t - values.squeeze().detach() + + surr1 = ratio * advantages + surr2 = torch.clamp(ratio, 1 - self.epsilon_clip, 1 + self.epsilon_clip) * advantages + actor_loss = -torch.min(surr1, surr2).mean() + critic_loss = F.mse_loss(values.squeeze(), returns_t) + loss = actor_loss + 0.5 * critic_loss + + self.optimizer.zero_grad() + loss.backward() + self.optimizer.step() + total_loss += float(loss.item()) + + return total_loss / self.k_epochs + + def save(self, path: str) -> None: + """Save policy weights to disk.""" + if TORCH_AVAILABLE and self.policy is not None: + import torch + + torch.save(self.policy.state_dict(), path) + + def load(self, path: str) -> None: + """Load policy weights from disk.""" + if TORCH_AVAILABLE and self.policy is not None: + import torch + + self.policy.load_state_dict(torch.load(path)) diff --git a/learning/rl/replay_buffer.py b/learning/rl/replay_buffer.py new file mode 100644 index 0000000..de49ee6 --- /dev/null +++ b/learning/rl/replay_buffer.py @@ -0,0 +1,116 @@ +""" +Replay buffer module for the RAG7 learning module. + +Provides an efficient circular experience replay buffer for +off-policy reinforcement learning algorithms. +""" + +import logging +import random +from collections import deque +from typing import Any, Deque, Dict, List, Tuple + +import numpy as np + +try: + import torch + + TORCH_AVAILABLE = True +except ImportError: + TORCH_AVAILABLE = False + + +class ReplayBuffer: + """Fixed-capacity circular replay buffer for experience replay. + + Stores (state, action, reward, next_state, done) transitions and + provides random sampling for training. + + Args: + capacity: Maximum number of transitions to store. + """ + + def __init__(self, capacity: int = 10_000) -> None: + """Initialize the replay buffer.""" + self._logger = logging.getLogger("rag7.learning.replay") + self._capacity = capacity + self._buffer: Deque[Tuple[Any, Any, float, Any, bool]] = deque( + maxlen=capacity + ) + + # ------------------------------------------------------------------ + # Public API + # ------------------------------------------------------------------ + + def push( + self, + state: Any, + action: Any, + reward: float, + next_state: Any, + done: bool, + ) -> None: + """Add a transition to the buffer. + + When the buffer is full the oldest transition is silently + evicted. + + Args: + state: Current observation. + action: Action taken. + reward: Observed scalar reward. + next_state: Resulting observation. + done: Episode termination flag. + """ + self._buffer.append((state, action, float(reward), next_state, bool(done))) + + def sample(self, batch_size: int) -> Dict[str, Any]: + """Sample a random mini-batch of transitions. + + Args: + batch_size: Number of transitions to sample. + + Returns: + Dictionary with keys ``states``, ``actions``, ``rewards``, + ``next_states``, ``dones`` as numpy arrays or torch tensors. + + Raises: + ValueError: If there are fewer transitions than batch_size. + """ + if len(self._buffer) < batch_size: + raise ValueError( + f"Buffer contains {len(self._buffer)} transitions " + f"but batch_size is {batch_size}." + ) + + transitions = random.sample(self._buffer, batch_size) + states, actions, rewards, next_states, dones = zip(*transitions) + + states_arr = np.asarray(states, dtype=np.float32) + actions_arr = np.asarray(actions, dtype=np.int64) + rewards_arr = np.asarray(rewards, dtype=np.float32) + next_states_arr = np.asarray(next_states, dtype=np.float32) + dones_arr = np.asarray(dones, dtype=np.float32) + + if TORCH_AVAILABLE: + import torch + + return { + "states": torch.from_numpy(states_arr), + "actions": torch.from_numpy(actions_arr), + "rewards": torch.from_numpy(rewards_arr), + "next_states": torch.from_numpy(next_states_arr), + "dones": torch.from_numpy(dones_arr), + } + + return { + "states": states_arr, + "actions": actions_arr, + "rewards": rewards_arr, + "next_states": next_states_arr, + "dones": dones_arr, + } + + def __len__(self) -> int: + """Return the current number of stored transitions.""" + return len(self._buffer) diff --git a/models/.gitkeep b/models/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/nlp/__init__.py b/nlp/__init__.py new file mode 100644 index 0000000..25c1632 --- /dev/null +++ b/nlp/__init__.py @@ -0,0 +1,9 @@ +""" +NLP package for the RAG7 AGI Robotics Framework. +""" + +from nlp.command_parser import CommandParser +from nlp.intent_classifier import IntentClassifier +from nlp.dialog_manager import DialogManager + +__all__ = ["CommandParser", "IntentClassifier", "DialogManager"] diff --git a/nlp/command_parser.py b/nlp/command_parser.py new file mode 100644 index 0000000..4fba256 --- /dev/null +++ b/nlp/command_parser.py @@ -0,0 +1,159 @@ +""" +Command parser module for the RAG7 NLP system. + +Converts natural language operator commands into structured action +dictionaries using LLM-based or rule-based parsing. +""" + +import logging +from typing import Any, Dict, List, Optional + +try: + from langchain_openai import ChatOpenAI # noqa: F401 + + LANGCHAIN_AVAILABLE = True +except ImportError: + LANGCHAIN_AVAILABLE = False + + +class CommandParser: + """Natural language command parser for robot control. + + Parses free-text operator commands into structured dictionaries + containing intent, action, target object, location, and confidence. + + Args: + llm_config: Optional LLM configuration dictionary. + """ + + def __init__(self, llm_config: Optional[Dict[str, Any]] = None) -> None: + """Initialize the command parser.""" + self._logger = logging.getLogger("rag7.nlp.parser") + self._llm_config = llm_config or {} + self._llm = None + + if LANGCHAIN_AVAILABLE and self._llm_config.get("provider") == "openai": + self._try_init_llm() + + # ------------------------------------------------------------------ + # Public API + # ------------------------------------------------------------------ + + def parse(self, text: str) -> Dict[str, Any]: + """Parse a natural language command into a structured dict. + + Args: + text: Raw operator input string. + + Returns: + Dictionary with keys: + - ``intent``: Detected intent string. + - ``action``: Primary action word. + - ``target_object``: Detected object target (or None). + - ``location``: Detected location (or None). + - ``confidence``: Parser confidence in [0, 1]. + """ + if self._llm is not None: + try: + return self._llm_parse(text) + except Exception as exc: # noqa: BLE001 + self._logger.warning("LLM parse failed: %s", exc) + + return self._rule_based_parse(text) + + def extract_entities(self, text: str) -> Dict[str, Any]: + """Extract named entities from a command string. + + Args: + text: Raw operator input string. + + Returns: + Dictionary with detected entity types as keys. + """ + words = text.lower().split() + entities: Dict[str, Any] = { + "objects": [], + "locations": [], + "actions": [], + "numbers": [], + } + + # Object keywords + object_words = ["box", "bottle", "cup", "chair", "door", "robot", "object", "item"] + for word in words: + clean = word.strip(".,!?") + if clean in object_words: + entities["objects"].append(clean) + elif clean.lstrip("-").replace(".", "").isdigit(): + entities["numbers"].append(float(clean)) + + # Location prepositions + location_preps = {"to", "at", "towards", "near", "beside", "on", "in"} + for i, word in enumerate(words): + if word in location_preps and i + 1 < len(words): + entities["locations"].append(words[i + 1].strip(".,!?")) + + return entities + + # ------------------------------------------------------------------ + # Private helpers + # ------------------------------------------------------------------ + + def _rule_based_parse(self, text: str) -> Dict[str, Any]: + """Keyword-based fallback parser. + + Args: + text: Raw operator input string. + + Returns: + Structured command dictionary. + """ + from nlp.intent_classifier import IntentClassifier + + classifier = IntentClassifier() + intent = classifier.classify(text) + entities = self.extract_entities(text) + target_object = entities["objects"][0] if entities["objects"] else None + location = entities["locations"][0] if entities["locations"] else None + + return { + "intent": intent, + "action": intent, + "target_object": target_object, + "location": location, + "confidence": 0.65, + } + + def _llm_parse(self, text: str) -> Dict[str, Any]: + """Parse command using LLM backend.""" + import json + + prompt = ( + f"Parse this robot command into JSON with fields " + f"intent, action, target_object, location, confidence:\n'{text}'" + ) + response = self._llm.invoke(prompt) + try: + return json.loads(response.content) + except json.JSONDecodeError: + return self._rule_based_parse(text) + + def _try_init_llm(self) -> None: + """Attempt to initialise the LangChain LLM client.""" + try: + import os + + from langchain_openai import ChatOpenAI + + api_key = os.environ.get( + self._llm_config.get("api_key_env", "OPENAI_API_KEY"), "" + ) + if not api_key: + return + self._llm = ChatOpenAI( + model=self._llm_config.get("model", "gpt-4"), + temperature=self._llm_config.get("temperature", 0.1), + openai_api_key=api_key, + ) + except Exception as exc: # noqa: BLE001 + self._logger.warning("CommandParser LLM init failed: %s", exc) diff --git a/nlp/dialog_manager.py b/nlp/dialog_manager.py new file mode 100644 index 0000000..fd927fe --- /dev/null +++ b/nlp/dialog_manager.py @@ -0,0 +1,119 @@ +""" +Dialog manager module for the RAG7 NLP system. + +Manages multi-turn conversations between human operators and the robot, +maintaining dialog history and generating contextually appropriate +responses. +""" + +import logging +from typing import Any, Dict, List, Optional + +from nlp.intent_classifier import IntentClassifier + + +class DialogManager: + """Multi-turn dialog manager for human-robot interaction. + + Maintains conversation history and generates contextually aware + responses based on detected intent and robot state. + + Args: + max_history: Maximum number of dialog turns to retain. + """ + + def __init__(self, max_history: int = 10) -> None: + """Initialize the dialog manager.""" + self._logger = logging.getLogger("rag7.nlp.dialog") + self._max_history = max_history + self._history: List[Dict[str, str]] = [] + self._classifier = IntentClassifier() + + # ------------------------------------------------------------------ + # Public API + # ------------------------------------------------------------------ + + def process( + self, user_input: str, robot_state: Optional[Dict[str, Any]] = None + ) -> str: + """Process a user utterance and return a robot response. + + Args: + user_input: Free-text message from the operator. + robot_state: Optional current robot state dictionary. + + Returns: + Response string from the robot. + """ + robot_state = robot_state or {} + intent = self._classifier.classify(user_input) + self.add_turn("human", user_input) + response = self._generate_response(intent, robot_state) + self.add_turn("robot", response) + return response + + def add_turn(self, role: str, content: str) -> None: + """Add a single dialog turn to the history. + + Args: + role: Speaker role (``"human"`` or ``"robot"``). + content: Utterance content string. + """ + self._history.append({"role": role, "content": content}) + if len(self._history) > self._max_history: + self._history.pop(0) + + def get_history(self) -> List[Dict[str, str]]: + """Return the current conversation history. + + Returns: + List of turn dictionaries with ``role`` and ``content`` keys. + """ + return list(self._history) + + def clear_history(self) -> None: + """Clear all stored dialog turns.""" + self._history.clear() + self._logger.debug("Dialog history cleared.") + + # ------------------------------------------------------------------ + # Private helpers + # ------------------------------------------------------------------ + + def _generate_response( + self, intent: str, robot_state: Dict[str, Any] + ) -> str: + """Generate a contextually appropriate robot response. + + Args: + intent: Classified intent string. + robot_state: Current robot state dictionary. + + Returns: + Response string. + """ + is_stopped = robot_state.get("is_stopped", False) + position = robot_state.get("position", {}) + pos_str = "" + if position: + pos_str = ( + f" Current position: ({position.get('x', 0):.2f}, " + f"{position.get('y', 0):.2f})." + ) + + if is_stopped: + return "Emergency stop is active. Please reset before issuing new commands." + + responses = { + "navigate": f"Understood. I will navigate to the specified location.{pos_str}", + "grasp": "Acknowledged. Initiating grasp sequence for the target object.", + "place": "Understood. I will place the object at the specified location.", + "inspect": f"Starting inspection scan.{pos_str}", + "stop": "Emergency stop engaged. All motion halted immediately.", + "query": f"System is operational.{pos_str} All agents are running.", + "unknown": ( + "I did not understand that command. " + "Please try: navigate, grasp, place, inspect, stop, or query." + ), + } + return responses.get(intent, responses["unknown"]) diff --git a/nlp/intent_classifier.py b/nlp/intent_classifier.py new file mode 100644 index 0000000..92e35a5 --- /dev/null +++ b/nlp/intent_classifier.py @@ -0,0 +1,126 @@ +""" +Intent classifier module for the RAG7 NLP system. + +Classifies operator commands into predefined intent categories using +keyword matching with confidence scoring. +""" + +import logging +from typing import Dict, List + + +class IntentClassifier: + """Keyword-based intent classifier for robot commands. + + Classifies text inputs into one of the predefined intents using + a weighted keyword matching approach. + + Class attributes: + INTENTS: List of supported intent label strings. + """ + + INTENTS: List[str] = [ + "navigate", + "grasp", + "place", + "inspect", + "stop", + "query", + "unknown", + ] + + # Mapping from intent to trigger keywords with weights + _KEYWORD_MAP: Dict[str, Dict[str, float]] = { + "navigate": { + "go": 1.0, "move": 1.0, "navigate": 1.5, "drive": 1.0, + "travel": 0.8, "head": 0.7, "proceed": 0.7, "reach": 0.7, + }, + "grasp": { + "grasp": 1.5, "pick": 1.0, "grab": 1.0, "take": 0.8, + "hold": 0.7, "lift": 0.8, "collect": 0.7, "retrieve": 0.8, + }, + "place": { + "place": 1.5, "put": 1.0, "drop": 0.8, "set": 0.7, + "release": 0.8, "deposit": 0.8, "leave": 0.6, + }, + "inspect": { + "inspect": 1.5, "look": 0.7, "examine": 1.0, "scan": 0.9, + "check": 0.8, "observe": 0.9, "analyse": 0.8, "survey": 0.8, + }, + "stop": { + "stop": 1.5, "halt": 1.5, "freeze": 1.0, "pause": 0.9, + "emergency": 1.0, "abort": 1.0, "cancel": 0.7, + }, + "query": { + "what": 0.8, "where": 0.8, "how": 0.7, "status": 1.0, + "tell": 0.6, "show": 0.6, "report": 0.9, "describe": 0.7, + }, + } + + def __init__(self) -> None: + """Initialize the intent classifier.""" + self._logger = logging.getLogger("rag7.nlp.intent") + + # ------------------------------------------------------------------ + # Public API + # ------------------------------------------------------------------ + + def classify(self, text: str) -> str: + """Classify the primary intent of an input text. + + Args: + text: Natural language input string. + + Returns: + Intent label string from :attr:`INTENTS`. + """ + scores = self._score_all(text) + if not scores: + return "unknown" + best_intent = max(scores, key=scores.get) # type: ignore[arg-type] + if scores[best_intent] == 0.0: + return "unknown" + return best_intent + + def get_confidence(self, text: str, intent: str) -> float: + """Return the classifier confidence for a specific intent. + + Args: + text: Natural language input string. + intent: Intent label to score. + + Returns: + Confidence value in the range [0, 1]. + """ + if intent not in self._KEYWORD_MAP: + return 0.0 + + scores = self._score_all(text) + total = sum(scores.values()) + if total == 0.0: + return 0.0 + return min(1.0, scores.get(intent, 0.0) / total) + + # ------------------------------------------------------------------ + # Private helpers + # ------------------------------------------------------------------ + + def _score_all(self, text: str) -> Dict[str, float]: + """Compute weighted keyword scores for all intents. + + Args: + text: Input text to score. + + Returns: + Dict mapping each intent to its raw keyword score. + """ + words = text.lower().split() + scores: Dict[str, float] = {intent: 0.0 for intent in self._KEYWORD_MAP} + + for intent, keywords in self._KEYWORD_MAP.items(): + for word in words: + clean = word.strip(".,!?;:") + if clean in keywords: + scores[intent] += keywords[clean] + + return scores diff --git a/perception/__init__.py b/perception/__init__.py new file mode 100644 index 0000000..6579f9b --- /dev/null +++ b/perception/__init__.py @@ -0,0 +1,19 @@ +""" +Perception package for the RAG7 AGI Robotics Framework. + +Provides vision, SLAM, and sensor fusion capabilities. +""" + +from perception.vision.object_detection import ObjectDetector +from perception.vision.segmentation import Segmenter +from perception.vision.tracking import ObjectTracker +from perception.slam.mapping import SLAMMapper +from perception.sensor_fusion import SensorFusion + +__all__ = [ + "ObjectDetector", + "Segmenter", + "ObjectTracker", + "SLAMMapper", + "SensorFusion", +] diff --git a/perception/sensor_fusion.py b/perception/sensor_fusion.py new file mode 100644 index 0000000..ef41b31 --- /dev/null +++ b/perception/sensor_fusion.py @@ -0,0 +1,138 @@ +""" +Sensor fusion module for the RAG7 perception system. + +Fuses data from RGB-D camera, 2-D LiDAR, and IMU into a unified +robot state estimate. +""" + +import logging +from typing import Any, Dict, Optional + +import numpy as np + + +class SensorFusion: + """Multi-modal sensor fusion processor. + + Combines camera depth, LiDAR range, and IMU orientation data into + a coherent fused state using a lightweight complementary-filter + approach. + + Attributes: + _state: Current fused state dictionary. + """ + + def __init__(self) -> None: + """Initialize the sensor fusion module.""" + self._logger = logging.getLogger("rag7.perception.fusion") + self._state: Dict[str, Any] = { + "position": {"x": 0.0, "y": 0.0, "z": 0.0}, + "orientation": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "velocity": {"linear": 0.0, "angular": 0.0}, + "obstacles": [], + } + + # ------------------------------------------------------------------ + # Public API + # ------------------------------------------------------------------ + + def fuse( + self, + camera_data: Optional[Dict[str, Any]], + lidar_data: Optional[Any], + imu_data: Optional[Dict[str, Any]], + ) -> Dict[str, Any]: + """Fuse data from multiple sensors into a unified state estimate. + + Args: + camera_data: Dict with optional ``depth`` array and + ``detections`` list. + lidar_data: Array-like of LiDAR range measurements. + imu_data: Dict with ``acceleration`` and/or orientation keys. + + Returns: + Fused state dictionary with ``position``, ``orientation``, + ``obstacles``, and ``confidence`` keys. + """ + fused: Dict[str, Any] = { + "position": dict(self._state["position"]), + "orientation": dict(self._state["orientation"]), + "obstacles": [], + "confidence": 0.0, + } + + confidence_sources = 0 + + if camera_data is not None: + depth = camera_data.get("depth") + if depth is not None: + pos_3d = self._camera_to_3d(camera_data, depth) + fused["position"].update(pos_3d) + confidence_sources += 1 + + if lidar_data is not None: + try: + ranges = np.asarray(lidar_data, dtype=float) + valid = ranges[np.isfinite(ranges) & (ranges > 0)] + if len(valid) > 0: + fused["obstacles"] = [float(np.min(valid))] + confidence_sources += 1 + except Exception as exc: # noqa: BLE001 + self._logger.warning("LiDAR fusion error: %s", exc) + + if imu_data is not None: + updated = self._apply_imu(fused, imu_data) + fused["orientation"] = updated["orientation"] + confidence_sources += 1 + + fused["confidence"] = confidence_sources / 3.0 + self._state = fused + return fused + + # ------------------------------------------------------------------ + # Private helpers + # ------------------------------------------------------------------ + + def _camera_to_3d( + self, camera_data: Dict[str, Any], depth: Any + ) -> Dict[str, float]: + """Project camera depth data to a 3-D position estimate. + + Uses the image centre pixel depth as a simplified projection. + + Args: + camera_data: Camera data dictionary (unused fields reserved + for future intrinsic calibration). + depth: Depth array (H x W) in metres. + + Returns: + Position dictionary with ``x``, ``y``, ``z`` keys. + """ + try: + depth_arr = np.asarray(depth, dtype=float) + if depth_arr.ndim == 2: + h, w = depth_arr.shape + centre_depth = float(depth_arr[h // 2, w // 2]) + return {"x": 0.0, "y": 0.0, "z": centre_depth} + except Exception as exc: # noqa: BLE001 + self._logger.warning("Camera projection error: %s", exc) + return {"x": 0.0, "y": 0.0, "z": 0.0} + + def _apply_imu( + self, state: Dict[str, Any], imu_data: Dict[str, Any] + ) -> Dict[str, Any]: + """Apply IMU measurements to correct the orientation estimate. + + Args: + state: Current fused state dict. + imu_data: IMU reading dict with optional ``roll``, ``pitch``, + ``yaw``, or ``angular_velocity`` keys. + + Returns: + Updated state dictionary. + """ + orientation = dict(state.get("orientation", {})) + orientation["roll"] = float(imu_data.get("roll", orientation.get("roll", 0.0))) + orientation["pitch"] = float(imu_data.get("pitch", orientation.get("pitch", 0.0))) + orientation["yaw"] = float(imu_data.get("yaw", orientation.get("yaw", 0.0))) + return {**state, "orientation": orientation} diff --git a/perception/slam/__init__.py b/perception/slam/__init__.py new file mode 100644 index 0000000..a17f57e --- /dev/null +++ b/perception/slam/__init__.py @@ -0,0 +1,7 @@ +""" +SLAM sub-package for the RAG7 perception module. +""" + +from perception.slam.mapping import SLAMMapper + +__all__ = ["SLAMMapper"] diff --git a/perception/slam/mapping.py b/perception/slam/mapping.py new file mode 100644 index 0000000..d311309 --- /dev/null +++ b/perception/slam/mapping.py @@ -0,0 +1,161 @@ +""" +SLAM mapping module for the RAG7 perception system. + +Provides an occupancy-grid-based mapping and localisation system +suitable for 2-D LiDAR data. +""" + +import logging +from typing import Any, Dict, List, Optional, Tuple + +import numpy as np + + +class SLAMMapper: + """Occupancy-grid SLAM mapper for 2-D LiDAR data. + + Maintains an occupancy grid map of the environment and provides + simple scan-matching localisation. + + Args: + map_resolution: Grid cell size in metres per cell. + map_size: Number of cells along each axis (creates a square map). + """ + + # Cell value constants + UNKNOWN = 0.5 + FREE = 0.0 + OCCUPIED = 1.0 + + def __init__( + self, + map_resolution: float = 0.05, + map_size: int = 100, + ) -> None: + """Initialize the SLAM mapper with an unknown occupancy grid.""" + self._logger = logging.getLogger("rag7.perception.slam") + self._resolution = map_resolution + self._size = map_size + self._map = np.full((map_size, map_size), self.UNKNOWN, dtype=np.float32) + self._pose: Dict[str, float] = {"x": 0.0, "y": 0.0, "theta": 0.0} + + # ------------------------------------------------------------------ + # Public API + # ------------------------------------------------------------------ + + def update(self, lidar_scan: Any, pose: Dict[str, float]) -> None: + """Integrate a LiDAR scan into the occupancy grid map. + + Uses a simplified ray-casting model to mark free and occupied + cells along each scan ray. + + Args: + lidar_scan: Array-like of range measurements in metres. + Values of ``inf`` or ``nan`` are ignored. + pose: Robot pose dictionary with ``x``, ``y``, and ``theta`` + keys (theta in radians). + """ + self._pose = dict(pose) + ranges = np.asarray(lidar_scan, dtype=float) + robot_cell = self._world_to_cell(pose["x"], pose["y"]) + num_rays = len(ranges) + + for i, r in enumerate(ranges): + if not np.isfinite(r) or r <= 0: + continue + angle = pose.get("theta", 0.0) + (2 * np.pi * i / num_rays) + end_x = pose["x"] + r * np.cos(angle) + end_y = pose["y"] + r * np.sin(angle) + end_cell = self._world_to_cell(end_x, end_y) + self._ray_cast(robot_cell, end_cell) + + self._logger.debug("Map updated at pose (%.2f, %.2f).", pose["x"], pose["y"]) + + def get_map(self) -> np.ndarray: + """Return the current occupancy grid map. + + Returns: + 2-D numpy array of shape (map_size, map_size) with cell + values in [0, 1] where 0 = free, 1 = occupied, 0.5 = unknown. + """ + return self._map.copy() + + def localize(self, scan: Any) -> Dict[str, float]: + """Estimate the robot pose from a LiDAR scan. + + This is a simplified placeholder that returns the last known + pose. A production implementation would use ICP or particle + filters. + + Args: + scan: LiDAR range measurements. + + Returns: + Estimated pose dictionary with ``x``, ``y``, ``theta``, + and ``confidence`` keys. + """ + return { + "x": self._pose["x"], + "y": self._pose["y"], + "theta": self._pose.get("theta", 0.0), + "confidence": 0.8, + } + + # ------------------------------------------------------------------ + # Private helpers + # ------------------------------------------------------------------ + + def _world_to_cell(self, x: float, y: float) -> Tuple[int, int]: + """Convert world coordinates to grid cell indices. + + Args: + x: World X coordinate in metres. + y: World Y coordinate in metres. + + Returns: + (row, col) cell indices clamped to the grid bounds. + """ + col = int(x / self._resolution) + self._size // 2 + row = int(y / self._resolution) + self._size // 2 + row = max(0, min(self._size - 1, row)) + col = max(0, min(self._size - 1, col)) + return row, col + + def _ray_cast( + self, start: Tuple[int, int], end: Tuple[int, int] + ) -> None: + """Mark cells along a ray using Bresenham's line algorithm. + + Cells between start and end are marked free; the end cell is + marked occupied. + + Args: + start: (row, col) of the robot cell. + end: (row, col) of the obstacle cell. + """ + r0, c0 = start + r1, c1 = end + dr = abs(r1 - r0) + dc = abs(c1 - c0) + sr = 1 if r1 > r0 else -1 + sc = 1 if c1 > c0 else -1 + err = dr - dc + r, c = r0, c0 + + while True: + if 0 <= r < self._size and 0 <= c < self._size: + if (r, c) == (r1, c1): + self._map[r, c] = min(1.0, self._map[r, c] + 0.2) + break + else: + self._map[r, c] = max(0.0, self._map[r, c] - 0.1) + else: + break + + e2 = 2 * err + if e2 > -dc: + err -= dc + r += sr + if e2 < dr: + err += dr + c += sc diff --git a/perception/vision/__init__.py b/perception/vision/__init__.py new file mode 100644 index 0000000..8b99c1d --- /dev/null +++ b/perception/vision/__init__.py @@ -0,0 +1,9 @@ +""" +Vision sub-package for the RAG7 perception module. +""" + +from perception.vision.object_detection import ObjectDetector +from perception.vision.segmentation import Segmenter +from perception.vision.tracking import ObjectTracker + +__all__ = ["ObjectDetector", "Segmenter", "ObjectTracker"] diff --git a/perception/vision/object_detection.py b/perception/vision/object_detection.py new file mode 100644 index 0000000..4bf3aa7 --- /dev/null +++ b/perception/vision/object_detection.py @@ -0,0 +1,165 @@ +""" +Object detection module for the RAG7 perception system. + +Provides a PyTorch-based object detector that gracefully falls back to +mock detections when no pre-trained model is available. +""" + +import logging +from collections import namedtuple +from typing import Any, List, Optional + +import numpy as np + +try: + import torch + import torch.nn as nn + + TORCH_AVAILABLE = True +except ImportError: + TORCH_AVAILABLE = False + +# Named tuple for structured detection output +Detection = namedtuple("Detection", ["class_id", "label", "confidence", "bbox"]) + + +class ObjectDetector: + """Real-time object detector using a PyTorch backbone. + + Falls back to mock detections when no model is loaded, enabling + testing without pre-trained weights. + + Args: + model_path: Optional path to a serialised PyTorch model file. + device: Compute device (``"cpu"`` or ``"cuda"``). + confidence_threshold: Minimum confidence to retain a detection. + """ + + # Default label map used when no model is loaded + DEFAULT_LABELS = ["person", "chair", "table", "box", "robot", "door"] + + def __init__( + self, + model_path: Optional[str] = None, + device: str = "cpu", + confidence_threshold: float = 0.5, + ) -> None: + """Initialize the object detector.""" + self._logger = logging.getLogger("rag7.perception.detection") + self._device = device + self._confidence_threshold = confidence_threshold + self._model: Optional[Any] = None + + if model_path is not None: + self.load_model(model_path) + + # ------------------------------------------------------------------ + # Public API + # ------------------------------------------------------------------ + + def detect(self, image: np.ndarray) -> List[Detection]: + """Run object detection on a single image. + + Args: + image: RGB image as a numpy array of shape (H, W, C) + with dtype uint8 or float32. + + Returns: + List of :class:`Detection` namedtuples filtered by the + configured confidence threshold. + """ + if image is None or image.size == 0: + return [] + + if self._model is not None and TORCH_AVAILABLE: + try: + tensor = self._preprocess(image) + import torch + + with torch.no_grad(): + outputs = self._model(tensor) + return self._parse_outputs(outputs) + except Exception as exc: # noqa: BLE001 + self._logger.warning("Model inference failed: %s", exc) + + return self._mock_detections(image) + + def load_model(self, path: str) -> bool: + """Load a serialised PyTorch model from disk. + + Args: + path: Filesystem path to the model file. + + Returns: + True if the model was loaded successfully. + """ + if not TORCH_AVAILABLE: + self._logger.warning("PyTorch not available; cannot load model.") + return False + try: + import torch + + self._model = torch.load(path, map_location=self._device) + self._model.eval() + self._logger.info("Model loaded from '%s'.", path) + return True + except Exception as exc: # noqa: BLE001 + self._logger.warning("Could not load model from '%s': %s", path, exc) + return False + + # ------------------------------------------------------------------ + # Private helpers + # ------------------------------------------------------------------ + + def _preprocess(self, image: np.ndarray) -> Any: + """Convert a numpy image to a normalised model input tensor. + + Args: + image: Raw numpy image (H, W, C). + + Returns: + Preprocessed torch Tensor with a batch dimension. + """ + import torch + + img = image.astype(np.float32) / 255.0 + tensor = torch.from_numpy(img).permute(2, 0, 1).unsqueeze(0) + return tensor.to(self._device) + + def _parse_outputs(self, outputs: Any) -> List[Detection]: + """Parse raw model output tensors into Detection objects. + + Args: + outputs: Raw model output (format depends on model). + + Returns: + List of Detection namedtuples. + """ + # Placeholder; real parsing depends on the model architecture + return [] + + def _mock_detections(self, image: np.ndarray) -> List[Detection]: + """Generate deterministic mock detections based on image shape. + + Args: + image: Input image (used only for shape information). + + Returns: + List of mock Detection objects above the confidence threshold. + """ + h, w = image.shape[:2] + mock = [ + Detection( + class_id=0, + label="person", + confidence=0.92, + bbox=[int(w * 0.2), int(h * 0.1), int(w * 0.5), int(h * 0.9)], + ), + Detection( + class_id=3, + label="box", + confidence=0.78, + bbox=[int(w * 0.6), int(h * 0.5), int(w * 0.85), int(h * 0.8)], + ), + ] + return [d for d in mock if d.confidence >= self._confidence_threshold] diff --git a/perception/vision/segmentation.py b/perception/vision/segmentation.py new file mode 100644 index 0000000..72359a4 --- /dev/null +++ b/perception/vision/segmentation.py @@ -0,0 +1,129 @@ +""" +Image segmentation module for the RAG7 perception system. + +Provides semantic segmentation with a graceful mock fallback. +""" + +import logging +from typing import Any, Dict, List, Optional + +import numpy as np + +try: + import torch + + TORCH_AVAILABLE = True +except ImportError: + TORCH_AVAILABLE = False + + +class Segmenter: + """Semantic image segmenter using a PyTorch backbone. + + Returns per-pixel class masks along with class labels and confidence + scores. Falls back to a mock output when no model is loaded. + + Args: + model_path: Optional path to a serialised segmentation model. + device: Compute device (``"cpu"`` or ``"cuda"``). + """ + + DEFAULT_CLASSES = ["background", "floor", "wall", "obstacle", "robot", "human"] + + def __init__( + self, + model_path: Optional[str] = None, + device: str = "cpu", + ) -> None: + """Initialize the segmenter.""" + self._logger = logging.getLogger("rag7.perception.segmentation") + self._device = device + self._model: Optional[Any] = None + + if model_path is not None: + self._try_load_model(model_path) + + # ------------------------------------------------------------------ + # Public API + # ------------------------------------------------------------------ + + def segment(self, image: np.ndarray) -> Dict[str, Any]: + """Segment an image into semantic regions. + + Args: + image: RGB image as a numpy array of shape (H, W, C). + + Returns: + Dictionary with: + - ``masks``: numpy array of shape (num_classes, H, W). + - ``labels``: list of class label strings. + - ``scores``: list of confidence floats per class. + """ + if image is None or image.size == 0: + return {"masks": np.array([]), "labels": [], "scores": []} + + if self._model is not None and TORCH_AVAILABLE: + try: + return self._model_segment(image) + except Exception as exc: # noqa: BLE001 + self._logger.warning("Segmentation inference failed: %s", exc) + + return self._mock_segment(image) + + # ------------------------------------------------------------------ + # Private helpers + # ------------------------------------------------------------------ + + def _mock_segment(self, image: np.ndarray) -> Dict[str, Any]: + """Return mock segmentation output. + + Args: + image: Input image (used for shape only). + + Returns: + Mock segmentation dictionary. + """ + h, w = image.shape[:2] + num_classes = len(self.DEFAULT_CLASSES) + masks = np.zeros((num_classes, h, w), dtype=np.float32) + # Floor mask: bottom third of the image + masks[1, h * 2 // 3 :, :] = 1.0 + # Wall mask: top half + masks[2, : h // 2, :] = 1.0 + + return { + "masks": masks, + "labels": list(self.DEFAULT_CLASSES), + "scores": [0.95, 0.88, 0.75, 0.60, 0.50, 0.40], + } + + def _model_segment(self, image: np.ndarray) -> Dict[str, Any]: + """Run model-based segmentation. + + Args: + image: Input image. + + Returns: + Segmentation output dictionary. + """ + import torch + + img = image.astype(np.float32) / 255.0 + tensor = torch.from_numpy(img).permute(2, 0, 1).unsqueeze(0).to(self._device) + with torch.no_grad(): + output = self._model(tensor) + # Placeholder parse; real parsing is model-specific + return {"masks": np.array([]), "labels": [], "scores": []} + + def _try_load_model(self, path: str) -> None: + """Attempt to load a segmentation model from disk.""" + if not TORCH_AVAILABLE: + return + try: + import torch + + self._model = torch.load(path, map_location=self._device) + self._model.eval() + self._logger.info("Segmentation model loaded from '%s'.", path) + except Exception as exc: # noqa: BLE001 + self._logger.warning("Could not load segmentation model: %s", exc) diff --git a/perception/vision/tracking.py b/perception/vision/tracking.py new file mode 100644 index 0000000..cc65f30 --- /dev/null +++ b/perception/vision/tracking.py @@ -0,0 +1,178 @@ +""" +Multi-object tracking module for the RAG7 perception system. + +Implements IoU-based multi-object tracking using a simple +Hungarian-style greedy matching algorithm. +""" + +import logging +from typing import Any, Dict, List, Optional, Tuple + + +class Track: + """Represents a single tracked object across frames. + + Attributes: + track_id: Unique integer track identifier. + bbox: Bounding box [x1, y1, x2, y2]. + label: Class label string. + age: Number of frames this track has existed. + hits: Number of consecutive frames with a detection. + last_seen: Frame index of the last successful update. + """ + + _id_counter = 0 + + def __init__(self, bbox: List[float], label: str) -> None: + """Initialize a new track from an initial detection.""" + Track._id_counter += 1 + self.track_id = Track._id_counter + self.bbox = list(bbox) + self.label = label + self.age: int = 1 + self.hits: int = 1 + self.last_seen: int = 0 + + +class ObjectTracker: + """IoU-based multi-object tracker. + + Maintains a set of active tracks and matches incoming detections to + existing tracks using Intersection-over-Union (IoU) overlap. + + Args: + max_age: Maximum frames a track can persist without a detection. + min_hits: Minimum consecutive hits before a track is reported. + """ + + def __init__(self, max_age: int = 30, min_hits: int = 3) -> None: + """Initialize the object tracker.""" + self._logger = logging.getLogger("rag7.perception.tracking") + self._max_age = max_age + self._min_hits = min_hits + self._tracks: List[Track] = [] + self._frame_count: int = 0 + + # ------------------------------------------------------------------ + # Public API + # ------------------------------------------------------------------ + + def update(self, detections: List[Any]) -> List[Dict[str, Any]]: + """Update tracks with the latest set of detections. + + Args: + detections: List of detection objects or dicts, each with + ``bbox`` (list[float]) and ``label`` (str) attributes/keys. + + Returns: + List of active track dictionaries with ``id``, ``bbox``, + and ``label`` keys. + """ + self._frame_count += 1 + det_bboxes = [] + det_labels = [] + + for det in detections: + if hasattr(det, "bbox"): + det_bboxes.append(det.bbox) + det_labels.append(getattr(det, "label", "unknown")) + elif isinstance(det, dict): + det_bboxes.append(det.get("bbox", [0, 0, 1, 1])) + det_labels.append(det.get("label", "unknown")) + + # Greedy IoU matching + matched, unmatched_dets, unmatched_trks = self._match(det_bboxes) + + # Update matched tracks + for det_idx, trk_idx in matched: + self._tracks[trk_idx].bbox = det_bboxes[det_idx] + self._tracks[trk_idx].label = det_labels[det_idx] + self._tracks[trk_idx].hits += 1 + self._tracks[trk_idx].age = 0 + self._tracks[trk_idx].last_seen = self._frame_count + + # Create new tracks for unmatched detections + for det_idx in unmatched_dets: + self._tracks.append(Track(det_bboxes[det_idx], det_labels[det_idx])) + + # Age out unmatched existing tracks + for trk_idx in unmatched_trks: + self._tracks[trk_idx].age += 1 + + # Remove dead tracks + self._tracks = [t for t in self._tracks if t.age <= self._max_age] + + # Return confirmed tracks + return [ + {"id": t.track_id, "bbox": t.bbox, "label": t.label} + for t in self._tracks + if t.hits >= self._min_hits + ] + + # ------------------------------------------------------------------ + # Private helpers + # ------------------------------------------------------------------ + + def _match( + self, det_bboxes: List[List[float]] + ) -> Tuple[List[Tuple[int, int]], List[int], List[int]]: + """Greedily match detections to existing tracks using IoU. + + Args: + det_bboxes: List of detection bounding boxes. + + Returns: + Tuple of (matched pairs, unmatched det indices, + unmatched track indices). + """ + if not self._tracks or not det_bboxes: + return [], list(range(len(det_bboxes))), list(range(len(self._tracks))) + + iou_matrix = [ + [self._iou(det, trk.bbox) for trk in self._tracks] + for det in det_bboxes + ] + + matched: List[Tuple[int, int]] = [] + used_trks: set = set() + used_dets: set = set() + + # Greedy best-match assignment + for det_idx in range(len(det_bboxes)): + best_iou = 0.3 # IoU threshold + best_trk = -1 + for trk_idx in range(len(self._tracks)): + if trk_idx in used_trks: + continue + if iou_matrix[det_idx][trk_idx] > best_iou: + best_iou = iou_matrix[det_idx][trk_idx] + best_trk = trk_idx + if best_trk >= 0: + matched.append((det_idx, best_trk)) + used_dets.add(det_idx) + used_trks.add(best_trk) + + unmatched_dets = [i for i in range(len(det_bboxes)) if i not in used_dets] + unmatched_trks = [i for i in range(len(self._tracks)) if i not in used_trks] + return matched, unmatched_dets, unmatched_trks + + @staticmethod + def _iou(box_a: List[float], box_b: List[float]) -> float: + """Compute Intersection-over-Union between two bounding boxes. + + Args: + box_a: Bounding box [x1, y1, x2, y2]. + box_b: Bounding box [x1, y1, x2, y2]. + + Returns: + IoU score in the range [0, 1]. + """ + xa = max(box_a[0], box_b[0]) + ya = max(box_a[1], box_b[1]) + xb = min(box_a[2], box_b[2]) + yb = min(box_a[3], box_b[3]) + inter = max(0.0, xb - xa) * max(0.0, yb - ya) + area_a = (box_a[2] - box_a[0]) * (box_a[3] - box_a[1]) + area_b = (box_b[2] - box_b[0]) * (box_b[3] - box_b[1]) + union = area_a + area_b - inter + return inter / union if union > 0 else 0.0 diff --git a/planning/__init__.py b/planning/__init__.py new file mode 100644 index 0000000..1ba456f --- /dev/null +++ b/planning/__init__.py @@ -0,0 +1,10 @@ +""" +Planning package for the RAG7 AGI Robotics Framework. +""" + +from planning.task_planner import TaskPlanner +from planning.path_planner import PathPlanner +from planning.motion_planner import MotionPlanner +from planning.decision_maker import DecisionMaker + +__all__ = ["TaskPlanner", "PathPlanner", "MotionPlanner", "DecisionMaker"] diff --git a/planning/decision_maker.py b/planning/decision_maker.py new file mode 100644 index 0000000..15779df --- /dev/null +++ b/planning/decision_maker.py @@ -0,0 +1,136 @@ +""" +Decision maker module for the RAG7 planning system. + +Implements rule-based decision making with safety checks to select +the optimal action from a set of candidates. +""" + +import logging +from typing import Any, Dict, List, Optional + + +class DecisionMaker: + """Rule-based decision maker with safety awareness. + + Evaluates candidate actions against the current robot state and + selects the best action based on a simple scoring heuristic. + + Args: + config: Optional configuration dictionary. + """ + + def __init__(self, config: Optional[Dict[str, Any]] = None) -> None: + """Initialize the decision maker.""" + self._logger = logging.getLogger("rag7.planning.decision") + self._config = config or {} + self._safety_threshold = self._config.get("safety_threshold", 0.3) + + # ------------------------------------------------------------------ + # Public API + # ------------------------------------------------------------------ + + def decide( + self, + state: Dict[str, Any], + options: List[Any], + ) -> Optional[Any]: + """Select the best option given the current robot state. + + Safety checks are applied first: if an emergency stop is active + or an obstacle is closer than the safety threshold only a + ``stop`` or ``wait`` action is permitted. + + Args: + state: Current robot state dictionary. + options: List of candidate action dicts or strings. + + Returns: + The chosen option, or None if no valid option exists. + """ + if not options: + return None + + # Safety gate + if state.get("is_stopped"): + safe = [o for o in options if self._is_safe_action(o, emergency=True)] + if not safe: + return None + options = safe + + nearest_obstacle = state.get("nearest_obstacle", float("inf")) + if nearest_obstacle < self._safety_threshold: + safe = [o for o in options if self._is_safe_action(o, obstacle=True)] + options = safe if safe else options + + if not options: + return None + + # Score each option and return the highest-scoring one + scored = [(self.evaluate(state, opt), opt) for opt in options] + scored.sort(key=lambda t: t[0], reverse=True) + chosen = scored[0][1] + self._logger.debug("Decision: %s (score=%.3f)", chosen, scored[0][0]) + return chosen + + def evaluate(self, state: Dict[str, Any], action: Any) -> float: + """Compute a scalar value score for an action given a state. + + Higher scores indicate more desirable actions. + + Args: + state: Current robot state dictionary. + action: Action to evaluate (dict or string). + + Returns: + Float score in the range [0, 1]. + """ + score = 0.5 # Neutral baseline + + action_type = ( + action.get("type", "") if isinstance(action, dict) else str(action) + ) + + # Prefer stop/safety actions when obstacles are near + nearest = state.get("nearest_obstacle", float("inf")) + if nearest < self._safety_threshold: + if action_type in ("stop", "wait", "reverse"): + score += 0.4 + else: + score -= 0.3 + + # Prefer navigation when goal is known and path is clear + if state.get("goal_known") and nearest > 1.0: + if action_type == "navigate": + score += 0.3 + + # Penalise repeated actions + last_action = state.get("last_action", "") + if action_type and action_type == last_action: + score -= 0.1 + + return max(0.0, min(1.0, score)) + + # ------------------------------------------------------------------ + # Private helpers + # ------------------------------------------------------------------ + + @staticmethod + def _is_safe_action(action: Any, emergency: bool = False, obstacle: bool = False) -> bool: + """Check whether an action is safe to execute. + + Args: + action: Action dict or string. + emergency: If True, only allow stop/reset actions. + obstacle: If True, disallow navigation-type actions. + + Returns: + True if the action is considered safe. + """ + action_type = ( + action.get("type", "") if isinstance(action, dict) else str(action) + ) + if emergency: + return action_type in ("stop", "reset_stop", "wait") + if obstacle: + return action_type not in ("navigate", "grasp", "place") + return True diff --git a/planning/motion_planner.py b/planning/motion_planner.py new file mode 100644 index 0000000..b070066 --- /dev/null +++ b/planning/motion_planner.py @@ -0,0 +1,124 @@ +""" +Motion planner module for the RAG7 planning system. + +Provides trajectory planning and kinematic computations for +multi-DOF robot arms. +""" + +import logging +import math +from typing import Any, Dict, List, Optional + +import numpy as np + + +class MotionPlanner: + """Trajectory planner for robot arm motion. + + Computes smooth joint trajectories and simplified kinematic + solutions for a robot arm with configurable DOF. + + Args: + dof: Degrees of freedom of the robot arm (default 6). + """ + + def __init__(self, dof: int = 6) -> None: + """Initialize the motion planner.""" + self._logger = logging.getLogger("rag7.planning.motion") + self._dof = dof + self._link_lengths = [0.3] * dof # Default uniform link lengths + + # ------------------------------------------------------------------ + # Public API + # ------------------------------------------------------------------ + + def plan_trajectory( + self, + start_config: List[float], + goal_config: List[float], + duration: float = 1.0, + num_points: int = 50, + ) -> Dict[str, Any]: + """Plan a smooth joint-space trajectory. + + Uses cubic polynomial interpolation between start and goal + configurations. + + Args: + start_config: Starting joint angles in radians. + goal_config: Goal joint angles in radians. + duration: Total trajectory duration in seconds. + num_points: Number of trajectory waypoints. + + Returns: + Dictionary with: + - ``trajectory``: List of joint configuration lists. + - ``timestamps``: List of time values in seconds. + """ + start = np.asarray(start_config, dtype=float) + goal = np.asarray(goal_config, dtype=float) + times = np.linspace(0.0, duration, num_points) + + # Cubic polynomial: q(t) = start + (goal-start) * (3τ² - 2τ³) + # where τ = t / duration + tau = times / duration + blend = 3 * tau**2 - 2 * tau**3 # shape (num_points,) + trajectory = [ + (start + (goal - start) * b).tolist() for b in blend + ] + + return {"trajectory": trajectory, "timestamps": times.tolist()} + + def compute_ik(self, pose: Dict[str, float]) -> List[float]: + """Compute simplified inverse kinematics for a target pose. + + This is a first-order approximation suitable for testing; a + production system would use a full analytical or numerical IK + solver. + + Args: + pose: Target end-effector pose with ``x``, ``y``, ``z`` + keys (theta keys optional). + + Returns: + List of joint angles in radians. + """ + x = pose.get("x", 0.0) + y = pose.get("y", 0.0) + z = pose.get("z", 0.0) + + # Simplified: distribute the reach across joints uniformly + total_reach = math.sqrt(x**2 + y**2 + z**2) + reach_per_link = total_reach / max(self._dof, 1) + target_angle = math.atan2(y, x) + + angles = [] + for i in range(self._dof): + # Crude alternating-sign approximation + angles.append(target_angle * ((-1) ** i) * (reach_per_link / 0.3)) + + self._logger.debug("IK angles (simplified): %s", angles) + return angles + + def compute_fk(self, joint_angles: List[float]) -> Dict[str, float]: + """Compute forward kinematics for a given joint configuration. + + Uses a simplified planar chain model (sum of link projections). + + Args: + joint_angles: List of joint angles in radians. + + Returns: + End-effector pose dict with ``x``, ``y``, ``z`` keys. + """ + x, y, z = 0.0, 0.0, 0.0 + cumulative_angle = 0.0 + + for i, angle in enumerate(joint_angles): + length = self._link_lengths[i] if i < len(self._link_lengths) else 0.3 + cumulative_angle += angle + x += length * math.cos(cumulative_angle) + y += length * math.sin(cumulative_angle) + + self._logger.debug("FK result: (%.3f, %.3f, %.3f)", x, y, z) + return {"x": x, "y": y, "z": z} diff --git a/planning/path_planner.py b/planning/path_planner.py new file mode 100644 index 0000000..7cb0636 --- /dev/null +++ b/planning/path_planner.py @@ -0,0 +1,180 @@ +""" +Path planner module for the RAG7 planning system. + +Implements A* search on a 2-D occupancy grid to compute +collision-free paths between two poses. +""" + +import heapq +import logging +from typing import Dict, List, Optional, Set, Tuple + + +class PathPlanner: + """Grid-based path planner using the A* algorithm. + + Operates on a discrete occupancy grid. Obstacle cells are treated + as impassable; all other cells have unit traversal cost. + + Args: + grid_resolution: Physical size of each grid cell in metres. + """ + + def __init__(self, grid_resolution: float = 0.1) -> None: + """Initialize the path planner.""" + self._logger = logging.getLogger("rag7.planning.path") + self._resolution = grid_resolution + + # ------------------------------------------------------------------ + # Public API + # ------------------------------------------------------------------ + + def plan( + self, + start: Tuple[float, float], + goal: Tuple[float, float], + obstacles: Optional[List[Tuple[float, float]]] = None, + ) -> List[Tuple[float, float]]: + """Compute a collision-free path from start to goal using A*. + + Args: + start: (x, y) start position in world coordinates. + goal: (x, y) goal position in world coordinates. + obstacles: Optional list of (x, y) obstacle positions in + world coordinates. + + Returns: + List of (x, y) waypoint tuples from start to goal, or an + empty list if no path is found. + """ + obstacles = obstacles or [] + obstacle_set: Set[Tuple[int, int]] = { + self._to_grid(ox, oy) for ox, oy in obstacles + } + start_cell = self._to_grid(*start) + goal_cell = self._to_grid(*goal) + + path_cells = self._astar(start_cell, goal_cell, obstacle_set) + + if path_cells is None: + self._logger.warning("No path found from %s to %s.", start, goal) + return [] + + return [self._to_world(r, c) for r, c in path_cells] + + # ------------------------------------------------------------------ + # Private A* implementation + # ------------------------------------------------------------------ + + def _astar( + self, + start: Tuple[int, int], + goal: Tuple[int, int], + obstacles: Set[Tuple[int, int]], + ) -> Optional[List[Tuple[int, int]]]: + """Run A* search on the grid. + + Args: + start: Grid (row, col) start cell. + goal: Grid (row, col) goal cell. + obstacles: Set of impassable grid cells. + + Returns: + List of grid cells forming the path, or None if unreachable. + """ + open_heap: List[Tuple[float, Tuple[int, int]]] = [] + heapq.heappush(open_heap, (0.0, start)) + came_from: Dict[Tuple[int, int], Optional[Tuple[int, int]]] = {start: None} + g_score: Dict[Tuple[int, int], float] = {start: 0.0} + + while open_heap: + _, current = heapq.heappop(open_heap) + + if current == goal: + return self._reconstruct(came_from, goal) + + for neighbour in self._neighbours(current): + if neighbour in obstacles: + continue + tentative_g = g_score[current] + 1.0 + if tentative_g < g_score.get(neighbour, float("inf")): + came_from[neighbour] = current + g_score[neighbour] = tentative_g + f = tentative_g + self._heuristic(neighbour, goal) + heapq.heappush(open_heap, (f, neighbour)) + + return None # No path found + + @staticmethod + def _heuristic( + a: Tuple[int, int], b: Tuple[int, int] + ) -> float: + """Manhattan distance heuristic for A*. + + Args: + a: First grid cell (row, col). + b: Second grid cell (row, col). + + Returns: + Manhattan distance between a and b. + """ + return float(abs(a[0] - b[0]) + abs(a[1] - b[1])) + + @staticmethod + def _neighbours(cell: Tuple[int, int]) -> List[Tuple[int, int]]: + """Return 4-connected grid neighbours of a cell. + + Args: + cell: (row, col) grid cell. + + Returns: + List of adjacent grid cells. + """ + r, c = cell + return [(r - 1, c), (r + 1, c), (r, c - 1), (r, c + 1)] + + @staticmethod + def _reconstruct( + came_from: Dict[Tuple[int, int], Optional[Tuple[int, int]]], + goal: Tuple[int, int], + ) -> List[Tuple[int, int]]: + """Reconstruct the path from the came_from map. + + Args: + came_from: Mapping from cell to its predecessor. + goal: Goal cell. + + Returns: + Ordered list of cells from start to goal. + """ + path = [] + current: Optional[Tuple[int, int]] = goal + while current is not None: + path.append(current) + current = came_from[current] + path.reverse() + return path + + def _to_grid(self, x: float, y: float) -> Tuple[int, int]: + """Convert world coordinates to grid cell indices. + + Args: + x: World X coordinate. + y: World Y coordinate. + + Returns: + (row, col) grid cell. + """ + return (int(round(y / self._resolution)), int(round(x / self._resolution))) + + def _to_world(self, row: int, col: int) -> Tuple[float, float]: + """Convert grid cell indices to world coordinates. + + Args: + row: Grid row index. + col: Grid column index. + + Returns: + (x, y) world coordinates. + """ + return (col * self._resolution, row * self._resolution) diff --git a/planning/task_planner.py b/planning/task_planner.py new file mode 100644 index 0000000..94ddddd --- /dev/null +++ b/planning/task_planner.py @@ -0,0 +1,186 @@ +""" +Task planner module for the RAG7 planning system. + +Decomposes high-level goals into ordered sequences of primitive actions, +using an LLM when available and falling back to rule-based planning. +""" + +import logging +from typing import Any, Dict, List, Optional + +try: + from langchain_openai import ChatOpenAI # noqa: F401 + + LANGCHAIN_AVAILABLE = True +except ImportError: + LANGCHAIN_AVAILABLE = False + + +class TaskPlanner: + """High-level task planner that decomposes goals into sub-tasks. + + When an LLM is configured and reachable it uses chain-of-thought + prompting to create plans; otherwise a deterministic rule-based + planner is used. + + Args: + llm_config: Optional LLM configuration dictionary. + """ + + def __init__(self, llm_config: Optional[Dict[str, Any]] = None) -> None: + """Initialize the task planner.""" + self._logger = logging.getLogger("rag7.planning.task") + self._llm_config = llm_config or {} + self._llm = None + + if LANGCHAIN_AVAILABLE and self._llm_config.get("provider") == "openai": + self._try_init_llm() + + # ------------------------------------------------------------------ + # Public API + # ------------------------------------------------------------------ + + def plan( + self, goal: str, context: Optional[Dict[str, Any]] = None + ) -> List[Dict[str, Any]]: + """Create a task plan to achieve the specified goal. + + Args: + goal: High-level goal description string. + context: Optional world-state context dictionary. + + Returns: + Ordered list of step dictionaries, each containing + ``step_id``, ``action``, and ``description`` keys. + """ + context = context or {} + if self._llm is not None: + try: + return self._llm_plan(goal, context) + except Exception as exc: # noqa: BLE001 + self._logger.warning("LLM planning failed: %s; using rule-based.", exc) + + return self.decompose(goal) + + def decompose(self, task: str) -> List[Dict[str, Any]]: + """Decompose a task string into primitive sub-tasks. + + Args: + task: Task or goal description. + + Returns: + List of step dictionaries. + """ + task_lower = task.lower() + + if any(kw in task_lower for kw in ["navigate", "go", "move", "drive"]): + return [ + {"step_id": 1, "action": "scan_environment", "description": "Scan for obstacles"}, + {"step_id": 2, "action": "compute_path", "description": "Plan collision-free path"}, + {"step_id": 3, "action": "navigate", "description": "Execute navigation"}, + {"step_id": 4, "action": "confirm_arrival", "description": "Verify goal reached"}, + ] + + if any(kw in task_lower for kw in ["grasp", "pick", "grab", "take"]): + return [ + {"step_id": 1, "action": "detect_object", "description": "Detect target object"}, + {"step_id": 2, "action": "plan_approach", "description": "Plan approach trajectory"}, + {"step_id": 3, "action": "execute_grasp", "description": "Perform grasp"}, + {"step_id": 4, "action": "verify_grasp", "description": "Check grasp success"}, + ] + + if any(kw in task_lower for kw in ["place", "put", "deposit"]): + return [ + {"step_id": 1, "action": "find_placement", "description": "Find placement location"}, + {"step_id": 2, "action": "navigate_to_placement", "description": "Move to placement"}, + {"step_id": 3, "action": "lower_object", "description": "Lower arm with object"}, + {"step_id": 4, "action": "release", "description": "Open gripper"}, + ] + + if any(kw in task_lower for kw in ["inspect", "check", "examine"]): + return [ + {"step_id": 1, "action": "approach_target", "description": "Move near target"}, + {"step_id": 2, "action": "capture_images", "description": "Take inspection images"}, + {"step_id": 3, "action": "analyse", "description": "Analyse inspection data"}, + {"step_id": 4, "action": "report", "description": "Generate inspection report"}, + ] + + # Generic fallback + return [ + {"step_id": 1, "action": "assess", "description": f"Assess: {task}"}, + {"step_id": 2, "action": "plan", "description": "Create execution plan"}, + {"step_id": 3, "action": "execute", "description": "Execute plan"}, + {"step_id": 4, "action": "verify", "description": "Verify outcome"}, + ] + + def replan( + self, + failed_step: Dict[str, Any], + context: Optional[Dict[str, Any]] = None, + ) -> List[Dict[str, Any]]: + """Generate a recovery plan after a step failure. + + Args: + failed_step: The step dictionary that failed. + context: Updated world-state context. + + Returns: + Recovery plan as a list of step dictionaries. + """ + context = context or {} + reason = context.get("failure_reason", "unknown") + self._logger.warning( + "Replanning after failed step '%s' (reason: %s).", + failed_step.get("action"), + reason, + ) + recovery = [ + {"step_id": 1, "action": "diagnose", "description": f"Diagnose failure: {reason}"}, + {"step_id": 2, "action": "recover", "description": "Execute recovery action"}, + ] + # Append original remaining steps after recovery + remaining = context.get("remaining_steps", []) + for i, step in enumerate(remaining, start=3): + step = dict(step) + step["step_id"] = i + recovery.append(step) + return recovery + + # ------------------------------------------------------------------ + # Private helpers + # ------------------------------------------------------------------ + + def _llm_plan( + self, goal: str, context: Dict[str, Any] + ) -> List[Dict[str, Any]]: + """Create a plan using the LLM backend.""" + prompt = ( + f"Robot task: {goal}\nContext: {context}\n" + "Provide a numbered step-by-step plan for the robot." + ) + response = self._llm.invoke(prompt) + lines = [l.strip() for l in response.content.split("\n") if l.strip()] + return [ + {"step_id": i, "action": "llm_step", "description": line} + for i, line in enumerate(lines, 1) + ] + + def _try_init_llm(self) -> None: + """Attempt to initialise the LangChain LLM client.""" + try: + import os + + from langchain_openai import ChatOpenAI + + api_key = os.environ.get( + self._llm_config.get("api_key_env", "OPENAI_API_KEY"), "" + ) + if not api_key: + return + self._llm = ChatOpenAI( + model=self._llm_config.get("model", "gpt-4"), + temperature=self._llm_config.get("temperature", 0.1), + openai_api_key=api_key, + ) + except Exception as exc: # noqa: BLE001 + self._logger.warning("TaskPlanner LLM init failed: %s", exc) diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..f155b85 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,18 @@ +torch>=2.0.0 +torchvision>=0.15.0 +numpy>=1.24.0 +scipy>=1.10.0 +opencv-python>=4.7.0 +langchain>=0.1.0 +langchain-openai>=0.0.5 +langchain-community>=0.0.20 +openai>=1.0.0 +transformers>=4.30.0 +pyyaml>=6.0 +rclpy # ROS2 Python client +geometry_msgs +sensor_msgs +nav_msgs +action_msgs +pytest>=7.0.0 +pytest-cov>=4.0.0 diff --git a/ros2_interface/__init__.py b/ros2_interface/__init__.py new file mode 100644 index 0000000..ac27938 --- /dev/null +++ b/ros2_interface/__init__.py @@ -0,0 +1,17 @@ +""" +ROS2 interface package for the RAG7 AGI Robotics Framework. + +Provides ROS2 node wrappers and the ROS2Interface communication layer. +""" + +from ros2_interface.ros2_interface import ROS2Interface +from ros2_interface.ros2_nodes.perception_node import PerceptionNode +from ros2_interface.ros2_nodes.planning_node import PlanningNode +from ros2_interface.ros2_nodes.control_node import ControlNode + +__all__ = [ + "ROS2Interface", + "PerceptionNode", + "PlanningNode", + "ControlNode", +] diff --git a/ros2_interface/launch/agi_system.launch.py b/ros2_interface/launch/agi_system.launch.py new file mode 100644 index 0000000..f3dcfc8 --- /dev/null +++ b/ros2_interface/launch/agi_system.launch.py @@ -0,0 +1,44 @@ +""" +ROS2 launch file for the RAG7 AGI system. + +Launches the perception, planning, and control nodes together. +""" + +try: + from launch import LaunchDescription + from launch_ros.actions import Node + + def generate_launch_description() -> LaunchDescription: + """Generate the ROS2 launch description for the RAG7 AGI system. + + Returns: + LaunchDescription with all three AGI nodes. + """ + perception_node = Node( + package="rag7_agi", + executable="perception_node", + name="perception_node", + output="screen", + ) + + planning_node = Node( + package="rag7_agi", + executable="planning_node", + name="planning_node", + output="screen", + ) + + control_node = Node( + package="rag7_agi", + executable="control_node", + name="control_node", + output="screen", + ) + + return LaunchDescription([perception_node, planning_node, control_node]) + +except ImportError: + # Provide a stub when launch packages are not installed + def generate_launch_description(): # type: ignore[misc] + """Stub launch description (ROS2 launch not available).""" + return None diff --git a/ros2_interface/msg/.gitkeep b/ros2_interface/msg/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/ros2_interface/ros2_interface.py b/ros2_interface/ros2_interface.py new file mode 100644 index 0000000..d0c5205 --- /dev/null +++ b/ros2_interface/ros2_interface.py @@ -0,0 +1,128 @@ +""" +ROS2 interface module for the RAG7 AGI Robotics Framework. + +Provides a thin wrapper around rclpy for publish/subscribe and +service-call operations. +""" + +import logging +from typing import Any, Callable, Dict, Optional + +try: + import rclpy + from rclpy.node import Node + + ROS2_AVAILABLE = True +except ImportError: + ROS2_AVAILABLE = False + + +class ROS2Interface: + """Thin abstraction layer over ROS2 communication primitives. + + Wraps rclpy publish, subscribe, and service-call operations. + Falls back to a mock implementation when rclpy is not installed. + + Args: + node_name: Name for the underlying ROS2 node. + """ + + def __init__(self, node_name: str = "rag7_interface") -> None: + """Initialize the ROS2 interface.""" + self._logger = logging.getLogger("rag7.ros2_interface") + self._node_name = node_name + self._node: Optional[Any] = None + self._publishers: Dict[str, Any] = {} + self._subscribers: Dict[str, Any] = {} + + if ROS2_AVAILABLE: + self._init_ros2() + + # ------------------------------------------------------------------ + # Public API + # ------------------------------------------------------------------ + + def publish(self, topic: str, message: Any) -> bool: + """Publish a message to a ROS2 topic. + + Args: + topic: Topic name string. + message: Message object to publish. + + Returns: + True if the message was published successfully. + """ + if not ROS2_AVAILABLE or self._node is None: + self._logger.debug("Mock publish to '%s': %s", topic, message) + return True + try: + if topic in self._publishers: + self._publishers[topic].publish(message) + return True + except Exception as exc: # noqa: BLE001 + self._logger.error("Publish failed on '%s': %s", topic, exc) + return False + + def subscribe(self, topic: str, callback: Callable[[Any], None]) -> bool: + """Subscribe to a ROS2 topic. + + Args: + topic: Topic name string. + callback: Function to call when a message is received. + + Returns: + True if the subscription was set up successfully. + """ + if not ROS2_AVAILABLE or self._node is None: + self._logger.debug("Mock subscribe to '%s'.", topic) + self._subscribers[topic] = callback + return True + try: + # Real subscription would require a message type; stored as stub + self._subscribers[topic] = callback + return True + except Exception as exc: # noqa: BLE001 + self._logger.error("Subscribe failed on '%s': %s", topic, exc) + return False + + def call_service(self, service: str, request: Any) -> Optional[Any]: + """Call a ROS2 service. + + Args: + service: Service name string. + request: Service request object. + + Returns: + Service response, or None on failure. + """ + if not ROS2_AVAILABLE or self._node is None: + self._logger.debug("Mock service call to '%s'.", service) + return {"status": "mock_response"} + try: + # Stub; real implementation would use rclpy service clients + return None + except Exception as exc: # noqa: BLE001 + self._logger.error("Service call failed on '%s': %s", service, exc) + return None + + def is_available(self) -> bool: + """Check whether the ROS2 interface is available. + + Returns: + True if rclpy is installed and the node is running. + """ + return ROS2_AVAILABLE and self._node is not None + + # ------------------------------------------------------------------ + # Private helpers + # ------------------------------------------------------------------ + + def _init_ros2(self) -> None: + """Attempt to initialise the underlying rclpy node.""" + try: + if not rclpy.ok(): + rclpy.init() + self._node = rclpy.create_node(self._node_name) + self._logger.info("ROS2 node '%s' created.", self._node_name) + except Exception as exc: # noqa: BLE001 + self._logger.warning("ROS2 initialisation failed: %s", exc) diff --git a/ros2_interface/ros2_nodes/__init__.py b/ros2_interface/ros2_nodes/__init__.py new file mode 100644 index 0000000..057fbb2 --- /dev/null +++ b/ros2_interface/ros2_nodes/__init__.py @@ -0,0 +1,9 @@ +""" +ROS2 nodes package for the RAG7 AGI Robotics Framework. +""" + +from ros2_interface.ros2_nodes.perception_node import PerceptionNode +from ros2_interface.ros2_nodes.planning_node import PlanningNode +from ros2_interface.ros2_nodes.control_node import ControlNode + +__all__ = ["PerceptionNode", "PlanningNode", "ControlNode"] diff --git a/ros2_interface/ros2_nodes/control_node.py b/ros2_interface/ros2_nodes/control_node.py new file mode 100644 index 0000000..00a06b4 --- /dev/null +++ b/ros2_interface/ros2_nodes/control_node.py @@ -0,0 +1,60 @@ +""" +ROS2 ControlNode for the RAG7 AGI Robotics Framework. + +Wraps the ControlAgent with ROS2 topic wiring. +Degrades gracefully when rclpy is not installed. +""" + +import logging +from typing import Any, Dict + +from agents.control_agent import ControlAgent + +try: + import rclpy + + ROS2_AVAILABLE = True +except ImportError: + ROS2_AVAILABLE = False + + +class ControlNode(ControlAgent): + """ROS2 node that exposes the ControlAgent over ROS2 topics. + + Args: + config: Control agent configuration dictionary. + """ + + def __init__(self, config: Dict[str, Any]) -> None: + """Initialize the control node.""" + super().__init__(config=config, ros2_enabled=ROS2_AVAILABLE) + self._ros_node: Any = None + self._publishers: Dict[str, Any] = {} + self._subscribers: Dict[str, Any] = {} + self._logger = logging.getLogger("rag7.ros2.control") + self._setup_node() + + def spin(self) -> None: + """Run the ROS2 node event loop (no-op in mock mode).""" + if not ROS2_AVAILABLE or self._ros_node is None: + self._logger.info("ControlNode spin (mock – no ROS2).") + return + try: + rclpy.spin(self._ros_node) + except KeyboardInterrupt: + pass + finally: + self._ros_node.destroy_node() + + def _setup_node(self) -> None: + """Create the ROS2 node and wire up topics.""" + if not ROS2_AVAILABLE: + self._logger.info("rclpy not available; running in mock mode.") + return + try: + if not rclpy.ok(): + rclpy.init() + self._ros_node = rclpy.create_node("control_node") + self._logger.info("ControlNode ROS2 node created.") + except Exception as exc: # noqa: BLE001 + self._logger.warning("Failed to create ROS2 control node: %s", exc) diff --git a/ros2_interface/ros2_nodes/perception_node.py b/ros2_interface/ros2_nodes/perception_node.py new file mode 100644 index 0000000..5797a9e --- /dev/null +++ b/ros2_interface/ros2_nodes/perception_node.py @@ -0,0 +1,83 @@ +""" +ROS2 PerceptionNode for the RAG7 AGI Robotics Framework. + +Wraps the PerceptionAgent with ROS2 subscriber/publisher wiring. +Degrades gracefully when rclpy is not installed. +""" + +import logging +from typing import Any, Dict + +from agents.perception_agent import PerceptionAgent + +try: + import rclpy + from rclpy.node import Node + + ROS2_AVAILABLE = True +except ImportError: + ROS2_AVAILABLE = False + + +class PerceptionNode(PerceptionAgent): + """ROS2 node that exposes the PerceptionAgent over ROS2 topics. + + Subscribes to camera, LiDAR, and IMU topics and publishes + processed perception outputs. + + Args: + config: Perception agent configuration dictionary. + device: Compute device for inference. + """ + + def __init__( + self, + config: Dict[str, Any], + device: str = "cpu", + ) -> None: + """Initialize the perception node.""" + super().__init__(config=config, device=device) + self._ros_node: Any = None + self._publishers: Dict[str, Any] = {} + self._subscribers: Dict[str, Any] = {} + self._logger = logging.getLogger("rag7.ros2.perception") + + self._setup_node() + + # ------------------------------------------------------------------ + # Node lifecycle + # ------------------------------------------------------------------ + + def spin(self) -> None: + """Run the ROS2 node event loop. + + Blocks until the node is shut down. In non-ROS mode, this is + a no-op. + """ + if not ROS2_AVAILABLE or self._ros_node is None: + self._logger.info("PerceptionNode spin (mock – no ROS2).") + return + try: + rclpy.spin(self._ros_node) + except KeyboardInterrupt: + pass + finally: + self._ros_node.destroy_node() + + # ------------------------------------------------------------------ + # Private helpers + # ------------------------------------------------------------------ + + def _setup_node(self) -> None: + """Create the ROS2 node and wire up topics.""" + if not ROS2_AVAILABLE: + self._logger.info("rclpy not available; running in mock mode.") + return + try: + if not rclpy.ok(): + rclpy.init() + self._ros_node = rclpy.create_node("perception_node") + # Real wiring would add typed subscribers/publishers here + self._logger.info("PerceptionNode ROS2 node created.") + except Exception as exc: # noqa: BLE001 + self._logger.warning("Failed to create ROS2 perception node: %s", exc) diff --git a/ros2_interface/ros2_nodes/planning_node.py b/ros2_interface/ros2_nodes/planning_node.py new file mode 100644 index 0000000..336b7e9 --- /dev/null +++ b/ros2_interface/ros2_nodes/planning_node.py @@ -0,0 +1,65 @@ +""" +ROS2 PlanningNode for the RAG7 AGI Robotics Framework. + +Wraps the PlanningAgent with ROS2 topic wiring. +Degrades gracefully when rclpy is not installed. +""" + +import logging +from typing import Any, Dict, Optional + +from agents.planning_agent import PlanningAgent + +try: + import rclpy + + ROS2_AVAILABLE = True +except ImportError: + ROS2_AVAILABLE = False + + +class PlanningNode(PlanningAgent): + """ROS2 node that exposes the PlanningAgent over ROS2 topics. + + Args: + config: Planning agent configuration dictionary. + llm_config: Optional LLM configuration dictionary. + """ + + def __init__( + self, + config: Dict[str, Any], + llm_config: Optional[Dict[str, Any]] = None, + ) -> None: + """Initialize the planning node.""" + super().__init__(config=config, llm_config=llm_config) + self._ros_node: Any = None + self._publishers: Dict[str, Any] = {} + self._subscribers: Dict[str, Any] = {} + self._logger = logging.getLogger("rag7.ros2.planning") + self._setup_node() + + def spin(self) -> None: + """Run the ROS2 node event loop (no-op in mock mode).""" + if not ROS2_AVAILABLE or self._ros_node is None: + self._logger.info("PlanningNode spin (mock – no ROS2).") + return + try: + rclpy.spin(self._ros_node) + except KeyboardInterrupt: + pass + finally: + self._ros_node.destroy_node() + + def _setup_node(self) -> None: + """Create the ROS2 node and wire up topics.""" + if not ROS2_AVAILABLE: + self._logger.info("rclpy not available; running in mock mode.") + return + try: + if not rclpy.ok(): + rclpy.init() + self._ros_node = rclpy.create_node("planning_node") + self._logger.info("PlanningNode ROS2 node created.") + except Exception as exc: # noqa: BLE001 + self._logger.warning("Failed to create ROS2 planning node: %s", exc) diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..0ef6400 --- /dev/null +++ b/setup.py @@ -0,0 +1,40 @@ +"""Setup script for the rag7_agi package.""" + +from setuptools import setup, find_packages + +setup( + name="rag7_agi", + version="0.1.0", + author="Stacey Williams", + description="Agentic AGI Robotics Framework", + long_description=open("README.md").read(), + long_description_content_type="text/markdown", + packages=find_packages(), + python_requires=">=3.10", + install_requires=[ + "torch>=2.0.0", + "torchvision>=0.15.0", + "numpy>=1.24.0", + "scipy>=1.10.0", + "opencv-python>=4.7.0", + "pyyaml>=6.0", + ], + extras_require={ + "llm": [ + "langchain>=0.1.0", + "langchain-openai>=0.0.5", + "langchain-community>=0.0.20", + "openai>=1.0.0", + "transformers>=4.30.0", + ], + "dev": [ + "pytest>=7.0.0", + "pytest-cov>=4.0.0", + ], + }, + classifiers=[ + "Programming Language :: Python :: 3", + "License :: OSI Approved :: MIT License", + "Operating System :: OS Independent", + ], +) diff --git a/simulation/__init__.py b/simulation/__init__.py new file mode 100644 index 0000000..131fe25 --- /dev/null +++ b/simulation/__init__.py @@ -0,0 +1,7 @@ +""" +Simulation package for the RAG7 AGI Robotics Framework. +""" + +from simulation.gazebo_env import GazeboEnv + +__all__ = ["GazeboEnv"] diff --git a/simulation/gazebo_env.py b/simulation/gazebo_env.py new file mode 100644 index 0000000..ac7b51a --- /dev/null +++ b/simulation/gazebo_env.py @@ -0,0 +1,191 @@ +""" +Gazebo simulation environment for the RAG7 AGI Robotics Framework. + +Provides an OpenAI Gym-compatible interface for simulated robot +environments. When Gazebo is not installed, the environment runs in +pure mock mode enabling development and testing without a simulator. +""" + +import logging +import random +from typing import Any, Dict, Optional, Tuple + +import numpy as np + + +class GazeboEnv: + """Gazebo-backed (or mock) robot simulation environment. + + Implements a Gym-style interface (reset / step / render / close) for + testing robot policies in simulation. + + Args: + world_name: Gazebo world file name (without path or extension). + robot_name: Name of the robot model in the simulation. + headless: If True, run without a graphical display. + """ + + def __init__( + self, + world_name: str = "empty", + robot_name: str = "robot", + headless: bool = True, + ) -> None: + """Initialize the Gazebo environment.""" + self._logger = logging.getLogger("rag7.simulation.gazebo") + self._world_name = world_name + self._robot_name = robot_name + self._headless = headless + self._is_running = False + self._step_count = 0 + self._max_steps = 1000 + + # Internal mock robot state + self._robot_state: Dict[str, Any] = { + "position": {"x": 0.0, "y": 0.0, "z": 0.0}, + "orientation": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "velocity": {"linear": 0.0, "angular": 0.0}, + } + self._goal: Dict[str, float] = {"x": 5.0, "y": 5.0} + self._logger.info( + "GazeboEnv initialised (world='%s', headless=%s).", + world_name, + headless, + ) + + # ------------------------------------------------------------------ + # Gym-style interface + # ------------------------------------------------------------------ + + def reset(self) -> Dict[str, Any]: + """Reset the environment to its initial state. + + Returns: + Initial observation dictionary. + """ + self._step_count = 0 + self._is_running = True + self._robot_state = { + "position": {"x": 0.0, "y": 0.0, "z": 0.0}, + "orientation": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "velocity": {"linear": 0.0, "angular": 0.0}, + } + self._logger.debug("Environment reset.") + return self._get_observation() + + def step( + self, action: Dict[str, Any] + ) -> Tuple[Dict[str, Any], float, bool, Dict[str, Any]]: + """Execute one simulation step. + + Args: + action: Action dictionary with optional ``linear_velocity`` + and ``angular_velocity`` keys. + + Returns: + Tuple of (observation, reward, done, info). + """ + if not self._is_running: + self._logger.warning("step() called before reset().") + return self._get_observation(), 0.0, True, {} + + self._step_count += 1 + self._apply_action(action) + obs = self._get_observation() + reward = self._compute_reward(self._robot_state, action) + done = self._is_terminal() + info = {"step": self._step_count, "world": self._world_name} + return obs, reward, done, info + + def render(self) -> Optional[np.ndarray]: + """Render the environment. + + Returns: + Mock RGB image array if headless, else None. + """ + if self._headless: + return np.zeros((480, 640, 3), dtype=np.uint8) + # In a real implementation this would trigger a Gazebo camera capture + return None + + def close(self) -> None: + """Shut down the environment.""" + self._is_running = False + self._logger.info("GazeboEnv closed.") + + # ------------------------------------------------------------------ + # Private helpers + # ------------------------------------------------------------------ + + def _get_observation(self) -> Dict[str, Any]: + """Build an observation dictionary from the current robot state. + + Returns: + Observation dict with camera image, LiDAR, and state info. + """ + # Mock camera image + camera = np.zeros((480, 640, 3), dtype=np.uint8) + # Mock LiDAR: 360 rays, all beyond range + lidar = np.full(360, 5.0, dtype=np.float32) + lidar[0] = random.uniform(0.5, 3.0) # Introduce one obstacle + + return { + "camera": camera, + "lidar": lidar, + "state": dict(self._robot_state), + "goal": dict(self._goal), + } + + def _compute_reward( + self, state: Dict[str, Any], action: Dict[str, Any] + ) -> float: + """Compute the reward signal for the current transition. + + Args: + state: Current robot state. + action: Action taken. + + Returns: + Scalar reward value. + """ + pos = state["position"] + dx = self._goal["x"] - pos["x"] + dy = self._goal["y"] - pos["y"] + distance = (dx**2 + dy**2) ** 0.5 + reward = -0.01 * distance # Dense distance reward + + if distance < 0.5: + reward += 10.0 # Goal bonus + + return float(reward) + + def _apply_action(self, action: Dict[str, Any]) -> None: + """Apply an action to update the mock robot state. + + Args: + action: Action dictionary. + """ + v = float(action.get("linear_velocity", 0.0)) + w = float(action.get("angular_velocity", 0.0)) + yaw = self._robot_state["orientation"]["yaw"] + + import math + + dt = 0.1 # 10 Hz simulation step + self._robot_state["position"]["x"] += v * math.cos(yaw) * dt + self._robot_state["position"]["y"] += v * math.sin(yaw) * dt + self._robot_state["orientation"]["yaw"] += w * dt + self._robot_state["velocity"] = {"linear": v, "angular": w} + + def _is_terminal(self) -> bool: + """Determine whether the episode has ended. + + Returns: + True if the goal is reached or the step limit is exceeded. + """ + pos = self._robot_state["position"] + dx = self._goal["x"] - pos["x"] + dy = self._goal["y"] - pos["y"] + if (dx**2 + dy**2) ** 0.5 < 0.5: + return True + return self._step_count >= self._max_steps diff --git a/simulation/worlds/.gitkeep b/simulation/worlds/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/tests/__init__.py b/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tests/test_agents.py b/tests/test_agents.py new file mode 100644 index 0000000..75ba340 --- /dev/null +++ b/tests/test_agents.py @@ -0,0 +1,371 @@ +""" +Unit tests for the agents module of the RAG7 AGI Robotics Framework. +""" + +import sys +import os + +# Ensure the project root is on the path +sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + +import numpy as np +import pytest + +from agents.base_agent import AgentState, BaseAgent +from agents.communication_agent import CommunicationAgent +from agents.control_agent import ControlAgent +from agents.coordination_agent import CoordinationAgent +from agents.perception_agent import PerceptionAgent +from agents.planning_agent import PlanningAgent +from agents.robotics_agi import RoboticsAGI + + +# --------------------------------------------------------------------------- +# Concrete minimal subclass used to test BaseAgent in isolation +# --------------------------------------------------------------------------- + + +class _ConcreteAgent(BaseAgent): + """Minimal concrete BaseAgent subclass for testing.""" + + def perceive(self, observation): + return observation + + def reason(self, context): + return {"context": context} + + def act(self, action): + return {"executed": action} + + +# =========================================================================== +# TestBaseAgent +# =========================================================================== + + +class TestBaseAgent: + """Tests for BaseAgent abstract base class.""" + + def test_instantiation(self): + """Concrete subclass should instantiate without errors.""" + agent = _ConcreteAgent("test_agent", {}) + assert agent.name == "test_agent" + + def test_update_state(self): + """update_state should store key-value pairs in state dict.""" + agent = _ConcreteAgent("a", {}) + agent.update_state("key", 42) + assert agent.state["key"] == 42 + + def test_add_and_get_memory(self): + """Memory should store and return items in FIFO order.""" + agent = _ConcreteAgent("a", {}) + agent.add_to_memory("item1") + agent.add_to_memory("item2") + mem = agent.get_memory(n=2) + assert "item1" in mem + assert "item2" in mem + + def test_clear_memory(self): + """clear_memory should remove all stored items.""" + agent = _ConcreteAgent("a", {}) + agent.add_to_memory("x") + agent.clear_memory() + assert agent.get_memory() == [] + + def test_register_and_get_tools(self): + """Registered tools should be retrievable via get_tools.""" + agent = _ConcreteAgent("a", {}) + + def dummy_tool(): + pass + + agent.register_tool(dummy_tool) + assert dummy_tool in agent.get_tools() + + def test_memory_capacity(self): + """Memory should not exceed configured capacity.""" + agent = _ConcreteAgent("a", {"memory_capacity": 3}) + for i in range(10): + agent.add_to_memory(i) + assert len(agent.memory) <= 3 + + +# =========================================================================== +# TestPerceptionAgent +# =========================================================================== + + +class TestPerceptionAgent: + """Tests for PerceptionAgent.""" + + @pytest.fixture + def agent(self): + return PerceptionAgent(config={"confidence_threshold": 0.5}) + + def test_perceive_with_image(self, agent): + """perceive should return detections when given a mock image.""" + image = np.zeros((480, 640, 3), dtype=np.uint8) + result = agent.perceive({"image": image}) + assert "detections" in result + assert isinstance(result["detections"], list) + + def test_perceive_with_lidar(self, agent): + """perceive should return nearest_obstacle from LiDAR data.""" + lidar = [5.0, 3.0, 2.0, 1.5, 4.0] + result = agent.perceive({"lidar": lidar}) + assert result["nearest_obstacle"] == pytest.approx(1.5) + + def test_perceive_with_imu(self, agent): + """perceive should extract orientation from IMU data.""" + imu = {"roll": 0.1, "pitch": 0.2, "yaw": 0.3} + result = agent.perceive({"imu": imu}) + assert result["orientation"]["yaw"] == pytest.approx(0.3) + + def test_perceive_empty_observation(self, agent): + """perceive should return valid defaults for an empty observation.""" + result = agent.perceive({}) + assert "detections" in result + assert "orientation" in result + + def test_reason(self, agent): + """reason should return a scene description dict.""" + agent.perceive({"lidar": [5.0, 6.0, 7.0]}) + scene = agent.reason(None) + assert "scene_description" in scene + assert "safe_to_proceed" in scene + + def test_act_update_threshold(self, agent): + """act should update the confidence threshold.""" + result = agent.act({"type": "update_threshold", "value": 0.8}) + assert result["status"] == "ok" + + +# =========================================================================== +# TestPlanningAgent +# =========================================================================== + + +class TestPlanningAgent: + """Tests for PlanningAgent.""" + + @pytest.fixture + def agent(self): + return PlanningAgent(config={}) + + def test_create_plan_navigate(self, agent): + """create_plan should return steps for a navigation task.""" + plan = agent.create_plan("navigate to the kitchen") + assert isinstance(plan, list) + assert len(plan) >= 2 + assert all("step_id" in s for s in plan) + + def test_create_plan_grasp(self, agent): + """create_plan should return steps for a grasp task.""" + plan = agent.create_plan("grasp the bottle") + assert isinstance(plan, list) + assert len(plan) >= 2 + + def test_plan_decomposition_generic(self, agent): + """_decompose_task should return a non-empty generic plan.""" + steps = agent._decompose_task("unknown complex task") + assert len(steps) >= 1 + + def test_reason_returns_plan(self, agent): + """reason should produce a plan dict with num_steps key.""" + result = agent.reason("navigate to room A") + assert "plan" in result + assert result["num_steps"] > 0 + + def test_act_advances_plan(self, agent): + """act should execute steps sequentially.""" + agent.reason("navigate somewhere") + result = agent.act({}) + assert result["status"] in ("executing", "plan_complete", "no_plan") + + +# =========================================================================== +# TestControlAgent +# =========================================================================== + + +class TestControlAgent: + """Tests for ControlAgent.""" + + @pytest.fixture + def agent(self): + return ControlAgent(config={"position_tolerance": 0.05}, ros2_enabled=False) + + def test_navigate_to(self, agent): + """navigate_to should return True and update position.""" + success = agent.navigate_to(3.0, 4.0, 0.0) + assert success is True + assert agent.state["position"]["x"] == pytest.approx(3.0) + + def test_execute_grasp(self, agent): + """execute_grasp should return True and record the object.""" + success = agent.execute_grasp("bottle_01") + assert success is True + assert agent.state["grasped_object"] == "bottle_01" + + def test_emergency_stop(self, agent): + """emergency_stop should set is_stopped flag.""" + agent.emergency_stop() + assert agent._is_stopped is True + + def test_navigate_blocked_by_estop(self, agent): + """navigate_to should fail when emergency stop is active.""" + agent.emergency_stop() + success = agent.navigate_to(1.0, 1.0, 0.0) + assert success is False + + def test_act_navigate(self, agent): + """act with type=navigate should navigate successfully.""" + result = agent.act({"type": "navigate", "x": 1.0, "y": 2.0, "theta": 0.0}) + assert result["success"] is True + + def test_act_stop(self, agent): + """act with type=stop should trigger emergency stop.""" + result = agent.act({"type": "stop"}) + assert result["success"] is True + assert agent._is_stopped is True + + +# =========================================================================== +# TestCommunicationAgent +# =========================================================================== + + +class TestCommunicationAgent: + """Tests for CommunicationAgent.""" + + @pytest.fixture + def agent(self): + return CommunicationAgent(config={"max_dialog_history": 5}) + + def test_parse_command_navigate(self, agent): + """parse_command should detect navigate intent.""" + result = agent.parse_command("go to the warehouse") + assert result["intent"] == "navigate" + + def test_parse_command_grasp(self, agent): + """parse_command should detect grasp intent.""" + result = agent.parse_command("pick up the red box") + assert result["intent"] == "grasp" + + def test_parse_command_stop(self, agent): + """parse_command should detect stop intent.""" + result = agent.parse_command("emergency stop now") + assert result["intent"] == "stop" + + def test_classify_intent_navigate(self, agent): + """classify_intent should return 'navigate' for navigation text.""" + assert agent.classify_intent("move to position A") == "navigate" + + def test_classify_intent_query(self, agent): + """classify_intent should return 'query' for status questions.""" + assert agent.classify_intent("what is the current status") == "query" + + def test_classify_intent_unknown(self, agent): + """classify_intent should return 'unknown' for unrecognised input.""" + assert agent.classify_intent("xyzzy florp bloop") == "unknown" + + def test_generate_response(self, agent): + """generate_response should return a non-empty string.""" + resp = agent.generate_response({"intent": "navigate"}) + assert isinstance(resp, str) + assert len(resp) > 0 + + +# =========================================================================== +# TestCoordinationAgent +# =========================================================================== + + +class TestCoordinationAgent: + """Tests for CoordinationAgent.""" + + @pytest.fixture + def agent(self): + return CoordinationAgent(config={"max_robots": 5}) + + def test_register_agent(self, agent): + """register_agent should add agents to the managed pool.""" + sub = _ConcreteAgent("sub1", {}) + agent.register_agent(sub) + assert "sub1" in agent.get_agent_status() + + def test_max_capacity(self, agent): + """Registering beyond max_robots should raise ValueError.""" + for i in range(5): + agent.register_agent(_ConcreteAgent(f"r{i}", {})) + with pytest.raises(ValueError): + agent.register_agent(_ConcreteAgent("overflow", {})) + + def test_allocate_task(self, agent): + """allocate_task should return one of the registered agents.""" + a1 = _ConcreteAgent("a1", {}) + a2 = _ConcreteAgent("a2", {}) + agent.register_agent(a1) + agent.register_agent(a2) + chosen = agent.allocate_task({"task_type": "navigate"}, [a1, a2]) + assert chosen in (a1, a2) + + def test_allocate_task_no_agents(self, agent): + """allocate_task with empty pool should return None.""" + result = agent.allocate_task({"task_type": "navigate"}, []) + assert result is None + + def test_coordinate(self, agent): + """coordinate should return allocation dict for each task.""" + a1 = _ConcreteAgent("coord_a1", {}) + agent.register_agent(a1) + tasks = [{"task_id": "t1", "task_type": "navigate"}] + allocations = agent.coordinate(tasks) + assert "t1" in allocations + + +# =========================================================================== +# TestRoboticsAGI +# =========================================================================== + + +class TestRoboticsAGI: + """Tests for the top-level RoboticsAGI orchestrator.""" + + @pytest.fixture + def agi(self, tmp_path): + """Create a RoboticsAGI instance pointing at the real config dir.""" + config_path = os.path.join( + os.path.dirname(os.path.dirname(os.path.abspath(__file__))), "config" + ) + return RoboticsAGI(config_path=config_path) + + def test_get_status(self, agi): + """get_status should return a dict with 'system' key.""" + status = agi.get_status() + assert isinstance(status, dict) + assert status["system"] == "online" + + def test_create_task(self, agi): + """create_task should return a dict with task_type.""" + task = agi.create_task("navigate", location={"x": 1.0, "y": 2.0}) + assert task["task_type"] == "navigate" + assert task["location"] == {"x": 1.0, "y": 2.0} + + def test_execute_command_navigate(self, agi): + """execute_command should handle a navigation command.""" + result = agi.execute_command("navigate to the storage area") + assert "intent" in result + assert result["intent"] == "navigate" + + def test_execute_command_stop(self, agi): + """execute_command should handle a stop command.""" + result = agi.execute_command("emergency stop") + assert result["success"] is True + + def test_execute_task_query(self, agi): + """execute_task with type=query should return system status.""" + task = agi.create_task("query") + result = agi.execute_task(task) + assert result["success"] is True diff --git a/tests/test_perception.py b/tests/test_perception.py new file mode 100644 index 0000000..7d163fa --- /dev/null +++ b/tests/test_perception.py @@ -0,0 +1,258 @@ +""" +Unit tests for the perception module of the RAG7 AGI Robotics Framework. +""" + +import sys +import os + +sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + +import numpy as np +import pytest + +from perception.vision.object_detection import Detection, ObjectDetector +from perception.vision.segmentation import Segmenter +from perception.vision.tracking import ObjectTracker +from perception.slam.mapping import SLAMMapper +from perception.sensor_fusion import SensorFusion + + +# =========================================================================== +# TestObjectDetector +# =========================================================================== + + +class TestObjectDetector: + """Tests for ObjectDetector.""" + + @pytest.fixture + def detector(self): + return ObjectDetector(confidence_threshold=0.5) + + def test_detect_returns_list(self, detector): + """detect should return a list.""" + image = np.zeros((480, 640, 3), dtype=np.uint8) + results = detector.detect(image) + assert isinstance(results, list) + + def test_detect_mock_detections(self, detector): + """Mock detections should have the correct namedtuple fields.""" + image = np.zeros((480, 640, 3), dtype=np.uint8) + results = detector.detect(image) + for det in results: + assert hasattr(det, "label") + assert hasattr(det, "confidence") + assert hasattr(det, "bbox") + assert det.confidence >= 0.5 + + def test_detect_empty_image(self, detector): + """detect with an empty array should return empty list.""" + empty = np.array([]) + results = detector.detect(empty) + assert results == [] + + def test_detect_various_shapes(self, detector): + """detect should work with different image sizes.""" + for h, w in [(240, 320), (720, 1280), (100, 100)]: + img = np.zeros((h, w, 3), dtype=np.uint8) + results = detector.detect(img) + assert isinstance(results, list) + + def test_load_model_nonexistent(self, detector): + """load_model with a bad path should return False gracefully.""" + result = detector.load_model("/nonexistent/model.pt") + assert result is False + + +# =========================================================================== +# TestSegmenter +# =========================================================================== + + +class TestSegmenter: + """Tests for Segmenter.""" + + @pytest.fixture + def segmenter(self): + return Segmenter() + + def test_segment_returns_dict(self, segmenter): + """segment should return a dict with masks, labels, scores.""" + image = np.zeros((480, 640, 3), dtype=np.uint8) + result = segmenter.segment(image) + assert "masks" in result + assert "labels" in result + assert "scores" in result + + def test_segment_mask_shape(self, segmenter): + """Mask array should have correct spatial dimensions.""" + image = np.zeros((120, 160, 3), dtype=np.uint8) + result = segmenter.segment(image) + if result["masks"].ndim == 3: + _, h, w = result["masks"].shape + assert h == 120 + assert w == 160 + + def test_segment_labels_list(self, segmenter): + """labels field should be a list of strings.""" + image = np.zeros((100, 100, 3), dtype=np.uint8) + result = segmenter.segment(image) + assert isinstance(result["labels"], list) + assert all(isinstance(l, str) for l in result["labels"]) + + def test_segment_empty_image(self, segmenter): + """segment with empty array should return safe defaults.""" + empty = np.array([]) + result = segmenter.segment(empty) + assert result["labels"] == [] + + +# =========================================================================== +# TestObjectTracker +# =========================================================================== + + +class TestObjectTracker: + """Tests for ObjectTracker.""" + + @pytest.fixture + def tracker(self): + return ObjectTracker(max_age=5, min_hits=1) + + def _make_detection(self, bbox, label="object"): + """Helper to build a detection dict.""" + return {"bbox": bbox, "label": label} + + def test_update_returns_list(self, tracker): + """update should always return a list.""" + dets = [self._make_detection([0, 0, 50, 50])] + tracks = tracker.update(dets) + assert isinstance(tracks, list) + + def test_track_has_required_fields(self, tracker): + """Each track should have id, bbox, label keys.""" + dets = [self._make_detection([10, 10, 60, 60, "person"])] + tracker.update(dets) + tracks = tracker.update(dets) + for t in tracks: + assert "id" in t + assert "bbox" in t + assert "label" in t + + def test_empty_detections(self, tracker): + """update with no detections should not crash.""" + tracks = tracker.update([]) + assert isinstance(tracks, list) + + def test_track_persistence(self, tracker): + """A track matched across multiple frames should persist.""" + det = self._make_detection([10, 10, 60, 60]) + for _ in range(5): + tracker.update([det]) + tracks = tracker.update([det]) + assert len(tracks) >= 1 + + def test_iou_identical_boxes(self, tracker): + """IoU of identical boxes should be 1.0.""" + box = [0.0, 0.0, 10.0, 10.0] + from perception.vision.tracking import ObjectTracker as OT + + iou = OT._iou(box, box) + assert iou == pytest.approx(1.0) + + def test_iou_non_overlapping(self, tracker): + """IoU of non-overlapping boxes should be 0.0.""" + box_a = [0, 0, 10, 10] + box_b = [20, 20, 30, 30] + from perception.vision.tracking import ObjectTracker as OT + + iou = OT._iou(box_a, box_b) + assert iou == pytest.approx(0.0) + + +# =========================================================================== +# TestSLAMMapper +# =========================================================================== + + +class TestSLAMMapper: + """Tests for SLAMMapper.""" + + @pytest.fixture + def mapper(self): + return SLAMMapper(map_resolution=0.1, map_size=50) + + def test_get_map_shape(self, mapper): + """get_map should return a square array of the configured size.""" + m = mapper.get_map() + assert m.shape == (50, 50) + + def test_update_changes_map(self, mapper): + """update should modify at least some cells in the map.""" + initial_map = mapper.get_map().copy() + lidar = [2.0] * 36 # 36 rays at 2 metres + mapper.update(lidar, {"x": 0.0, "y": 0.0, "theta": 0.0}) + updated_map = mapper.get_map() + assert not np.array_equal(initial_map, updated_map) + + def test_localize_returns_pose(self, mapper): + """localize should return a dict with x, y, theta keys.""" + lidar = [3.0] * 36 + pose = mapper.localize(lidar) + assert "x" in pose + assert "y" in pose + assert "theta" in pose + assert "confidence" in pose + + def test_update_with_empty_scan(self, mapper): + """update with an empty scan should not crash.""" + mapper.update([], {"x": 0.0, "y": 0.0, "theta": 0.0}) + m = mapper.get_map() + assert m is not None + + +# =========================================================================== +# TestSensorFusion +# =========================================================================== + + +class TestSensorFusion: + """Tests for SensorFusion.""" + + @pytest.fixture + def fusion(self): + return SensorFusion() + + def test_fuse_returns_dict(self, fusion): + """fuse should return a dict with required keys.""" + result = fusion.fuse(None, None, None) + assert "position" in result + assert "orientation" in result + assert "confidence" in result + + def test_fuse_with_lidar(self, fusion): + """fuse should extract nearest obstacle from LiDAR.""" + lidar = [5.0, 3.0, 1.5, 4.0] + result = fusion.fuse(None, lidar, None) + assert len(result["obstacles"]) > 0 + assert result["obstacles"][0] == pytest.approx(1.5) + + def test_fuse_with_imu(self, fusion): + """fuse should propagate IMU orientation values.""" + imu = {"roll": 0.1, "pitch": 0.2, "yaw": 1.0} + result = fusion.fuse(None, None, imu) + assert result["orientation"]["yaw"] == pytest.approx(1.0) + + def test_fuse_confidence_range(self, fusion): + """Confidence should be in [0, 1].""" + lidar = [2.0, 3.0] + imu = {"yaw": 0.5} + result = fusion.fuse(None, lidar, imu) + assert 0.0 <= result["confidence"] <= 1.0 + + def test_fuse_with_camera(self, fusion): + """fuse with camera depth data should update position.""" + depth = np.ones((10, 10)) * 2.5 + camera = {"depth": depth} + result = fusion.fuse(camera, None, None) + assert "position" in result diff --git a/tests/test_planning.py b/tests/test_planning.py new file mode 100644 index 0000000..320a5f3 --- /dev/null +++ b/tests/test_planning.py @@ -0,0 +1,263 @@ +""" +Unit tests for the planning module of the RAG7 AGI Robotics Framework. +""" + +import sys +import os + +sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + +import pytest + +from planning.task_planner import TaskPlanner +from planning.path_planner import PathPlanner +from planning.motion_planner import MotionPlanner +from planning.decision_maker import DecisionMaker + + +# =========================================================================== +# TestTaskPlanner +# =========================================================================== + + +class TestTaskPlanner: + """Tests for TaskPlanner.""" + + @pytest.fixture + def planner(self): + return TaskPlanner() + + def test_plan_returns_list(self, planner): + """plan should return a non-empty list of steps.""" + steps = planner.plan("navigate to the dock") + assert isinstance(steps, list) + assert len(steps) > 0 + + def test_plan_step_structure(self, planner): + """Each step should have step_id, action, description keys.""" + steps = planner.plan("grasp the red cube") + for step in steps: + assert "step_id" in step + assert "action" in step + assert "description" in step + + def test_plan_navigate_keywords(self, planner): + """Navigation keywords should produce a navigation plan.""" + steps = planner.plan("go to room B") + actions = [s["action"] for s in steps] + # At least one navigation-related action + assert any( + kw in a for a in actions for kw in ("navigate", "path", "scan", "arrive", "confirm") + ) + + def test_decompose_grasp(self, planner): + """decompose should return grasp steps for pick commands.""" + steps = planner.decompose("pick up the bottle") + assert any("grasp" in s["action"] or "detect" in s["action"] for s in steps) + + def test_decompose_inspect(self, planner): + """decompose should handle inspect tasks.""" + steps = planner.decompose("inspect the machinery") + assert len(steps) >= 2 + + def test_decompose_generic(self, planner): + """decompose should return a generic plan for unknown tasks.""" + steps = planner.decompose("do something unusual") + assert len(steps) >= 1 + + def test_replan_includes_recovery(self, planner): + """replan should start with a diagnose/recover step.""" + failed = {"step_id": 2, "action": "navigate"} + steps = planner.replan(failed, {"failure_reason": "obstacle"}) + actions = [s["action"] for s in steps] + assert "diagnose" in actions or "recover" in actions + + def test_plan_with_context(self, planner): + """plan should accept a context dict without errors.""" + steps = planner.plan("place object", context={"location": "shelf_A"}) + assert isinstance(steps, list) + + +# =========================================================================== +# TestPathPlanner +# =========================================================================== + + +class TestPathPlanner: + """Tests for PathPlanner A* path planning.""" + + @pytest.fixture + def planner(self): + return PathPlanner(grid_resolution=1.0) + + def test_plan_returns_path(self, planner): + """plan should return a list of waypoints.""" + path = planner.plan(start=(0, 0), goal=(5, 5)) + assert isinstance(path, list) + assert len(path) > 0 + + def test_path_starts_at_start(self, planner): + """First waypoint should be close to the start position.""" + start = (0.0, 0.0) + path = planner.plan(start=start, goal=(5.0, 5.0)) + assert path[0][0] == pytest.approx(0.0, abs=1.5) + assert path[0][1] == pytest.approx(0.0, abs=1.5) + + def test_path_ends_at_goal(self, planner): + """Last waypoint should be close to the goal position.""" + goal = (5.0, 5.0) + path = planner.plan(start=(0.0, 0.0), goal=goal) + assert path[-1][0] == pytest.approx(5.0, abs=1.5) + assert path[-1][1] == pytest.approx(5.0, abs=1.5) + + def test_plan_no_obstacles(self, planner): + """A* should find a path with no obstacles.""" + path = planner.plan((0, 0), (3, 3), obstacles=[]) + assert len(path) >= 2 + + def test_plan_with_obstacles(self, planner): + """A* should route around obstacles.""" + obstacles = [(1, 0), (1, 1), (1, 2), (1, 3)] + path = planner.plan((0, 0), (3, 0), obstacles=obstacles) + # Path may be empty if blocked, but should not crash + assert isinstance(path, list) + + def test_heuristic_same_cell(self, planner): + """Heuristic of same cell should be 0.""" + h = PathPlanner._heuristic((3, 3), (3, 3)) + assert h == pytest.approx(0.0) + + def test_heuristic_manhattan(self, planner): + """Heuristic should return Manhattan distance.""" + h = PathPlanner._heuristic((0, 0), (3, 4)) + assert h == pytest.approx(7.0) + + def test_same_start_and_goal(self, planner): + """When start equals goal, path should be a single point.""" + path = planner.plan((2, 2), (2, 2)) + assert len(path) == 1 + + +# =========================================================================== +# TestMotionPlanner +# =========================================================================== + + +class TestMotionPlanner: + """Tests for MotionPlanner.""" + + @pytest.fixture + def planner(self): + return MotionPlanner(dof=6) + + def test_plan_trajectory_returns_dict(self, planner): + """plan_trajectory should return dict with trajectory and timestamps.""" + start = [0.0] * 6 + goal = [0.5] * 6 + result = planner.plan_trajectory(start, goal, duration=1.0) + assert "trajectory" in result + assert "timestamps" in result + + def test_trajectory_length(self, planner): + """Trajectory should have the requested number of waypoints.""" + result = planner.plan_trajectory([0.0] * 6, [1.0] * 6, duration=2.0, num_points=20) + assert len(result["trajectory"]) == 20 + assert len(result["timestamps"]) == 20 + + def test_trajectory_start_end(self, planner): + """First and last configs should be close to start and goal.""" + start = [0.0] * 6 + goal = [1.0] * 6 + result = planner.plan_trajectory(start, goal) + traj = result["trajectory"] + # First waypoint ~ start + assert all(abs(traj[0][i] - start[i]) < 0.1 for i in range(6)) + # Last waypoint ~ goal + assert all(abs(traj[-1][i] - goal[i]) < 0.1 for i in range(6)) + + def test_compute_ik_returns_list(self, planner): + """compute_ik should return a list of joint angles.""" + angles = planner.compute_ik({"x": 0.5, "y": 0.3, "z": 0.2}) + assert isinstance(angles, list) + assert len(angles) == 6 + + def test_compute_fk_returns_pose(self, planner): + """compute_fk should return a pose dict.""" + angles = [0.0] * 6 + pose = planner.compute_fk(angles) + assert "x" in pose + assert "y" in pose + assert "z" in pose + + def test_fk_zero_angles(self, planner): + """With all-zero joint angles, end-effector should be on x-axis.""" + pose = planner.compute_fk([0.0] * 6) + assert pose["y"] == pytest.approx(0.0, abs=1e-6) + + def test_ik_fk_consistency(self, planner): + """FK(IK(pose)) should produce a pose in the correct hemisphere.""" + target = {"x": 0.3, "y": 0.1, "z": 0.0} + angles = planner.compute_ik(target) + pose = planner.compute_fk(angles) + # Weak check: the result should be a valid pose dict + assert isinstance(pose["x"], float) + + +# =========================================================================== +# TestDecisionMaker +# =========================================================================== + + +class TestDecisionMaker: + """Tests for DecisionMaker.""" + + @pytest.fixture + def dm(self): + return DecisionMaker(config={"safety_threshold": 0.5}) + + def test_decide_returns_option(self, dm): + """decide should return one of the provided options.""" + options = [{"type": "navigate"}, {"type": "wait"}] + state = {"nearest_obstacle": 5.0} + chosen = dm.decide(state, options) + assert chosen in options + + def test_decide_empty_options(self, dm): + """decide with empty options should return None.""" + result = dm.decide({}, []) + assert result is None + + def test_decide_prefers_stop_near_obstacle(self, dm): + """decide should prefer stop/wait when obstacle is close.""" + options = [{"type": "navigate"}, {"type": "stop"}] + state = {"nearest_obstacle": 0.1} + chosen = dm.decide(state, options) + # Should not choose navigate when obstacle is very close + assert chosen is not None + + def test_decide_respects_estop(self, dm): + """With emergency stop active, only safe actions are chosen.""" + options = [{"type": "navigate"}, {"type": "stop"}] + state = {"is_stopped": True} + chosen = dm.decide(state, options) + if chosen is not None: + assert chosen.get("type") in ("stop", "wait", "reset_stop") + + def test_evaluate_returns_float(self, dm): + """evaluate should return a float in [0, 1].""" + score = dm.evaluate({"nearest_obstacle": 3.0}, {"type": "navigate"}) + assert isinstance(score, float) + assert 0.0 <= score <= 1.0 + + def test_evaluate_stop_preferred_when_close(self, dm): + """stop action should score higher than navigate when close to obstacle.""" + state = {"nearest_obstacle": 0.1} + stop_score = dm.evaluate(state, {"type": "stop"}) + nav_score = dm.evaluate(state, {"type": "navigate"}) + assert stop_score > nav_score + + def test_decide_single_option(self, dm): + """decide with a single option should return that option.""" + option = {"type": "navigate"} + chosen = dm.decide({"nearest_obstacle": 5.0}, [option]) + assert chosen == option