diff --git a/.gitignore b/.gitignore index 61c35fae1..e2d0e670a 100644 --- a/.gitignore +++ b/.gitignore @@ -100,6 +100,9 @@ workspace/.training_pending/ # Per-skill persistent key-value stores (Skill.storage, skills/types.py). workspace/skill_storage/ +# Generated on-robot by the skill catalog (typed physical-skill refs). +workspace/physical_skills/ + # recordings folder recordings/ @@ -116,7 +119,6 @@ data/.last_mode arm_wave/ .vscode -ros2_ws/src/brain/brain_client/innate/skills.pyi workspace/innate_skills/**/*.engine node_modules/ diff --git a/AGENTS.md b/AGENTS.md index 2cbb13d24..699ea86e2 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -32,6 +32,52 @@ Run `innate` with no arguments to print the current system status (version, mode | `innate volume` | Get or set speaker volume | | `innate --help` | Show all commands | +## Writing Skills + +Skills live in `workspace/` (see [workspace/README.md](workspace/README.md)). A skill is a +`Skill` subclass; everything it consumes is declared with a type annotation. + +### Never `time.sleep` — always `self.sleep` + +**In skill code, use `self.sleep(seconds)`. Never `time.sleep(seconds)`.** + +`self.sleep` wakes and raises `SkillCancelled` the moment a Stop lands; `time.sleep` blocks +to completion, so a skill that uses it keeps running (and keeps the robot moving) after the +user pressed Stop. Sleeping is the only cancel point a loop needs — write the loop as if +cancel didn't exist and let the framework halt the base and report `CANCELLED`. + +```python +while traveled < target: + self.mobility.send_cmd_vel(linear_x=velocity, duration=0.5) + self.sleep(0.1) # ✅ cancellable + # time.sleep(0.1) # ❌ Stop is ignored until the sleep finishes +``` + +`time` itself is fine for *measuring* — `time.time()` / `time.monotonic()` for deadlines and +elapsed checks. The rule is only about blocking. + +Related cancel-aware helpers, all of which raise `SkillCancelled` too: + +| Call | Use for | +|---|---| +| `self.sleep(seconds)` | Any pause in skill code | +| `self.wait_for(read, timeout)` | Block until a reader returns non-`None` | +| `self.check_cancelled()` | A checkpoint with no sleep (e.g. before an irreversible commit) | +| `self.cancelled` | Read the latch without raising | + +Cleanup belongs in `try/finally` inside `execute()`. `self.on_cancel(hook)` is only for +forwarding a cancel to an external action goal — braking the base is automatic. + +### The one exception: committed, non-cancellable sections + +Teardown and already-committed physical actions must **not** be cancellable, so they use +`time.sleep` on purpose. Once `pick_any_object` closes the gripper, a cancel must not unwind +mid-grip and drop the object over the floor, so `_close_twist_lift` sleeps with `time.sleep` +and the run finishes carrying the object home. + +If you write such a section, say so in a comment — otherwise the next reader "fixes" it back +to `self.sleep` and reintroduces the bug. Everywhere else, `self.sleep`. + ## Key ROS Packages | Package | Role | diff --git a/CLAUDE.md b/CLAUDE.md index 1fc1e8e7e..84b8b24d2 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -1,86 +1,35 @@ # CLAUDE.md -Behavioral guidelines to reduce common LLM coding mistakes. Merge with project-specific instructions as needed. +Project instructions for Claude. See [AGENTS.md](AGENTS.md) for the system overview, the +`innate` CLI, and the ROS package map. -**Tradeoff:** These guidelines bias toward caution over speed. For trivial tasks, use judgment. +## Writing skills -## 1. Think Before Coding +### Never `time.sleep` — always `self.sleep` -**Don't assume. Don't hide confusion. Surface tradeoffs.** +**In skill code, use `self.sleep(seconds)`. Never `time.sleep(seconds)`.** -Before implementing: +`self.sleep` wakes and raises `SkillCancelled` the moment a Stop lands; `time.sleep` blocks +to completion, so a skill that uses it keeps running (and keeps the robot moving) after the +user pressed Stop. Sleeping is the only cancel point a loop needs — write the loop as if +cancel didn't exist and let the framework halt the base and report `CANCELLED`. -- State your assumptions explicitly. If uncertain, ask. -- If multiple interpretations exist, present them - don't pick silently. -- If a simpler approach exists, say so. Push back when warranted. -- If something is unclear, stop. Name what's confusing. Ask. - -## 2. Simplicity First - -**Minimum code that solves the problem. Nothing speculative.** - -- No features beyond what was asked. -- No abstractions for single-use code. -- No "flexibility" or "configurability" that wasn't requested. -- No error handling for impossible scenarios. -- If you write 200 lines and it could be 50, rewrite it. - -Ask yourself: "Would a senior engineer say this is overcomplicated?" If yes, simplify. - -## 3. Surgical Changes - -**Touch only what you must. Clean up only your own mess.** - -When editing existing code: - -- Don't "improve" adjacent code, comments, or formatting. -- Don't refactor things that aren't broken. -- Match existing style, even if you'd do it differently. -- If you notice unrelated dead code, mention it - don't delete it. - -When your changes create orphans: - -- Remove imports/variables/functions that YOUR changes made unused. -- Don't remove pre-existing dead code unless asked. - -The test: Every changed line should trace directly to the user's request. - -## 4. Goal-Driven Execution - -**Define success criteria. Loop until verified.** - -Transform tasks into verifiable goals: - -- "Add validation" → "Write tests for invalid inputs, then make them pass" -- "Fix the bug" → "Write a test that reproduces it, then make it pass" -- "Refactor X" → "Ensure tests pass before and after" - -For multi-step tasks, state a brief plan: - -``` -1. [Step] → verify: [check] -2. [Step] → verify: [check] -3. [Step] → verify: [check] +```python +while traveled < target: + self.mobility.send_cmd_vel(linear_x=velocity, duration=0.5) + self.sleep(0.1) # ✅ cancellable + # time.sleep(0.1) # ❌ Stop is ignored until the sleep finishes ``` -Strong success criteria let you loop independently. Weak criteria ("make it work") require constant clarification. - -## 5. Clean, Readable Code - -**You are a senior engineer. Write clean, maintainable code.** - -Clean code is understood without reading the comments: - -- Use clear, descriptive variable and function names that state intent. -- Keep complexity low. Avoid deep, multiple-levels-of-nesting indentation. -- Use early returns to flatten control flow instead of nesting. -- Keep things simple, always. - -When handling errors, don't overdo it: +`time` itself is fine for *measuring* — `time.time()` / `time.monotonic()` for deadlines and +elapsed checks. The rule is only about blocking. -- Avoid scattering multiple try/catch blocks where one is enough. -- Catch errors at the level where you can actually do something about them. +`self.wait_for(read, timeout)` and `self.check_cancelled()` are cancel-aware too; cleanup +belongs in `try/finally`. ---- +**The one exception:** teardown and already-committed physical actions must *not* be +cancellable, so they use `time.sleep` deliberately — e.g. once `pick_any_object` closes the +gripper, a cancel must not unwind mid-grip and drop the object. If you write such a section, +comment it, or the next reader will "fix" it back to `self.sleep` and reintroduce the bug. -**These guidelines are working if:** fewer unnecessary changes in diffs, fewer rewrites due to overcomplication, and clarifying questions come before implementation rather than after mistakes. +See [AGENTS.md](AGENTS.md#writing-skills) for the full cancellation contract. diff --git a/README.md b/README.md index d3b271ba6..93497bb02 100644 --- a/README.md +++ b/README.md @@ -135,6 +135,9 @@ You will find skills in two different directories: - **Built-in skills** — Located in `workspace/innate_skills/`. - **Your custom skills** — Stored in `workspace/custom_skills/`. Gitignored and yours to play with. +- **Skill packs** — Any other folder dropped into `workspace/` loads as its own package (ids `/`). A pack that lives elsewhere on disk is symlinked in (`ln -s /opt/team/skills workspace/team_skills`) and works the same, hot reload included. + +Helpers work like normal Python: any `.py` in your skills folder that doesn't define a `Skill` is just a module — `import` it, use relative imports inside subfolders, share across packages by bare name (`from innate_skills import arm_utils`). Device helpers are methods on the interfaces (`self.manipulation.go(...)`, `self.mobility.rotate_by(...)`); camera math and Gemini live under `innate` (`from innate import geometry, vision, gemini`). ### Skill definition @@ -164,52 +167,27 @@ You will find skills in two different directories: Code skill — call the mobility interface to move forward.
Saved as workspace/custom_skills/move_forward.py: -
from brain_client.skills.types import Interface, InterfaceType, Skill, SkillResult
-import time
+      
from innate import Mobility, Skill, SkillReturn
 
 
 class MoveForward(Skill):
-    """Move the robot forward by a given distance."""
-
-    mobility = Interface(InterfaceType.MOBILITY)
-
-    def __init__(self, logger):
-        super().__init__(logger)
-        self._cancelled = False
+    """Move the robot forward by a given distance in meters."""
 
-    @property
-    def name(self):
-        return "move_forward"
-
-    def guidelines(self):
-        return "Move the robot forward by a given distance in meters."
-
-    def execute(self, distance_m: float = 0.5):
-        self._cancelled = False
-
-        if self.mobility is None:
-            return "Mobility interface not available", SkillResult.FAILURE
+    mobility: Mobility          # declare what you use; the runtime injects it
 
+    def execute(self, distance_m: float = 0.5) -> SkillReturn:
         speed = 0.2  # m/s
         duration = distance_m / speed
         self.mobility.send_cmd_vel(linear_x=speed, duration=duration)
-
-        stop_at = time.time() + duration
-        while True:
-            remaining = stop_at - time.time()
-            if not remaining > 0:
-                break
-            if self._cancelled:
-                self.mobility.send_cmd_vel(linear_x=0.0)
-                return "Move cancelled", SkillResult.CANCELLED
-            time.sleep(min(0.05, remaining))
-
-        return f"Moved forward {distance_m} m", SkillResult.SUCCESS
-
-    def cancel(self):
-        self._cancelled = True
-        return "Move cancelled"
+        self.sleep(duration)    # like time.sleep, but a Stop unwinds it
+        return f"Moved forward {distance_m} m"
 
+ The return value is the run's result message; call + self.fail(message) to end the run as a failure. + Cancellation is the framework's job: self.sleep (and every + blocking framework call) raises the moment a Stop lands, the base is + braked automatically, and the run reports CANCELLED — skills carry no + cancel code. @@ -336,7 +314,7 @@ Input devices live in [`workspace/inputs/`](workspace/inputs/) and are pure Pyth import threading import time -from brain_client.input_types import InputDevice +from brain_client.inputs.types import InputDevice def read_thermometer_celsius() -> float: diff --git a/ci/run_integration_tests.sh b/ci/run_integration_tests.sh index 423c0d0e2..158dac58e 100755 --- a/ci/run_integration_tests.sh +++ b/ci/run_integration_tests.sh @@ -83,7 +83,6 @@ fi echo "=== unit tests (fast, no ROS) ===" PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest -q \ src/brain/brain_client/test/test_fake_cloud_selftest.py \ - src/brain/brain_client/test/test_backwards_compat.py \ src/brain/manipulation/test/test_config_validation.py echo "=== unit tests: webapp front door (aiohttp, no ROS) ===" diff --git a/config/settings.yaml.template b/config/settings.yaml.template index d28ab4245..7527742be 100644 --- a/config/settings.yaml.template +++ b/config/settings.yaml.template @@ -167,12 +167,13 @@ # cmd_publish_hz: 50.0 # cmd_vel republish rate during a move # poll_period_sec: 0.02 # action-loop poll interval -# ── Extra agent / skill directories ─────────────────────────────────── -# Scan agents/skills from extra locations, on top of the built-in workspace/ dirs. -# Absolute paths anywhere on the machine, scanned in place (never created); in a Docker/sim -# setup, host paths must also be mounted into the container. (Read directly by the agent / -# skill loaders — not a ROS node, so leave the section name as `script_paths`.) -# script_paths: -# ros__parameters: -# extra_agent_dirs: ["/home/me/my-agents", "/opt/team/agents"] -# extra_skill_dirs: ["/home/me/my-skills"] +# Extra agent/skill directories (script_paths.extra_agent_dirs / extra_skill_dirs) +# were removed in 0.7: agents and skills load only from workspace/. To add a +# skill pack, drop its directory into workspace/ — it is picked up (and +# hot-reloaded) without a restart. A pack that must stay elsewhere on disk is +# symlinked in instead, and behaves the same: +# ln -s /opt/team/skills ~/innate-os/workspace/team_skills +# Dirs a 0.6.x settings.yaml configured are symlinked into workspace/ +# automatically on update (scripts/update/migrate_user_data.sh): skill dirs as +# custom_skills/ subpackages (keeping their local/ skill ids), agent-dir +# entries file-by-file into custom_agents/. diff --git a/docs/INPUT_DEVICES.md b/docs/INPUT_DEVICES.md index e7cd1edc8..a0a41832f 100644 --- a/docs/INPUT_DEVICES.md +++ b/docs/INPUT_DEVICES.md @@ -23,7 +23,7 @@ class HelloWorld(Directive): ```python # workspace/inputs/my_sensor_input.py -from brain_client.input_types import InputDevice +from brain_client.inputs.types import InputDevice import threading import time @@ -215,9 +215,9 @@ innate-os/ │ └── hello_world_directive.py # Uses get_inputs() └── ros2_ws/src/brain/brain_client/ └── brain_client/ - ├── input_types.py # Base class - ├── input_loader.py # Auto-discovery - └── input_manager_node.py # ROS bridge + ├── inputs/types.py # Base class + ├── inputs/loader.py # Auto-discovery + └── nodes/input_manager.py # ROS bridge ``` ## Summary diff --git a/docs/PARAMETERS.md b/docs/PARAMETERS.md index b237529a8..221aef73f 100644 --- a/docs/PARAMETERS.md +++ b/docs/PARAMETERS.md @@ -54,7 +54,6 @@ To tune something, **uncomment a whole stanza** (the `node:`, `ros__parameters:` | `navigation_grid_localizer` | `max_score_threshold`, `max_range`, `auto_localize_timeout` | `0.3`, `12.0`, `30.0` | | `brain_client_node` | `cartesia_voice_id` (TTS voice), `vertical_fov`, `pose_image_interval`, `scan_stale_after_sec`, `send_depth`, `send_arm_camera_image`, `log_everything`, STT/transcribe models | see template | | `uninavid_node` (VLN) | `forward_speed`, `turn_speed`, `cmd_duration_sec`, `image_send_hz`, `consecutive_stops_to_complete`, `cmd_publish_hz`, `poll_period_sec` | `0.3` / `0.8`, rest see template | -| `script_paths` | `extra_agent_dirs`, `extra_skill_dirs` (extra dirs scanned on top of `workspace/`) | `[]` | > **Driving caps vs the safety clamp.** `motion_control` is the *driving feel* cap: the > joystick, keyboard, and app drive joystick all ship the same `0.4` m/s / `1.0` rad/s diff --git a/pyproject.toml b/pyproject.toml index a31be185b..c8729c862 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -20,7 +20,7 @@ ignore = [ ] [tool.ruff.lint.isort] -known-first-party = ["mars_nav", "mars_control", "mars_cam", "mars_arm", "brain_client", "manipulation"] +known-first-party = ["mars_nav", "mars_control", "mars_cam", "mars_arm", "brain_client", "manipulation", "workspace", "innate", "innate_proxy"] [tool.ruff.format] quote-style = "double" diff --git a/pyrightconfig.json b/pyrightconfig.json new file mode 100644 index 000000000..7b077efd3 --- /dev/null +++ b/pyrightconfig.json @@ -0,0 +1,30 @@ +{ + "pythonVersion": "3.10", + "pythonPlatform": "Linux", + // Scoped to the tree that is actually clean under standard mode: the skills + // API and the skills written against it. Unscoped, pyright checks all 291 + // files in the repo and reports ~230 real diagnostics from code that was + // never typed — burying the ones a skill author needs to see. Add paths + // here as they get cleaned (test/ and the rest of ros2_ws are not yet). + "include": [ + "ros2_ws/src/brain/brain_client/brain_client/skills", + "ros2_ws/src/brain/brain_client/innate", + "workspace/innate_skills" + ], + "extraPaths": [ + "workspace", + "ros2_ws/src/brain/brain_client", + "ros2_ws/src/cloud/clients/proxy-client", + "ros2_ws/install/brain_client/local/lib/python3.10/dist-packages", + "ros2_ws/install/brain_messages/local/lib/python3.10/dist-packages", + "ros2_ws/install/innate_cloud_msgs/local/lib/python3.10/dist-packages", + "ros2_ws/install/mars_msgs/local/lib/python3.10/dist-packages", + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages", + // System site-packages (pydantic, etc.). Needed when the language server + // has no usable interpreter and would otherwise only see the paths above. + "/usr/local/lib/python3.10/dist-packages", + "/usr/lib/python3/dist-packages", + "/usr/lib/python3.10/dist-packages" + ] +} diff --git a/ros2_ws/pip-requirements.txt b/ros2_ws/pip-requirements.txt index 60061f8b3..85eb2afb9 100644 --- a/ros2_ws/pip-requirements.txt +++ b/ros2_ws/pip-requirements.txt @@ -36,7 +36,6 @@ dynamixel-sdk pyserial trimesh smbus2 -sounddevice bluezero python-chess diff --git a/ros2_ws/src/brain/brain_client/CMakeLists.txt b/ros2_ws/src/brain/brain_client/CMakeLists.txt index 06ba41661..7b744eaa1 100644 --- a/ros2_ws/src/brain/brain_client/CMakeLists.txt +++ b/ros2_ws/src/brain/brain_client/CMakeLists.txt @@ -15,7 +15,7 @@ ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR brain_client ) -# Public authoring namespace for skill files (`from innate.skills import ...`) +# Public authoring namespace for skill files (`from innate import Skill, Mobility, ...`) ament_python_install_package(innate PACKAGE_DIR innate ) diff --git a/ros2_ws/src/brain/brain_client/brain_client/agent_types.py b/ros2_ws/src/brain/brain_client/brain_client/agent_types.py deleted file mode 100644 index ab4848b07..000000000 --- a/ros2_ws/src/brain/brain_client/brain_client/agent_types.py +++ /dev/null @@ -1,9 +0,0 @@ -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -"""Deprecated: moved to brain_client.agents.types. - -Backward-compat shim for custom agents that still import from the old path. -Remove in a future release once external agent files have migrated. -""" - -from brain_client.agents.types import * # noqa: F401, F403 diff --git a/ros2_ws/src/brain/brain_client/brain_client/agents/initializer.py b/ros2_ws/src/brain/brain_client/brain_client/agents/initializer.py index 48220d41a..ebba8d528 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/agents/initializer.py +++ b/ros2_ws/src/brain/brain_client/brain_client/agents/initializer.py @@ -8,16 +8,17 @@ to keep the main brain_client_node.py clean and focused. """ -from typing import Any - from brain_client.agents.loader import AgentLoader +from brain_client.agents.types import Agent from brain_client.common.script_paths import ( ensure_user_directories, get_agent_directories, + get_workspace_dir, ) +from brain_client.skills.physical_refs import render_refs, write_refs -def initialize_agents(logger, skills_dict: dict[str, Any] | None = None) -> tuple[dict[str, Any], Any | None]: +def initialize_agents(logger, skills_dict: dict[str, dict] | None = None) -> tuple[dict[str, Agent], Agent | None]: """ Initialize all agents using dynamic loading. @@ -36,6 +37,12 @@ def initialize_agents(logger, skills_dict: dict[str, Any] | None = None) -> tupl # and, if present, ~/agents (in place — never moved). ensure_user_directories() + # Agent files may `from physical_skills import X`, so make sure the + # generated package exists before importing them (a fresh workspace where + # agents load before the skills server has written it). The skills server + # is the authoritative writer; this only fills the ordering gap. + _regenerate_physical_refs(logger, skills_dict) + agents_directories = [str(p) for p in get_agent_directories()] # Load all agents dynamically from all directories @@ -64,3 +71,22 @@ def initialize_agents(logger, skills_dict: dict[str, Any] | None = None) -> tupl logger.error("No agents loaded! This will cause issues.") return agents, default_agent + + +def _regenerate_physical_refs(logger, skills_dict: dict[str, dict] | None) -> None: + """Write workspace/physical_skills/ from the roster metadata, only when + the generated package doesn't exist yet. Skipped when no roster is + available (nothing to generate from) or when the skills server has + already written the package: the server regenerates it on every load and + publish from its full pre-dedupe roster, while this roster has + display-name dedupe applied — rewriting here from the (possibly smaller) + deduped set would make the two processes overwrite each other's file + forever, each write triggering the watcher's full reload.""" + if not skills_dict: + return + if (get_workspace_dir() / "physical_skills" / "__init__.py").exists(): + return + # Everything on the roster that isn't a code skill is a physical skill + # (learned/replay/eval/poses/...; broken entries never reach the registry). + entries = [meta for meta in skills_dict.values() if meta.get("type") != "code"] + write_refs(get_workspace_dir() / "physical_skills", render_refs(entries), logger) diff --git a/ros2_ws/src/brain/brain_client/brain_client/agents/loader.py b/ros2_ws/src/brain/brain_client/brain_client/agents/loader.py index 6c3890cf4..50f75e885 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/agents/loader.py +++ b/ros2_ws/src/brain/brain_client/brain_client/agents/loader.py @@ -16,6 +16,7 @@ from brain_client.agents.types import Agent from brain_client.common.dynamic_loader import DynamicLoader from brain_client.common.script_paths import classify_source +from brain_client.skills.workspace_import import ensure_import_roots class AgentLoader(DynamicLoader): @@ -26,6 +27,13 @@ class AgentLoader(DynamicLoader): base_class = Agent name_suffixes = ("Agent", "Directive") + def __init__(self, logger): + super().__init__(logger) + # Agent files may reference skill classes directly + # (`from innate_skills.navigate_to_position import NavigateToPosition`), + # so workspace packages must be importable in this process too. + ensure_import_roots() + def _iter_candidate_files(self, directory: Path) -> list[Path]: # Look for Python files (excluding __init__.py, types.py, and _-prefixed) return [ @@ -110,7 +118,7 @@ def reload_agent_by_name(self, agent_name: str, directories: list[str]) -> tuple def create_agent_instances( self, agent_classes: dict[str, tuple[type[Agent], Path]], - available_skills: dict[str, any] | None = None, + available_skills: dict[str, dict] | None = None, ) -> dict[str, Agent]: """ Create instances of agent classes. @@ -155,9 +163,6 @@ def _load_display_icon(self, agent_instance: Agent, agents_directory: str | None agent_instance: The agent instance agents_directory: Path to the agents directory """ - # Initialize the attribute for storing base64 icon data - agent_instance.display_icon_data = None - if not agent_instance.display_icon or not agents_directory: return @@ -171,7 +176,7 @@ def _load_display_icon(self, agent_instance: Agent, agents_directory: str | None except Exception as e: self.logger.warning(f"Failed to load icon for agent '{agent_instance.id}': {e}") - def _validate_agent_skills(self, agent_instance: Agent, available_skills: dict[str, any]) -> None: + def _validate_agent_skills(self, agent_instance: Agent, available_skills: dict[str, dict]) -> None: """ Validates that all skills referenced by an agent have corresponding skill files available. @@ -184,7 +189,7 @@ def _validate_agent_skills(self, agent_instance: Agent, available_skills: dict[s Warning if a skill is not found (logged, not raised) """ try: - agent_skills = agent_instance.get_skills() + agent_skills = agent_instance.skill_ids() missing_skills = [] for skill_name in agent_skills: diff --git a/ros2_ws/src/brain/brain_client/brain_client/agents/types.py b/ros2_ws/src/brain/brain_client/brain_client/agents/types.py index 72ce57ffe..692b7729e 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/agents/types.py +++ b/ros2_ws/src/brain/brain_client/brain_client/agents/types.py @@ -8,6 +8,23 @@ """ from abc import ABC, abstractmethod +from typing import TYPE_CHECKING, TypeAlias, Union + +from brain_client.common.script_paths import Source + +if TYPE_CHECKING: + from brain_client.inputs.types import InputDevice + from brain_client.skills.types import Skill, TrainedSkill + +# What get_skills() may list: the Skill class itself for code skills, the +# generated TrainedSkill ref for physical skills (both typed — an import +# error or rename is caught by the editor, not at runtime on the robot), or +# an id string. +SkillRef: TypeAlias = Union["type[Skill]", "type[TrainedSkill]", str] + +# What get_inputs() may list: the InputDevice class itself (typed, same +# rationale as SkillRef) or a device-name string. +InputRef: TypeAlias = Union["type[InputDevice]", str] class Agent(ABC): @@ -21,7 +38,11 @@ class Agent(ABC): # Stamped by the loader to "shipped" or "user" based on origin directory. # Subclasses must not set this themselves. - source: str = "user" + source: Source = "user" + + # Stamped by the loader: the display_icon file base64-encoded, when the + # agent declares one and it loads. + display_icon_data: str | None = None @property @abstractmethod @@ -42,17 +63,50 @@ def display_name(self) -> str: pass @abstractmethod - def get_skills(self) -> list[str]: + def get_skills(self) -> list[SkillRef]: """ - Returns a list of skill IDs that should be available - when this agent is active (e.g. "innate-os/navigate_to_position" - or "local/wave-hello"). IDs are matched exactly against each - available skill's id during registration — not by display name. + Returns the skills that should be available when this agent is + active. Prefer classes: the Skill itself for code skills, the + generated ref (``physical_skills`` package) for physical skills:: + + from innate_skills.navigate_to_position import NavigateToPosition + from physical_skills import PickSocks + + def get_skills(self): + return [NavigateToPosition, PickSocks] + + Id strings (e.g. "innate-os/navigate_to_position") are equivalent. + Ids are matched exactly against each available skill's id during + registration — not by display name. Subclasses must implement this method. """ pass + def skill_ids(self) -> list[str]: + """get_skills() normalized to id strings — the only form the rest of + the system (registration, cloud agent, webapp) ever consumes. A class + resolves through skill_id_for_class, the same derivation the catalog + uses to id it, so a class reference and its id are interchangeable.""" + # lazy: keeps this module importable without the skill framework + from brain_client.skills.types import Skill, TrainedSkill + from brain_client.skills.workspace_import import skill_id_for_class + + ids = [] + for ref in self.get_skills(): + if isinstance(ref, str): + ids.append(ref) + elif isinstance(ref, type) and issubclass(ref, TrainedSkill): + ids.append(ref.skill_id) + elif isinstance(ref, type) and issubclass(ref, Skill): + ids.append(skill_id_for_class(ref)) + else: + raise TypeError( + f"{type(self).__name__}.get_skills() entries must be Skill classes, " + f"physical_skills refs, or skill-id strings, got {ref!r}" + ) + return ids + @abstractmethod def get_prompt(self) -> str | None: """ @@ -76,19 +130,45 @@ def display_icon(self) -> str | None: """ return None - def get_inputs(self) -> list[str]: + def get_inputs(self) -> list[InputRef]: """ - Returns a list of input device names that should be active - when this directive is running. + Returns the input devices that should be active when this agent is + running. Prefer the InputDevice class over its name string:: + + from inputs.micro_input import MicroInput + + def get_inputs(self): + return [MicroInput] + + Name strings (e.g. "micro") are equivalent; they are matched exactly + against each device's registered name. Subclasses can override this method to specify required inputs. Default: return empty list (no input devices required). - - Example: - return ["micro", "camera"] """ return [] + def input_names(self) -> list[str]: + """get_inputs() normalized to device-name strings — the only form the + input manager consumes. A class resolves through input_name_for_class, + the same derivation the loader registers it under, so a class + reference and its name are interchangeable.""" + # lazy: keeps this module importable without the input framework + from brain_client.inputs.types import InputDevice, input_name_for_class + + names = [] + for ref in self.get_inputs(): + if isinstance(ref, str): + names.append(ref) + elif isinstance(ref, type) and issubclass(ref, InputDevice): + names.append(input_name_for_class(ref)) + else: + raise TypeError( + f"{type(self).__name__}.get_inputs() entries must be InputDevice classes " + f"or device-name strings, got {ref!r}" + ) + return names + def uses_gaze(self) -> bool: """ Whether this agent uses person-tracking gaze. diff --git a/ros2_ws/src/brain/brain_client/brain_client/common/dynamic_loader.py b/ros2_ws/src/brain/brain_client/brain_client/common/dynamic_loader.py index 13daa2e22..4358a8e7e 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/common/dynamic_loader.py +++ b/ros2_ws/src/brain/brain_client/brain_client/common/dynamic_loader.py @@ -1,18 +1,18 @@ #!/usr/bin/env python3 # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc -"""Shared machinery for the dynamic skill/agent/input loaders. +"""Shared machinery for the agent and input-device loaders. -Each loader discovers subclasses of a base type in user/shipped script -directories and loads them by file path. The per-type logic (class validation, -naming, and instance creation) lives in the subclasses; the file discovery and -module-execution boilerplate lives here. +Discovers subclasses of a base type in script directories and loads them by +file path. Per-type validation/naming lives in subclasses. Skills no longer +use this (see skills/workspace_import.py); agents and inputs still do. """ from __future__ import annotations import importlib.util import inspect +import os import re import sys from pathlib import Path @@ -30,23 +30,47 @@ def class_name_to_snake_case(class_name: str, *, strip_suffixes: tuple[str, ...] return re.sub("([a-z0-9])([A-Z])", r"\1_\2", class_name).lower() +def evict_modules_under(directories: list[str]) -> list[str]: + """Drop cached modules whose source file lives under one of ``directories``. + + Enables reload by forcing the next discovery pass to re-import. Namespace + packages (no ``__file__``) are skipped. Returns the evicted names. + """ + roots = [Path(os.path.realpath(d)) for d in directories] + evicted = [] + for name, module in list(sys.modules.items()): + module_file = getattr(module, "__file__", None) + if not module_file: + continue + path = Path(os.path.realpath(module_file)) + if not any(root in path.parents for root in roots): + continue + del sys.modules[name] + evicted.append(name) + # Also drop parent.attr bindings or `from pkg import child` stays stale. + if "." in name: + parent_name, _, child = name.rpartition(".") + parent = sys.modules.get(parent_name) + if parent is not None and hasattr(parent, child): + delattr(parent, child) + return evicted + + class DynamicLoader: """Discovers and loads classes of a given base type from script directories. Subclasses set :attr:`base_class` (and optionally :attr:`name_suffixes`) and - implement :meth:`_validate_class` and :meth:`_get_name`. The discovery/loading - flow, ``INNATE_OS_ROOT`` handling, and conflict reporting are shared here. + implement :meth:`_validate_class` and :meth:`_get_name`. """ - #: Base class that discovered classes must inherit from. Set by subclasses. base_class: type - #: Class-name suffixes stripped when deriving a fallback snake_case name. name_suffixes: tuple[str, ...] = () def __init__(self, logger): self.logger = logger + # Load errors by file path; cleared on success. Surfaced as broken entries in the UI. + self.file_errors: dict[str, str] = {} - # --- subclass hooks --- def _iter_candidate_files(self, directory: Path) -> list[Path]: """Python files in ``directory`` that may define classes. Override for custom globs.""" return [f for f in directory.glob("*.py") if f.name != "__init__.py" and not f.name.startswith("_")] @@ -65,38 +89,61 @@ def _entry_module(self, entry) -> str: """Module name of a stored entry, for conflict reporting.""" return entry[0].__module__ - # --- shared machinery --- def _exec_module(self, file_path: Path) -> ModuleType | None: """Import a module from a file path, exposing ``INNATE_OS_ROOT`` on ``sys.path``. - Returns the executed module, or ``None`` if it could not be loaded. + Returns the executed module, or ``None`` if it could not be loaded — + in which case ``file_errors`` records why. """ module_name = file_path.stem spec = importlib.util.spec_from_file_location(module_name, file_path) if spec is None or spec.loader is None: self.logger.warning(f"Could not load spec for {file_path}") + self.file_errors[str(file_path)] = "could not load module spec" return None module = importlib.util.module_from_spec(spec) + # Scoped to this exec (removed in the finally): leaking these would + # leave every script directory permanently at the front of sys.path, + # where a workspace package named `types` or `logging` shadows the + # stdlib on the next lazy import anywhere in the process. + added_paths = [] root = str(get_innate_os_root()) - added = root not in sys.path - if added: - sys.path.insert(0, root) + workspace = str(get_innate_os_root() / "workspace") + parent = str(file_path.parent) + for entry in (root, workspace, parent): + if entry not in sys.path: + sys.path.insert(0, entry) + added_paths.append(entry) + # Temporary sys.modules entry so annotation resolution works during exec. + previous = sys.modules.get(module_name) + sys.modules[module_name] = module try: spec.loader.exec_module(module) except ModuleNotFoundError as e: self.logger.warning(f"Skipping module {module_name}: missing dependency '{e.name or e}'") + self.file_errors[str(file_path)] = f"missing dependency '{e.name or e}'" return None except ImportError as e: self.logger.warning(f"Skipping module {module_name}: import failed ({e})") + self.file_errors[str(file_path)] = f"import failed: {e}" return None except Exception as e: self.logger.error(f"Error executing module {module_name}: {e}") + self.file_errors[str(file_path)] = f"{type(e).__name__}: {e}" return None finally: - if added: - sys.path.remove(root) + for entry in added_paths: + try: + sys.path.remove(entry) + except ValueError: + pass + if previous is not None: + sys.modules[module_name] = previous + else: + sys.modules.pop(module_name, None) + self.file_errors.pop(str(file_path), None) return module def _find_classes(self, module: ModuleType) -> list[type]: @@ -107,6 +154,16 @@ def _find_classes(self, module: ModuleType) -> list[type]: continue if obj.__module__ != module.__name__: continue + if name.startswith("_"): + self.logger.debug(f"Skipping helper base class {name} in {module.__name__}") + continue + if inspect.isabstract(obj): + missing = ", ".join(sorted(getattr(obj, "__abstractmethods__", ()))) + self.logger.warning( + f"Skipping abstract class {name} in {module.__name__} (unimplemented: {missing}); " + "implement the missing methods, or prefix the class with '_' if it is a helper base." + ) + continue if self._validate_class(obj): found.append(obj) else: diff --git a/ros2_ws/src/brain/brain_client/brain_client/common/logging.py b/ros2_ws/src/brain/brain_client/brain_client/common/logging.py index 4fd6526f8..c67d046bf 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/common/logging.py +++ b/ros2_ws/src/brain/brain_client/brain_client/common/logging.py @@ -9,8 +9,6 @@ import os -from launch.actions import SetEnvironmentVariable - def _env_flag(name: str, default: bool) -> bool: value = os.getenv(name) @@ -95,6 +93,13 @@ def get_logging_env_vars(): Returns: list: A list of SetEnvironmentVariable actions. """ + # Lazy: importing `launch` swaps the global logger class to LaunchLogger + # (propagate=False) as an import side effect, which silently breaks stdlib + # log propagation (and pytest's caplog) for every logger created afterwards. + # Only launch files call this, and there `launch` is already loaded — the + # import must not ride along with UniversalLogger into non-launch code. + from launch.actions import SetEnvironmentVariable + # Default to the standard format (inherit RCUTILS_CONSOLE_OUTPUT_FORMAT from # the environment — see config/dds/setup_dds.zsh) so brain_client's lines # carry their node name and timestamp like every other node, and the diff --git a/ros2_ws/src/brain/brain_client/brain_client/common/script_paths.py b/ros2_ws/src/brain/brain_client/brain_client/common/script_paths.py index 7cded9c3a..793ed2d8c 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/common/script_paths.py +++ b/ros2_ws/src/brain/brain_client/brain_client/common/script_paths.py @@ -8,26 +8,18 @@ $INNATE_OS_ROOT/workspace/custom_agents/ # user agents (gitignored) $INNATE_OS_ROOT/workspace/innate_skills/ # shipped skills (tracked) $INNATE_OS_ROOT/workspace/custom_skills/ # user skills (gitignored) + $INNATE_OS_ROOT/workspace// # a skill package, dropped in whole (see below) $INNATE_OS_ROOT/workspace/inputs/ # input devices - $INNATE_OS_ROOT/agents/ # legacy user agents (<= 0.5.x, in place) - $INNATE_OS_ROOT/skills/ # legacy user skills (<= 0.5.x, in place) - $INNATE_OS_ROOT/inputs/ # legacy input devices (<= 0.5.x, in place) - ~/agents/ # user agents (alternative, in place) - ~/skills/ # user skills (alternative, in place) - # extra dirs from config/settings.yaml - -Extra scan dirs: the ``script_paths`` section of config/settings.yaml -(``extra_agent_dirs`` / ``extra_skill_dirs``) lets a user point at agent/skill -directories anywhere on the machine; they are scanned in place (never created) and, -because the hot-reload watchers consume these same getters, are hot-reloadable too. - -Backwards compatibility: through release 0.5.x, agents/skills/inputs were loaded -from $INNATE_OS_ROOT/{agents,skills,inputs} and ~/{agents,skills}. Those locations -are still scanned (in place — never moved or created) so content deployed against -older releases keeps loading. See test/test_backwards_compat.py. Provenance is determined by which directory a script came from: "shipped" if the path is under innate_*/, "user" otherwise. + +Skill packages: every other directory under workspace/ (not agents/inputs/lib +machinery, not hidden or ``_``-prefixed) is scanned as a skill package — a +folder of skills and their helpers that installs by being dropped in whole. +Skills in a package get ids namespaced by the package directory name +(``john_skills/chess``); ``innate_skills`` and ``custom_skills`` keep their +historical ``innate-os/`` and ``local/`` prefixes so nothing persisted breaks. """ from __future__ import annotations @@ -36,8 +28,6 @@ from pathlib import Path from typing import Literal -from mars_bringup.config_loader import load_extra_script_dirs - Source = Literal["shipped", "user"] @@ -49,6 +39,11 @@ def _workspace() -> Path: return get_innate_os_root() / "workspace" +def get_workspace_dir() -> Path: + """workspace/ itself — the root new skill packages are dropped into.""" + return _workspace() + + def get_innate_agents_dir() -> Path: return _workspace() / "innate_agents" @@ -65,99 +60,99 @@ def get_custom_skills_dir() -> Path: return _workspace() / "custom_skills" -def get_innate_inputs_dir() -> Path: - return _workspace() / "inputs" - - -def get_legacy_root_agents_dir() -> Path: - """Legacy in-place location $INNATE_OS_ROOT/agents (used through 0.5.x).""" - return get_innate_os_root() / "agents" - - -def get_legacy_root_skills_dir() -> Path: - """Legacy in-place location $INNATE_OS_ROOT/skills (used through 0.5.x).""" - return get_innate_os_root() / "skills" - - -def get_legacy_root_inputs_dir() -> Path: - """Legacy in-place location $INNATE_OS_ROOT/inputs (used through 0.5.x).""" - return get_innate_os_root() / "inputs" - - -def get_home_agents_dir() -> Path: - """Optional user location ~/agents (an alternative to custom_agents).""" - return Path.home() / "agents" - - -def get_home_skills_dir() -> Path: - """Optional user location ~/skills (an alternative to custom_skills).""" - return Path.home() / "skills" - - -def _extra_dirs(settings_key: str) -> list[Path]: - """Resolve extra scan dirs from config/settings.yaml ``script_paths.``. - - Blank entries are dropped; ``~`` and ``$VARS`` are expanded. +# workspace/ directories that are never skill packages: agent/input/lib +# machinery and per-skill storage. skill_lib/ and the pre-workspace agents// +# skills/ names stay listed so a stale checkout directory is never scanned. +NON_PACKAGE_DIR_NAMES = frozenset( + { + "innate_agents", + "custom_agents", + "inputs", + "skill_lib", + "skill_storage", + "agents", + "skills", + # generated TrainedSkill refs (see skills/physical_refs.py) — importable + # like any workspace package, but never scanned for skills + "physical_skills", + } +) + + +def get_workspace_package_dirs() -> list[Path]: + """Skill-package directories under workspace/ beyond the two standard ones. + + Any directory not claimed by other machinery is a package: a folder of + skills and helpers that installs by being dropped in whole. Hidden and + ``_``-prefixed names are skipped. Sorted for a deterministic scan order. """ - dirs: list[Path] = [] - for part in load_extra_script_dirs(settings_key): - part = part.strip() - if part: - dirs.append(Path(os.path.expandvars(os.path.expanduser(part)))) - return dirs + workspace = _workspace() + if not workspace.is_dir(): + return [] + packages = [] + for child in sorted(workspace.iterdir()): + name = child.name + if not child.is_dir() or name.startswith((".", "_")): + continue + if name in NON_PACKAGE_DIR_NAMES or name in ("innate_skills", "custom_skills"): + continue + packages.append(child) + return packages -def get_extra_agent_dirs() -> list[Path]: - """Extra agent dirs from settings.yaml ``script_paths.extra_agent_dirs`` (anywhere on the machine).""" - return _extra_dirs("extra_agent_dirs") +def skill_id_prefix_for(path: str | os.PathLike) -> str: + """The skill-id namespace for a script at ``path``. + ``innate_skills`` and ``custom_skills`` keep their historical prefixes — + every persisted id, the webapp, and cloud registration already speak them. + Any other workspace package namespaces by its directory name, which is what + makes a dropped-in pack collision-proof. Anything outside workspace/ + stays ``local``. + """ + resolved = Path(path).resolve() + for root, prefix in ((get_innate_skills_dir(), "innate-os"), (get_custom_skills_dir(), "local")): + try: + resolved.relative_to(root.resolve()) + return prefix + except ValueError: + continue + try: + rel = resolved.relative_to(_workspace().resolve()) + except ValueError: + return "local" + if rel.parts and rel.parts[0] not in NON_PACKAGE_DIR_NAMES: + return rel.parts[0] + return "local" -def get_extra_skill_dirs() -> list[Path]: - """Extra skill dirs from settings.yaml ``script_paths.extra_skill_dirs`` (anywhere on the machine).""" - return _extra_dirs("extra_skill_dirs") +def get_innate_inputs_dir() -> Path: + return _workspace() / "inputs" -def _scan_dirs(required: list[Path], optional: list[Path]) -> list[Path]: - """Ordered, de-duplicated scan list. - ``required`` directories are always included (loaders tolerate missing ones). - ``optional`` directories — legacy $INNATE_OS_ROOT/* and home locations kept - for backwards compatibility — are appended only when they exist, so they act - as in-place alternatives that are never created or moved. - """ - dirs = list(required) + [d for d in optional if d.is_dir()] +def _dedupe(dirs: list[Path]) -> list[Path]: + """Ordered, de-duplicated scan list (loaders tolerate missing dirs).""" seen: set[str] = set() - deduped: list[Path] = [] + out: list[Path] = [] for d in dirs: - key = str(d) - if key not in seen: - seen.add(key) - deduped.append(d) - return deduped + if str(d) not in seen: + seen.add(str(d)) + out.append(d) + return out def get_agent_directories() -> list[Path]: - """Agent scan dirs: workspace innate + custom + config-configured extras, then - legacy $INNATE_OS_ROOT/agents and ~/agents (kept for backwards compatibility).""" - return _scan_dirs( - [get_innate_agents_dir(), get_custom_agents_dir(), *get_extra_agent_dirs()], - [get_legacy_root_agents_dir(), get_home_agents_dir()], - ) + """Agent scan dirs under workspace/.""" + return _dedupe([get_innate_agents_dir(), get_custom_agents_dir()]) def get_skill_directories() -> list[Path]: - """Skill scan dirs: workspace innate + custom + config-configured extras, then - legacy $INNATE_OS_ROOT/skills and ~/skills (kept for backwards compatibility).""" - return _scan_dirs( - [get_innate_skills_dir(), get_custom_skills_dir(), *get_extra_skill_dirs()], - [get_legacy_root_skills_dir(), get_home_skills_dir()], - ) + """Skill scan dirs: the two standard packages plus any dropped-in package.""" + return _dedupe([get_innate_skills_dir(), get_custom_skills_dir(), *get_workspace_package_dirs()]) def get_input_directories() -> list[Path]: - """Input-device scan dirs: workspace/inputs, then legacy $INNATE_OS_ROOT/inputs - (kept for backwards compatibility, scanned in place).""" - return _scan_dirs([get_innate_inputs_dir()], [get_legacy_root_inputs_dir()]) + """Input-device scan dirs under workspace/.""" + return _dedupe([get_innate_inputs_dir()]) def classify_source(path: str | os.PathLike) -> Source: diff --git a/ros2_ws/src/brain/brain_client/brain_client/core/lifecycle.py b/ros2_ws/src/brain/brain_client/brain_client/core/lifecycle.py index 265080ac9..90acaea10 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/core/lifecycle.py +++ b/ros2_ws/src/brain/brain_client/brain_client/core/lifecycle.py @@ -81,7 +81,7 @@ def activate_directive_inputs(self) -> None: if not directive or not self._state.is_brain_active: return try: - required_inputs = directive.get_inputs() + required_inputs = directive.input_names() self._active_inputs_pub.publish(String(data=json.dumps({"inputs": required_inputs}))) if required_inputs: self._logger.info(f"🔌 Activated inputs for directive '{directive.id}': {required_inputs}") @@ -200,7 +200,7 @@ def set_directive(self, name: str) -> None: self._logger.error(f"Unknown directive: {name}") return self._state.current_directive = self._state.directives[name] - self._state.active_skill_ids = list(self._state.current_directive.get_skills()) + self._state.active_skill_ids = list(self._state.current_directive.skill_ids()) self._logger.info(f"Activated directive: {name}") self._chat.clear() self._ws.send_message(MessageIn(type=MessageInType.RESET, payload={"memory_state": "clear"})) diff --git a/ros2_ws/src/brain/brain_client/brain_client/input_types.py b/ros2_ws/src/brain/brain_client/brain_client/input_types.py deleted file mode 100644 index 93999c43c..000000000 --- a/ros2_ws/src/brain/brain_client/brain_client/input_types.py +++ /dev/null @@ -1,9 +0,0 @@ -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -"""Deprecated: moved to brain_client.inputs.types. - -Backward-compat shim for custom input devices that still import from the old -path. Remove in a future release once external input files have migrated. -""" - -from brain_client.inputs.types import * # noqa: F401, F403 diff --git a/ros2_ws/src/brain/brain_client/brain_client/inputs/loader.py b/ros2_ws/src/brain/brain_client/brain_client/inputs/loader.py index 88bd6dde3..67b7cac62 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/inputs/loader.py +++ b/ros2_ws/src/brain/brain_client/brain_client/inputs/loader.py @@ -11,7 +11,7 @@ from pathlib import Path from brain_client.common.dynamic_loader import DynamicLoader -from brain_client.inputs.types import InputDevice +from brain_client.inputs.types import InputDevice, input_name_for_class class InputLoader(DynamicLoader): @@ -81,12 +81,9 @@ def _validate_class(self, input_class: type[InputDevice]) -> bool: return False def _get_name(self, input_class: type[InputDevice]) -> str: - """Gets the input device name by creating a temporary instance.""" - try: - return input_class().name - except Exception as e: - self.logger.debug(f"Could not get name from input device {input_class.__name__}: {e}") - return self._fallback_name(input_class) + """The device's registered name — shared with Agent.input_names(), so + a typed class reference in get_inputs() resolves identically.""" + return input_name_for_class(input_class) def create_input_instances(self, input_classes: dict[str, type[InputDevice]], logger) -> dict[str, InputDevice]: """ diff --git a/ros2_ws/src/brain/brain_client/brain_client/inputs/types.py b/ros2_ws/src/brain/brain_client/brain_client/inputs/types.py index d855bf335..c8ab31a4c 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/inputs/types.py +++ b/ros2_ws/src/brain/brain_client/brain_client/inputs/types.py @@ -13,9 +13,9 @@ from enum import Enum from typing import Any -from innate_proxy import ProxyClient - +from brain_client.common.dynamic_loader import class_name_to_snake_case from brain_client.common.logging import UniversalLogger +from innate_proxy import ProxyClient class InputDeviceType(Enum): @@ -265,3 +265,14 @@ def get_description(self) -> str: Description string """ return self.name + + +def input_name_for_class(cls: type[InputDevice]) -> str: + """The name a device class registers under: the instance ``name`` when the + class instantiates cleanly, else snake_case(ClassName) minus the ``Input`` + suffix. The loader resolves through this too, so a class reference and its + name string are interchangeable.""" + try: + return cls().name + except Exception: + return class_name_to_snake_case(cls.__name__, strip_suffixes=("Input",)) diff --git a/ros2_ws/src/brain/brain_client/brain_client/logging_config.py b/ros2_ws/src/brain/brain_client/brain_client/logging_config.py deleted file mode 100644 index dfd1ef734..000000000 --- a/ros2_ws/src/brain/brain_client/brain_client/logging_config.py +++ /dev/null @@ -1,9 +0,0 @@ -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -"""Deprecated: moved to brain_client.common.logging. - -Backward-compat shim for custom files that still import from the old path. -Remove in a future release once external files have migrated. -""" - -from brain_client.common.logging import * # noqa: F401, F403 diff --git a/ros2_ws/src/brain/brain_client/brain_client/nodes/brain_client_node.py b/ros2_ws/src/brain/brain_client/brain_client/nodes/brain_client_node.py index f5a2ec464..31708b17b 100755 --- a/ros2_ws/src/brain/brain_client/brain_client/nodes/brain_client_node.py +++ b/ros2_ws/src/brain/brain_client/brain_client/nodes/brain_client_node.py @@ -289,7 +289,7 @@ def _startup(self) -> None: self.get_logger(), self.state.registry.primitives or None ) self.state.active_skill_ids = ( - list(self.state.current_directive.get_skills()) if self.state.current_directive else [] + list(self.state.current_directive.skill_ids()) if self.state.current_directive else [] ) if self.state.current_directive: self.lifecycle.activate_directive_inputs() @@ -497,7 +497,7 @@ def _svc_get_directives(self, request, response): "display_name": directive.display_name, "display_icon": directive.display_icon_data, "prompt": directive.get_prompt(), - "skills": directive.get_skills(), + "skills": directive.skill_ids(), "source": getattr(directive, "source", "user"), } ) diff --git a/ros2_ws/src/brain/brain_client/brain_client/nodes/input_manager.py b/ros2_ws/src/brain/brain_client/brain_client/nodes/input_manager.py index c219ec03d..cd9bb8621 100755 --- a/ros2_ws/src/brain/brain_client/brain_client/nodes/input_manager.py +++ b/ros2_ws/src/brain/brain_client/brain_client/nodes/input_manager.py @@ -11,7 +11,6 @@ from __future__ import annotations import rclpy -from innate_proxy import ProxyClient from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy, QoSProfile from std_msgs.msg import Bool, String @@ -19,6 +18,7 @@ from brain_client.common.logging import UniversalLogger from brain_client.inputs.manager import InputDeviceManager +from innate_proxy import ProxyClient class InputManagerNode(Node): diff --git a/ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py b/ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py index e80004305..c57d9eddd 100755 --- a/ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py +++ b/ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py @@ -3,30 +3,22 @@ # Copyright (c) 2026 Innate Inc """Skills action server: executes skills dispatched as ExecuteSkill goals. -Focused on the action/execution flow — code-skill execution, physical-skill -delegation to behavior_server (incl. cancellation + CLI worker), and the goal -lifecycle. Skill discovery/metadata/publishing/reload lives in -:mod:`brain_client.skills.catalog`; live robot-state injection lives in -:mod:`brain_client.skills.robot_state`. This node wires those together and owns -the action server. +Discovery/reload lives in skills.catalog; robot-state injection in +skills.robot_state. This node wires those together and owns the action server. """ from __future__ import annotations -import inspect import json import os -import queue import threading import time import traceback import uuid -from typing import get_args import rclpy from brain_messages.action import ExecuteBehavior, ExecuteSkill from brain_messages.srv import CreatePhysicalSkill, DeleteSkill, ReloadSkillsAgents, SaveAsReplaySkill -from innate.skills import SkillCancelled, SkillFailed, use_invoker from rclpy.action import ActionClient, ActionServer, CancelResponse, GoalResponse from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.executors import ExternalShutdownException, MultiThreadedExecutor @@ -35,45 +27,39 @@ from std_srvs.srv import Trigger from brain_client.perception.camera_provider import CameraProvider -from brain_client.robot.head import HeadInterface -from brain_client.robot.manipulation import ManipulationInterface -from brain_client.robot.mobility import MobilityInterface +from brain_client.robot.head import Head +from brain_client.robot.manipulation import Manipulation +from brain_client.robot.mobility import Mobility from brain_client.skills.catalog import SkillRepository from brain_client.skills.cli_bridge import SkillCliBridge, SkillCliGoalHandle from brain_client.skills.invoker import SkillInvoker from brain_client.skills.robot_state import RobotStateProvider -from brain_client.skills.types import RobotStateType, SkillResult, normalize_skill_result - - -def _annotation_is_float(annotation) -> bool: - """True if a param annotation is ``float`` (or a union including it). - - Handles both real type objects and string annotations (skills using - ``from __future__ import annotations`` expose ``"float"`` instead). +from brain_client.skills.types import ( + RobotStateType, + SkillCancelled, + SkillFailed, + SkillOutput, + SkillResult, + normalize_skill_result, + swap_run_cancel, +) + +# result status -> (goal-handle finalize method, ExecuteSkill success flag) +_FINALIZE = { + SkillResult.SUCCESS: ("succeed", True), + SkillResult.CANCELLED: ("succeed", True), + SkillResult.FAILURE: ("abort", False), +} + + +def _coerce_numeric_inputs(entry, inputs: dict) -> dict: + """Widen whole-number ints to floats for float-annotated params. + + JSON has one number type and ROS float64 setters reject ints. """ - if annotation is float or annotation == "float": - return True - return any(arg is float or arg == "float" for arg in get_args(annotation)) - - -def _coerce_numeric_inputs(skill, inputs: dict) -> dict: - """Widen whole-number ints to floats for ``float``-annotated params. - - JSON has a single number type, so a UI/agent serializing a float param - whose value happens to be whole (e.g. ``x=3``) sends an int. ROS - ``float64`` setters reject ints (``must be of type 'float'``), so we match - each value to its declared ``execute()`` annotation before dispatch. bools - are left alone (``bool`` subclasses ``int``).""" - try: - signature = inspect.signature(skill.execute) - except (TypeError, ValueError): - return inputs coerced = dict(inputs) for name, value in inputs.items(): - if not isinstance(value, int) or isinstance(value, bool): - continue - param = signature.parameters.get(name) - if param is not None and _annotation_is_float(param.annotation): + if isinstance(value, int) and not isinstance(value, bool) and name in entry.float_params: coerced[name] = float(value) return coerced @@ -85,7 +71,6 @@ class SkillsActionServer(Node): def __init__(self): super().__init__("skills_action_server") - # Camera images handled by a dedicated lightweight node (own thread) self._camera_node = CameraProvider() self.declare_parameter("cmd_vel_topic", "/cmd_vel") @@ -95,12 +80,10 @@ def __init__(self): self.declare_parameter("head_current_position_topic", "/mars/head/current_position") self.head_current_position_topic = self.get_parameter("head_current_position_topic").value - # Robot interfaces injected into skills. - self.manipulation = ManipulationInterface(self, self.get_logger(), lazy=True) - self.mobility = MobilityInterface(self, self.get_logger(), self.cmd_vel_topic) - self.head = HeadInterface(self, self.get_logger(), self.head_position_topic) + self.manipulation = Manipulation(self, self.get_logger(), lazy=True) + self.mobility = Mobility(self, self.get_logger(), self.cmd_vel_topic) + self.head = Head(self, self.get_logger(), self.head_position_topic) - # Robot-state provider (subscriptions, interface injection, live state). self.robot_state = RobotStateProvider( self, self._camera_node, @@ -110,50 +93,29 @@ def __init__(self): head_current_position_topic=self.head_current_position_topic, ) - # The robot runs one skill at a time. Guards execute_callback so a goal - # that arrives while another skill is still executing is aborted promptly - # (see execute_callback) instead of contending for the arm/robot state. - # The condition variable lets one incoming goal wait briefly for a - # cancelling skill to finish tearing down (Stop→Run grace window) - # instead of being rejected during the handover. - # Also gates disposal of reload-retired skill instances: destroying a - # retired instance's ROS entities mid-run would crash the skill that is - # still spinning them, so disposal waits until execution ends. + # One skill at a time; Condition lets Stop→Run wait out teardown briefly. self._skill_execution_lock = threading.Lock() self._skill_free = threading.Condition(self._skill_execution_lock) self._skill_running = False self._active_goal_handle = None self._teardown_waiter = False - self._pending_retired_skills = [] + self._active_code_skill = None + # Cancel that arrived before the run finished wiring up. + self._pending_cancel_goal = None - # Skill catalog (discovery, metadata, publishing, reload). - self.catalog = SkillRepository( - self, - interface_injector=self.robot_state.inject_required_interfaces, - retire_instances=self._retire_skill_instances, - ) + self.catalog = SkillRepository(self) - # Broadcasts every run's lifecycle (running/completed/failed/interrupted) so any - # client — webapp, mobile app, another webapp tab — can see a skill someone else - # triggered, not just the one that sent the goal. Payload mirrors ChatManager's - # (webapp consumers read either), keyed by a fresh id per run so repeats of the - # same skill don't collapse into one chat entry. self._skill_status_pub = self.create_publisher(String, "/brain/skill_status_update", 10) - # Behavior delegation for physical skills. + # At most one behavior goal in flight; owner is the skill goal handle. self._behavior_client = ActionClient(self, ExecuteBehavior, "/behavior/execute") - self._behavior_goal_lock = threading.Lock() - self._behavior_goal_handles = {} - self._behavior_goal_cancel_requested = set() - self._behavior_goal_cancel_sent = set() - - # ReentrantCallbackGroup so a cancel request can be serviced *while* a - # skill's execute_callback is blocked waiting on the behavior result. - # With the default MutuallyExclusiveCallbackGroup the cancel callback is - # skipped until execute returns, so a running physical/policy skill can't - # be interrupted (the cancel never reaches behavior_server). The awaited - # result future still completes inside execute's nested spin; only the - # separate cancel service callback was being blocked by the group. + self._behavior_lock = threading.Lock() + self._behavior_owner = None + self._behavior_handle = None + self._behavior_cancel_requested = False + self._behavior_cancel_sent = False + + # Reentrant so cancels are serviced while execute_callback blocks. self._action_server = ActionServer( self, ExecuteSkill, @@ -164,14 +126,8 @@ def __init__(self): callback_group=ReentrantCallbackGroup(), ) - # CLI skill worker (runs CLI-submitted skills off the main spin thread). - self._cli_skill_tasks = queue.Queue() - self._cli_skill_worker_stop = threading.Event() - self._cli_skill_worker = threading.Thread(target=self._run_cli_skill_worker, daemon=True) - self._cli_skill_worker.start() self._skill_cli_bridge = SkillCliBridge(self.get_logger(), self._submit_cli_skill) - # Services delegate to the catalog. self._reload_srv = self.create_service(Trigger, "/brain/reload_primitives", self._handle_reload_skills) self._create_physical_skill_srv = self.create_service( CreatePhysicalSkill, "/brain/create_physical_skill", self._handle_create_physical_skill @@ -190,27 +146,9 @@ def __init__(self): self.catalog.publish_skills_list() self.catalog.start_watcher() - # Heartbeat: the roster is published once (latched), but the webapp - # reaches /brain/available_skills through rws, which subscribes after - # boot and never receives the latched sample. Re-emit the cached roster - # at a low rate so late-joining clients get it within one interval. + # rws may miss the latched roster at boot; re-emit periodically. self._skills_heartbeat_timer = self.create_timer(3.0, self.catalog.republish_cached) - # ================= retired skill instances ================= - def _retire_skill_instances(self, instances): - """Dispose skill instances a reload replaced, deferring while a skill runs. - - The running skill may itself be a retired instance (reload mid-run), and - its execute() is still spinning the ROS entities it owns — so disposal - waits for the run to end (drained in execute_callback's finally). - """ - with self._skill_execution_lock: - if self._skill_running: - self._pending_retired_skills.extend(instances) - return - SkillRepository.dispose_instances(instances, self.get_logger()) - - # ================= service handlers (delegate to catalog) ================= def _handle_reload_skills(self, request, response): try: self.catalog.reload_all() @@ -253,25 +191,17 @@ def _handle_delete_skill(self, request, response): return response def _handle_cancel_skill(self, request, response): - """Cancel the currently running skill, whoever started it. - - The action protocol only lets the goal's sender cancel; a client that - merely sees the run on /brain/skill_status_update (the mobile app, - another webapp tab) holds no goal handle — this service is its Stop. - """ + """Cancel the currently running skill (for clients without a goal handle).""" with self._skill_execution_lock: goal_handle = self._active_goal_handle if self._skill_running else None - if goal_handle is None: - response.success = False - response.message = "No skill is running" - return response - skill_type = goal_handle.request.skill_type - # Dispatch under the lock: released, the slot could be freed and - # re-claimed by a new run of the same skill, and the cancel would - # hit that fresh execution instead. - self.cancel_callback(goal_handle) + if goal_handle is None: + response.success = False + response.message = "No skill is running" + return response + # Outside the lock: on_cancel hooks must not deadlock the slot. + self.cancel_callback(goal_handle) response.success = True - response.message = f"Cancellation requested for '{skill_type}'" + response.message = f"Cancellation requested for '{goal_handle.request.skill_type}'" return response def _handle_reload_skills_agents(self, request, response): @@ -289,7 +219,6 @@ def _handle_reload_skills_agents(self, request, response): response.reloaded_agents = [] return response - # ================= action lifecycle ================= def goal_callback(self, goal_request): self.get_logger().debug(f"Received goal for skill: '{goal_request.skill_type}'") return GoalResponse.ACCEPT @@ -297,100 +226,90 @@ def goal_callback(self, goal_request): def cancel_callback(self, goal_handle): try: skill_type = goal_handle.request.skill_type - code_entry = self.catalog.get_code_skill(skill_type) - is_physical = self.catalog.get_physical_skill(skill_type) is not None - - if code_entry is not None: - _name, instance = code_entry + with self._skill_execution_lock: + live = goal_handle is self._active_goal_handle + if live: + # Latch so cancels that land before wiring still take effect. + self._pending_cancel_goal = goal_handle + skill = self._active_code_skill if live else None + # Code path first — same resolution order as execute_callback. + if skill is not None: self.get_logger().debug(f"Canceling code skill: {skill_type}") - instance.cancel() - elif is_physical: + skill.cancel() + elif self.catalog.get_physical_skill(skill_type) is not None: self.get_logger().debug(f"Canceling physical skill: {skill_type}") self._request_behavior_goal_cancel(goal_handle, skill_type) else: - self.get_logger().warning(f"Unknown skill type: {skill_type}") - except Exception as e: + self.get_logger().debug(f"No live run to cancel for '{skill_type}'") + except (Exception, SkillCancelled) as e: + # SkillCancelled is a BaseException; still return ACCEPT. self.get_logger().error(f"Error in cancel_callback: {str(e)}") - self.get_logger().debug("Attempting to cancel all code skills") - for sid, (_name, instance) in self.catalog.all_code_skills(): - try: - instance.cancel() - except Exception as cancel_error: - self.get_logger().error(f"Error canceling {sid}: {str(cancel_error)}") return CancelResponse.ACCEPT + def _abort_result(self, goal_handle, skill_type: str, message: str): + goal_handle.abort() + return ExecuteSkill.Result( + success=False, message=message, skill_type=skill_type, success_type=SkillResult.FAILURE.value + ) + def execute_callback(self, goal_handle): - self.get_logger().debug(f"[SAS] execute_callback ENTER for skill: '{goal_handle.request.skill_type}'") + skill_type = goal_handle.request.skill_type + self.get_logger().debug(f"[SAS] execute_callback ENTER for skill: '{skill_type}'") try: inputs = json.loads(goal_handle.request.inputs) except Exception as e: self.get_logger().error(f"Invalid JSON for inputs: {str(e)}") - goal_handle.abort() - return ExecuteSkill.Result( - success=False, message="Invalid inputs JSON", success_type=SkillResult.FAILURE.value - ) + return self._abort_result(goal_handle, skill_type, "Invalid inputs JSON") - skill_type = goal_handle.request.skill_type + # Resolve once so a mid-run reload cannot swap the entry out. + entry = self.catalog.get_code_skill(skill_type) + physical = None if entry is not None else self.catalog.get_physical_skill(skill_type) - # No skill-status broadcast for a refused goal — it never ran. if not self._claim_skill_slot(goal_handle): self.get_logger().warn(f"Skill '{skill_type}' requested but another skill is already running") - goal_handle.abort() - return ExecuteSkill.Result( - success=False, - message="Another skill is already running", - skill_type=skill_type, - success_type=SkillResult.FAILURE.value, - ) - - name = self._skill_display_name(skill_type) + return self._abort_result(goal_handle, skill_type, "Another skill is already running") + + if entry is not None: + name = entry.display_name + elif physical is not None: + name = physical.metadata.get("name", skill_type) + else: + name = skill_type run_id = uuid.uuid4().hex self._publish_skill_status(run_id, skill_type, name, "running") try: - if self.catalog.get_code_skill(skill_type) is not None: - result = self._execute_code_skill(goal_handle, skill_type, inputs) - elif self.catalog.get_physical_skill(skill_type) is not None: - result = self._execute_physical_skill(goal_handle, skill_type, inputs) + if entry is not None: + result = self._execute_code_skill(goal_handle, skill_type, inputs, entry) + elif physical is not None: + result = self._execute_physical_skill(goal_handle, skill_type, physical) else: - self.get_logger().error(f"Skill '{skill_type}' not available") - self.get_logger().error(f"Available skills: {self.catalog.all_skill_ids()}") - goal_handle.abort() - result = ExecuteSkill.Result( - success=False, - message="Skill not available", - skill_type=skill_type, - success_type=SkillResult.FAILURE.value, - ) + reason = self.catalog.unavailable_reason(skill_type) + message = f"Skill '{skill_type}' {reason}" if reason else "Skill not available" + self.get_logger().error(f"Skill '{skill_type}' not available: {self.catalog.all_skill_ids()}") + result = self._abort_result(goal_handle, skill_type, message) + # Terminal status goes out BEFORE the slot is released: a queued + # goal claims the slot the moment it frees, and its "running" + # overtaking this run's terminal status would make latest-message + # consumers (the web app's external-run banner) clear the banner + # for the run that just started. + status, reason = self._terminal_skill_status(result) + self._publish_skill_status(run_id, skill_type, name, status, reason) except Exception as e: - # A 'running' broadcast went out above — a terminal status MUST - # follow or every client shows this skill as active forever. + # Clients stick on "running" forever without a terminal status. self._publish_skill_status(run_id, skill_type, name, "failed", str(e) or "internal error") raise finally: self._release_skill_slot() - status, reason = self._terminal_skill_status(result) - self._publish_skill_status(run_id, skill_type, name, status, reason) return result @staticmethod def _publish_initial_feedback(goal_handle): - """Emit one "running" feedback the moment execution starts: rws only - relays its assigned goal_id — which an app cancel must bind to — on the - first feedback, and skills may produce none of their own for a while.""" + """Emit early "running" feedback so clients can bind a cancel to goal_id.""" feedback = ExecuteSkill.Feedback() feedback.feedback = "running" feedback.image_b64 = "" goal_handle.publish_feedback(feedback) - def _skill_display_name(self, skill_type: str) -> str: - code_entry = self.catalog.get_code_skill(skill_type) - if code_entry is not None: - return code_entry[0] - physical = self.catalog.get_physical_skill(skill_type) - if physical is not None: - return physical.get("metadata", {}).get("name", skill_type) - return skill_type - @staticmethod def _terminal_skill_status(result) -> tuple[str, str | None]: if result.success and result.success_type == SkillResult.SUCCESS.value: @@ -414,20 +333,50 @@ def _publish_skill_status( payload["reason"] = reason self._skill_status_pub.publish(String(data=json.dumps(payload))) - # ================= execution ================= - def _execute_code_skill(self, goal_handle, skill_type, inputs): - entry = self.catalog.get_code_skill(skill_type) - if entry is None: - self.get_logger().error(f"Code skill '{skill_type}' disappeared during reload") - goal_handle.abort() - return ExecuteSkill.Result( - success=False, - message=f"Skill '{skill_type}' was removed during a concurrent reload", - skill_type=skill_type, - success_type=SkillResult.FAILURE.value, - ) - _name, skill = entry + def _create_run_node(self): + """Throwaway node for one run — destroyed whole at end (avoids #497).""" + run_node = Node( + f"skill_run_{uuid.uuid4().hex[:12]}", + context=self.context, + enable_rosout=False, + start_parameter_services=False, + ) + if self.executor is not None: + self.executor.add_node(run_node) + return run_node + + def _destroy_run_node(self, run_node) -> None: + try: + if self.executor is not None: + self.executor.remove_node(run_node) + run_node.destroy_node() + except Exception as e: + self.get_logger().error(f"Error destroying run node: {e}") + + def _instantiate_for_run(self, entry, run_node, invoker, publish_feedback): + """Fresh, fully wired instance for one run; caller owns disposal.""" + def wire(skill_class): + skill = skill_class(self.get_logger()) + skill.node = run_node + self.robot_state.inject_required_interfaces(skill) + skill.set_feedback_callback(publish_feedback) + skill.skills = invoker + return skill + + skill = wire(entry.skill_class) + skill.source = entry.source + skill.wire_subskills(wire) + return skill + + def _dispose_run_instance(self, skill) -> None: + try: + skill.shutdown() + except (Exception, SkillCancelled) as e: + # SkillCancelled is a BaseException; must not escape the caller's finally. + self.get_logger().error(f"Error shutting down {type(skill).__name__} run instance: {e}") + + def _execute_code_skill(self, goal_handle, skill_type, inputs, entry): def _publish_feedback(update_message: str, image_b64: str = None): feedback_msg = ExecuteSkill.Feedback() feedback_msg.feedback = update_message @@ -435,188 +384,176 @@ def _publish_feedback(update_message: str, image_b64: str = None): goal_handle.publish_feedback(feedback_msg) self.get_logger().debug(f"Published feedback for '{skill_type}': {update_message}") - skill.set_feedback_callback(_publish_feedback) - skill.skills = SkillInvoker(self, goal_handle, _publish_feedback) + run_node = None + try: + run_node = self._create_run_node() + invoker = SkillInvoker(self, goal_handle, _publish_feedback, run_node) + skill = self._instantiate_for_run(entry, run_node, invoker, _publish_feedback) + except Exception as e: + if run_node is not None: + self._destroy_run_node(run_node) + self.get_logger().error(f"Error constructing skill '{skill_type}': {e}") + return self._abort_result(goal_handle, skill_type, f"Skill construction failed: {e}") + with self._skill_execution_lock: + self._active_code_skill = skill + cancel_raced_start = self._pending_cancel_goal is goal_handle + if cancel_raced_start: + skill.cancel() try: self._publish_initial_feedback(goal_handle) self.robot_state.start_subscriptions() - result_message, result_status = self._run_code_skill_body(skill, skill_type, inputs, goal_handle) - - if result_status == SkillResult.SUCCESS: - self.get_logger().info(f"Skill '{skill_type}' succeeded: {result_message}") - goal_handle.succeed() - return ExecuteSkill.Result( - success=True, message=result_message, skill_type=skill_type, success_type=SkillResult.SUCCESS.value - ) - elif result_status == SkillResult.CANCELLED: - self.get_logger().info(f"Skill '{skill_type}' cancelled: {result_message}") - goal_handle.succeed() - return ExecuteSkill.Result( - success=True, - message=result_message, - skill_type=skill_type, - success_type=SkillResult.CANCELLED.value, - ) - else: # SkillResult.FAILURE - self.get_logger().info(f"Skill '{skill_type}' failed: {result_message}") - goal_handle.abort() - return ExecuteSkill.Result( - success=False, message=result_message, skill_type=skill_type, success_type=SkillResult.FAILURE.value - ) - except Exception as e: - self.get_logger().error(f"Error executing skill: {str(e)}") - goal_handle.abort() + output = self._run_code_skill_body(skill, entry, skill_type, inputs, goal_handle) + self.get_logger().info(f"Skill '{skill_type}' {output.status.value}: {output.message}") + finalize, success = _FINALIZE[output.status] + getattr(goal_handle, finalize)() return ExecuteSkill.Result( - success=False, message=str(e), skill_type=skill_type, success_type=SkillResult.FAILURE.value + success=success, message=output.message, skill_type=skill_type, success_type=output.status.value ) + except Exception as e: + self.get_logger().error(f"Error executing skill: {str(e)}") + return self._abort_result(goal_handle, skill_type, str(e)) finally: + with self._skill_execution_lock: + self._active_code_skill = None + # Motion never outlives a run; dispose while interfaces are live — + # teardowns may command hardware. + skill._halt_interfaces() + self._dispose_run_instance(skill) self.robot_state.stop_subscriptions() + self._destroy_run_node(run_node) - def _run_code_skill_body(self, skill, skill_type, inputs, goal_handle): - """Prepare robot state for a code skill and run its ``execute()``. + def _run_code_skill_body(self, skill, entry, skill_type, inputs, goal_handle) -> SkillOutput: + """Prepare robot state and run execute(); returns the SkillOutput. - Returns ``(message, SkillResult)``; goal finalization and subscriptions - stay with the caller (the top-level goal owns them, so they stay up - while a chaining skill runs its children). Nesting-safe: the 50 Hz - state slot suspends/resumes (see RobotStateProvider) and the camera is - refcounted. + Binds this skill's cancel latch as the process-wide run latch for the + duration (save/restore, so a chained child rebinds and its parent's + latch comes back): interface helpers block on it via cancellable_sleep + and unwind on cancel without any plumbing from the skill. """ + previous_run_cancel = swap_run_cancel(skill._cancel_latch()) + try: + return self._run_code_skill_prepared(skill, entry, skill_type, inputs, goal_handle) + finally: + swap_run_cancel(previous_run_cancel) + + def _run_code_skill_prepared(self, skill, entry, skill_type, inputs, goal_handle) -> SkillOutput: skill._begin_run(goal_handle) - if skill._cancelled: + if skill.cancelled: self.get_logger().info(f"Skill '{skill_type}' cancelled before it started") - return "Skill cancelled before it started", SkillResult.CANCELLED - - required_states = skill.get_required_robot_states() - needs_camera = required_states and ( - RobotStateType.LAST_MAIN_CAMERA_IMAGE_B64 in required_states - or RobotStateType.LAST_WRIST_CAMERA_IMAGE_B64 in required_states - ) + return SkillOutput("Skill cancelled before it started", status=SkillResult.CANCELLED) + + missing = skill.missing_required_interfaces() + if missing: + names = "/".join(m.capitalize() for m in missing) + plural = "s" if len(missing) > 1 else "" + return SkillOutput(f"{names} interface{plural} not available", status=SkillResult.FAILURE) + + declared_states = skill.declared_robot_state_types() + camera_feeds = { + feed + for state_type, feed in ( + (RobotStateType.LAST_MAIN_CAMERA_IMAGE_B64, "main"), + (RobotStateType.LAST_WRIST_CAMERA_IMAGE_B64, "wrist"), + (RobotStateType.LAST_DEPTH_IMAGE, "depth"), + ) + if state_type in declared_states + } + began_continuous_updates = False try: - if needs_camera: - self._camera_node.start() - # singleton instances keep state from previous runs; drop it so a - # skill never mistakes a stale value for fresh sensor data - skill.clear_robot_state() - self.robot_state.update_skill_robot_state(skill) - if needs_camera: - # The camera subscription above is brand new; wait (bounded) - # for first frames so execute() doesn't race them and fail. - self.robot_state.wait_for_camera_states(skill, required_states) - if required_states: + if camera_feeds: + self._camera_node.start(camera_feeds) + self.robot_state.wait_for_required_states(skill) + if skill.cancelled: + return SkillOutput("Skill cancelled before it started", status=SkillResult.CANCELLED) + missing = skill.missing_required_robot_states() + if missing: + return SkillOutput(f"No data from the {' / '.join(missing)}", status=SkillResult.FAILURE) + if declared_states: self.robot_state.begin_continuous_updates(skill) + began_continuous_updates = True self.get_logger().info(f"Started continuous state updates for '{skill_type}' at 50Hz") - # innate.skills proxies route to this skill's invoker while - # execute() runs - with use_invoker(skill.skills): - return normalize_skill_result(skill.execute(**_coerce_numeric_inputs(skill, inputs))) + return normalize_skill_result( + skill.execute(**_coerce_numeric_inputs(entry, inputs)), skill.name, logger=skill.logger + ) except SkillCancelled as e: - return str(e) or "Skill cancelled", SkillResult.CANCELLED + return SkillOutput(str(e) or "Skill cancelled", status=SkillResult.CANCELLED) except SkillFailed as e: - return str(e) or "Skill failed", SkillResult.FAILURE + return SkillOutput(str(e) or "Skill failed", status=SkillResult.FAILURE) finally: - if required_states: + if began_continuous_updates: self.robot_state.end_continuous_updates() - if needs_camera: - self._camera_node.stop() + if camera_feeds: + self._camera_node.stop(camera_feeds) - def _execute_physical_skill(self, goal_handle, skill_type, inputs): + def _execute_physical_skill(self, goal_handle, skill_type, physical_data): self.get_logger().info(f"Delegating physical skill '{skill_type}' to behavior_server") - physical_data = self.catalog.get_physical_skill(skill_type) - if physical_data is None: - self.get_logger().error(f"Physical skill '{skill_type}' disappeared during reload") - goal_handle.abort() - return ExecuteSkill.Result( - success=False, - message=f"Skill '{skill_type}' was removed during a concurrent reload", - skill_type=skill_type, - success_type=SkillResult.FAILURE.value, - ) self.robot_state.start_subscriptions() try: - success, message, success_type, finalize = self._run_physical_skill(goal_handle, skill_type, physical_data) + output = self._run_physical_skill(goal_handle, skill_type, physical_data) + finalize, success = _FINALIZE[output.status] getattr(goal_handle, finalize)() return ExecuteSkill.Result( - success=success, message=message, skill_type=skill_type, success_type=success_type + success=success, message=output.message, skill_type=skill_type, success_type=output.status.value ) finally: self.robot_state.stop_subscriptions() - def _run_physical_skill(self, goal_handle, skill_type, physical_data): - """Send a physical skill to behavior_server and wait for its result. - - Returns ``(success, message, success_type, finalize)`` where ``finalize`` - is the goal-handle method the caller should invoke ("succeed", "abort" - or "canceled"). Does not finalize the goal, so a chaining skill can run - a physical child on its own goal without ending the parent. - """ - metadata = physical_data["metadata"] + def _run_physical_skill(self, goal_handle, skill_type, physical_data) -> SkillOutput: + """Delegate to behavior_server; returns the SkillOutput.""" + metadata = physical_data.metadata + self._begin_behavior_goal(goal_handle) try: self._publish_initial_feedback(goal_handle) if not self._behavior_client.wait_for_server(timeout_sec=5.0): self.get_logger().error("Behavior server not available!") - return False, "Behavior server not available", SkillResult.FAILURE.value, "abort" + return SkillOutput("Behavior server not available", status=SkillResult.FAILURE) behavior_goal = ExecuteBehavior.Goal() - behavior_goal.skill_dir = physical_data["directory"] + behavior_goal.skill_dir = physical_data.directory behavior_goal.behavior_config = json.dumps(metadata) self.get_logger().info(f"Sending behavior goal to behavior_server: {skill_type}") send_goal_future = self._behavior_client.send_goal_async(behavior_goal) - # Wait on the behavior goal via the executor (event-based) instead of - # re-spinning this node. The dedicated MultiThreadedExecutor services the - # behavior response on another thread while we block here; re-spinning - # required rclpy's global executor and raced Nav2's own global spins. - goal_ready = self._wait_for_cli_future(send_goal_future, timeout_sec=10.0) == "done" + goal_ready = self._wait_for_future(send_goal_future, timeout_sec=10.0) == "done" if not goal_ready: self.get_logger().error("Timeout waiting for behavior goal acceptance") self._cancel_behavior_goal_when_ready(send_goal_future, skill_type) - return False, "Timeout waiting for behavior goal acceptance", SkillResult.FAILURE.value, "abort" + return SkillOutput("Timeout waiting for behavior goal acceptance", status=SkillResult.FAILURE) behavior_goal_handle = send_goal_future.result() if not behavior_goal_handle.accepted: self.get_logger().error("Behavior goal rejected by behavior_server") - return False, "Behavior goal rejected by behavior_server", SkillResult.FAILURE.value, "abort" + return SkillOutput("Behavior goal rejected by behavior_server", status=SkillResult.FAILURE) - self._register_behavior_goal_handle(goal_handle, behavior_goal_handle, skill_type) + self._attach_behavior_goal(goal_handle, behavior_goal_handle, skill_type) self.get_logger().info("Behavior goal accepted, waiting for result...") result_future = behavior_goal_handle.get_result_async() - result_wait_state = self._wait_for_cli_future(result_future, server_ready_check=self._behavior_server_ready) + result_wait_state = self._wait_for_future(result_future, server_ready_check=self._behavior_server_ready) if result_wait_state == "server_unavailable": self.get_logger().error(f"Behavior server became unavailable while running '{skill_type}'") - return ( - False, - "Behavior server became unavailable while waiting for result", - SkillResult.FAILURE.value, - "abort", + return SkillOutput( + "Behavior server became unavailable while waiting for result", status=SkillResult.FAILURE ) - if not result_future.done(): - self.get_logger().info(f"Physical skill '{skill_type}' cancelled before behavior result was ready") - return True, "Physical skill cancelled", SkillResult.CANCELLED.value, "canceled" - behavior_result = result_future.result().result if behavior_result.success: self.get_logger().info(f"Physical skill '{skill_type}' succeeded: {behavior_result.message}") - return True, behavior_result.message, SkillResult.SUCCESS.value, "succeed" + return SkillOutput(behavior_result.message) if "cancel" in behavior_result.message.lower(): self.get_logger().info(f"Physical skill '{skill_type}' cancelled: {behavior_result.message}") - return True, behavior_result.message, SkillResult.CANCELLED.value, "succeed" + return SkillOutput(behavior_result.message, status=SkillResult.CANCELLED) self.get_logger().error(f"Physical skill '{skill_type}' failed: {behavior_result.message}") - return False, behavior_result.message, SkillResult.FAILURE.value, "abort" + return SkillOutput(behavior_result.message, status=SkillResult.FAILURE) except Exception as e: self.get_logger().error(f"Unexpected error executing physical skill '{skill_type}': {e}") - return False, f"Unexpected error executing physical skill: {e}", SkillResult.FAILURE.value, "abort" + return SkillOutput(f"Unexpected error executing physical skill: {e}", status=SkillResult.FAILURE) finally: - self._unregister_behavior_goal_handle(goal_handle) - - # ================= behavior goal tracking ================= - def _skill_goal_key(self, goal_handle) -> int: - return id(goal_handle) + self._end_behavior_goal(goal_handle) def _skill_goal_cancel_requested(self, goal_handle) -> bool: try: @@ -626,18 +563,13 @@ def _skill_goal_cancel_requested(self, goal_handle) -> bool: def _cancelling_teardown_in_progress(self) -> bool: """True when the running skill has a cancel in flight.""" - return self._active_goal_handle is not None and self._skill_goal_cancel_requested(self._active_goal_handle) + handle = self._active_goal_handle + if handle is None: + return False + return self._pending_cancel_goal is handle or self._skill_goal_cancel_requested(handle) def _claim_skill_slot(self, goal_handle) -> bool: - """Claim the one-skill-at-a-time slot; False means another skill kept it. - - A goal that arrives while another skill is executing is refused — the - caller aborts it so the app gets a prompt result (rejecting in - goal_callback instead would surface nothing back through the rws - bridge). One exception: rapid Stop→Run, where the previous skill is - mid-teardown from a cancel — that goal waits the teardown out instead - of failing with "already running". - """ + """Claim the one-skill-at-a-time slot; False if another skill kept it.""" with self._skill_free: if self._skill_running: self._await_cancelling_teardown() @@ -648,11 +580,7 @@ def _claim_skill_slot(self, goal_handle) -> bool: return True def _await_cancelling_teardown(self): - """Wait for a cancelling skill to release the slot. Call holding _skill_free. - - Only one goal waits; a pile-up keeps the prompt rejection. The first - short wait covers a Run that beat its preceding Stop into the server. - """ + """Wait for a cancelling skill to release the slot. Call holding _skill_free.""" if self._teardown_waiter: return self._teardown_waiter = True @@ -670,36 +598,49 @@ def _release_skill_slot(self): with self._skill_free: self._skill_running = False self._active_goal_handle = None - retired, self._pending_retired_skills = self._pending_retired_skills, [] + self._pending_cancel_goal = None self._skill_free.notify_all() - SkillRepository.dispose_instances(retired, self.get_logger()) - def _register_behavior_goal_handle(self, skill_goal_handle, behavior_goal_handle, skill_type: str) -> None: - key = self._skill_goal_key(skill_goal_handle) - with self._behavior_goal_lock: - self._behavior_goal_handles[key] = behavior_goal_handle - cancel_requested = key in self._behavior_goal_cancel_requested + def _begin_behavior_goal(self, skill_goal_handle) -> None: + """Claim the behavior slot for this skill goal, before the goal is sent.""" + with self._behavior_lock: + self._behavior_owner = skill_goal_handle + self._behavior_handle = None + self._behavior_cancel_requested = False + self._behavior_cancel_sent = False + with self._skill_execution_lock: + cancel_raced_start = self._pending_cancel_goal is skill_goal_handle + if cancel_raced_start: + self._request_behavior_goal_cancel(skill_goal_handle, skill_goal_handle.request.skill_type) + + def _attach_behavior_goal(self, skill_goal_handle, behavior_goal_handle, skill_type: str) -> None: + """Record the accepted behavior goal; re-dispatch a cancel that raced it.""" + with self._behavior_lock: + if self._behavior_owner is not skill_goal_handle: + return + self._behavior_handle = behavior_goal_handle + cancel_requested = self._behavior_cancel_requested if cancel_requested or self._skill_goal_cancel_requested(skill_goal_handle): self.get_logger().info(f"Cancel was already requested for physical skill '{skill_type}'") self._request_behavior_goal_cancel(skill_goal_handle, skill_type) - def _unregister_behavior_goal_handle(self, skill_goal_handle) -> None: - key = self._skill_goal_key(skill_goal_handle) - with self._behavior_goal_lock: - self._behavior_goal_handles.pop(key, None) - self._behavior_goal_cancel_requested.discard(key) - self._behavior_goal_cancel_sent.discard(key) + def _end_behavior_goal(self, skill_goal_handle) -> None: + with self._behavior_lock: + if self._behavior_owner is skill_goal_handle: + self._behavior_owner = None + self._behavior_handle = None + self._behavior_cancel_requested = False + self._behavior_cancel_sent = False def _request_behavior_goal_cancel(self, skill_goal_handle, skill_type: str) -> None: - key = self._skill_goal_key(skill_goal_handle) - with self._behavior_goal_lock: - self._behavior_goal_cancel_requested.add(key) - behavior_goal_handle = self._behavior_goal_handles.get(key) - if behavior_goal_handle is None: + with self._behavior_lock: + if self._behavior_owner is not skill_goal_handle: return - if key in self._behavior_goal_cancel_sent: + self._behavior_cancel_requested = True + behavior_goal_handle = self._behavior_handle + if behavior_goal_handle is None or self._behavior_cancel_sent: return - self._behavior_goal_cancel_sent.add(key) + self._behavior_cancel_sent = True try: self.get_logger().info(f"Requesting behavior_server cancel for physical skill '{skill_type}'") behavior_goal_handle.cancel_goal_async() @@ -718,50 +659,36 @@ def _cancel_when_ready(future): send_goal_future.add_done_callback(_cancel_when_ready) - # ================= CLI skill worker ================= def _submit_cli_skill(self, task): + # One thread per task; the skill slot already enforces one-at-a-time. goal_handle = SkillCliGoalHandle(task) task.set_cancel_handler(lambda: self.cancel_callback(goal_handle)) - self._cli_skill_tasks.put((task, goal_handle)) + threading.Thread(target=self._run_cli_skill, args=(task, goal_handle), daemon=True).start() - def _run_cli_skill_worker(self): - while not self._cli_skill_worker_stop.is_set(): - try: - item = self._cli_skill_tasks.get(timeout=0.2) - except queue.Empty: - continue - if item is None: - return - task, goal_handle = item - try: - task.mark_started() - if task.cancel_event.is_set(): - task.set_error("Skill execution was cancelled before start") - continue - try: - result = self.execute_callback(goal_handle) - except Exception as e: - self.get_logger().error(f"Unexpected error executing CLI skill '{task.skill_type}': {e}") - task.set_error(f"Skill execution failed: {e}") - continue - if result is None: - task.set_error("Skill execution returned no result") - else: - task.set_result(result) - finally: - self._unregister_behavior_goal_handle(goal_handle) + def _run_cli_skill(self, task, goal_handle): + task.mark_started() + if task.cancel_event.is_set(): + task.set_error("Skill execution was cancelled before start") + return + try: + result = self.execute_callback(goal_handle) + except Exception as e: + self.get_logger().error(f"Unexpected error executing CLI skill '{task.skill_type}': {e}") + task.set_error(f"Skill execution failed: {e}") + return + if result is None: + task.set_error("Skill execution returned no result") + else: + task.set_result(result) def _behavior_server_ready(self) -> bool: try: - checker = getattr(self._behavior_client, "server_is_ready", None) - if checker is not None: - return bool(checker()) - return bool(self._behavior_client.wait_for_server(timeout_sec=0.0)) + return bool(self._behavior_client.server_is_ready()) except Exception as e: self.get_logger().error(f"Could not check behavior_server readiness: {e}") return False - def _wait_for_cli_future(self, future, timeout_sec=None, server_ready_check=None): + def _wait_for_future(self, future, timeout_sec=None, server_ready_check=None): """Wait for a ROS future while the node executor spins in the main thread.""" if future.done(): return "done" @@ -780,14 +707,10 @@ def _wait_for_cli_future(self, future, timeout_sec=None, server_ready_check=None if server_ready_check is not None and not server_ready_check(): return "server_unavailable" - # ================= teardown ================= def destroy(self): self.catalog.stop_watcher() if hasattr(self, "_skill_cli_bridge"): self._skill_cli_bridge.stop() - self._cli_skill_worker_stop.set() - self._cli_skill_tasks.put(None) - self._cli_skill_worker.join(timeout=1.0) self.manipulation.shutdown() self._camera_node.shutdown() self._action_server.destroy() @@ -797,19 +720,8 @@ def destroy(self): def main(args=None): rclpy.init(args=args) action_server = SkillsActionServer() - # Spin on a dedicated executor instead of rclpy's global one. Skills that drive - # Nav2 (mobility.rotate, navigate_to_position) call BasicNavigator, whose blocking - # helpers spin the *global* executor; sharing it with this node lets those Nav2 - # action clients enter our wait set and corrupt it ("wait set index ... out of - # bounds" -> SIGABRT). A dedicated MultiThreadedExecutor isolates us and lets a - # blocked physical-skill execute() wait on behavior results serviced by another - # thread (see _wait_for_cli_future) without re-spinning this node. - # - # Floor the thread pool well above the max expected concurrent skill count. - # execute_callback runs on a pool thread and blocks in _wait_for_cli_future - # until another pool thread dispatches the behavior response it waits on; with - # only os.cpu_count() threads (4 on a Jetson), enough concurrent skills could - # occupy every thread and leave none to resolve their futures -> deadlock. + # Dedicated executor — never share rclpy's global one with Nav2 (SIGABRT risk). + # Floor threads well above CPU count so blocked callbacks can't deadlock futures. executor = MultiThreadedExecutor(num_threads=max(8, (os.cpu_count() or 4) + 4)) executor.add_node(action_server) try: @@ -817,14 +729,9 @@ def main(args=None): except (KeyboardInterrupt, ExternalShutdownException): pass except Exception: - # An exception escaping spin() (e.g. InvalidHandle from an entity - # destroyed while the executor was using it) must not unwind past the - # teardown below: exiting with live zenoh entities panics rmw_zenoh's - # Rust runtime (SIGABRT). Log it and exit through the ordered teardown; - # launch respawns us either way, but from a clean exit. + # Log and tear down cleanly — live zenoh entities on exit can SIGABRT. action_server.get_logger().fatal(f"Executor spin crashed:\n{traceback.format_exc()}") action_server.destroy() - # Guard against double-shutdown: avoids a teardown RCLError that exits 1. if rclpy.ok(): rclpy.shutdown() diff --git a/ros2_ws/src/brain/brain_client/brain_client/perception/camera_provider.py b/ros2_ws/src/brain/brain_client/brain_client/perception/camera_provider.py index edbf8a245..6399a7f86 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/perception/camera_provider.py +++ b/ros2_ws/src/brain/brain_client/brain_client/perception/camera_provider.py @@ -6,21 +6,23 @@ in its own spin thread, storing raw compressed bytes. Runs independently of the main executor so camera callbacks are never -starved by long-running action-server work. Base64 encoding is deferred -to property access so the callback stays as fast as possible. +starved by long-running action-server work. Callbacks store the raw +bytes; consumers (skills/robot_state.py) wrap them lazily. Subscriptions are created on-demand via start()/stop() so the node consumes zero CPU when no skill needs camera data. """ -import base64 import threading +import numpy as np import rclpy import rclpy.executors from rclpy.node import Node from rclpy.qos import QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy -from sensor_msgs.msg import CompressedImage +from sensor_msgs.msg import CompressedImage, Image + +from brain_client.perception.camera import _DEPTH_DTYPES class CameraProvider(Node): @@ -50,68 +52,138 @@ def __init__(self): self._main_camera_raw: bytes | None = None self._wrist_camera_raw: bytes | None = None + self._depth_msg: Image | None = None self._main_sub = None self._wrist_sub = None + self._depth_sub = None self._executor: rclpy.executors.SingleThreadedExecutor | None = None self._thread: threading.Thread | None = None self._running = False # start()/stop() are refcounted: a chained child that needs the camera # must not tear it down on exit while its parent still does self._users = 0 + # per-feed refcounts, so a feed only a nested child declared stops + # streaming when the child ends instead of for the parent's whole run + self._feed_users = {"main": 0, "wrist": 0, "depth": 0} # ---- lifecycle ---- - def start(self): - """Create subscriptions and begin spinning in a background thread.""" + def start(self, feeds=("main", "wrist", "depth")): + """Create subscriptions for ``feeds`` ("main"/"wrist"/"depth") and + begin spinning in a background thread. + + Only the requested feeds are subscribed — raw depth in particular is + ~600 KB/frame uncompressed, not worth streaming for a skill that never + declared it. A feed a nested caller adds while already running gets + its subscription on demand (creating on a spinning node is safe; + destroying is what races the executor).""" self._users += 1 + for feed in feeds: + if feed in self._feed_users: + self._feed_users[feed] += 1 + if "main" in feeds and self._main_sub is None: + self._main_sub = self.create_subscription( + CompressedImage, + "/mars/main_camera/left/image_raw/compressed", + self._main_camera_cb, + self._IMAGE_QOS, + ) + if "wrist" in feeds and self._wrist_sub is None: + self._wrist_sub = self.create_subscription( + CompressedImage, + "/mars/arm/image_raw/compressed", + self._wrist_camera_cb, + self._IMAGE_QOS, + ) + if "depth" in feeds and self._depth_sub is None: + self._depth_sub = self.create_subscription( + Image, + "/camera/depth/image_raw", + self._depth_cb, + self._IMAGE_QOS, + ) if self._running: return - self._main_sub = self.create_subscription( - CompressedImage, - "/mars/main_camera/left/image_raw/compressed", - self._main_camera_cb, - self._IMAGE_QOS, - ) - self._wrist_sub = self.create_subscription( - CompressedImage, - "/mars/arm/image_raw/compressed", - self._wrist_camera_cb, - self._IMAGE_QOS, - ) - self._executor = rclpy.executors.SingleThreadedExecutor() - self._executor.add_node(self) - self._thread = threading.Thread(target=self._spin, daemon=True) - self._thread.start() + self._start_spin() self._running = True self.get_logger().info("Camera subscriptions started") - def stop(self): - """Destroy subscriptions and stop the background thread. + def stop(self, feeds=("main", "wrist", "depth")): + """Release ``feeds`` and stop the background thread when unused. - Refcounted with start(): only the last outstanding user stops it. + Refcounted with start(), per feed and overall: only the last + outstanding user stops the node, but a feed only this caller needed + (a nested child's depth, say) is dropped right away rather than + streaming for the rest of an enclosing skill's run. """ if not self._running: return self._users = max(0, self._users - 1) + for feed in feeds: + if feed in self._feed_users: + self._feed_users[feed] = max(0, self._feed_users[feed] - 1) if self._users: + self._drop_unused_feeds() return - if self._executor is not None: - self._executor.shutdown() - if self._thread is not None: - self._thread.join(timeout=2.0) - self._thread = None - self._executor = None - for sub in (self._main_sub, self._wrist_sub): + self._stop_spin() + for sub in (self._main_sub, self._wrist_sub, self._depth_sub): if sub is not None: self.destroy_subscription(sub) self._main_sub = None self._wrist_sub = None + self._depth_sub = None self._main_camera_raw = None self._wrist_camera_raw = None + self._depth_msg = None + self._feed_users = dict.fromkeys(self._feed_users, 0) self._running = False self.get_logger().info("Camera subscriptions stopped") + def _start_spin(self): + self._executor = rclpy.executors.SingleThreadedExecutor() + self._executor.add_node(self) + self._thread = threading.Thread(target=self._spin, daemon=True) + self._thread.start() + + def _stop_spin(self): + if self._executor is not None: + self._executor.shutdown() + if self._thread is not None: + self._thread.join(timeout=2.0) + self._thread = None + self._executor = None + + def _drop_unused_feeds(self): + """Destroy subscriptions whose last user left while others remain. + + Destroying an entity under a spinning executor races it (#497), so + park the spin thread first, drop the dead subscriptions, and resume — + the surviving feeds miss at most one frame interval. + """ + dead = [ + feed + for feed, sub in (("main", self._main_sub), ("wrist", self._wrist_sub), ("depth", self._depth_sub)) + if sub is not None and not self._feed_users[feed] + ] + if not dead: + return + self._stop_spin() + if "main" in dead: + self.destroy_subscription(self._main_sub) + self._main_sub = None + self._main_camera_raw = None + if "wrist" in dead: + self.destroy_subscription(self._wrist_sub) + self._wrist_sub = None + self._wrist_camera_raw = None + if "depth" in dead: + self.destroy_subscription(self._depth_sub) + self._depth_sub = None + self._depth_msg = None + self._start_spin() + self.get_logger().info(f"Camera feeds dropped: {', '.join(dead)}") + # ---- callbacks (as cheap as possible) ---- def _spin(self): @@ -126,25 +198,53 @@ def _main_camera_cb(self, msg: CompressedImage): def _wrist_camera_cb(self, msg: CompressedImage): self._wrist_camera_raw = bytes(msg.data) - # ---- lazy base64 properties ---- + def _depth_cb(self, msg: Image): + self._depth_msg = msg + + # ---- frame properties ---- @property - def last_main_camera_b64(self) -> str | None: - """Return the latest main camera frame as a base64 string, or None.""" - raw = self._main_camera_raw - if raw is None: - return None - return base64.b64encode(raw).decode("utf-8") + def last_main_camera_jpeg(self) -> bytes | None: + """The latest main camera frame as raw JPEG bytes, or None.""" + return self._main_camera_raw @property - def last_wrist_camera_b64(self) -> str | None: - """Return the latest wrist camera frame as a base64 string, or None.""" - raw = self._wrist_camera_raw - if raw is None: + def last_wrist_camera_jpeg(self) -> bytes | None: + """The latest wrist camera frame as raw JPEG bytes, or None.""" + return self._wrist_camera_raw + + @property + def last_depth_image(self) -> "np.ndarray | None": + """Return the latest depth frame as a (height, width) numpy array, or + None. Dtype follows the sensor encoding (uint16 mm or float32 m); + frombuffer is a view, so this stays cheap on every read.""" + msg = self._depth_msg + if msg is None: + return None + dtype = _DEPTH_DTYPES.get(msg.encoding) + if dtype is None: + self.get_logger().warn(f"Unexpected depth encoding: {msg.encoding}") + return None + try: + return np.frombuffer(msg.data, dtype=dtype).reshape((msg.height, msg.width)) + except ValueError: + # Padded/truncated frame (data length ≠ height*width*itemsize). + # This property feeds the pre-run state wait and the 50 Hz update + # thread — a malformed frame must read as "no frame", not raise + # out of the skills server. + self.get_logger().warn( + f"Depth frame does not match {msg.height}x{msg.width} {msg.encoding} (len={len(msg.data)})" + ) return None - return base64.b64encode(raw).decode("utf-8") # ---- cleanup ---- def shutdown(self): + """Process teardown: force the full stop whatever the refcount. + + A run still in flight (or a leaked count) holds ``_users`` above 1; + the refcounted stop() would only decrement, leaving the spin thread + alive into rclpy.shutdown() — live entities there SIGABRT rmw_zenoh. + """ + self._users = min(self._users, 1) self.stop() diff --git a/ros2_ws/src/brain/brain_client/brain_client/perception/gaze.py b/ros2_ws/src/brain/brain_client/brain_client/perception/gaze.py index eca8d84ec..f868c1b39 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/perception/gaze.py +++ b/ros2_ws/src/brain/brain_client/brain_client/perception/gaze.py @@ -23,8 +23,8 @@ from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy from sensor_msgs.msg import Image -from brain_client.robot.head import HeadInterface -from brain_client.robot.mobility import MobilityInterface +from brain_client.robot.head import Head +from brain_client.robot.mobility import Mobility class FaceDetector: @@ -171,8 +171,8 @@ def __init__(self, node, camera_topic: str = "/mars/main_camera/left/image_raw") self._frame_lock = threading.Lock() # Hardware interfaces - self._head = HeadInterface(node, node.get_logger()) - self._mobility = MobilityInterface(node, node.get_logger(), "/cmd_vel") + self._head = Head(node, node.get_logger()) + self._mobility = Mobility(node, node.get_logger(), "/cmd_vel") # Gaze controller self._gaze = GazeController( diff --git a/ros2_ws/src/brain/brain_client/brain_client/robot/head.py b/ros2_ws/src/brain/brain_client/brain_client/robot/head.py index 91ad446bb..d27b1e718 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/robot/head.py +++ b/ros2_ws/src/brain/brain_client/brain_client/robot/head.py @@ -2,7 +2,7 @@ # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc """ -HeadInterface - Provides head tilt control capabilities to skills. +Head - Provides head tilt control capabilities to skills. This interface allows skills to: 1. Set the head tilt position @@ -15,10 +15,12 @@ from brain_client.common.logging import UniversalLogger -class HeadInterface: +class Head: """High-level interface for head (tilt) control. - Skills should use this instead of directly publishing to head topics. + Injected as ``self.head`` when a skill declares ``head: Head``; the + framework constructs it. Skills should use this instead of directly + publishing to head topics. """ def __init__(self, node: Node, logger, head_position_topic: str = "/mars/head/set_position"): @@ -29,7 +31,7 @@ def __init__(self, node: Node, logger, head_position_topic: str = "/mars/head/se # Publisher for head position commands self._head_position_pub = self.node.create_publisher(Int32, self.head_position_topic, 10) - self.logger.info(f"HeadInterface initialized with position topic: {self.head_position_topic}") + self.logger.info(f"Head initialized with position topic: {self.head_position_topic}") def set_position(self, angle_degrees: int) -> None: """Set the head tilt position. @@ -48,4 +50,4 @@ def set_position(self, angle_degrees: int) -> None: self._head_position_pub.publish(msg) - self.logger.debug(f"HeadInterface: set head position to {angle_degrees} degrees") + self.logger.debug(f"Head: set head position to {angle_degrees} degrees") diff --git a/ros2_ws/src/brain/brain_client/brain_client/robot/manipulation.py b/ros2_ws/src/brain/brain_client/brain_client/robot/manipulation.py index fd266dc9a..e517374b4 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/robot/manipulation.py +++ b/ros2_ws/src/brain/brain_client/brain_client/robot/manipulation.py @@ -2,7 +2,7 @@ # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc """ -ManipulationInterface - Provides IK and arm control capabilities to skills. +Manipulation - Provides IK and arm control capabilities to skills. This interface allows skills to: 1. Request inverse kinematics solutions for Cartesian poses @@ -10,6 +10,7 @@ 3. Get current end-effector pose (forward kinematics) """ +import math import threading import time @@ -18,17 +19,28 @@ from geometry_msgs.msg import PoseStamped, Twist from mars_msgs.srv import GotoJS, GotoJSTrajectory from rclpy.node import Node +from rclpy.subscription import Subscription from sensor_msgs.msg import JointState from std_msgs.msg import Float64MultiArray from std_srvs.srv import Trigger -class ManipulationInterface: +class ArmUnhealthy(RuntimeError): + """Servo brownout/refusal — abort, don't continue limp.""" + + +class ArmFailed(RuntimeError): + """Joint/cartesian command rejected or did not complete.""" + + +class Manipulation: """ Interface for arm manipulation using IK and joint control. - This class provides high-level methods for skills to control the arm - without needing to know the details of ROS topics and services. + Injected as ``self.manipulation`` when a skill declares + ``manipulation: Manipulation``; the framework constructs it. Provides + high-level methods for skills to control the arm without needing to know + the details of ROS topics and services. """ def __init__(self, node: Node, logger, lazy: bool = False): @@ -47,8 +59,8 @@ def __init__(self, node: Node, logger, lazy: bool = False): # always-alive subscriptions just rotate in the bounded rmw queues at # no Python cost — dispatching them through an executor costs ~half a # Jetson core at the ~600 msgs/s these feeds add up to. - self._executor = None - self._executor_thread = None + self._executor: rclpy.executors.SingleThreadedExecutor | None = None + self._executor_thread: threading.Thread | None = None self._lifecycle_lock = threading.Lock() self._ik_lock = threading.Lock() @@ -56,11 +68,11 @@ def __init__(self, node: Node, logger, lazy: bool = False): self._ik_target_pub = self.node.create_publisher(Twist, "/ik_delta", 10) # Cached state - self._ik_solution = None - self._ik_solution_fk = None - self._fk_pose = None - self._arm_state = None - self._torque_enabled = None + self._ik_solution: JointState | None = None + self._ik_solution_fk: PoseStamped | None = None + self._fk_pose: PoseStamped | None = None + self._arm_state: JointState | None = None + self._torque_enabled: bool | None = None # Subscription handles (created once in start(); kept for the node's # lifetime — destroying one while the private executor thread spins @@ -68,10 +80,10 @@ def __init__(self, node: Node, logger, lazy: bool = False): # stop() instead gates the callbacks via _active and parks the # executor. self._active = False - self._ik_solution_sub = None - self._ik_solution_fk_sub = None - self._fk_pose_sub = None - self._arm_state_sub = None + self._ik_solution_sub: Subscription | None = None + self._ik_solution_fk_sub: Subscription | None = None + self._fk_pose_sub: Subscription | None = None + self._arm_state_sub: Subscription | None = None if not lazy: self.start() @@ -87,13 +99,13 @@ def __init__(self, node: Node, logger, lazy: bool = False): self._torque_off_client = self.node.create_client(Trigger, "/mars/arm/torque_off") self._reboot_servos_client = self.node.create_client(Trigger, "/mars/arm/reboot") - self.logger.info("ManipulationInterface initialized") + self.logger.info("Manipulation initialized") def _spin_executor(self, executor): try: executor.spin() except Exception as e: - self.logger.error(f"[ManipulationInterface] Executor stopped unexpectedly: {e}") + self.logger.error(f"[Manipulation] Executor stopped unexpectedly: {e}") def start(self): """Enable arm-state feeds: spin up the private executor and create the @@ -131,7 +143,8 @@ def stop(self): self._active = False if self._executor is not None: self._executor.shutdown() - self._executor_thread.join(timeout=2.0) + if self._executor_thread is not None: + self._executor_thread.join(timeout=2.0) self._executor = None self._executor_thread = None self._ik_solution = None @@ -179,6 +192,12 @@ def _arm_state_callback(self, msg: JointState): if self._active: self._arm_state = msg + @property + def last_fk_pose(self) -> PoseStamped | None: + """The latest cached /fk_pose message, or None. No spin, no warning — + for high-rate ambient reads (RobotStateProvider.current_arm).""" + return self._fk_pose + def get_current_end_effector_pose(self) -> dict | None: """ Get the current end-effector pose in Cartesian space. @@ -287,17 +306,19 @@ def solve_ik( # Validate that we received a non-empty solution if len(joint_positions) == 0: - self.logger.error("[ManipulationInterface] IK solver returned empty solution (IK failed)") + self.logger.error("[Manipulation] IK solver returned empty solution (IK failed)") return None return joint_positions iteration += 1 - self.logger.error(f"[ManipulationInterface] IK solution timeout after {timeout}s ({iteration} iterations)") + self.logger.error(f"[Manipulation] IK solution timeout after {timeout}s ({iteration} iterations)") return None - def move_to_joint_positions(self, joint_positions: list[float], duration: int = 3, blocking: bool = False) -> bool: + def move_to_joint_positions( + self, joint_positions: list[float], duration: float = 3.0, blocking: bool = False + ) -> bool: """ Move the arm to specified joint positions using smooth trajectory. @@ -313,17 +334,17 @@ def move_to_joint_positions(self, joint_positions: list[float], duration: int = self.logger.error(f"Expected 6 joint positions, got {len(joint_positions)}") return False if self._torque_enabled is False: - self.logger.error("[ManipulationInterface] Arm torque is disabled") + self.logger.error("[Manipulation] Arm torque is disabled") return False # Ensure GotoJS client is available if self._goto_js_client is None: - self.logger.error("[ManipulationInterface] GotoJS v2 client is not initialized") + self.logger.error("[Manipulation] GotoJS v2 client is not initialized") return False # Non-blocking check for service readiness if not self._goto_js_client.service_is_ready(): - self.logger.error("[ManipulationInterface] GotoJS v2 service is not ready") + self.logger.error("[Manipulation] GotoJS v2 service is not ready") return False # Build request @@ -338,7 +359,7 @@ def move_to_joint_positions(self, joint_positions: list[float], duration: int = if blocking and not self._await_motion_result(future, "GotoJS v2", duration + 1.0): return False except Exception as e: - self.logger.error(f"[ManipulationInterface] Exception calling GotoJS v2: {e}") + self.logger.error(f"[Manipulation] Exception calling GotoJS v2: {e}") return False return True @@ -351,7 +372,7 @@ def move_to_cartesian_pose( roll: float = 0.0, pitch: float = 0.0, yaw: float = 0.0, - duration: int = 3, + duration: float = 3.0, ik_timeout: float = 2.0, blocking: bool = False, gripper_position: float | None = None, @@ -423,10 +444,10 @@ def move_cartesian_trajectory( True if successful, False otherwise. """ if len(poses) < 2: - self.logger.error("[ManipulationInterface] Need at least 2 poses for trajectory") + self.logger.error("[Manipulation] Need at least 2 poses for trajectory") return False if self._torque_enabled is False: - self.logger.error("[ManipulationInterface] Arm torque is disabled") + self.logger.error("[Manipulation] Arm torque is disabled") return False # Resolve gripper position once @@ -450,7 +471,7 @@ def move_cartesian_trajectory( timeout=ik_timeout, ) if joints is None: - self.logger.error(f"[ManipulationInterface] IK failed for trajectory pose {i}: {p}") + self.logger.error(f"[Manipulation] IK failed for trajectory pose {i}: {p}") return False # Append gripper (IK returns 5 joints) if len(joints) == 5: @@ -459,10 +480,10 @@ def move_cartesian_trajectory( # Check service readiness if self._goto_js_traj_client is None: - self.logger.error("[ManipulationInterface] GotoJSTrajectory client not initialized") + self.logger.error("[Manipulation] GotoJSTrajectory client not initialized") return False if not self._goto_js_traj_client.service_is_ready(): - self.logger.error("[ManipulationInterface] GotoJSTrajectory service not ready") + self.logger.error("[Manipulation] GotoJSTrajectory service not ready") return False # Build flat waypoint array and segment durations @@ -488,7 +509,7 @@ def move_cartesian_trajectory( if not self._await_motion_result(future, "GotoJSTrajectory", total_time + 5.0): return False except Exception as e: - self.logger.error(f"[ManipulationInterface] Exception calling GotoJSTrajectory: {e}") + self.logger.error(f"[Manipulation] Exception calling GotoJSTrajectory: {e}") return False return True @@ -496,21 +517,21 @@ def move_cartesian_trajectory( def _await_motion_result(self, future, name: str, timeout_sec: float) -> bool: """Block on a motion-service future; return True iff it completed successfully.""" if not self._wait_for_future(future, timeout_sec=timeout_sec): - self.logger.error(f"[ManipulationInterface] {name} call timed out") + self.logger.error(f"[Manipulation] {name} call timed out") return False result = future.result() if result is None: - self.logger.error(f"[ManipulationInterface] {name} call timed out") + self.logger.error(f"[Manipulation] {name} call timed out") return False if not result.success: - self.logger.error(f"[ManipulationInterface] {name} returned failure") + self.logger.error(f"[Manipulation] {name} returned failure") return False return True def _call_trigger(self, client, action_name: str, success_msg: str, timeout_sec: float = 2.0) -> bool: """Call a std_srvs/Trigger service, log the outcome, and return whether it succeeded.""" if not client.service_is_ready(): - self.logger.error(f"[ManipulationInterface] {action_name} service not ready") + self.logger.error(f"[Manipulation] {action_name} service not ready") return False try: @@ -560,59 +581,188 @@ def reboot_servos(self) -> bool: # Gripper position constants (radians) GRIPPER_CLOSED = 0.0 GRIPPER_OPEN = 0.85 + # Below this j6 the claw is still (tripped) shut after an open command. + GRIPPER_SHUT_J6 = 0.10 + # Squeezing past the closed stop by more than this overcurrent-trips the + # servo on a real object (0.7 and 0.8 both tripped on hardware; recovery + # needs a reboot). + GRIPPER_MAX_STRENGTH = 0.6 + + def _command_gripper(self, j6: float, duration: float, blocking: bool) -> bool: + """Send a joint command that moves only the gripper to ``j6``.""" + if self._arm_state is None: + self.logger.error("No arm state available") + return False + + self.spin_node_to_refresh_topics(count=5, timeout_sec=0.01) + + positions = list(self._arm_state.position) + if len(positions) < 6: + positions.extend([0.0] * (6 - len(positions))) + + positions[5] = j6 + return self.move_to_joint_positions(positions, duration=duration, blocking=blocking) def open_gripper(self, percent: float = 100.0, duration: float = 0.5, blocking: bool = False) -> bool: """ Open the gripper (joint6). + When blocking, verifies the claw actually opened — the servo can + overcurrent-trip and stay shut — and reboots + retries once if not. + Args: percent: How open to make the gripper, 0-100% (default 100% = fully open) duration: Time for gripper motion - blocking: If True, block until motion completes + blocking: If True, block until motion completes and verify the claw opened Returns: True if successful, False otherwise """ - # Get current joint positions - if self._arm_state is None: - self.logger.error("No arm state available") - return False - - self.spin_node_to_refresh_topics(count=5, timeout_sec=0.01) - - positions = list(self._arm_state.position) - if len(positions) < 6: - positions.extend([0.0] * (6 - len(positions))) - - # Clamp percent to 0-100 percent = max(0.0, min(100.0, percent)) - - # Interpolate between closed and open based on percent - positions[5] = self.GRIPPER_CLOSED + (self.GRIPPER_OPEN - self.GRIPPER_CLOSED) * (percent / 100.0) - return self.move_to_joint_positions(positions, duration=duration, blocking=blocking) + target = self.GRIPPER_CLOSED + (self.GRIPPER_OPEN - self.GRIPPER_CLOSED) * (percent / 100.0) + for attempt in (1, 2): + if not self._command_gripper(target, duration, blocking): + return False + # Can only verify a blocking move, and only when the target itself + # clears the shut threshold. + if not blocking or target < self.GRIPPER_SHUT_J6: + return True + self.spin_node_to_refresh_topics(count=5, timeout_sec=0.01) + j6 = self.gripper_j6(self._arm_state) + if j6 is None or j6 >= self.GRIPPER_SHUT_J6: + return True + if attempt == 1: + self.logger.warning(f"Gripper did not open (j6={j6:.3f}); rebooting servos, then retrying") + self.recover() + self.logger.error("Gripper did not open (servo tripped shut)") + return False def close_gripper(self, strength: float = 0.0, duration: float = 0.5, blocking: bool = False) -> bool: """ Close the gripper (joint6). Args: - strength: Additional radians to close beyond 0.0 (e.g. 0.1 = close to -0.1 rad) + strength: Additional radians to close beyond 0.0 for a firmer grip + (e.g. 0.1 = close to -0.1 rad). Clamped to GRIPPER_MAX_STRENGTH. duration: Time for gripper motion blocking: If True, block until motion completes Returns: True if successful, False otherwise """ - # Get current joint positions - if self._arm_state is None: - self.logger.error("No arm state available") - return False - - self.spin_node_to_refresh_topics(count=5, timeout_sec=0.01) + strength = min(abs(strength), self.GRIPPER_MAX_STRENGTH) + return self._command_gripper(self.GRIPPER_CLOSED - strength, duration, blocking) + + # --- arm primitives --- + # Skills call these as methods: self.manipulation.go(...), .move_checked(...). + + # Grasp reach box (base_link m). + REACH_X = (0.22, 0.40) + REACH_Y = (-0.10, 0.10) + + # joints 1-6 = base yaw, shoulder, elbow, wrist pitch, wrist roll, gripper. + # Folded rest with j4 lifted so the gripper clears the floor (verified live: + # ee_link z ~0.042 m). j1/j2 clamp to their limits, so this is what the arm + # actually reaches and holds. + REST = [1.5708, -1.2195, 1.5723, 0.30, 0.0, 0.0031] + ZERO = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + @classmethod + def clamp_reach(cls, x, y): + return (max(cls.REACH_X[0], min(cls.REACH_X[1], x)), max(cls.REACH_Y[0], min(cls.REACH_Y[1], y))) + + def ee_xyz(self): + """FK end-effector (x,y,z), or None.""" + pose = self.get_current_end_effector_pose() + if pose is None: + return None + try: + p = pose["position"] + return (float(p["x"]), float(p["y"]), float(p["z"])) + except (KeyError, TypeError): + return None - positions = list(self._arm_state.position) - if len(positions) < 6: - positions.extend([0.0] * (6 - len(positions))) + @staticmethod + def gripper_j6(joint_states): + """Gripper joint j6, or None.""" + try: + return joint_states.position[5] if joint_states else None + except (AttributeError, IndexError, TypeError): + return None - positions[5] = self.GRIPPER_CLOSED - abs(strength) - return self.move_to_joint_positions(positions, duration=duration, blocking=blocking) + @staticmethod + def with_gripper(joints, j6): + """Copy of joints with j6 set (no-op if j6 is None).""" + out = list(joints) + if j6 is not None: + out[5] = float(j6) + return out + + @classmethod + def rest_joints(cls, joint_states=None, keep_gripper=True): + """Joint target for the folded rest pose.""" + if keep_gripper: + return cls.with_gripper(cls.REST, cls.gripper_j6(joint_states)) + return list(cls.REST) + + def go(self, joints, duration=3.0, *, times=1, pause=0.3, logger=None): + """Move to joint positions, blocking for each move. Raises ArmFailed. + ``times`` + ``pause`` repeat the move so the arm can settle (pick + teardown). Deliberately not cancel-interruptible: an arm move is a + short atomic commitment, and this must stay safe in teardown paths + that run after a cancel.""" + logger = logger or self.logger + joints = list(joints) + for i in range(times): + logger.info(f"[arm] joints {[round(j, 3) for j in joints]} over {duration}s") + if not self.move_to_joint_positions(joint_positions=joints, duration=duration, blocking=True): + raise ArmFailed("Failed to send arm command") + if pause and i + 1 < times: + time.sleep(pause) + + def recover(self, logger=None): + """Reboot servos + torque on (clears overcurrent trip / brownout).""" + (logger or self.logger).warning("[arm] recovering (reboot + torque on)") + self.reboot_servos() + time.sleep(2.0) + self.torque_on() + time.sleep(0.5) + + def move_checked(self, x, y, z, pitch, duration=1.5, tol_xy=0.05, tol_z=0.10, gripper=None, logger=None): + """Cartesian move; verify FK within per-axis tolerances, recover+retry once, else raise. + + tol_z is looser than tol_xy on purpose: a z shortfall usually means the + fingers met the object/floor early (expected while descending), while xy + error means the grasp is off target. + + ``gripper``: j6 command to hold through the move. Pass the grip goal + when moving with an object in the fingers — the default (None) re-seeds + j6 from the measured position, and under current-based position control + (mode 5) the standing position error IS the grip force, so re-seeding + it releases the object. + """ + logger = logger or self.logger + for attempt in (1, 2): + ok = self.move_to_cartesian_pose( + x=x, + y=y, + z=z, + roll=0.0, + pitch=pitch, + yaw=0.0, + duration=duration, + blocking=True, + gripper_position=gripper, + ) + cur = self.ee_xyz() + err_xy = math.hypot(cur[0] - x, cur[1] - y) if cur is not None else None + err_z = abs(cur[2] - z) if cur is not None else None + if ok and err_xy is not None and err_z is not None and err_xy <= tol_xy and err_z <= tol_z: + return True + logger.warning( + f"[arm] not tracking (ok={ok} err_xy={err_xy} err_z={err_z}) — " + f"{'recovering' if attempt == 1 else 'giving up'}" + ) + if attempt == 1: + self.recover(logger) + raise ArmUnhealthy(f"arm failed to reach ({x:.2f},{y:.2f},{z:.2f})") diff --git a/ros2_ws/src/brain/brain_client/brain_client/robot/mobility.py b/ros2_ws/src/brain/brain_client/brain_client/robot/mobility.py index 269a82184..edc03e049 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/robot/mobility.py +++ b/ros2_ws/src/brain/brain_client/brain_client/robot/mobility.py @@ -2,7 +2,7 @@ # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc """ -MobilityInterface - Provides base movement (wheels) control capabilities to skills. +Mobility - Provides base movement (wheels) control capabilities to skills. This interface allows skills to: 1. Send velocity commands on the common cmd_vel topic @@ -16,14 +16,18 @@ from geometry_msgs.msg import PoseStamped, Twist from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult from rclpy.node import Node +from rclpy.timer import Timer from brain_client.common.logging import UniversalLogger +from brain_client.skills.types import SkillCancelled, cancellable_sleep -class MobilityInterface: +class Mobility: """High-level interface for base (wheel) motion. - Skills should use this instead of directly publishing to cmd_vel. + Injected as ``self.mobility`` when a skill declares ``mobility: Mobility``; + the framework constructs it. Skills should use this instead of directly + publishing to cmd_vel. """ def __init__(self, node: Node, logger, cmd_vel_topic: str = "/cmd_vel"): @@ -32,13 +36,13 @@ def __init__(self, node: Node, logger, cmd_vel_topic: str = "/cmd_vel"): self.cmd_vel_topic = cmd_vel_topic self._cmd_vel_pub = self.node.create_publisher(Twist, self.cmd_vel_topic, 10) - self._stop_timer: object | None = None + self._stop_timer: Timer | None = None # Nav2 navigator for precise movements self._navigator = BasicNavigator(namespace="") self._navigator_mapfree = BasicNavigator(namespace="mapfree") - self.logger.info(f"MobilityInterface initialized with cmd_vel topic: {self.cmd_vel_topic}") + self.logger.info(f"Mobility initialized with cmd_vel topic: {self.cmd_vel_topic}") def _schedule_stop(self, duration: float): # One timer, created once and retargeted per command. Destroying a @@ -55,9 +59,10 @@ def _schedule_stop(self, duration: float): self._stop_timer.reset() def _on_stop_timer(self): - self._stop_timer.cancel() # one-shot: stays cancelled until the next _schedule_stop + if self._stop_timer is not None: + self._stop_timer.cancel() # one-shot: stays cancelled until the next _schedule_stop self._cmd_vel_pub.publish(Twist()) - self.logger.debug("MobilityInterface: stop command published") + self.logger.debug("Mobility: stop command published") def send_cmd_vel( self, @@ -79,7 +84,7 @@ def send_cmd_vel( self._cmd_vel_pub.publish(twist) self.logger.debug( - f"MobilityInterface: cmd_vel published (linear_x={linear_x}, angular_z={angular_z}, duration={duration})" + f"Mobility: cmd_vel published (linear_x={linear_x}, angular_z={angular_z}, duration={duration})" ) if duration is not None and duration > 0.0: @@ -118,24 +123,130 @@ def rotate(self, angle_radians: float) -> bool: goal_pose.pose.orientation.z = math.sin(angle_radians / 2.0) goal_pose.pose.orientation.w = math.cos(angle_radians / 2.0) - self.logger.info(f"MobilityInterface: rotating {math.degrees(angle_radians):.1f}° via Nav2") + self.logger.info(f"Mobility: rotating {math.degrees(angle_radians):.1f}° via Nav2") # Get path to verify it's possible path = self._navigator_mapfree.getPath(goal_pose, goal_pose, use_start=False) if path is None: - self.logger.error("MobilityInterface: failed to get rotation path") + self.logger.error("Mobility: failed to get rotation path") return False # Execute rotation (blocking) self._navigator.goToPose(goal_pose, behavior_tree="mapfree") while not self._navigator.isTaskComplete(): - time.sleep(0.1) # block until done; a bare pass busy-spins a full core + try: + cancellable_sleep(0.1) + except SkillCancelled: + self._navigator.cancelTask() + raise result = self._navigator.getResult() if result == TaskResult.SUCCEEDED: - self.logger.info("MobilityInterface: rotation complete") + self.logger.info("Mobility: rotation complete") return True else: - self.logger.error(f"MobilityInterface: rotation failed with {result}") + self.logger.error(f"Mobility: rotation failed with {result}") return False + + # --- base primitives --- + # Skills call these as methods: self.mobility.rotate_by(...), .drive(...). + + def stop(self): + self.send_cmd_vel(0.0, 0.0, 0.1) + + halt = stop # the framework brake (Skill._halt_interfaces) + + @staticmethod + def odom_xyt(odom): + """(x, y, theta) from nested dict or flat dataclass, or None.""" + if odom is None: + return None + try: + if isinstance(odom, dict): + p = odom["pose"]["pose"]["position"] + return (float(p["x"]), float(p["y"]), math.radians(float(odom["theta_degrees"]))) + return (float(odom.x), float(odom.y), float(odom.theta)) + except (KeyError, AttributeError, TypeError): + return None + + @staticmethod + def servo_vel(err_px, gain, v_min, v_max, deadband_px): + """Pixel P-servo axis: gain per 100 px, clamp to [v_min, v_max], 0 in deadband.""" + if abs(err_px) <= deadband_px: + return 0.0 + v = max(-v_max, min(v_max, -gain / 100.0 * err_px)) + return math.copysign(v_min, v) if abs(v) < v_min else v + + def rotate_by( + self, get_xyt, angle, *, kp=1.2, wz_max=0.5, wz_min=0.15, tol=math.radians(2.5), timeout=12.0, logger=None + ): + """Rotate in place by `angle` rad, closed on odometry yaw (open-loop + if get_xyt yields None). Returns True when the target (or open-loop + best effort) was reached, False on timeout or odometry loss.""" + logger = logger or self.logger + try: + xyt = get_xyt() + if xyt is None: + logger.warning("[mobility] no odom — open-loop rotate") + duration = abs(angle) / 0.35 + self.send_cmd_vel(0.0, math.copysign(0.35, angle), duration) + cancellable_sleep(duration + 0.4) + return True + target = xyt[2] + angle + err = math.atan2(math.sin(angle), math.cos(angle)) + t0 = time.time() + while time.time() - t0 < timeout: + xyt = get_xyt() + if xyt is None: + logger.warning("[mobility] odom lost mid-rotate — stopping short") + return False + err = math.atan2(math.sin(target - xyt[2]), math.cos(target - xyt[2])) + if abs(err) < tol: + return True + wz = max(-wz_max, min(wz_max, kp * err)) + if abs(wz) < wz_min: + wz = math.copysign(wz_min, wz) + self.send_cmd_vel(0.0, wz, 0.15) + cancellable_sleep(0.08) + logger.warning( + f"[mobility] rotate_by timed out after {timeout}s " + f"({math.degrees(angle):.1f}° requested, {math.degrees(err):.1f}° short)" + ) + return False + finally: + self.stop() + + def drive(self, get_xyt, dist, *, kp=0.3, v_max=0.10, v_min=0.04, tol=0.015, timeout=15.0, logger=None): + """Drive straight by `dist` m, closed on odometry position (open-loop + if get_xyt yields None). Returns True when the distance (or open-loop + best effort) was covered, False on timeout or odometry loss.""" + logger = logger or self.logger + if abs(dist) < tol: + return True + try: + xyt = get_xyt() + if xyt is None: + logger.warning("[mobility] no odom — open-loop drive") + duration = abs(dist) / 0.08 + self.send_cmd_vel(math.copysign(0.08, dist), 0.0, duration) + cancellable_sleep(duration + 0.4) + return True + x0, y0 = xyt[0], xyt[1] + err = abs(dist) + t0 = time.time() + while time.time() - t0 < timeout: + xyt = get_xyt() + if xyt is None: + logger.warning("[mobility] odom lost mid-drive — stopping short") + return False + err = abs(dist) - math.hypot(xyt[0] - x0, xyt[1] - y0) + if err < tol: + return True + v = math.copysign(max(v_min, min(v_max, kp * err)), dist) + self.send_cmd_vel(v, 0.0, 0.15) + cancellable_sleep(0.08) + logger.warning(f"[mobility] drive timed out after {timeout}s ({dist:.2f} m requested, {err:.2f} m short)") + return False + finally: + self.stop() diff --git a/ros2_ws/src/brain/brain_client/brain_client/skill_types.py b/ros2_ws/src/brain/brain_client/brain_client/skill_types.py deleted file mode 100644 index 3cc4e8a43..000000000 --- a/ros2_ws/src/brain/brain_client/brain_client/skill_types.py +++ /dev/null @@ -1,9 +0,0 @@ -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -"""Deprecated: moved to brain_client.skills.types. - -Backward-compat shim for custom skills that still import from the old path. -Remove in a future release once external skill files have migrated. -""" - -from brain_client.skills.types import * # noqa: F401, F403 diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/README.md b/ros2_ws/src/brain/brain_client/brain_client/skills/README.md index d9b821acb..f1f553d99 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/skills/README.md +++ b/ros2_ws/src/brain/brain_client/brain_client/skills/README.md @@ -16,5 +16,12 @@ - `hot_reload.py` — full/selective reload coordination and the file-watcher queue. - `loader.py` / `cli_bridge.py` / `hot_reload_watcher.py` — skill loading + CLI bridge + the filesystem watcher. -- `types.py` — the public `Skill` / `SkillResult` SDK base classes that user skills - in `workspace/` import. Keep this import path stable. +- `types.py` — the public `Skill` / `SkillOutput` / `SkillResult` SDK base classes + that user skills in `workspace/` import. Keep this import path stable. The return + contract: `execute()` returns the message str (or `SkillOutput(message, data)`, + or None); `self.fail(message)` fails the run. Legacy `(message, SkillResult)` + tuple returns are deprecated but still normalize. + +The skill-facing state snapshot types (`Odometry`, `Pose`, `Battery`, ...) are +not framework code and live in `../state/` — ROS-free dataclasses converted +per-message from the live feeds. diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/catalog.py b/ros2_ws/src/brain/brain_client/brain_client/skills/catalog.py index c07ad2469..888d6dc26 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/skills/catalog.py +++ b/ros2_ws/src/brain/brain_client/brain_client/skills/catalog.py @@ -2,10 +2,9 @@ # Copyright (c) 2026 Innate Inc """Skill catalog: discovery, metadata, publishing, reload, and physical skills. -Owns the loaded code/physical/in-training skill dicts (guarded by a lock against -the hot-reload thread), the latched ``/brain/available_skills`` publisher, the -skill cache, and the hot-reload watcher. The executor asks this for skills via the -thread-safe getters; the node's reload/create services delegate here. +Code skills are stored as classes plus metadata harvested at discovery — never +live instances; the skills server constructs a fresh instance per run, so a +reload here is just an entry swap. """ from __future__ import annotations @@ -18,51 +17,128 @@ import threading import time import types +from dataclasses import dataclass from pathlib import Path -from typing import Literal, get_args, get_origin +from typing import Literal, cast, get_args, get_origin import h5py from brain_messages.msg import AvailableSkills, SkillInfo from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy +from brain_client.common.dynamic_loader import class_name_to_snake_case, evict_modules_under from brain_client.common.script_paths import ( classify_source, ensure_user_directories, get_custom_skills_dir, get_innate_skills_dir, get_skill_directories, + get_workspace_dir, + skill_id_prefix_for, ) from brain_client.skills.hot_reload_watcher import HotReloadWatcher -from brain_client.skills.loader import SkillLoader +from brain_client.skills.physical import ( + get_episode_count, + has_physical_metadata, + shutdown_quietly, + validate_physical_skill, +) +from brain_client.skills.physical_refs import ( + prune_dir_shims, + render_dir_shims, + render_refs, + write_dir_shims, + write_refs, +) from brain_client.skills.replay_conversion import recording_action_to_replay -from brain_client.skills.types import Skill +from brain_client.skills.workspace_import import ( + import_workspace_packages, + module_skill_id, + registered_workspace_skills, +) + + +def _annotation_is_float(annotation) -> bool: + """True if a param annotation is ``float`` (or a union including it). + + Handles both real type objects and string annotations (skills using + ``from __future__ import annotations`` expose ``"float"`` instead). + """ + if annotation is float or annotation == "float": + return True + return any(arg is float or arg == "float" for arg in get_args(annotation)) + + +@dataclass(frozen=True) +class CodeSkillEntry: + """A discovered code skill: its class plus metadata harvested at discovery. + execute() is introspected exactly once — the published schema, input + coercion and invoker validation all read these fields.""" + + display_name: str + skill_class: type + source: str # "shipped" | "user" — stamped onto each run's instance + group: str # folder path inside the package ("" = root) — UI grouping only + guidelines: str + guidelines_when_running: str + inputs: dict # {param: schema} from execute()'s signature + inputs_json: str # the same schema, serialized for the roster message + float_params: frozenset[str] # params to widen int -> float before dispatch + accepts_extra_inputs: bool # execute() takes **kwargs (compat plumbing) + + +@dataclass(frozen=True) +class PhysicalSkillEntry: + """A discovered physical skill: its metadata.json plus validation results. + + No episode count here: episodes accumulate while a skill trains, so the + roster re-reads them at publish time (_build_physical_skill_info).""" + + metadata: dict + directory: str + in_training: bool class SkillRepository: - def __init__(self, node, *, interface_injector, retire_instances=None): + def __init__(self, node): self._node = node self._logger = node.get_logger() - self._inject = interface_injector - # Called with the skill instances a reload replaced. The skills server - # passes a gate that defers disposal while a skill is executing; without - # a callback they are disposed inline. - self._retire_instances = retire_instances - self.skill_loader = SkillLoader(self._logger) self._skills_directories = self._resolve_skills_directories() - # Guards _code_skills / _physical_skills / _in_training_skills against the - # HotReloadWatcher background thread. + # guards the skill dicts against the HotReloadWatcher thread self._skills_lock = threading.Lock() - self._code_skills: dict[str, tuple[str, Skill]] = {} # {id: (display_name, instance)} - self._physical_skills: dict[str, dict] = {} - self._in_training_skills: dict[str, dict] = {} - - self._code_skills = self._load_code_skills(self._skills_directories) + self._code_skills: dict[str, CodeSkillEntry] = {} + self._physical_skills: dict[str, PhysicalSkillEntry] = {} + self._in_training_skills: dict[str, PhysicalSkillEntry] = {} + # skill_id -> load-error text for files that failed to load. Published on + # the roster (SkillInfo.load_error) so a broken skill shows up in the UI + # with its error instead of silently vanishing. Never registered with the + # cloud agent and not runnable. + self._broken_skills: dict[str, str] = {} + # module name -> skill ids it registered on its last clean import, + # carried across reloads: a module that later fails to import rosters + # every skill it used to define as broken (see _load_code_skills). + self._skill_ids_by_module: dict[str, list[str]] = {} + + # Evict before the first load too: in a fresh process this is a no-op, + # but it makes construction deterministic when workspace modules are + # already cached (tests, re-instantiation) — same path as reload_all. + self._evict_workspace_modules() + # Physical first, refs second, code last: skills `from physical_skills + # import X`, and on a fresh workspace that package doesn't exist until + # we write it — importing code first would roster every such skill + # broken for the whole first pass. + self._physical_skills, self._in_training_skills, physical_broken = self._load_physical_skills( + self._skills_directories + ) + self._refresh_physical_refs(self._physical_skills, self._in_training_skills) + self._code_skills, code_broken = self._load_code_skills() self._logger.info(f"Successfully loaded {len(self._code_skills)} code skills") - self._physical_skills, self._in_training_skills = self._load_physical_skills(self._skills_directories) + self._broken_skills = {**code_broken, **physical_broken} self._logger.info(f"Successfully loaded {len(self._physical_skills)} physical skills") self._logger.info(f"Found {len(self._in_training_skills)} in-training skills") + if self._broken_skills: + self._logger.warning(f"{len(self._broken_skills)} skills failed to load: {list(self._broken_skills)}") qos = QoSProfile( depth=1, @@ -70,10 +146,8 @@ def __init__(self, node, *, interface_injector, retire_instances=None): reliability=QoSReliabilityPolicy.RELIABLE, ) self._skills_publisher = node.create_publisher(AvailableSkills, "/brain/available_skills", qos) - # Last roster we published, kept so the heartbeat can re-emit it cheaply - # without re-inspecting every skill. The topic is latched, but bridges - # (rws) that subscribe after boot don't get the latched sample, so a - # periodic re-publish is what actually reaches late-joining webapp clients. + # kept for the heartbeat: rws subscribes after boot and never gets the + # latched sample, so periodic re-publish is what reaches the webapp self._last_published_msg: AvailableSkills | None = None self._hot_reload_watcher = None @@ -87,48 +161,106 @@ def code_count(self) -> int: def physical_count(self) -> int: return len(self._physical_skills) - def get_code_skill(self, skill_id: str): + def get_code_skill(self, skill_id: str) -> CodeSkillEntry | None: with self._skills_lock: return self._code_skills.get(skill_id) - def get_physical_skill(self, skill_id: str): + def get_physical_skill(self, skill_id: str) -> PhysicalSkillEntry | None: with self._skills_lock: return self._physical_skills.get(skill_id) - def all_skill_ids(self) -> list[str]: - with self._skills_lock: - return list(self._code_skills.keys()) + list(self._physical_skills.keys()) + def bare_id_candidates(self, skill_id: str) -> list[str]: + """Ids a possibly-bare name may resolve to, in precedence order. - def all_code_skills(self) -> list[tuple[str, tuple[str, Skill]]]: + ``local/`` beats ``innate-os/`` (a user override wins over the shipped + skill of the same name), then any dropped-in package that could hold + the name. Already-qualified ids resolve only to themselves. + """ + if "/" in skill_id: + return [skill_id] + candidates = [f"local/{skill_id}", f"innate-os/{skill_id}"] with self._skills_lock: - return list(self._code_skills.items()) - - # --- retired-instance disposal --- - @staticmethod - def dispose_instances(instances: list[Skill], logger) -> None: - """shutdown() retired skill instances, logging (never raising) failures.""" - for instance in instances: - try: - instance.shutdown() - except Exception as e: - logger.error(f"Error shutting down retired {type(instance).__name__} instance: {e}") - - def _retire(self, instances: list[Skill]) -> None: - """Dispose instances a reload replaced (or hand them to the server's gate). + live = [*self._code_skills, *self._physical_skills, *self._broken_skills] + for namespace in dict.fromkeys(i.split("/", 1)[0] for i in live if "/" in i): + if namespace not in ("local", "innate-os"): + candidates.append(f"{namespace}/{skill_id}") + return candidates + + def get_load_error(self, skill_id: str) -> str | None: + """The load error for a broken skill, resolving bare names like the + invoker does (see :meth:`bare_id_candidates`). None if not broken.""" + candidates = self.bare_id_candidates(skill_id) + with self._skills_lock: + for candidate in candidates: + error = self._broken_skills.get(candidate) + if error is not None: + return error + # A broken module in a subpackage rosters by its dotted module + # path (custom_skills/boards/chess.py -> local/boards.chess), + # which no candidate ever spells — the working class's id was + # local/chess. Match by namespace + leaf module name so a call to + # that id surfaces the load error instead of "unknown skill". + # Several same-leaf broken modules (boards/chess.py AND + # demos/chess.py) are ambiguous — report all of them rather than + # quoting whichever iterates first and pointing the author at a + # file they never touched. + for candidate in candidates: + namespace, _, leaf = candidate.partition("/") + matches = { + broken_id: error + for broken_id, error in self._broken_skills.items() + if broken_id.partition("/")[0] == namespace + and broken_id.partition("/")[2].rpartition(".")[2] == leaf + } + if len(matches) == 1: + return next(iter(matches.values())) + if matches: + return "; ".join(f"{broken_id}: {error}" for broken_id, error in sorted(matches.items())) + return None + + def in_training_reason(self, skill_id: str) -> str | None: + """The in-training entry a bare or qualified id matches: on the + roster, but its runnable data (checkpoint or recorded trajectory) + isn't on disk yet. Resolves bare names like the invoker (see + :meth:`bare_id_candidates`). None if no candidate is training.""" + candidates = self.bare_id_candidates(skill_id) + with self._skills_lock: + for candidate in candidates: + if candidate in self._in_training_skills: + return f"'{candidate}' has no runnable data yet (still training, or its recording is not on this robot)" + return None + + def unavailable_reason(self, skill_id: str) -> str | None: + """Why an id that matches *something* on the roster cannot run: a + broken module's load error, an in-training skill's missing + checkpoint, or both (a broken local entry shadowing a training skill + of the same bare name — without the training note, the load error + alone points the user at the wrong skill). Phrased to read after + "Skill '' ...". None when the id matches nothing (the caller + says "unknown skill").""" + load_error = self.get_load_error(skill_id) + training = self.in_training_reason(skill_id) + if load_error and training: + return f"failed to load: {load_error} (note: {training})" + if load_error: + return f"failed to load: {load_error}" + if training: + return f"is not runnable: {training}" + return None - A replaced instance still holds live ROS entities (e.g. BasicNavigator - nodes). Left to the GC it is cyclic garbage: its graph entities persist - until an eventual gen-2 pass, so reloads leak subscriptions and memory. - """ - if not instances: - return - if self._retire_instances is not None: - self._retire_instances(instances) - else: - self.dispose_instances(instances, self._logger) + def all_skill_ids(self) -> list[str]: + with self._skills_lock: + return list(self._code_skills.keys()) + list(self._physical_skills.keys()) # --- watcher --- def start_watcher(self) -> None: + # Every callback is a full reload now, so the workspace root (recursive + # — covers every package, incl. ones dropped in after boot) is the only + # watch that needs scheduling. The skill dirs are still passed: a + # physical skill is *data*, so its reload trigger is a metadata.json or + # episode_*.h5 write, and only skills_directories resolves a non-.py + # path to the skill that owns it. The watcher skips scheduling the ones + # the root already covers. self._hot_reload_watcher = HotReloadWatcher( logger=self._logger, skills_directories=self._skills_directories, @@ -136,6 +268,7 @@ def start_watcher(self) -> None: on_reload=self._on_skills_file_changed, debounce_seconds=1.0, recursive=True, # physical skills live in subdirs (metadata.json + assets) + workspace_roots=[str(get_workspace_dir())], ) self._hot_reload_watcher.start() @@ -144,64 +277,156 @@ def stop_watcher(self) -> None: self._hot_reload_watcher.stop() def _on_skills_file_changed(self, skill_names: list, _agent_names: list) -> None: - """Called by HotReloadWatcher when skill files change. - - Names are code-skill stems (``foo.py`` -> ``foo``) or physical-skill - directory names (``foo/metadata.json`` -> ``foo``); both resolve below. - """ - self._logger.info(f"Hot reload triggered for skills: {skill_names}") - if not skill_names: - self.reload_all() - return - # drop entries whose source is gone so a rename doesn't ghost-publish - # both the old and new id - removed = self._prune_stale_skills() - skill_ids = [] - for stem in skill_names: - for d in self._skills_directories: - py_file = Path(d) / f"{stem}.py" - subdir = Path(d) / stem - source = py_file if py_file.exists() else subdir if subdir.is_dir() else None - if source is None: - continue - skill_ids.append(self._compute_skill_id(source)) - break - if skill_ids: - self.reload_selective(skill_ids) # publishes when done - elif removed: - self.publish_skills_list() - else: - self.reload_all() # couldn't resolve the change; rebuild from disk + """Watcher callback. Every change is a full reload under the import + model — reload_all re-resolves directories (new drop-in packages + included), evicts the workspace subtree, and re-imports.""" + self._logger.info(f"Hot reload triggered (skills hint: {skill_names or 'n/a'}) - full reload") + self.reload_all() # --- loading --- - def _load_code_skills(self, skills_directories) -> dict[str, tuple[str, Skill]]: - discovered_skills = self.skill_loader.load_from_directories(skills_directories) - self._logger.info(f"Discovered skills: {list(discovered_skills.keys())} in directories {skills_directories}") + def _load_code_skills(self) -> tuple[dict[str, CodeSkillEntry], dict[str, str]]: + """Returns ``(loaded entries, broken: skill_id -> load-error text)``. + + Workspace packages are *imported* — defining a Skill subclass registers + it (see workspace_import.py). The distinction between skill, helper, + and broken is therefore exact, not guessed: a module that raises is + broken (keyed by its module name), a module that imports clean and + registers nothing is a helper, and each registered class is a skill. + """ + broken: dict[str, str] = {} + import_errors = import_workspace_packages(self._logger) + # Aggregate by skill id: same-name skills in different namespaces are + # distinct entries — display-name collisions are resolved at publish + # time by _dedupe_display_names, not by silently dropping one here. + id_keyed: dict[str, tuple[str, type, Path]] = dict(registered_workspace_skills(self._logger)) + + # A failed import leaves no classes to key broken entries by, so a + # multi-skill module would otherwise collapse to one module-derived + # row — every other skill in the file vanishing from the roster with + # no error. Roster the ids the module registered on its last clean + # import instead; the module-derived row is only for modules that + # never imported cleanly in this process. + ids_by_module: dict[str, list[str]] = {} + for skill_id, (_class_name, cls, _src_path) in id_keyed.items(): + ids_by_module.setdefault(cls.__module__, []).append(skill_id) + for module_name, error in import_errors.items(): + known_ids = self._skill_ids_by_module.get(module_name) + if known_ids: + ids_by_module[module_name] = known_ids # carry through the breakage + for skill_id in known_ids: + broken[skill_id] = error + else: + broken[module_skill_id(module_name)] = error + self._skill_ids_by_module = ids_by_module - id_keyed: dict[str, tuple[str, type, Path]] = {} - for display_name, (cls, src_path) in discovered_skills.items(): - id_keyed[self._compute_skill_id(src_path)] = (display_name, cls, src_path) + self._logger.info(f"Discovered skills: {list(id_keyed.keys())}") - code_skills: dict[str, tuple[str, Skill]] = {} - for skill_id, (display_name, skill_class, src_path) in id_keyed.items(): + code_skills: dict[str, CodeSkillEntry] = {} + for skill_id, (_class_name, skill_class, src_path) in id_keyed.items(): try: - instance = self._instantiate(skill_class, src_path) - code_skills[skill_id] = (display_name, instance) - self._logger.info(f"Loaded code skill: {skill_id} ({display_name}) [source={instance.source}]") + code_skills[skill_id] = self._harvest_entry(skill_id, skill_class, src_path) except Exception as e: - self._logger.error(f"Error instantiating skill {skill_id}: {e}") - return code_skills + self._logger.error(f"Error loading skill {skill_id}: {e}") + broken[skill_id] = f"{type(e).__name__}: {e}" + return code_skills, broken - def _instantiate(self, skill_class, src_path): + def _harvest_entry(self, skill_id: str, skill_class: type, src_path) -> CodeSkillEntry: + """Build a skill's metadata from one throwaway instance — name and + guidelines() are instance-level API (0.6.0 contract).""" instance = skill_class(self._logger) - instance.node = self._node - instance.source = classify_source(src_path) - self._inject(instance) - return instance + try: + try: + display_name = str(instance.name) + except Exception: # noqa: BLE001 — a broken .name property must not hide the skill + display_name = class_name_to_snake_case(skill_class.__name__) + inputs, float_params, accepts_extra = self._inspect_skill_inputs(skill_id, instance) + try: + inputs_json = json.dumps(inputs) + except (TypeError, ValueError) as e: + self._logger.error(f"Could not serialize inputs for code skill '{skill_id}': {e}; using empty") + inputs, inputs_json = {}, "{}" + entry = CodeSkillEntry( + display_name=display_name, + skill_class=skill_class, + source=classify_source(src_path), + # the folder IS the group: innate_skills.chess.x -> "chess" + group="/".join(skill_class.__module__.split(".")[1:-1]), + guidelines=self._safe_skill_string(skill_id, instance, "guidelines"), + guidelines_when_running=self._safe_skill_string(skill_id, instance, "guidelines_when_running"), + inputs=inputs, + inputs_json=inputs_json, + float_params=float_params, + accepts_extra_inputs=accepts_extra, + ) + declared = instance.describe_feeds() + self._logger.info( + f"Loaded code skill: {skill_id} ({display_name}) [source={entry.source}]" + + (f" [declares: {declared}]" if declared else "") + ) + # Suspicious bare annotations (typo'd feed declarations) recorded + # at class creation — surface them here, at load/reload time, + # where an author debugging "why is my feed None" will look. + for issue in getattr(skill_class, "_declaration_issues", ()): + self._logger.warning(f"{Path(src_path).name}: {issue}") + return entry + finally: + # A legacy __init__ may own ROS entities; never let the throwaway leak. + shutdown_quietly(instance, self._logger) + + def _read_physical_skill(self, skill_dir: str) -> PhysicalSkillEntry | None: + """metadata.json -> validated entry, or None (with a log) if invalid.""" + metadata_path = os.path.join(skill_dir, "metadata.json") + with open(metadata_path) as f: + metadata = json.load(f) + if not isinstance(metadata, dict): + self._logger.warn(f"Skipped {metadata_path}: top-level JSON is {type(metadata).__name__}, expected object") + return None + is_valid, is_in_training = validate_physical_skill(skill_dir, metadata, self._logger) + if not is_valid: + self._logger.warn(f"Skipped invalid physical skill: {skill_dir}") + return None + return PhysicalSkillEntry( + metadata=metadata, + directory=skill_dir, + in_training=is_in_training, + ) + + def _reload_physical_skill(self, skill_id: str) -> bool: + """Re-read one physical skill's directory into the live dicts. + + Used by create_physical_skill: a full reload_all() takes seconds and + overruns the bridge's service timeout. + """ + basename = skill_id.split("/", 1)[-1] + for skills_directory in self._skills_directories: + skill_path = os.path.join(skills_directory, basename) + if not has_physical_metadata(skill_path): + continue + try: + entry = self._read_physical_skill(skill_path) + except Exception as e: + self._logger.error(f"Error reloading physical skill {skill_id}: {e}") + return False + if entry is None: + return False + with self._skills_lock: + live, retired = ( + (self._in_training_skills, self._physical_skills) + if entry.in_training + else (self._physical_skills, self._in_training_skills) + ) + live[skill_id] = entry + retired.pop(skill_id, None) + self._broken_skills.pop(skill_id, None) + self._logger.info(f"Reloaded physical skill: {skill_id}") + return True + return False def _load_physical_skills(self, skills_directories): + """Returns ``(physical, in_training, broken: skill_id -> load-error text)``.""" physical_skills = {} in_training_skills = {} + broken: dict[str, str] = {} for skills_directory in skills_directories: if not os.path.exists(skills_directory): continue @@ -213,47 +438,29 @@ def _load_physical_skills(self, skills_directories): item_path = os.path.join(skills_directory, item) if not os.path.isdir(item_path): continue - metadata_path = os.path.join(item_path, "metadata.json") - if not os.path.exists(metadata_path): + if not has_physical_metadata(item_path): continue + skill_id = self._compute_skill_id(Path(item_path)) try: - with open(metadata_path) as f: - metadata = json.load(f) - if not isinstance(metadata, dict): - self._logger.warn( - f"Skipped {metadata_path}: top-level JSON is {type(metadata).__name__}, expected object" - ) + entry = self._read_physical_skill(item_path) + if entry is None: continue - skill_id = self._compute_skill_id(Path(item_path)) - is_valid, is_in_training, episode_count = self.skill_loader.validate_physical_skill( - item_path, metadata + kind = "in-training" if entry.in_training else "physical" + target = in_training_skills if entry.in_training else physical_skills + target[skill_id] = entry + self._logger.info( + f"Loaded {kind} skill: {skill_id} (type: {entry.metadata.get('type', 'unknown')})" ) - if not is_valid: - self._logger.warn(f"Skipped invalid physical skill: {skill_id}") - continue - skill_data = { - "metadata": metadata, - "directory": item_path, - "in_training": is_in_training, - "episode_count": episode_count, - } - if is_in_training: - in_training_skills[skill_id] = skill_data - self._logger.info( - f"Loaded in-training skill: {skill_id} (type: {metadata.get('type', 'unknown')})" - ) - else: - physical_skills[skill_id] = skill_data - self._logger.info( - f"Loaded physical skill: {skill_id} (type: {metadata.get('type', 'unknown')})" - ) except json.JSONDecodeError as e: - self._logger.error(f"Skipped {metadata_path}: invalid JSON ({e})") + self._logger.error(f"Skipped {item_path}: invalid JSON ({e})") + broken[skill_id] = f"metadata.json is invalid JSON: {e}" except OSError as e: - self._logger.error(f"Skipped {metadata_path}: read failed ({e})") + self._logger.error(f"Skipped {item_path}: read failed ({e})") + broken[skill_id] = f"read failed: {e}" except Exception as e: self._logger.error(f"Skipped physical skill at {item_path}: {e}") - return physical_skills, in_training_skills + broken[skill_id] = f"{type(e).__name__}: {e}" + return physical_skills, in_training_skills, broken def _resolve_skills_directories(self) -> list[str]: innate_skills_dir = str(get_innate_skills_dir()) @@ -267,132 +474,52 @@ def _resolve_skills_directories(self) -> list[str]: return directories def _compute_skill_id(self, path: str | Path) -> str: - path_str = str(Path(path)) - basename = Path(path_str).stem if path_str.endswith(".py") else Path(path_str).name - prefix = "innate-os" if classify_source(path) == "shipped" else "local" - return f"{prefix}/{basename}" + """Id for a path-identified entry: physical skill dirs and legacy-lane + code files. Workspace code skills get ids from their class instead + (workspace_import.skill_id_for_class).""" + p = Path(path) + basename = p.stem if p.suffix == ".py" else p.name + return f"{skill_id_prefix_for(path)}/{basename}" # --- reload --- + def _evict_workspace_modules(self) -> None: + """Drop every cached module under workspace/ (skills and helpers, via + any import form) plus helper modules in legacy skill dirs. Re-import + happens in _load_code_skills.""" + workspace_root = get_workspace_dir().resolve() + legacy = [d for d in self._skills_directories if not Path(d).resolve().is_relative_to(workspace_root)] + evicted = evict_modules_under([str(get_workspace_dir()), *legacy]) + if evicted: + self._logger.info(f"Evicted workspace modules for reload: {evicted}") + def reload_all(self) -> None: self._logger.info("Reloading skills...") self._skills_directories = self._resolve_skills_directories() - new_code_skills = self._load_code_skills(self._skills_directories) - new_physical, new_in_training = self._load_physical_skills(self._skills_directories) + self._evict_workspace_modules() + # Physical before code, refs in between — see __init__ for why. + new_physical, new_in_training, physical_broken = self._load_physical_skills(self._skills_directories) + self._refresh_physical_refs(new_physical, new_in_training) + new_code_skills, code_broken = self._load_code_skills() with self._skills_lock: - old_code_skills = self._code_skills self._code_skills = new_code_skills self._physical_skills = new_physical self._in_training_skills = new_in_training - self._retire([instance for _name, instance in old_code_skills.values()]) + self._broken_skills = {**code_broken, **physical_broken} self._logger.info(f"Reloaded {len(new_code_skills)} code + {len(new_physical)} physical skills") self.publish_skills_list() def reload_selective(self, skill_ids: list[str]) -> list[str]: - """Reload specific skills by ID. Empty list means reload all.""" - if not skill_ids: - self.reload_all() - with self._skills_lock: - return list(self._code_skills.keys()) + list(self._physical_skills.keys()) - - self._logger.info(f"Selectively reloading skills: {skill_ids}") - reloaded = [] - for skill_id in skill_ids: - basename = skill_id.split("/", 1)[-1] if "/" in skill_id else skill_id - with self._skills_lock: - is_code = skill_id in self._code_skills or self._is_code_skill_id(skill_id) - is_physical = (not is_code) and ( - skill_id in self._physical_skills or skill_id in self._in_training_skills - ) - if is_code: - result = self.skill_loader.reload_skill_by_file_stem(basename, self._skills_directories) - if result is not None: - cls, src_path = result - display_name = self.skill_loader._get_name(cls) - try: - instance = self._instantiate(cls, src_path) - with self._skills_lock: - replaced = self._code_skills.get(skill_id) - self._code_skills[skill_id] = (display_name, instance) - if replaced is not None: - self._retire([replaced[1]]) - reloaded.append(skill_id) - self._logger.info(f"Reloaded code skill: {skill_id}") - except Exception as e: - self._logger.error(f"Error instantiating {skill_id}: {e}") - elif is_physical: - if self._reload_physical_skill(skill_id): - reloaded.append(skill_id) - self._logger.info(f"Selectively reloaded {len(reloaded)} skills") - self.publish_skills_list() - return reloaded - - def _is_code_skill_id(self, skill_id: str) -> bool: - basename = skill_id.split("/", 1)[-1] if "/" in skill_id else skill_id - return any((Path(d) / f"{basename}.py").exists() for d in self._skills_directories) - - def _prune_stale_skills(self) -> list[str]: - """Drop catalog entries whose source file/directory is gone; returns removed ids.""" - removed = [] - pruned_instances = [] - with self._skills_lock: - for skill_id in list(self._code_skills): - if not self._code_source_exists(skill_id): - pruned_instances.append(self._code_skills.pop(skill_id)[1]) - removed.append(skill_id) - for skills in (self._physical_skills, self._in_training_skills): - for skill_id, data in list(skills.items()): - if not os.path.exists(os.path.join(data.get("directory", ""), "metadata.json")): - del skills[skill_id] - removed.append(skill_id) - self._retire(pruned_instances) - if removed: - self._logger.info(f"Pruned stale skills (source removed): {removed}") - return removed - - def _code_source_exists(self, skill_id: str) -> bool: - """True if some scan dir still holds a .py that maps to this exact id. - - Prefix-aware, unlike _is_code_skill_id: a deleted local/foo must not be - kept alive by a shipped innate-os/foo of the same stem. + """Reload skills. Under the import model every reload is a full one — + imports are cached, module identity is exact, and partial reloads were + the source of the flat/folder, stem-resolution, and stale-helper bug + family. ``skill_ids`` is accepted for service compatibility; the reply + lists everything now live. """ - stem = skill_id.split("/", 1)[-1] if "/" in skill_id else skill_id - for d in self._skills_directories: - py_file = Path(d) / f"{stem}.py" - if py_file.exists() and self._compute_skill_id(py_file) == skill_id: - return True - return False - - def _reload_physical_skill(self, skill_id: str) -> bool: - basename = skill_id.split("/", 1)[-1] if "/" in skill_id else skill_id - for skills_directory in self._skills_directories: - skill_path = os.path.join(skills_directory, basename) - metadata_path = os.path.join(skill_path, "metadata.json") - if os.path.exists(metadata_path): - try: - with open(metadata_path) as f: - metadata = json.load(f) - is_valid, is_in_training, episode_count = self.skill_loader.validate_physical_skill( - skill_path, metadata - ) - if is_valid: - skill_data = { - "metadata": metadata, - "directory": skill_path, - "in_training": is_in_training, - "episode_count": episode_count, - } - with self._skills_lock: - if is_in_training: - self._in_training_skills[skill_id] = skill_data - self._physical_skills.pop(skill_id, None) - else: - self._physical_skills[skill_id] = skill_data - self._in_training_skills.pop(skill_id, None) - self._logger.info(f"Reloaded physical skill: {skill_id}") - return True - except Exception as e: - self._logger.error(f"Error reloading physical skill {skill_id}: {e}") - return False + if skill_ids: + self._logger.info(f"Selective reload requested for {skill_ids} — performing a full reload") + self.reload_all() + with self._skills_lock: + return list(self._code_skills.keys()) + list(self._physical_skills.keys()) @staticmethod def _slugify(display_name: str) -> str: @@ -416,13 +543,9 @@ def _write_json_atomic(path: Path, payload: dict) -> None: raise def create_physical_skill(self, display_name: str, kind: str = "learned") -> tuple[bool, str, str, str]: - """Create a physical-skill directory with metadata.json. Returns (ok, msg, dir, id). - - ``kind`` is the intended final type ("learned", "replay", or "eval"). - An "eval" dataset only collects policy-rollout episodes for review; it is - never trained (training selection filters to type=="learned") and never - run as a policy, so it gets no execution config. - """ + """Create a physical-skill directory with metadata.json; returns + (ok, msg, dir, id). An "eval" dataset is never trained or run, so it + gets no execution config.""" try: display_name = display_name.strip() if not display_name: @@ -483,13 +606,9 @@ def create_physical_skill(self, display_name: str, kind: str = "learned") -> tup def save_recording_as_replay_skill( self, task_directory: str, display_name: str, guidelines: str = "", episode_id: int = 0 ) -> tuple[str, str, bool]: - """Promote a recorded teleop episode into a deterministic replay skill. - - Reads the recording, converts it to the replay layout (6 arm joints + 2 - base cmd_vel, plus an optional head channel), writes a ``type: "replay"`` - skill under ``custom_skills/``, drops the raw recording, and republishes. - Returns ``(skill_dir, skill_id, wheeled)``. - """ + """Promote a recorded teleop episode into a replay skill under + custom_skills/, dropping the raw recording. Returns + (skill_dir, skill_id, wheeled).""" display_name = display_name.strip() if not display_name: raise ValueError("Skill name cannot be empty.") @@ -497,9 +616,10 @@ def save_recording_as_replay_skill( recorder_meta, episode_path = self._resolve_recording_episode(task_directory, episode_id) with h5py.File(episode_path, "r") as f: # head_command is its own dataset (replay-only, never in /action); older - # recordings without it just yield an arm+base trajectory. - head = f["head_command"][:] if "head_command" in f else None - action, wheeled = recording_action_to_replay(f["action"][:], head=head) + # recordings without it just yield an arm+base trajectory. The casts + # narrow h5py's Group | Dataset | Datatype lookup union. + head = cast(h5py.Dataset, f["head_command"])[:] if "head_command" in f else None + action, wheeled = recording_action_to_replay(cast(h5py.Dataset, f["action"])[:], head=head) if len(action) == 0: raise ValueError("Recorded episode has no timesteps.") @@ -567,12 +687,9 @@ def _resolve_recording_episode(self, task_directory: str, episode_id: int) -> tu return meta, episode_path def _resolve_replay_skill_dir(self, display_name: str, task_directory: str) -> tuple[Path, bool]: - """Pick custom_skills/, enforce the overwrite policy, and return (skill_dir, in_place). - - Overwriting our own replay skill is fine; refuse a trained model or a different - existing skill — only the in-place name-first flow converts its own scratch. - Writes nothing. - """ + """Pick custom_skills/ and enforce the overwrite policy: + overwriting our own replay skill is fine; refuse a trained model or a + different existing skill. Writes nothing.""" dir_name = self._slugify(display_name) if not dir_name: raise ValueError(f"Cannot derive a directory name from '{display_name}'.") @@ -655,48 +772,55 @@ def publish_skills_list(self) -> None: code_skills_snapshot = dict(self._code_skills) physical_skills_snapshot = dict(self._physical_skills) in_training_skills_snapshot = dict(self._in_training_skills) + broken_skills_snapshot = dict(self._broken_skills) + + for skill_id, entry in code_skills_snapshot.items(): + skills.append( + self._build_skill_info( + skill_id=skill_id, + name=entry.display_name, + skill_type="code", + group=entry.group, + guidelines=entry.guidelines, + guidelines_when_running=entry.guidelines_when_running, + inputs_json=entry.inputs_json, + ) + ) - for skill_id, (display_name, skill_instance) in code_skills_snapshot.items(): - try: - inputs = self._inspect_skill_inputs(skill_id, skill_instance) - guidelines = self._safe_skill_string(skill_id, skill_instance, "guidelines") - guidelines_when_running = self._safe_skill_string(skill_id, skill_instance, "guidelines_when_running") + physical_infos = [] + physical_dirs_by_id = {} + for snapshot in (physical_skills_snapshot, in_training_skills_snapshot): + for skill_id, entry in snapshot.items(): try: - inputs_json = json.dumps(inputs) - except (TypeError, ValueError) as e: - self._logger.error(f"Could not serialize inputs for code skill '{skill_id}': {e}; using empty") - inputs_json = "{}" - skills.append( - self._build_skill_info( - skill_id=skill_id, - name=display_name, - skill_type="code", - guidelines=guidelines, - guidelines_when_running=guidelines_when_running, - inputs_json=inputs_json, - ) + info = self._build_physical_skill_info(skill_id, entry) + except Exception as e: + self._logger.error(f"Skipping physical skill '{skill_id}' in available_skills: {e}") + continue + skills.append(info) + physical_infos.append(info) + physical_dirs_by_id[skill_id] = entry.directory + self._write_physical_refs(physical_infos, physical_dirs_by_id) + + # Broken skills ride the roster too — the UI shows them with their error + # instead of them silently vanishing. Consumers that run or register + # skills skip any entry with a non-empty load_error. + for skill_id, error in broken_skills_snapshot.items(): + # A broken module in a subfolder rosters as "/chess.foo": + # show it as "foo" grouped under "chess", like its healthy peers. + dotted = skill_id.split("/", 1)[-1] + group, _, leaf = dotted.rpartition(".") + skills.append( + self._build_skill_info( + skill_id=skill_id, + name=leaf, + skill_type="broken", + group=group.replace(".", "/"), + guidelines="", + guidelines_when_running="", + inputs_json="{}", + load_error=error, ) - except Exception as e: - self._logger.error(f"Skipping code skill '{skill_id}' in available_skills: {e}") - continue - - for skill_id, physical_data in physical_skills_snapshot.items(): - try: - info = self._build_physical_skill_info(skill_id, physical_data, in_training=False) - if info is not None: - skills.append(info) - except Exception as e: - self._logger.error(f"Skipping physical skill '{skill_id}' in available_skills: {e}") - continue - - for skill_id, physical_data in in_training_skills_snapshot.items(): - try: - info = self._build_physical_skill_info(skill_id, physical_data, in_training=True) - if info is not None: - skills.append(info) - except Exception as e: - self._logger.error(f"Skipping in-training skill '{skill_id}' in available_skills: {e}") - continue + ) filtered_skills = self._dedupe_display_names(skills) @@ -708,72 +832,28 @@ def publish_skills_list(self) -> None: self._logger.info(f"Published {len(filtered_skills)} skills on /brain/available_skills") except Exception as e: self._logger.error(f"Failed to publish AvailableSkills (had {len(filtered_skills)} entries): {e}") - self._write_import_stub(filtered_skills) - - def _write_import_stub(self, skills: list[SkillInfo]) -> None: - """Regenerate ``innate/skills.pyi`` so IDEs complete the proxy imports. - Best effort — never load-bearing, so failures only log.""" - try: - import innate - - stub_path = Path(innate.__file__).with_name("skills.pyi") - stub_path.write_text(self._render_import_stub(skills)) - except Exception as e: - self._logger.warn(f"Could not write innate/skills.pyi: {e}") - - @staticmethod - def _render_import_stub(skills: list[SkillInfo]) -> str: - """One typed def per importable skill, from the published inputs schema.""" - type_names = {"str": "str", "float": "float", "int": "int", "bool": "bool"} - lines = [ - "# Auto-generated from the skill catalog on every (re)load. Do not edit.", - "# A .pyi replaces the module's visible API, so the real names live here too.", - "from contextlib import contextmanager", - "from typing import Any, Iterator", - "", - "from brain_client.skills.types import SkillOutput as SkillOutput", - "", - "class SkillFailed(Exception): ...", - "class SkillCancelled(Exception): ...", - "@contextmanager", - "def use_invoker(invoker: Any) -> Iterator[None]: ...", - ] - # bare import name = id minus prefix; local/ wins, matching SkillInvoker._resolve - by_stem: dict[str, SkillInfo] = {} - for skill in skills: - stem = skill.id.split("/", 1)[-1] - if not stem.isidentifier(): # dashed dirs etc. — self.skills.run() only - continue - if stem not in by_stem or skill.id.startswith("local/"): - by_stem[stem] = skill - for stem, skill in sorted(by_stem.items()): - try: - inputs = json.loads(skill.inputs_json or "{}") - except json.JSONDecodeError: - inputs = {} - params = [] - for param_name, schema in inputs.items(): - if not isinstance(schema, dict) or not param_name.isidentifier(): - continue - annotation = type_names.get(schema.get("type"), "Any") - default = "" if schema.get("required") else " = ..." - params.append(f"{param_name}: {annotation}{default}") - params.append("*, timeout: float | None = ...") - lines.append("") - lines.append(f"def {stem}({', '.join(params)}) -> SkillOutput:") - guidelines = (skill.guidelines or "").replace('"""', "'''").strip() - lines.append(f' """{guidelines}"""' if guidelines else " ...") - return "\n".join(lines) + "\n" def _dedupe_display_names(self, skills: list[SkillInfo]) -> list[SkillInfo]: - """Enforce unique display names (the LLM can't disambiguate duplicates). + """Enforce unique display names (the LLM can't disambiguate them). + A runnable skill outranks a broken one, then a ``local/`` skill claims + the plain name; a same-name skill from any other namespace (shipped or + a dropped-in package) stays published under ``name ()`` so + full-id references keep working. Same-namespace duplicates are a + mistake: first wins.""" + + def namespace(skill: SkillInfo) -> str: + return skill.id.split("/", 1)[0] + + def plain_name_winner(first: SkillInfo, second: SkillInfo) -> tuple[SkillInfo, SkillInfo]: + """(keeps the plain name, gets qualified). A broken skill never + takes the plain name from a runnable one: an unimportable + custom_skills/foo.py would otherwise rename the shipped foo the + agent calls by name — and broken entries aren't registered with + the cloud at all, so the plain name would resolve to nothing.""" + if bool(first.load_error) != bool(second.load_error): + return (second, first) if first.load_error else (first, second) + return (second, first) if namespace(second) == "local" else (first, second) - A user skill (local/) claims the plain name — the same precedence as - SkillInvoker._resolve. The shipped skill stays published under a - qualified name: dropping it would silently unregister directives that - reference it by full id. Duplicates within the same source are a - mistake: the first wins and the rest are skipped. - """ deduped: list[SkillInfo] = [] by_name: dict[str, SkillInfo] = {} for skill in skills: @@ -782,40 +862,79 @@ def _dedupe_display_names(self, skills: list[SkillInfo]) -> list[SkillInfo]: by_name[skill.name] = skill deduped.append(skill) continue - if skill.id.startswith("local/") == existing.id.startswith("local/"): + if namespace(skill) == namespace(existing): self._logger.error( f"DUPLICATE skill name '{skill.name}' between {existing.id} and {skill.id}. " f"Skipping '{skill.id}' — rename the skill to fix this." ) continue - user, shipped = (skill, existing) if skill.id.startswith("local/") else (existing, skill) - qualified = f"{user.name} (innate-os)" - if qualified in by_name: # a second shipped skill with the same name + # Different namespaces: runnable beats broken, then local keeps the + # plain name; between two equal-rank namespaces the first seen keeps + # it (scan order: shipped before packages). The other is published + # qualified, not dropped. + keeps_plain, qualify = plain_name_winner(existing, skill) + qualified = f"{qualify.name} ({namespace(qualify)})" + if qualified in by_name: self._logger.error( - f"DUPLICATE skill name '{qualified}' between {by_name[qualified].id} and {shipped.id}. " - f"Skipping '{shipped.id}' — rename the skill to fix this." + f"DUPLICATE skill name '{qualified}' between {by_name[qualified].id} and {qualify.id}. " + f"Skipping '{qualify.id}' — rename the skill to fix this." ) continue self._logger.warning( - f"Skill '{user.name}': user skill {user.id} overrides shipped {shipped.id}; " - f"publishing the shipped skill as '{qualified}'" + f"Skill '{keeps_plain.name}': {keeps_plain.id} keeps the plain name; " + f"publishing {qualify.id} as '{qualified}'" ) - shipped.name = qualified - by_name[user.name] = user - by_name[shipped.name] = shipped + qualify.name = qualified + by_name[keeps_plain.name] = keeps_plain + by_name[qualified] = qualify # whichever was seen first is already in deduped; append the newcomer deduped.append(skill) return deduped - def republish_cached(self) -> None: - """Re-emit the last published roster (no rebuild). + def _refresh_physical_refs( + self, physical: dict[str, PhysicalSkillEntry], in_training: dict[str, PhysicalSkillEntry] + ) -> None: + """Regenerate the refs from freshly loaded entries — the pre-pass run + before each code-skill import (see __init__). publish_skills_list + writes them again from the roster; the content-compare in write_refs + makes the second write a no-op.""" + infos = [] + dirs_by_id = {} + for snapshot in (physical, in_training): + for skill_id, entry in snapshot.items(): + try: + infos.append(self._build_physical_skill_info(skill_id, entry)) + dirs_by_id[skill_id] = entry.directory + except Exception as e: + self._logger.error(f"Skipping physical skill '{skill_id}' in physical refs: {e}") + self._write_physical_refs(infos, dirs_by_id) + + def _write_physical_refs(self, physical_infos: list[SkillInfo], dirs_by_id: dict[str, str]) -> None: + """Regenerate workspace/physical_skills/ — the typed refs agents and + skills import instead of id strings — plus the per-recording-folder + ``__init__.py`` shims that make the pack-local import spelling work. + Content-compared inside, so an unchanged roster writes nothing (and + can't loop the file watcher).""" + entries = [ + { + "id": info.id, + "guidelines": info.guidelines, + "type": info.type, + "episode_count": info.episode_count, + "in_training": info.in_training, + "dir": dirs_by_id.get(info.id), + } + for info in physical_infos + ] + write_refs(get_workspace_dir() / "physical_skills", render_refs(entries), self._logger) + shims = render_dir_shims(entries) + write_dir_shims(shims, self._logger) + prune_dir_shims(self._skills_directories, shims, self._logger) - Latched delivery only reaches subscribers present at publish time; the - webapp connects through rws after boot and never receives the latched - sample. A low-rate heartbeat calling this lets late subscribers pick up - the roster within one interval. Downstream consumers dedupe by content, - so re-emitting the same message is a no-op for the UI. - """ + def republish_cached(self) -> None: + """Re-emit the last published roster, no rebuild — the heartbeat for + late subscribers (see _last_published_msg). Consumers dedupe by + content, so repeats are a no-op for the UI.""" if self._last_published_msg is not None: self._skills_publisher.publish(self._last_published_msg) @@ -827,15 +946,18 @@ def _build_skill_info( guidelines: str, guidelines_when_running: str, inputs_json: str, + group: str = "", in_training: bool = False, episode_count: int = 0, directory: str = "", wheeled: bool = False, + load_error: str = "", ) -> SkillInfo: msg = SkillInfo() msg.id = skill_id or "" msg.name = name or "" msg.type = skill_type or "" + msg.group = group or "" msg.guidelines = guidelines or "" msg.guidelines_when_running = guidelines_when_running or "" msg.inputs_json = inputs_json or "" @@ -843,24 +965,38 @@ def _build_skill_info( msg.episode_count = int(episode_count or 0) msg.directory = directory or "" msg.wheeled = bool(wheeled) + msg.load_error = load_error or "" return msg - def _inspect_skill_inputs(self, skill_id: str, skill_instance) -> dict: - """Best-effort introspection of a code skill's execute() signature.""" + def _inspect_skill_inputs(self, skill_id: str, skill_instance) -> tuple[dict, frozenset[str], bool]: + """Best-effort introspection of a code skill's execute() signature. + + Returns ``(inputs schema, float-annotated params, accepts **kwargs)``. + The permissive fallback for an un-introspectable signature is + ``({}, frozenset(), True)``: nothing coerces, and validation lets the + call itself decide. + """ if not hasattr(skill_instance, "execute"): - return {} + return {}, frozenset(), True try: signature = inspect.signature(skill_instance.execute) except (TypeError, ValueError) as e: self._logger.warn(f"Could not inspect execute() signature for '{skill_id}': {e}") - return {} + return {}, frozenset(), True inputs: dict = {} + float_params: set[str] = set() + accepts_extra = False for param_name, param in signature.parameters.items(): if param_name == "self": continue - if param.kind in (inspect.Parameter.VAR_KEYWORD, inspect.Parameter.VAR_POSITIONAL): + if param.kind == inspect.Parameter.VAR_KEYWORD: + accepts_extra = True continue # *args/**kwargs are compat plumbing, not inputs + if param.kind == inspect.Parameter.VAR_POSITIONAL: + continue + if _annotation_is_float(param.annotation): + float_params.add(param_name) param_type = "any" enum_values = None try: @@ -897,7 +1033,7 @@ def _inspect_skill_inputs(self, skill_id: str, skill_instance) -> dict: except (TypeError, ValueError): pass inputs[param_name] = param_schema - return inputs + return inputs, frozenset(float_params), accepts_extra def _safe_skill_string(self, skill_id: str, skill_instance, attr: str) -> str: if not hasattr(skill_instance, attr): @@ -909,21 +1045,8 @@ def _safe_skill_string(self, skill_id: str, skill_instance, attr: str) -> str: return "" return str(value) if value is not None else "" - def _build_physical_skill_info(self, skill_id: str, physical_data: dict, *, in_training: bool) -> SkillInfo | None: - metadata = physical_data.get("metadata") - if not isinstance(metadata, dict): - self._logger.error( - f"Physical skill '{skill_id}' has malformed metadata (type={type(metadata).__name__}); skipping" - ) - return None - - directory = physical_data.get("directory", "") or "" - try: - episode_count = self.skill_loader._get_episode_count(directory) - except Exception as e: - self._logger.warn(f"Could not read episode count for '{skill_id}': {e}; defaulting to 0") - episode_count = 0 - + def _build_physical_skill_info(self, skill_id: str, entry: PhysicalSkillEntry) -> SkillInfo: + metadata = entry.metadata try: inputs_json = json.dumps(metadata.get("inputs", {})) except (TypeError, ValueError) as e: @@ -937,9 +1060,12 @@ def _build_physical_skill_info(self, skill_id: str, physical_data: dict, *, in_t guidelines=metadata.get("guidelines", ""), guidelines_when_running=metadata.get("guidelines_when_running", ""), inputs_json=inputs_json, - in_training=in_training, - episode_count=episode_count, - directory=directory, + in_training=entry.in_training, + # re-read at publish time, not entry.episode_count: episodes + # accumulate while a skill trains, and on a robot without the + # file watcher a count frozen at load would never move + episode_count=get_episode_count(entry.directory, self._logger), + directory=entry.directory, wheeled=bool(metadata.get("wheeled", False)), ) @@ -958,6 +1084,7 @@ def _skill_info_to_cache_dict(self, skill: SkillInfo) -> dict: "id": skill.id, "name": skill.name, "type": skill.type, + "group": skill.group, "inputs": inputs, "guidelines": skill.guidelines, "guidelines_when_running": skill.guidelines_when_running, @@ -965,6 +1092,7 @@ def _skill_info_to_cache_dict(self, skill: SkillInfo) -> dict: "episode_count": skill.episode_count, "directory": skill.directory, "wheeled": skill.wheeled, + "load_error": skill.load_error, } def _write_skill_cache(self, skills: list[SkillInfo]) -> None: diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/hot_reload.py b/ros2_ws/src/brain/brain_client/brain_client/skills/hot_reload.py index 14eeaf811..e0c7deeab 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/skills/hot_reload.py +++ b/ros2_ws/src/brain/brain_client/brain_client/skills/hot_reload.py @@ -161,7 +161,7 @@ def perform_full(self) -> None: self._logger, self._state.registry.primitives ) self._state.active_skill_ids = ( - list(self._state.current_directive.get_skills()) if self._state.current_directive else [] + list(self._state.current_directive.skill_ids()) if self._state.current_directive else [] ) self._catalog.register() self._logger.info( @@ -190,10 +190,14 @@ def _await_available_skills(self, timeout_sec: float): waiter = rclpy.create_node("brain_client_reload_skills_waiter") executor = SingleThreadedExecutor() try: + + def on_skills(msg: AvailableSkills) -> None: + received.setdefault("registry", registry_from_skills_msg(msg)) + waiter.create_subscription( AvailableSkills, "/brain/available_skills", - lambda msg: received.setdefault("registry", registry_from_skills_msg(msg)), + on_skills, AVAILABLE_SKILLS_QOS, ) executor.add_node(waiter) @@ -228,9 +232,9 @@ def _reload_agents(self, agent_names: list) -> list: reloaded.append(agent_name) if self._state.current_directive and self._state.current_directive.id == agent_name: self._state.current_directive = agent_instance - previous_skill_ids = set(self._state.active_skill_ids or agent_instance.get_skills()) + previous_skill_ids = set(self._state.active_skill_ids or agent_instance.skill_ids()) self._state.active_skill_ids = [ - skill_id for skill_id in agent_instance.get_skills() if skill_id in previous_skill_ids + skill_id for skill_id in agent_instance.skill_ids() if skill_id in previous_skill_ids ] self._logger.info(f"Updated current directive: {agent_name}") self._logger.info(f"Reloaded agent: {agent_name} [source={agent_instance.source}]") diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/hot_reload_watcher.py b/ros2_ws/src/brain/brain_client/brain_client/skills/hot_reload_watcher.py index 61fbdce80..da9ed13f1 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/skills/hot_reload_watcher.py +++ b/ros2_ws/src/brain/brain_client/brain_client/skills/hot_reload_watcher.py @@ -10,16 +10,31 @@ import threading from collections.abc import Callable from pathlib import Path +from typing import TYPE_CHECKING -try: +# watchdog is optional: the robot falls back to the reload service when it is +# absent. The fallbacks below rebind the names, which a type checker cannot +# use as types — so under TYPE_CHECKING it reads the real ones (start() +# refuses to run unless WATCHDOG_AVAILABLE, so they are always real at use). +if TYPE_CHECKING: from watchdog.events import FileSystemEventHandler from watchdog.observers import Observer + # Observer itself is a per-platform alias (a variable, not a class), so + # annotations need the base class. + from watchdog.observers.api import BaseObserver + WATCHDOG_AVAILABLE = True -except ImportError: - WATCHDOG_AVAILABLE = False - Observer = None - FileSystemEventHandler = object +else: + try: + from watchdog.events import FileSystemEventHandler + from watchdog.observers import Observer + + WATCHDOG_AVAILABLE = True + except ImportError: + WATCHDOG_AVAILABLE = False + Observer = None + FileSystemEventHandler = object class HotReloadWatcher: @@ -36,6 +51,7 @@ def __init__( on_reload: Callable[[list[str], list[str]], None], debounce_seconds: float = 1.0, recursive: bool = False, + workspace_roots: list[str] | None = None, ): """ Initialize the hot reload watcher. @@ -50,17 +66,25 @@ def __init__( live in a per-skill subdirectory (``//metadata.json`` and assets); a change to any file in there reloads that skill. Agents are flat ``.py`` files, so they watch non-recursively. + workspace_roots: Roots holding skill packages (``workspace/``), watched + recursively. Any ``.py`` change under one reloads everything — a + package's modules import each other freely, so no single skill owns + an edit — and because it is one recursive watch on the root, a pack + dropped in after boot (``cp -r john_skills workspace/``) is picked up + without a restart. """ self.logger = logger self.skills_directories = skills_directories self.agents_directories = agents_directories + self.workspace_roots = workspace_roots or [] self.on_reload = on_reload self.debounce_seconds = debounce_seconds self.recursive = recursive - self._observer: Observer | None = None + self._observer: BaseObserver | None = None self._pending_skills: set[str] = set() self._pending_agents: set[str] = set() + self._pending_reload_all = False self._lock = threading.Lock() self._debounce_timer: threading.Timer | None = None self._running = False @@ -78,7 +102,8 @@ def start(self): self.logger.warn("Hot reload watcher already running") return True - self._observer = Observer() + observer = Observer() + self._observer = observer # Create event handler handler = _InternalHandler( @@ -87,13 +112,23 @@ def start(self): ) # Skills honor self.recursive (physical skills live in subdirs); agents are flat. + # A directory already inside a workspace root is skipped: the recursive + # root watch delivers its events, and scheduling both would report every + # change twice. It still resolves through skills_directories, so a + # physical skill's non-.py files map to their skill (see _record_change). watched_count = 0 - watch_specs = [(d, self.recursive) for d in self.skills_directories] + [ - (d, False) for d in self.agents_directories - ] + watch_specs = ( + [(d, self.recursive) for d in self.skills_directories if not self._inside_workspace_root(d)] + + [(d, False) for d in self.agents_directories if not self._inside_workspace_root(d)] + + [(d, True) for d in self.workspace_roots] + ) for directory, recursive in watch_specs: if os.path.exists(directory): - self._observer.schedule(handler, directory, recursive=recursive) + # Watch the resolved path: inotify delivers zero events for a + # watch scheduled through a symlink (a symlinked-in pack), and + # _resolve/_is_workspace_change realpath both sides already, so + # the real-path events still map back to their skills. + observer.schedule(handler, os.path.realpath(directory), recursive=recursive) self.logger.info(f"👁️ Watching for changes: {directory}") watched_count += 1 @@ -101,7 +136,7 @@ def start(self): self.logger.warn("No valid directories to watch for hot reload") return False - self._observer.start() + observer.start() self._running = True self.logger.info(f"🔥 Hot reload watcher started ({watched_count} directories)") return True @@ -121,24 +156,66 @@ def stop(self): self.logger.info("Hot reload watcher stopped") def _on_path_changed(self, file_path: str): - """Called when any watched file changes; resolves it to the skill/agent it belongs to.""" + """Called when any watched file changes; records what it reloads and arms the timer. + + Runs on watchdog's dispatch thread, where an escaped exception kills the + observer — ending hot reload for the rest of the process with nothing but + a one-time traceback — so any failure is logged and the event dropped. + """ + try: + with self._lock: + if not self._record_change(file_path): + return + + # Cancel existing timer and schedule new one + if self._debounce_timer is not None: + self._debounce_timer.cancel() + + self._debounce_timer = threading.Timer(self.debounce_seconds, self._execute_reload) + self._debounce_timer.start() + except Exception as e: # noqa: BLE001 — see docstring: never raise into the observer + self.logger.error(f"Hot reload watcher failed to handle change to {file_path}: {e}") + + def _record_change(self, file_path: str) -> bool: + """Note what a changed file reloads; False if it's noise. Caller holds the lock.""" + if self._is_workspace_change(file_path): + # Workspace packages import each other freely, so no single skill + # owns an edit: empty lists mean "reload everything". + self._pending_reload_all = True + return True resolved = self._resolve(file_path) if resolved is None: - return + return False item_name, is_skill = resolved + target = self._pending_skills if is_skill else self._pending_agents + target.add(item_name) + return True - with self._lock: - if is_skill: - self._pending_skills.add(item_name) - else: - self._pending_agents.add(item_name) + def _inside_workspace_root(self, directory: str) -> bool: + """True when a recursive workspace-root watch already covers ``directory``.""" + path = Path(os.path.realpath(directory)) + for root in self.workspace_roots: + resolved = Path(os.path.realpath(root)) + if path == resolved or resolved in path.parents: + return True + return False - # Cancel existing timer and schedule new one - if self._debounce_timer is not None: - self._debounce_timer.cancel() + def _is_workspace_change(self, file_path: str) -> bool: + """True for a ``.py`` change under a watched workspace root. - self._debounce_timer = threading.Timer(self.debounce_seconds, self._execute_reload) - self._debounce_timer.start() + Skipping ``__pycache__`` is what stops a reload loop: the reload this + triggers writes ``.pyc`` files back under the same root. + """ + path = Path(os.path.realpath(file_path)) + if path.suffix != ".py": + return False + for root in self.workspace_roots: + try: + rel = path.relative_to(Path(os.path.realpath(root))) + except ValueError: + continue + return not any(part == "__pycache__" or part.startswith(".") for part in rel.parts) + return False def _resolve(self, file_path: str) -> tuple[str, bool] | None: """Map a changed file to ``(item_name, is_skill)``, or ``None`` if it's noise. @@ -164,17 +241,28 @@ def _resolve(self, file_path: str) -> tuple[str, bool] | None: def _execute_reload(self): """Execute pending reloads.""" with self._lock: + reload_all = self._pending_reload_all skills = list(self._pending_skills) agents = list(self._pending_agents) + self._pending_reload_all = False self._pending_skills.clear() self._pending_agents.clear() - if skills or agents: + if not reload_all and not skills and not agents: + return + + if reload_all: + # Empty lists mean "reload everything", which also covers whatever + # individual skills changed inside the same debounce window. + skills, agents = [], [] + self.logger.info("🔄 Hot reload triggered - shared helper changed, reloading all skills") + else: self.logger.info(f"🔄 Hot reload triggered - skills: {skills}, agents: {agents}") - try: - self.on_reload(skills, agents) - except Exception as e: - self.logger.error(f"Hot reload failed: {e}") + + try: + self.on_reload(skills, agents) + except Exception as e: + self.logger.error(f"Hot reload failed: {e}") def _flat_py_name_for(root: Path, path: Path) -> str | None: @@ -205,6 +293,8 @@ def _skill_name_for(root: Path, path: Path) -> str | None: except ValueError: return None parts = rel.parts + if not parts: + return None # the event is on the root itself (e.g. the directory was removed) if len(parts) == 1: return _flat_py_name_for(root, path) # top-level code skill if any(part.startswith(".") or part == "__pycache__" for part in parts): @@ -233,19 +323,19 @@ def __init__(self, logger, on_path_changed: Callable[[str], None]): def on_modified(self, event): if not event.is_directory: - self.on_path_changed(event.src_path) + self.on_path_changed(os.fsdecode(event.src_path)) def on_created(self, event): if not event.is_directory: - self.on_path_changed(event.src_path) + self.on_path_changed(os.fsdecode(event.src_path)) def on_deleted(self, event): # Deleting a skill's .py or a physical skill's file should reload it so the # catalog drops the stale entry. if not event.is_directory: - self.on_path_changed(event.src_path) + self.on_path_changed(os.fsdecode(event.src_path)) def on_moved(self, event): # Atomic saves arrive as a rename onto the target, not a modify. if not event.is_directory and event.dest_path: - self.on_path_changed(event.dest_path) + self.on_path_changed(os.fsdecode(event.dest_path)) diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/invoker.py b/ros2_ws/src/brain/brain_client/brain_client/skills/invoker.py index 65d28068b..1f0b9fe87 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/skills/invoker.py +++ b/ros2_ws/src/brain/brain_client/brain_client/skills/invoker.py @@ -1,21 +1,16 @@ # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc -"""self.skills.run(...) — lets one skill run others, in order. - -Children reuse the same server execution paths as a top-level skill, so any -kind of skill (python, learned, replay) works. They run on the parent's goal: -each child's start/finish is tagged onto the parent's feedback stream and -decoded back into a per-step status by PrimitiveRunner._on_feedback. -""" +"""self.skills.run(...) — lets one skill run others, in order. Children run +on the parent's goal; their start/finish is tagged onto the parent's feedback +stream and decoded back by PrimitiveRunner.""" from __future__ import annotations -import inspect import threading import uuid from brain_client.skills.lifecycle import encode_substep_feedback -from brain_client.skills.types import SkillResult +from brain_client.skills.types import SkillCancelled, SkillOutput, SkillResult _STEP_EVENT = { SkillResult.SUCCESS: "completed", @@ -27,104 +22,152 @@ class SkillInvoker: """Runs child skills on behalf of a parent skill's execute_skill goal.""" - def __init__(self, server, goal_handle, publish_feedback): + def __init__(self, server, goal_handle, publish_feedback, run_node): self._server = server self._goal_handle = goal_handle self._publish_feedback = publish_feedback + # children live on the top-level run's node; entities die with it + self._run_node = run_node self._logger = server.get_logger() self._cancelled = False - # guards cancel() against re-entry: a child's default cancel() delegates - # right back to the invoker that is cancelling it. + # a child's default cancel() delegates right back to the invoker + # cancelling it — guard against the re-entry. The thread identity + # tells that re-entry (same thread, mid-_cancel_active_child) apart + # from a genuine Stop arriving on another thread. self._cancelling = False + self._cancelling_thread = None self._active_code_skill = None self._active_physical_skill = None - def run(self, skill_id, *, timeout=None, **inputs): - """Run one child skill to completion. Returns (message, SkillResult). - - Accepts a full catalog id ("innate-os/wave") or a bare name ("wave"); - bare names try local/ before innate-os/. A child still running when - ``timeout`` (seconds) expires is cancelled and reported as a FAILURE, - without cancelling the rest of the routine. - """ + def run(self, skill_id, *, timeout=None, **inputs) -> SkillOutput: + """Run one child to completion; returns a SkillOutput whose status is + SUCCESS or FAILURE. A child outliving ``timeout`` seconds is + cancelled and reported as a FAILURE without cancelling the rest of + the routine. A cancelled routine raises SkillCancelled instead of + returning, so callers never handle CANCELLED themselves.""" if self._cancelled: - return "Routine cancelled", SkillResult.CANCELLED + raise SkillCancelled("Routine cancelled") skill_id, code, physical = self._resolve(skill_id) - if code is None and physical is None: - self._logger.error(f"[invoker] unknown skill '{skill_id}'") - return f"Unknown skill '{skill_id}'", SkillResult.FAILURE - if code is not None: - problem = self._invalid_inputs(code[1], skill_id, inputs) + problem = self._invalid_inputs(code, skill_id, inputs) if problem: self._logger.error(f"[invoker] {problem}") - return problem, SkillResult.FAILURE - - name = code[1].name if code else physical["metadata"].get("name", skill_id) + return SkillOutput(problem, status=SkillResult.FAILURE) + name = code.display_name + elif physical is not None: + name = physical.metadata.get("name", skill_id) + else: + reason = self._server.catalog.unavailable_reason(skill_id) + if reason: + self._logger.error(f"[invoker] skill '{skill_id}' {reason}") + return SkillOutput(f"Skill '{skill_id}' {reason}", status=SkillResult.FAILURE) + self._logger.error(f"[invoker] unknown skill '{skill_id}'") + return SkillOutput(f"Unknown skill '{skill_id}'", status=SkillResult.FAILURE) step_id = uuid.uuid4().hex self._logger.info(f"[invoker] running '{skill_id}' inputs={inputs} timeout={timeout}") self._step(step_id, name, skill_id, "running") timed_out = threading.Event() watchdog = None + # The settle handshake closes the window between the child finishing + # (its finally restores the active-skill slot to the parent) and run() + # cancelling the timer below: a timer expiring in that window would + # cancel whatever ancestor now owns the slot. The child's finally + # settles under the lock BEFORE restoring the slot, so _expire either + # sees settled (no-op) or cancels while the slot still points at the + # child. + settle_lock = threading.Lock() + settled = False + + def _settle(): + nonlocal settled + with settle_lock: + settled = True + if timeout: def _expire(): - timed_out.set() - self._cancel_active_child() + with settle_lock: + if settled: + return # child already finished — the slot may belong to an ancestor + timed_out.set() + # Scope the cancel to THIS call's subtree. When the timed-out + # child is physical, _active_code_skill (if any) is the code + # skill that dispatched it — an ancestor, not the child — and + # cancelling it would unwind the very routine the timeout is + # documented not to cancel. A code child's descendants (deeper + # code, a physical it delegated to) are fair game: execution + # is strictly nested, so anything active below it is its own. + self._cancel_active_child(include_code=code is not None) watchdog = threading.Timer(timeout, _expire) watchdog.start() try: if code is not None: - message, status = self._run_code(code[1], skill_id, inputs) + output = self._run_code(code, skill_id, inputs, settle=_settle) else: - message, status = self._run_physical(skill_id, physical) + output = self._run_physical(skill_id, physical, settle=_settle) except Exception as e: self._logger.error(f"[invoker] '{skill_id}' raised: {e}") self._step(step_id, name, skill_id, "failed", reason=str(e)) - return str(e), SkillResult.FAILURE + return SkillOutput(str(e), status=SkillResult.FAILURE) finally: if watchdog is not None: watchdog.cancel() - if timed_out.is_set() and status is SkillResult.CANCELLED: + if timed_out.is_set() and output.status is SkillResult.CANCELLED: # only this child timed out, not the routine — report a step # failure so later steps still run - message, status = f"'{skill_id}' timed out after {timeout}s", SkillResult.FAILURE + output = SkillOutput(f"'{skill_id}' timed out after {timeout}s", status=SkillResult.FAILURE) self._step( step_id, name, skill_id, - _STEP_EVENT.get(status, "completed"), - reason=message if status is SkillResult.FAILURE else None, - output=message if status is SkillResult.SUCCESS else None, + _STEP_EVENT.get(output.status, "completed"), + reason=output.message if output.status is SkillResult.FAILURE else None, + output=output.message if output.status is SkillResult.SUCCESS else None, ) - self._cancelled = self._cancelled or status is SkillResult.CANCELLED - return message, status + if output.status is SkillResult.CANCELLED: + self._cancelled = True + raise SkillCancelled(output.message) + return output def cancel(self): - """Stop the routine: the child running right now, and every step after it. - - Call from the parent skill's cancel(). The timeout watchdog instead uses - _cancel_active_child directly so it never marks the whole routine cancelled. - """ + """Stop the routine: the running child and every step after it. The + timeout watchdog uses _cancel_active_child directly so it never marks + the whole routine cancelled.""" if self._cancelling: - return "Routine cancelled" # re-entered from the child we're cancelling + if threading.current_thread() is self._cancelling_thread: + return "Routine cancelled" # re-entered from the child we're cancelling + # a Stop racing the watchdog's child-cancel: the child is already + # being cancelled, but the routine itself must still latch + self._cancelled = True + return "Routine cancelled" self._cancelled = True return self._cancel_active_child() - def _cancel_active_child(self): + def _cancel_active_child(self, include_code=True): + """Cancel the running children. ``include_code=False`` is the timeout + watchdog for a physical child: the active code skill (if any) is the + one that DISPATCHED that child — cancelling it would unwind the + routine the timeout must not cancel — so only the behavior goal is + touched. The physical cancel always runs here (not via the code + skill's own cancel()): its forward to invoker.cancel() re-enters on + this thread and no-ops.""" if self._cancelling: return "Routine cancelled" + self._cancelling_thread = threading.current_thread() self._cancelling = True try: - if self._active_code_skill is not None: + if include_code and self._active_code_skill is not None: try: self._active_code_skill.cancel() - except Exception as e: + except (Exception, SkillCancelled) as e: + # SkillCancelled (a BaseException) included: a child's + # cancel() override may raise it, and the physical-goal + # cancel below must still be dispatched. self._logger.error(f"[invoker] error cancelling child: {e}") if self._active_physical_skill is not None: self._server._request_behavior_goal_cancel(self._goal_handle, self._active_physical_skill) @@ -133,22 +176,33 @@ def _cancel_active_child(self): self._cancelling = False @staticmethod - def _invalid_inputs(skill, skill_id, inputs): - """An error message if inputs don't fit execute()'s signature, else None.""" - try: - signature = inspect.signature(skill.execute) - except (TypeError, ValueError): - return None # can't introspect — let the call itself decide - try: - signature.bind(**inputs) + def _invalid_inputs(entry, skill_id, inputs): + """An error message if inputs don't fit the harvested schema, else + None — checked before an instance exists.""" + missing = [name for name, spec in entry.inputs.items() if spec.get("required") and name not in inputs] + unknown = [] if entry.accepts_extra_inputs else [name for name in inputs if name not in entry.inputs] + if not missing and not unknown: return None - except TypeError as e: - expected = ", ".join(p for p in signature.parameters if p != "self") or "no inputs" - return f"Invalid inputs for '{skill_id}': {e}. Expected: ({expected})" + problems = [] + if missing: + problems.append(f"missing required: {', '.join(missing)}") + if unknown: + problems.append(f"unexpected: {', '.join(unknown)}") + expected = ", ".join(entry.inputs) or "no inputs" + return f"Invalid inputs for '{skill_id}': {'; '.join(problems)}. Expected: ({expected})" + + def find(self, skill_id: str) -> str | None: + """The resolved id for ``skill_id``, or None if no such skill. + + Used at wire time to fail a declared physical skill up front rather + than mid-routine. + """ + resolved, code, physical = self._resolve(skill_id) + return resolved if (code is not None or physical is not None) else None def _resolve(self, skill_id): """Look skill_id up in the catalog. Returns (resolved_id, code_entry, physical_entry).""" - candidates = [skill_id] if "/" in skill_id else [f"local/{skill_id}", f"innate-os/{skill_id}"] + candidates = self._server.catalog.bare_id_candidates(skill_id) for candidate in candidates: code = self._server.catalog.get_code_skill(candidate) if code is not None: @@ -158,28 +212,29 @@ def _resolve(self, skill_id): return candidate, None, physical return skill_id, None, None - def _run_code(self, skill, skill_id, inputs): - skill.set_feedback_callback(self._publish_feedback) - skill.skills = self # a child can chain too + def _run_code(self, entry, skill_id, inputs, settle=None): + skill = self._server._instantiate_for_run(entry, self._run_node, self, self._publish_feedback) # save/restore, not clear: in a nested chain A→B→C the slot must hand # back to B when C ends, or cancel() can no longer reach B prev = self._active_code_skill self._active_code_skill = skill try: - return self._server._run_code_skill_body(skill, skill_id, inputs, self._goal_handle) + return self._server._run_code_skill_body(skill, entry, skill_id, inputs, self._goal_handle) finally: + if settle is not None: + settle() # before the slot changes hands — see run()'s settle handshake self._active_code_skill = prev + self._server._dispose_run_instance(skill) - def _run_physical(self, skill_id, physical): + def _run_physical(self, skill_id, physical, settle=None): prev = self._active_physical_skill self._active_physical_skill = skill_id try: - success, message, success_type, _finalize = self._server._run_physical_skill( - self._goal_handle, skill_id, physical - ) + return self._server._run_physical_skill(self._goal_handle, skill_id, physical) finally: + if settle is not None: + settle() # before the slot changes hands — see run()'s settle handshake self._active_physical_skill = prev - return message, SkillResult(success_type) def _step(self, step_id, name, skill_id, event, reason=None, output=None): self._publish_feedback( diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/loader.py b/ros2_ws/src/brain/brain_client/brain_client/skills/loader.py deleted file mode 100644 index 2a972b611..000000000 --- a/ros2_ws/src/brain/brain_client/brain_client/skills/loader.py +++ /dev/null @@ -1,207 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -""" -Dynamic Skill Loader - -This module provides functionality to dynamically discover and load skill classes -from specified directories. It validates that skills inherit from the Skill base class -and can automatically register them with the execution server. -""" - -import json -import logging -import os -from pathlib import Path - -import h5py - -from brain_client.common.dynamic_loader import DynamicLoader -from brain_client.skills.types import Skill - - -class SkillLoader(DynamicLoader): - """ - Dynamically loads skill classes from specified directories. - """ - - base_class = Skill - - def _validate_class(self, skill_class: type[Skill]) -> bool: - # Check that required abstract methods are implemented - required_methods = ["name", "execute", "cancel"] - for method_name in required_methods: - if not hasattr(skill_class, method_name): - self.logger.error(f"Skill {skill_class.__name__} missing required method: {method_name}") - return False - - # Check that name is a property - if not hasattr(skill_class, "name") or not isinstance(skill_class.name, property): - self.logger.error(f"Skill {skill_class.__name__} name must be a property") - return False - - return True - - def _get_name(self, skill_class: type[Skill]) -> str: - try: - temp_logger = logging.getLogger(f"temp_{skill_class.__name__}") - instance = skill_class(temp_logger) - try: - return instance.name - finally: - # The throwaway instance may own ROS entities (e.g. BasicNavigator - # nodes); shut them down or every discovery pass leaks them. - self._shutdown_quietly(instance) - except Exception as e: - self.logger.debug(f"Could not get name from skill {skill_class.__name__}: {e}") - return self._fallback_name(skill_class) - - def _shutdown_quietly(self, instance: Skill) -> None: - try: - instance.shutdown() - except Exception as e: - self.logger.debug(f"Error shutting down temp {type(instance).__name__} instance: {e}") - - def reload_skill_by_file_stem(self, file_stem: str, directories: list[str]) -> tuple[type[Skill], Path] | None: - """ - Reload a code skill by its file stem (e.g. 'navigate_to_position'). - - Returns: - (class, source_file) or None if not found. - """ - for directory in directories: - py_file = Path(directory) / f"{file_stem}.py" - if not py_file.exists(): - continue - try: - discovered = self.discover_in_file(py_file) - # Return the first skill found in the file - for _name, entry in discovered.items(): - self.logger.info(f"Reloaded skill from {py_file}") - return entry - except Exception as e: - self.logger.debug(f"Error reloading {py_file}: {e}") - - self.logger.warning(f"Could not find code skill file '{file_stem}.py' in any directory") - return None - - def validate_physical_skill(self, skill_dir: str, metadata: dict) -> tuple: - """Validate a physical skill. - - Returns: - tuple: (is_valid: bool, is_in_training: bool, episode_count: int) - - is_valid: True if the skill can be loaded (either ready or in training) - - is_in_training: True if the skill is a learned type missing its checkpoint - - episode_count: Number of recorded episodes (0 if not applicable or not found) - """ - skill_type = metadata.get("type", "").lower() - execution = metadata.get("execution", {}) - - if skill_type == "learned": - is_valid, is_in_training = self._validate_learned_skill(skill_dir, execution) - episode_count = self._get_episode_count(skill_dir) - return (is_valid, is_in_training, episode_count) - elif skill_type == "eval": - # A rollout-capture dataset: always valid, never "in training" (it has - # no checkpoint and is never trained), episodes counted like a dataset. - return (True, False, self._get_episode_count(skill_dir)) - elif skill_type == "replay": - # A replay draft (created up front, still being recorded into data/) has no - # replay_file yet — treat it as in_training rather than invalid so it loads - # cleanly until the take is saved and the trajectory is written. - if not execution.get("replay_file") and os.path.isdir(os.path.join(skill_dir, "data")): - return (True, True, 0) - is_valid = self._validate_replay_skill(skill_dir, execution) - return ( - is_valid, - False, - 0, - ) # Replay skills are never "in training", no episodes - else: - self.logger.warning(f"Unknown skill type '{skill_type}' in {skill_dir}") - return (True, False, 0) # Allow unknown types but log warning - - def _validate_learned_skill(self, skill_dir: str, execution: dict) -> tuple: - """Validate a learned skill. - - Returns: - tuple: (is_valid: bool, is_in_training: bool) - """ - checkpoint_file = execution.get("checkpoint") - # If no checkpoint specified, it's in training - if not checkpoint_file: - self.logger.info(f"Learned skill in {skill_dir} has no checkpoint - marked as in_training") - return (True, True) # Valid but in training - - checkpoint_path = os.path.join(skill_dir, checkpoint_file) - if not os.path.exists(checkpoint_path): - self.logger.info(f"Learned skill checkpoint not found: {checkpoint_path} - marked as in_training") - return (True, True) # Valid but in training - - # Check for stats file (optional but commonly needed) - stats_file = execution.get("stats_file", "dataset_stats.pt") - stats_path = os.path.join(skill_dir, stats_file) - if not os.path.exists(stats_path): - self.logger.warning(f"Learned skill stats file not found: {stats_path} (optional)") - - self.logger.info(f"Learned skill validation passed: {skill_dir}") - return (True, False) # Valid and ready - - def _get_episode_count(self, skill_dir: str) -> int: - """Get the number of recorded episodes for a learned skill. - - Looks for data/dataset_metadata.json and reads the number_of_episodes field. - - Args: - skill_dir: Path to the skill directory. - - Returns: - int: Number of episodes, or 0 if not found. - """ - dataset_metadata_path = os.path.join(skill_dir, "data", "dataset_metadata.json") - - if not os.path.exists(dataset_metadata_path): - return 0 - - try: - with open(dataset_metadata_path) as f: - dataset_metadata = json.load(f) - return dataset_metadata.get("number_of_episodes", 0) - except Exception as e: - self.logger.warning(f"Error reading dataset metadata from {dataset_metadata_path}: {e}") - return 0 - - def _validate_replay_skill_internal(self, skill_dir: str, execution: dict) -> bool: - """Internal validation for replay skills. Returns bool for validity.""" - replay_file = execution.get("replay_file") - if not replay_file: - self.logger.warning(f"Replay skill in {skill_dir} missing replay_file in execution config") - return False - - replay_path = os.path.join(skill_dir, replay_file) - if not os.path.exists(replay_path): - self.logger.warning(f"Replay skill file not found: {replay_path}") - return False - - # Validate H5 file structure - try: - with h5py.File(replay_path, "r") as h5file: - if "action" not in h5file: - self.logger.warning(f"Replay file {replay_path} missing required 'action' dataset") - return False - - actions = h5file["action"][:] - if actions.shape[0] == 0: - self.logger.warning(f"Replay file {replay_path} contains no actions") - return False - - except Exception as e: - self.logger.warning(f"Failed to validate replay file {replay_path}: {e}") - return False - - self.logger.info(f"Replay skill validation passed: {skill_dir}") - return True - - def _validate_replay_skill(self, skill_dir: str, execution: dict) -> bool: - """Validate a replay skill. Returns bool for backwards compatibility.""" - return self._validate_replay_skill_internal(skill_dir, execution) diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/physical.py b/ros2_ws/src/brain/brain_client/brain_client/skills/physical.py new file mode 100644 index 000000000..28e9f53fb --- /dev/null +++ b/ros2_ws/src/brain/brain_client/brain_client/skills/physical.py @@ -0,0 +1,172 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +"""Physical-skill validation: learned checkpoints, replay trajectories, episodes. + +Physical skills are data (``metadata.json`` + checkpoints/H5), not Python, so +they are read and validated rather than imported — the one part of discovery +the import model does not cover. +""" + +import json +import os +from typing import cast + +import h5py + + +def has_physical_metadata(skill_dir) -> bool: + """True if ``skill_dir`` holds a physical skill's ``metadata.json``. + + Existence alone is not enough: the training client's file lock + (``skill_manager._locked_metadata``) historically touched 0-byte + ``metadata.json`` files into arbitrary ``custom_skills/`` subdirs, and a + crash mid-write can leave one behind. Real metadata is always written + whole, so an empty file carries no user intent and must read as "absent" — + everywhere, consistently: the catalog and workspace import both classify + "data dir vs code package" off this file, so disagreeing on empties would + either roster phantom broken skills or silently drop a code package from + import. + """ + try: + return os.path.getsize(os.path.join(str(skill_dir), "metadata.json")) > 0 + except OSError: + return False + + +def shutdown_quietly(instance, logger) -> None: + """Shut down a throwaway skill instance; log rather than raise.""" + try: + instance.shutdown() + except Exception as e: # noqa: BLE001 — teardown must not mask the real error + logger.debug(f"Error shutting down temp {type(instance).__name__} instance: {e}") + + +def validate_physical_skill(skill_dir: str, metadata: dict, logger) -> tuple: + """Validate a physical skill. + + Returns: + tuple: (is_valid: bool, is_in_training: bool) + - is_valid: True if the skill can be loaded (either ready or in training) + - is_in_training: True if the skill's runnable data isn't on disk yet (a + learned skill's checkpoint, or a replay skill's trajectory) + + Episode counts are deliberately NOT part of validation: episodes accumulate + while a skill trains, so the roster re-reads them at publish time + (catalog._build_physical_skill_info -> get_episode_count). + """ + skill_type = metadata.get("type", "").lower() + execution = metadata.get("execution", {}) + + if skill_type == "learned": + return _validate_learned_skill(skill_dir, execution, logger) + elif skill_type == "eval": + # A rollout-capture dataset: always valid, never "in training" (it has + # no checkpoint and is never trained). + return (True, False) + elif skill_type == "replay": + # A replay draft (created up front, still being recorded into data/) has no + # replay_file yet — treat it as in_training rather than invalid so it loads + # cleanly until the take is saved and the trajectory is written. + if not execution.get("replay_file") and os.path.isdir(os.path.join(skill_dir, "data")): + return (True, True) + # A named replay_file that isn't on disk is the not-fetched-yet state, not + # damage: recording folders ship in git with metadata.json + the generated + # ref shim but without the trajectory (see metadata["downloads"]). Mirror + # the missing-checkpoint treatment learned skills get — roster it as + # in_training so the typed ref keeps existing (agents importing it stay + # loadable) and execution is refused with a reason, not "unknown skill". + replay_file = execution.get("replay_file") + if replay_file and not os.path.exists(os.path.join(skill_dir, replay_file)): + logger.info( + f"Replay trajectory not on disk yet: {os.path.join(skill_dir, replay_file)} - marked as in_training" + ) + return (True, True) + return (_validate_replay_skill(skill_dir, execution, logger), False) + else: + logger.warning(f"Unknown skill type '{skill_type}' in {skill_dir}") + return (True, False) # Allow unknown types but log warning + + +def _validate_learned_skill(skill_dir: str, execution: dict, logger) -> tuple: + """Validate a learned skill. + + Returns: + tuple: (is_valid: bool, is_in_training: bool) + """ + checkpoint_file = execution.get("checkpoint") + # If no checkpoint specified, it's in training + if not checkpoint_file: + logger.info(f"Learned skill in {skill_dir} has no checkpoint - marked as in_training") + return (True, True) # Valid but in training + + checkpoint_path = os.path.join(skill_dir, checkpoint_file) + if not os.path.exists(checkpoint_path): + logger.info(f"Learned skill checkpoint not found: {checkpoint_path} - marked as in_training") + return (True, True) # Valid but in training + + # Check for stats file (optional but commonly needed) + stats_file = execution.get("stats_file", "dataset_stats.pt") + stats_path = os.path.join(skill_dir, stats_file) + if not os.path.exists(stats_path): + logger.warning(f"Learned skill stats file not found: {stats_path} (optional)") + + logger.info(f"Learned skill validation passed: {skill_dir}") + return (True, False) # Valid and ready + + +def get_episode_count(skill_dir: str, logger) -> int: + """Get the number of recorded episodes for a learned skill. + + Looks for data/dataset_metadata.json and reads the number_of_episodes field. + + Args: + skill_dir: Path to the skill directory. + + Returns: + int: Number of episodes, or 0 if not found. + """ + dataset_metadata_path = os.path.join(skill_dir, "data", "dataset_metadata.json") + + if not os.path.exists(dataset_metadata_path): + return 0 + + try: + with open(dataset_metadata_path) as f: + dataset_metadata = json.load(f) + return dataset_metadata.get("number_of_episodes", 0) + except Exception as e: + logger.warning(f"Error reading dataset metadata from {dataset_metadata_path}: {e}") + return 0 + + +def _validate_replay_skill(skill_dir: str, execution: dict, logger) -> bool: + """Validate a replay skill's trajectory file. Returns bool for validity.""" + replay_file = execution.get("replay_file") + if not replay_file: + logger.warning(f"Replay skill in {skill_dir} missing replay_file in execution config") + return False + + replay_path = os.path.join(skill_dir, replay_file) + if not os.path.exists(replay_path): + logger.warning(f"Replay skill file not found: {replay_path}") + return False + + # Validate H5 file structure + try: + with h5py.File(replay_path, "r") as h5file: + if "action" not in h5file: + logger.warning(f"Replay file {replay_path} missing required 'action' dataset") + return False + + # cast: narrows h5py's Group | Dataset | Datatype lookup union + actions = cast(h5py.Dataset, h5file["action"])[:] + if actions.shape[0] == 0: + logger.warning(f"Replay file {replay_path} contains no actions") + return False + + except Exception as e: + logger.warning(f"Failed to validate replay file {replay_path}: {e}") + return False + + logger.info(f"Replay skill validation passed: {skill_dir}") + return True diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/physical_refs.py b/ros2_ws/src/brain/brain_client/brain_client/skills/physical_refs.py new file mode 100644 index 000000000..7738b082f --- /dev/null +++ b/ros2_ws/src/brain/brain_client/brain_client/skills/physical_refs.py @@ -0,0 +1,256 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +"""Generate the ``physical_skills`` package: one TrainedSkill subclass per +physical skill, so agents and skills reference them typed instead of by id +string. Each recording folder also gets a generated ``__init__.py`` shim +next to its metadata.json (see render_dir_shims), so the pack-local spelling +``from innate_skills.pick_socks import PickSocks`` resolves to the same +class — the folder becomes an importable package while staying a data dir +to the scanner (workspace_import skips metadata dirs from proactive import; +the shim loads transitively). Rendering is pure (testable without ROS); the +catalog calls the writers on every roster publish, and the content-compare +inside makes regeneration a no-op when nothing changed — which also keeps +the workspace hot-reload watcher from looping on our own writes.""" + +from __future__ import annotations + +import os +import re +import textwrap +from pathlib import Path + +from .physical import has_physical_metadata + +_HEADER = '''\ +# AUTO-GENERATED by the skill catalog from the physical skills on this robot. +# Regenerated on every skill change — do not edit; edits are overwritten. +"""Typed references to this robot's physical skills. + +Import a ref from its recording folder; this package is the generated +definition site the folder shims re-export from: + + from innate_skills.pick_socks import PickSocks # the usual spelling + from physical_skills import PickSocks # same class + +Use them anywhere a skill id goes: in an Agent's get_skills() list, or as a +declaration on a Skill (``pick: PickSocks``). +""" + +from innate import TrainedSkill + +''' + +# First line of every dir shim — write_dir_shims only ever overwrites or +# removes files that start with it, so a hand-written __init__.py is safe. +_SHIM_MARKER = "# AUTO-GENERATED physical-skill ref" + +# Dir shims land in workspace/ and get committed, so they have to survive +# `ruff format` untouched: hence "{name}" and not {name!r}, whose single +# quotes ruff rewrites and CI then fails on. Safe because class_name_for() +# only ever returns a valid identifier. + +_DIR_SHIM = ( + _SHIM_MARKER + + ''' by the skill catalog — do not edit; edits are overwritten. +"""Typed ref to {skill_id!r}, the recording in this folder. + +Same class either way: + + from physical_skills import {name} +""" + +from physical_skills import {name} + +__all__ = ["{name}"] +''' +) + + +def class_name_for(skill_id: str) -> str: + """``local/pick-socks`` -> ``PickSocks``. '' if no valid identifier results.""" + leaf = skill_id.rsplit("/", 1)[-1] + parts = [p for p in re.split(r"[-_. ]+", leaf) if p] + name = "".join(p[:1].upper() + p[1:] for p in parts) + return name if name.isidentifier() else "" + + +def _docstring(entry: dict) -> str: + """The ref's docstring: guidelines, then a summary line. Escaped so + arbitrary guidelines text can't break out of the literal.""" + guidelines = (entry.get("guidelines") or "").strip() + guidelines = guidelines.replace("\\", "\\\\").replace('"""', '\\"\\"\\"') + kind = entry.get("type") or "physical" + summary = f"{kind} skill · {entry.get('episode_count', 0)} episodes" + if entry.get("in_training"): + summary += " · still in training" + if not guidelines: + return f' """{summary}"""' + return textwrap.indent(f'"""{guidelines}\n\n{summary}\n"""', " ") + + +def _claimed(entries: list[dict]) -> dict[str, dict]: + """``{class_name: winning entry}`` in deterministic claim order. + + local/ first: on a class-name collision the user's skill claims the + name, mirroring the catalog's plain-display-name precedence. Shared by + render_refs and render_dir_shims so the two can't disagree on winners.""" + claims: dict[str, dict] = {} + for entry in sorted(entries, key=lambda e: (not e["id"].startswith("local/"), e["id"])): + name = class_name_for(entry["id"]) + if name and name not in claims: + claims[name] = entry + return claims + + +def render_refs(entries: list[dict]) -> dict[str, str]: + """The full ``physical_skills`` package for ``entries`` (dicts with at + least ``id``; optional guidelines/type/episode_count/in_training), as + ``{filename: source}`` — write_refs prunes anything else, so a file + dropped from this mapping is deleted on the next publish. Sorted for + deterministic output; collisions and non-identifier names are skipped + with a visible comment.""" + lines = [_HEADER] + claims = _claimed(entries) + by_name: dict[str, str] = {} + for entry in sorted(entries, key=lambda e: (not e["id"].startswith("local/"), e["id"])): + skill_id = entry["id"] + name = class_name_for(skill_id) + if not name: + lines.append(f"# skipped {skill_id}: cannot derive a Python class name\n") + continue + if claims[name] is not entry: + lines.append(f"# skipped {skill_id}: class name {name} already taken by {claims[name]['id']}\n") + continue + by_name[name] = skill_id + lines.append(f"\nclass {name}(TrainedSkill):\n{_docstring(entry)}\n\n skill_id = {skill_id!r}\n") + available = ", ".join(sorted(by_name)) or "none — no physical skills on this robot yet" + lines.append( + "\n\ndef __getattr__(name):\n" + " raise AttributeError(\n" + ' f"No physical skill named {name!r} on this robot (deleted, renamed, ' + 'or not created yet?). "\n' + f" {'Available: ' + available!r}\n" + " )\n" + ) + return {"__init__.py": "".join(lines)} + + +def write_refs(package_dir: str | Path, files: dict[str, str], logger) -> None: + """Write the ``physical_skills`` package atomically: each file in + ``files`` (skipping identical content), then remove generated modules + whose skill is gone. Failure is logged, never raised — refs are a + convenience and must not break skill loading.""" + try: + package_dir = Path(package_dir) + package_dir.mkdir(parents=True, exist_ok=True) + changed = 0 + for filename, content in files.items(): + target = package_dir / filename + try: + if target.read_text() == content: + continue + except OSError: + pass + # pid in the tmp name: the skills server and brain_client both + # write these files, and a shared tmp path would let them interleave + tmp = target.with_name(f"{filename}.{os.getpid()}.tmp") + tmp.write_text(content) + os.replace(tmp, target) + changed += 1 + # the whole directory is generated (and gitignored), so any .py we + # didn't just render is a stale ref for a deleted or renamed skill + for stale in package_dir.glob("*.py"): + if stale.name not in files: + stale.unlink(missing_ok=True) + changed += 1 + if changed: + logger.info(f"Regenerated physical skill refs at {package_dir} ({changed} files)") + except OSError as e: + logger.warning(f"Could not write physical skill refs: {e}") + + +def render_dir_shims(entries: list[dict]) -> dict[str, str | None]: + """``{/__init__.py: source}`` for entries that carry a ``dir``. + + The shim makes the recording folder an importable package, so the + pack-local spelling (``from innate_skills.wave import Wave``) resolves to + the class defined in ``physical_skills``. ``None`` marks a shim to + *remove*: the entry exists but lost its class name (collision loser or + no derivable identifier), so a previously written shim must not linger + and re-export a name that now belongs to another skill.""" + claims = _claimed(entries) + shims: dict[str, str | None] = {} + for name, entry in claims.items(): + if entry.get("dir"): + path = os.path.join(entry["dir"], "__init__.py") + shims[path] = _DIR_SHIM.format(skill_id=entry["id"], name=name) + winner_ids = {entry["id"] for entry in claims.values()} + for entry in entries: + if entry.get("dir") and entry["id"] not in winner_ids: + shims.setdefault(os.path.join(entry["dir"], "__init__.py"), None) + return shims + + +def write_dir_shims(shims: dict[str, str | None], logger) -> None: + """Write (or remove, for ``None``) ref shims inside recording folders. + + Only files whose first line is the shim marker are ever overwritten or + deleted — a hand-written ``__init__.py`` in a skill folder is left alone + with a warning. Content-compared like write_refs so an unchanged roster + writes nothing. Failure is logged, never raised.""" + for path_str, content in shims.items(): + try: + path = Path(path_str) + try: + existing = path.read_text() + except OSError: + existing = None + if content is None: + if existing is not None and existing.startswith(_SHIM_MARKER): + path.unlink(missing_ok=True) + logger.info(f"Removed stale physical skill ref {path}") + continue + if existing == content: + continue + if existing is not None and not existing.startswith(_SHIM_MARKER): + logger.warning(f"Not writing physical skill ref over hand-written {path}") + continue + if not path.parent.is_dir(): + continue # skill dir vanished between roster build and now + tmp = path.with_name(f"__init__.py.{os.getpid()}.tmp") + tmp.write_text(content) + os.replace(tmp, path) + logger.info(f"Wrote physical skill ref {path}") + except OSError as e: + logger.warning(f"Could not write physical skill ref {path_str}: {e}") + + +def prune_dir_shims(package_dirs: list[str], live_shims: dict[str, str | None], logger) -> None: + """Remove generated ref shims left behind in folders that stopped being + recording folders (metadata.json deleted or emptied while the generated + ``__init__.py`` stayed) — render_dir_shims can't list those, since it + only sees rostered entries. A folder that still holds real metadata + keeps its shim even when its skill is off the roster this publish + (corrupt trajectory, unreadable metadata): shims are committed alongside + metadata, so deleting one over a transient/fixable fault would dirty git + and break ``from innate_skills. import `` everywhere. Scans one + level under each skill package root, the only place recording folders + live; only marker-bearing files are ever deleted, so hand-written + subpackage ``__init__.py`` files are untouched. Failure is logged, never + raised.""" + keep = {os.path.realpath(p) for p, content in live_shims.items() if content is not None} + for root in package_dirs: + try: + for init in Path(root).glob("*/__init__.py"): + if os.path.realpath(init) in keep: + continue + if has_physical_metadata(init.parent): + continue # still a recording folder — off-roster isn't orphaned + try: + if init.read_text().startswith(_SHIM_MARKER): + init.unlink(missing_ok=True) + logger.info(f"Removed orphaned physical skill ref {init}") + except OSError: + continue + except OSError as e: + logger.warning(f"Could not prune physical skill refs under {root}: {e}") diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/registration.py b/ros2_ws/src/brain/brain_client/brain_client/skills/registration.py index d2debbcc6..b58f4b41f 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/skills/registration.py +++ b/ros2_ws/src/brain/brain_client/brain_client/skills/registration.py @@ -1,12 +1,7 @@ # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc -"""Skill catalog: track available skills and register them with the cloud agent. - -Subscribes to the latched ``/brain/available_skills`` topic, rebuilds the shared -:class:`~brain_client.skills.registry.SkillRegistry`, and sends the registration -payload (skills filtered to the current directive + the directive prompt). Also -handles re-registration deferral while a primitive is running. -""" +"""Track available skills (from /brain/available_skills) and register them +with the cloud agent, deferring re-registration while a primitive runs.""" from __future__ import annotations @@ -28,13 +23,15 @@ REGISTRATION_RETRY_SEC = 0.5 -def registry_from_skills_msg(msg: AvailableSkills, on_duplicate=None) -> SkillRegistry: - """Build a :class:`SkillRegistry` from an ``AvailableSkills`` message.""" - metadata = [ +def _metadata_from_msg(msg: AvailableSkills) -> list[dict]: + # Skills with a load_error are on the roster for the UI only — never + # registered with the cloud agent (they aren't runnable). + return [ { "id": s.id, "name": s.name, "type": s.type, + "group": s.group, "guidelines": s.guidelines, "guidelines_when_running": s.guidelines_when_running, "inputs": json.loads(s.inputs_json) if s.inputs_json else {}, @@ -44,8 +41,13 @@ def registry_from_skills_msg(msg: AvailableSkills, on_duplicate=None) -> SkillRe "wheeled": s.wheeled, } for s in msg.skills + if not s.load_error ] - return SkillRegistry.from_metadata(metadata, on_duplicate=on_duplicate) + + +def registry_from_skills_msg(msg: AvailableSkills, on_duplicate=None) -> SkillRegistry: + """Build a :class:`SkillRegistry` from an ``AvailableSkills`` message.""" + return SkillRegistry.from_metadata(_metadata_from_msg(msg), on_duplicate=on_duplicate) class SkillCatalog: @@ -55,45 +57,27 @@ def __init__(self, node, ws_bridge, state, *, execute_skill_ready=None): self._ws = ws_bridge self._state = state - # Callable returning whether the execute_skill action server is - # discoverable (PrimitiveRunner.action_client.server_is_ready); None - # disables the wait. + # returns whether the execute_skill server is discoverable; None disables the wait self._execute_skill_ready = execute_skill_ready self._register_retry_timer = None - self._last_skills_signature: tuple | None = None + self._last_skills_metadata: list | None = None self._sub = node.create_subscription( AvailableSkills, "/brain/available_skills", self._on_available_skills, AVAILABLE_SKILLS_QOS ) def _on_available_skills(self, msg: AvailableSkills) -> None: - # The roster is latched and re-published on a heartbeat so late - # subscribers (the webapp, via rws) can catch it. Ignore unchanged - # repeats here so each beat doesn't rebuild the registry and re-hit the - # cloud agent. - signature = tuple( - ( - s.id, - s.name, - s.type, - s.guidelines, - s.guidelines_when_running, - s.inputs_json, - s.in_training, - s.episode_count, - s.directory, - s.wheeled, - ) - for s in msg.skills - ) - if signature == self._last_skills_signature: + # ignore unchanged heartbeat repeats so each beat doesn't rebuild the + # registry and re-hit the cloud agent + metadata = _metadata_from_msg(msg) + if metadata == self._last_skills_metadata: return - self._last_skills_signature = signature + self._last_skills_metadata = metadata def _warn_dup(name, existing_id, new_id): self._logger.warn(f"Duplicate skill name '{name}': ID '{existing_id}' overwritten by '{new_id}'") - self._state.registry = registry_from_skills_msg(msg, on_duplicate=_warn_dup) + self._state.registry = SkillRegistry.from_metadata(metadata, on_duplicate=_warn_dup) counts = {t: sum(1 for s in msg.skills if s.type == t) for t in ("code", "learned", "replay")} self._logger.info( @@ -110,13 +94,9 @@ def _warn_dup(name, existing_id, new_id): self._state.pending_reregistration = True def register(self) -> None: - """Send the skills + directive registration to the cloud. - - Registering is the cloud's licence to trigger skills, so it waits for - the execute_skill server to be reachable first — otherwise the very - first trigger races graph discovery and the dispatch fails (INN-711's - root cause). While the server isn't ready, a short timer re-runs this. - """ + """Send the skills + directive registration to the cloud, waiting for + the execute_skill server first — registering earlier lets the first + trigger race graph discovery and fail (INN-711).""" if self._state.current_directive is None: self._stop_waiting_for_server() return @@ -167,7 +147,7 @@ def active_skill_ids_for_registration(self) -> list[str]: current_skill_ids = ( self._state.active_skill_ids if self._state.active_skill_ids is not None - else list(self._state.current_directive.get_skills()) + else list(self._state.current_directive.skill_ids()) ) current_skill_set = set(current_skill_ids) return [skill_id for skill_id in self.available_skill_ids() if skill_id in current_skill_set] diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/registry.py b/ros2_ws/src/brain/brain_client/brain_client/skills/registry.py index de5bf33f6..91aa67a18 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/skills/registry.py +++ b/ros2_ws/src/brain/brain_client/brain_client/skills/registry.py @@ -1,66 +1,39 @@ # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc -"""Skill registry — pure name<->id bookkeeping, no ROS. - -The cloud agent and the LLM refer to skills sometimes by deterministic *id* -(e.g. ``innate-os/navigate_to_position``) and sometimes by human *name* -(``navigate_to_position``). This module is the single source of truth for that -mapping, replacing three inconsistent inline translations in the old node. - -The ROS layer converts an ``AvailableSkills`` message into a list of plain metadata -dicts and calls :meth:`SkillRegistry.from_metadata`; nothing here imports rclpy. -""" +"""Skill registry — pure name<->id bookkeeping, no ROS. The single source of +truth for resolving cloud/LLM skill references (ids or display names).""" from __future__ import annotations from dataclasses import dataclass, field -class PrimitiveStub: - """Lightweight stand-in holding a skill's metadata (no executable body). - - Exposes the ``guidelines`` accessors the registration payload reads. - """ - - def __init__(self, metadata: dict): - self.metadata = metadata - - def guidelines(self) -> str: - return self.metadata.get("guidelines", "") - - def guidelines_when_running(self) -> str: - return self.metadata.get("guidelines_when_running", "") - - @dataclass class SkillRegistry: """Immutable-ish view of the currently available skills. - ``primitives`` is keyed by skill id; ``metadata`` is the ordered list used for - registration. + ``primitives`` maps skill id to its metadata dict; ``metadata`` is the + ordered list used for registration. """ - primitives: dict[str, PrimitiveStub] = field(default_factory=dict) + primitives: dict[str, dict] = field(default_factory=dict) metadata: list[dict] = field(default_factory=list) name_to_id: dict[str, str] = field(default_factory=dict) id_to_name: dict[str, str] = field(default_factory=dict) @classmethod def from_metadata(cls, metadata_list: list[dict], on_duplicate=None) -> SkillRegistry: - """Build a registry from a list of skill metadata dicts. - - Each dict must contain at least ``id`` and ``name``. ``on_duplicate`` is an - optional callback ``(name, existing_id, new_id)`` invoked when two skills - share a name (the later one wins, matching prior behaviour). - """ - primitives: dict[str, PrimitiveStub] = {} + """Build from metadata dicts (each with at least id + name); + ``on_duplicate(name, existing_id, new_id)`` fires on name clashes + (later wins).""" + primitives: dict[str, dict] = {} name_to_id: dict[str, str] = {} id_to_name: dict[str, str] = {} for meta in metadata_list: skill_id = meta["id"] name = meta["name"] - primitives[skill_id] = PrimitiveStub(meta) + primitives[skill_id] = meta if name in name_to_id and on_duplicate is not None: on_duplicate(name, name_to_id[name], skill_id) name_to_id[name] = skill_id diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/robot_state.py b/ros2_ws/src/brain/brain_client/brain_client/skills/robot_state.py index 72de974ac..4f3b5df10 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/skills/robot_state.py +++ b/ros2_ws/src/brain/brain_client/brain_client/skills/robot_state.py @@ -1,30 +1,34 @@ # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc -"""Robot-state provision for skill execution. - -Owns the on-demand state subscriptions (odom / map / head) and the camera + robot -interface references, and injects required interfaces and live robot state into the -running skill — including the 50 Hz continuous-update thread. Pulled out of the -skills action server so the node is left with just the action/execution flow. -""" +"""Robot-state provision for skill execution: the state subscriptions, +interface injection, and the 50 Hz continuous-update thread. Ambient reads +land on the current_* accessors, converting the newest cached message.""" from __future__ import annotations -import base64 import json -import math import threading import time +from collections.abc import Callable -import numpy as np +from geometry_msgs.msg import PoseWithCovarianceStamped from nav_msgs.msg import OccupancyGrid from nav_msgs.msg import Odometry as OdometryMsg -from sensor_msgs.msg import BatteryState, JointState +from rclpy.qos import qos_profile_sensor_data +from sensor_msgs.msg import BatteryState, JointState, LaserScan from std_msgs.msg import String from brain_client.common.geometry import quaternion_to_yaw -from brain_client.skills.odometry import Odometry -from brain_client.skills.types import InterfaceType, RobotStateType +from brain_client.skills.types import _DEFAULT_STATE_GRACE_S, InterfaceType, RobotStateType, _state_grace_s +from brain_client.state.arm import Arm +from brain_client.state.battery import Battery +from brain_client.state.head import HeadState +from brain_client.state.image import DepthMap, MainImage, WristImage +from brain_client.state.joint_states import JointStates +from brain_client.state.lidar import Lidar +from brain_client.state.map import Map +from brain_client.state.odometry import Odometry +from brain_client.state.pose import Pose class RobotStateProvider: @@ -42,26 +46,35 @@ def __init__(self, node, camera_node, *, manipulation, mobility, head, head_curr self.last_head_position = None self.last_joint_states = None self.last_battery = None + self.last_amcl_pose = None + self.last_scan = None + self._lidar_cache = None # (msg, Lidar) of the last converted scan + # (msg, Map) of the last converted map — a fresh Map per 50 Hz tick + # would discard Map.grid's cached_property and re-decode the whole + # grid on every skill read + self._map_cache = None + # (jpeg, Image) of the last converted frame per camera — the b64 + # encode is far too expensive to redo at 50 Hz for a ~15 Hz camera + self._main_image_cache = None + self._wrist_image_cache = None self._odom_sub = None self._map_sub = None self._head_position_sub = None self._joint_states_sub = None self._battery_sub = None - # Feeds are gated by this flag instead of destroying the subscriptions: - # destroying a subscription an executor has already selected as - # "ready" races _take_subscription and crashes the process - # (InvalidHandle -> rmw_zenoh Rust panic -> SIGABRT), easiest to hit - # right after a skill cancellation. The subscriptions live on the - # manipulation interface's private node, whose executor is parked - # between skills — always-alive high-rate feeds (/joint_states + head - # position alone are ~400 msgs/s) would otherwise cost ~half a Jetson - # core in executor dispatch even while idle. + self._amcl_pose_sub = None + self._scan_sub = None + # Gate feeds with this flag, never by destroying subscriptions: + # destroying one the executor already selected as "ready" crashes the + # process (InvalidHandle -> rmw_zenoh SIGABRT), easiest to hit right + # after a cancel. The subs live on manipulation's private node, parked + # between skills — always-alive feeds (~400 msgs/s) would otherwise + # cost ~half a Jetson core while idle. self._active = False - # warn once per missing state, not at 50 Hz while a slow topic - # (battery publishes at 0.2 Hz) sends its first message - self._warned_missing = set() + self._warned_missing = set() # warn once per missing state, not at 50 Hz + # (skill, injection pairs) holding the 50 Hz slot; None when idle self._current_skill = None self._current_skill_lock = threading.Lock() # parents suspended while a chained child holds the 50 Hz slot @@ -69,10 +82,25 @@ def __init__(self, node, camera_node, *, manipulation, mobility, head, head_curr self._state_update_thread = None self._state_update_stop_event = threading.Event() + # feed enum -> snapshot accessor; see _state_getters + self._state_getter_map = { + RobotStateType.LAST_MAIN_CAMERA_IMAGE_B64: self.current_main_image, + RobotStateType.LAST_WRIST_CAMERA_IMAGE_B64: self.current_wrist_image, + RobotStateType.LAST_DEPTH_IMAGE: self.current_depth, + RobotStateType.LAST_ODOM: self.current_odom, + RobotStateType.LAST_MAP: self.current_map, + RobotStateType.LAST_JOINT_STATES: self.current_joint_states, + RobotStateType.LAST_BATTERY: self.current_battery, + RobotStateType.LAST_HEAD_POSITION: self.current_head_position, + RobotStateType.LAST_POSE: self.current_pose, + RobotStateType.LAST_LIDAR: self.current_lidar, + RobotStateType.LAST_ARM: self.current_arm, + } + # --- interface injection (at skill load + before execution) --- def inject_required_interfaces(self, skill) -> None: """Inject only the interfaces declared by the skill via Interface descriptors.""" - for interface_type in skill.get_required_interfaces(): + for interface_type in skill.declared_interface_types(): if interface_type == InterfaceType.MANIPULATION: skill.inject_interface(interface_type, self._manipulation) elif interface_type == InterfaceType.MOBILITY: @@ -98,14 +126,16 @@ def start_subscriptions(self) -> None: ) self._joint_states_sub = feed_node.create_subscription(JointState, "/joint_states", self._on_joint_states, 10) self._battery_sub = feed_node.create_subscription(BatteryState, "/battery_state", self._on_battery, 10) + self._amcl_pose_sub = feed_node.create_subscription( + PoseWithCovarianceStamped, "/amcl_pose", self._on_amcl_pose, 10 + ) + # the lidar driver publishes with sensor-data QoS (best effort); a + # reliable subscription would never match it + self._scan_sub = feed_node.create_subscription(LaserScan, "/scan", self._on_scan, qos_profile_sensor_data) def stop_subscriptions(self) -> None: - """Deactivate robot-state feeds when no skill is running. - - The subscriptions are deliberately NOT destroyed (see __init__); the - callbacks early-return while inactive, and parking the private - executor (manipulation.stop()) stops them being invoked at all. - """ + """Deactivate feeds — subscriptions are deliberately NOT destroyed + (see __init__); callbacks early-return and the private executor parks.""" self._active = False self._manipulation.stop() self.last_odom = None @@ -113,6 +143,12 @@ def stop_subscriptions(self) -> None: self.last_head_position = None self.last_joint_states = None self.last_battery = None + self.last_amcl_pose = None + self.last_scan = None + self._lidar_cache = None + self._map_cache = None + self._main_image_cache = None + self._wrist_image_cache = None def _on_odom(self, msg: OdometryMsg) -> None: if self._active: @@ -130,6 +166,14 @@ def _on_battery(self, msg: BatteryState) -> None: if self._active: self.last_battery = msg + def _on_amcl_pose(self, msg: PoseWithCovarianceStamped) -> None: + if self._active: + self.last_amcl_pose = msg + + def _on_scan(self, msg: LaserScan) -> None: + if self._active: + self.last_scan = msg + def _on_head_position(self, msg: String) -> None: if not self._active: return @@ -145,17 +189,19 @@ def _warn_missing(self, state_name: str) -> None: # --- continuous updates --- def begin_continuous_updates(self, skill) -> None: - """Start (or hand over) the 50 Hz state feed for ``skill``. + """Start (or hand over) the 50 Hz state feed. Nesting-safe: a parent + holding the slot is suspended and resumes when the child ends. - Nesting-safe: if a skill already holds the slot (a parent running a - chained child), it is suspended and resumes when the child ends. - """ + The declared-feed set is fixed once the run is wired, so the + (state, getter) pairs are computed here, once — not by re-walking the + sub-skill tree on every 50 Hz tick.""" + entry = (skill, self._injection_pairs(skill)) with self._current_skill_lock: if self._current_skill is not None: self._skill_stack.append(self._current_skill) - self._current_skill = skill + self._current_skill = entry return # update thread already running - self._current_skill = skill + self._current_skill = entry self._state_update_stop_event.clear() self._state_update_thread = threading.Thread(target=self._state_update_thread_func, daemon=True) self._state_update_thread.start() @@ -177,145 +223,237 @@ def _state_update_thread_func(self) -> None: while not self._state_update_stop_event.is_set(): with self._current_skill_lock: if self._current_skill is not None: + skill, pairs = self._current_skill try: - self.update_skill_robot_state(self._current_skill) + self._inject(skill, pairs) except Exception as e: self._logger.error(f"Error in continuous state update: {e}") self._state_update_stop_event.wait(0.02) - def wait_for_camera_states(self, skill, required_states, timeout_s: float = 3.0) -> None: - """Block briefly until required camera frames exist, then re-inject. - - The camera subscription is created at skill start, so the first frame - is always one publish period away (more in sim, where cameras render - on demand) -- without this, a skill reading its image in the first - lines of execute() fails cold and only succeeds on retry. Bounded: - on timeout the skill still sees the missing state and handles it.""" - wanted = [] - if RobotStateType.LAST_MAIN_CAMERA_IMAGE_B64 in required_states: - wanted.append(lambda: self._camera.last_main_camera_b64) - if RobotStateType.LAST_WRIST_CAMERA_IMAGE_B64 in required_states: - wanted.append(lambda: self._camera.last_wrist_camera_b64) - if not wanted: - return - deadline = time.monotonic() + timeout_s - while time.monotonic() < deadline: - if all(get() is not None for get in wanted): - break - time.sleep(0.05) - else: - self._logger.warn(f"Camera frames still missing after {timeout_s:.0f}s; the skill sees no image.") + def _state_getters(self) -> dict: + # static per provider (also the test seam: tests stub this per instance) + return self._state_getter_map + + def wait_for_required_states(self, skill) -> None: + """Block, bounded per feed, until every required state has a first + value, then re-inject. + + Feeds start at run start, so the first message is at least one + publish period away — without this, a required read in execute()'s + opening lines would fail cold. Anything still missing afterwards + fails the run (Skill.missing_required_robot_states), never execute(). + + Optional declarations (``| None``, legacy descriptors) never block + the start — a dead feed would stall every run for nothing. They are + injected as messages land (the 50 Hz refresh); a skill wanting one + early waits itself: ``self.wait_for(lambda: self.image)``.""" + getters = self._state_getters() + start = time.monotonic() + # per-feed grace bounds come from the spec table (types._feed_specs) + deadlines = { + state_type: start + _state_grace_s().get(state_type, _DEFAULT_STATE_GRACE_S) + for state_type in skill.required_robot_state_types() + if state_type in getters + } + pending = dict(deadlines) + while pending and not skill.cancelled: + now = time.monotonic() + pending = {t: deadline for t, deadline in pending.items() if getters[t]() is None and now < deadline} + if pending: + time.sleep(0.05) + timed_out = [t.name for t in deadlines if getters[t]() is None] + if timed_out: + self._logger.warn(f"Required state still missing after its grace period: {', '.join(timed_out)}") self.update_skill_robot_state(skill) - # --- state injection --- - def update_skill_robot_state(self, skill) -> None: - """Update a skill's robot state from current sensor data.""" - required_states = skill.get_required_robot_states() - if not required_states: - return + # --- snapshot accessors --- + # Ambient state reads (skill.battery, skill.odom, ...) land here: each + # call converts the newest cached message, so every read is as fresh as + # the topic. + def current_main_image(self) -> MainImage | None: + # memoized per frame, like lidar: re-encoding ~100 KB JPEGs to base64 + # at 50 Hz is ~10 MB/s of allocations for frames that change at ~15 Hz + jpeg = self._camera.last_main_camera_jpeg + if jpeg is None: + return None + cached = self._main_image_cache + if cached is not None and cached[0] is jpeg: + return cached[1] + image = MainImage.from_jpeg(jpeg) + self._main_image_cache = (jpeg, image) + return image + + def current_wrist_image(self) -> WristImage | None: + jpeg = self._camera.last_wrist_camera_jpeg + if jpeg is None: + return None + cached = self._wrist_image_cache + if cached is not None and cached[0] is jpeg: + return cached[1] + image = WristImage.from_jpeg(jpeg) + self._wrist_image_cache = (jpeg, image) + return image + + def current_depth(self) -> DepthMap | None: + depth = self._camera.last_depth_image + # view-cast: same buffer, feed-identifying type + return None if depth is None else depth.view(DepthMap) + + def current_odom(self) -> Odometry | None: + msg = self.last_odom + if msg is None: + return None + pos = msg.pose.pose.position + twist = msg.twist.twist + return Odometry( + x=pos.x, + y=pos.y, + theta=quaternion_to_yaw(msg.pose.pose.orientation), + linear_velocity=twist.linear.x, + angular_velocity=twist.angular.z, + stamp=msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9, + frame_id=msg.header.frame_id, + child_frame_id=msg.child_frame_id, + # .raw builds lazily from the message, so + # high-rate reads pay nothing for the full-fidelity dict + raw_source=msg, + ) - robot_state_to_inject = {} + def current_map(self) -> Map | None: + msg = self.last_map + if msg is None: + return None + # memoized per message, like lidar: a fresh Map every 50 Hz tick would + # throw away Map.grid's cached_property, making every skill read of + # .grid re-decode the whole grid + cached = self._map_cache + if cached is not None and cached[0] is msg: + return cached[1] + # cheap: the grid itself decodes lazily on Map.grid access + current = Map( + resolution=msg.info.resolution, + width=msg.info.width, + height=msg.info.height, + origin_x=msg.info.origin.position.x, + origin_y=msg.info.origin.position.y, + origin_theta=quaternion_to_yaw(msg.info.origin.orientation), + stamp=msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9, + frame_id=msg.header.frame_id, + raw_source=msg, + ) + self._map_cache = (msg, current) + return current + + def current_joint_states(self) -> JointStates | None: + msg = self.last_joint_states + if msg is None: + return None + return JointStates( + name=tuple(msg.name), + position=tuple(msg.position), + velocity=tuple(msg.velocity), + effort=tuple(msg.effort), + ) - if RobotStateType.LAST_MAIN_CAMERA_IMAGE_B64 in required_states: - b64 = self._camera.last_main_camera_b64 - if b64 is not None: - robot_state_to_inject[RobotStateType.LAST_MAIN_CAMERA_IMAGE_B64.value] = b64 - else: - self._warn_missing("LAST_MAIN_CAMERA_IMAGE_B64") + def current_battery(self) -> Battery | None: + msg = self.last_battery + if msg is None: + return None + return Battery( + percentage=msg.percentage, + voltage=msg.voltage, + current=msg.current, + charging=msg.power_supply_status == BatteryState.POWER_SUPPLY_STATUS_CHARGING, + ) - if RobotStateType.LAST_WRIST_CAMERA_IMAGE_B64 in required_states: - b64 = self._camera.last_wrist_camera_b64 - if b64 is not None: - robot_state_to_inject[RobotStateType.LAST_WRIST_CAMERA_IMAGE_B64.value] = b64 - else: - self._warn_missing("LAST_WRIST_CAMERA_IMAGE_B64") - - if RobotStateType.LAST_ODOM in required_states: - if self.last_odom is not None: - msg = self.last_odom - pos = msg.pose.pose.position - twist = msg.twist.twist - robot_state_to_inject[RobotStateType.LAST_ODOM.value] = Odometry( - x=pos.x, - y=pos.y, - theta=quaternion_to_yaw(msg.pose.pose.orientation), - linear_velocity=twist.linear.x, - angular_velocity=twist.angular.z, - stamp=msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9, - frame_id=msg.header.frame_id, - child_frame_id=msg.child_frame_id, - # .raw is built lazily from the message on first access, so - # this 50 Hz path pays nothing for the full-fidelity dict - raw_source=msg, - ) - else: - self._warn_missing("LAST_ODOM") - - if RobotStateType.LAST_MAP in required_states: - if self.last_map is not None: - map_data_bytes = np.array(self.last_map.data, dtype=np.int8).tobytes() - ori_map = self.last_map.info.origin.orientation - yaw_map = quaternion_to_yaw(ori_map) - robot_state_to_inject[RobotStateType.LAST_MAP.value] = { - "header": { - "stamp": { - "sec": self.last_map.header.stamp.sec, - "nanosec": self.last_map.header.stamp.nanosec, - }, - "frame_id": self.last_map.header.frame_id, - }, - "info": { - "map_load_time": { - "sec": self.last_map.info.map_load_time.sec, - "nanosec": self.last_map.info.map_load_time.nanosec, - }, - "resolution": self.last_map.info.resolution, - "width": self.last_map.info.width, - "height": self.last_map.info.height, - "origin": { - "position": { - "x": self.last_map.info.origin.position.x, - "y": self.last_map.info.origin.position.y, - "z": self.last_map.info.origin.position.z, - }, - "orientation": {"x": ori_map.x, "y": ori_map.y, "z": ori_map.z, "w": ori_map.w}, - "yaw_degrees": math.degrees(yaw_map), - }, - }, - "data_b64": base64.b64encode(map_data_bytes).decode("utf-8"), - } - else: - self._warn_missing("LAST_MAP") - - if RobotStateType.LAST_JOINT_STATES in required_states: - if self.last_joint_states is not None: - js = self.last_joint_states - robot_state_to_inject[RobotStateType.LAST_JOINT_STATES.value] = { - "name": list(js.name), - "position": list(js.position), - "velocity": list(js.velocity), - "effort": list(js.effort), - } - else: - self._warn_missing("LAST_JOINT_STATES") - - if RobotStateType.LAST_BATTERY in required_states: - if self.last_battery is not None: - b = self.last_battery - robot_state_to_inject[RobotStateType.LAST_BATTERY.value] = { - "percentage": b.percentage, - "voltage": b.voltage, - "current": b.current, - "charging": b.power_supply_status == BatteryState.POWER_SUPPLY_STATUS_CHARGING, - } - else: - self._warn_missing("LAST_BATTERY") + def current_head_position(self) -> HeadState | None: + payload = self.last_head_position + if payload is None: + return None + pitch = payload.get("current_position") + if pitch is None: + return None + return HeadState( + pitch_degrees=pitch, + min_degrees=payload.get("min_angle"), + max_degrees=payload.get("max_angle"), + default_degrees=payload.get("default_angle"), + raw_source=payload, + ) - if RobotStateType.LAST_HEAD_POSITION in required_states: - if self.last_head_position is not None: - robot_state_to_inject[RobotStateType.LAST_HEAD_POSITION.value] = self.last_head_position - else: - self._warn_missing("LAST_HEAD_POSITION") + def current_pose(self) -> Pose | None: + msg = self.last_amcl_pose + if msg is None: + return None + pose = msg.pose.pose + return Pose( + x=pose.position.x, + y=pose.position.y, + theta=quaternion_to_yaw(pose.orientation), + stamp=msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9, + frame_id=msg.header.frame_id, + ) + + def current_lidar(self) -> Lidar | None: + msg = self.last_scan + if msg is None: + return None + # memoized per message: tuple(msg.ranges) copies ~1100 floats at 50 Hz + # for a ~10 Hz scan + cached = self._lidar_cache + if cached is not None and cached[0] is msg: + return cached[1] + lidar = Lidar( + ranges=tuple(msg.ranges), + angle_min=msg.angle_min, + angle_increment=msg.angle_increment, + range_min=msg.range_min, + range_max=msg.range_max, + stamp=msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9, + frame_id=msg.header.frame_id, + ) + self._lidar_cache = (msg, lidar) + return lidar + + def current_arm(self) -> Arm | None: + # last_fk_pose, not get_current_end_effector_pose(): the dict getter + # spins the node and warn-logs on None — both wrong at 50 Hz + fk = self._manipulation.last_fk_pose + if fk is None: + return None + js = self.last_joint_states + gripper = js.position[5] if js is not None and len(js.position) > 5 else None + pose = fk.pose + return Arm( + x=pose.position.x, + y=pose.position.y, + z=pose.position.z, + qx=pose.orientation.x, + qy=pose.orientation.y, + qz=pose.orientation.z, + qw=pose.orientation.w, + gripper=gripper, + frame_id=fk.header.frame_id, + ) - if robot_state_to_inject: - skill.update_robot_state(**robot_state_to_inject) + # --- state injection --- + def _injection_pairs(self, skill) -> list[tuple[RobotStateType, Callable[[], object]]]: + """(state, getter) for every declared feed, sub-skills included.""" + getters = self._state_getters() + return [(t, getters[t]) for t in skill.declared_robot_state_types() if t in getters] + + def update_skill_robot_state(self, skill) -> None: + """Inject a current value for every state the skill declared (one-shot + path: the 50 Hz thread injects via precomputed pairs instead).""" + self._inject(skill, self._injection_pairs(skill)) + + def _inject(self, skill, pairs) -> None: + to_inject = {} + for state_type, getter in pairs: + value = getter() + if value is not None: + to_inject[state_type.value] = value + else: + self._warn_missing(state_type.name) + if to_inject: + skill.update_robot_state(**to_inject) diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/runner.py b/ros2_ws/src/brain/brain_client/brain_client/skills/runner.py index cf1efad64..f6719e7b1 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/skills/runner.py +++ b/ros2_ws/src/brain/brain_client/brain_client/skills/runner.py @@ -264,8 +264,8 @@ def _on_result(self, future) -> None: self._maybe_run_pending(result) def _is_code_skill(self, skill_id: str) -> bool: - stub = self._state.registry.primitives.get(skill_id) - return stub is not None and stub.metadata.get("type") == "code" + meta = self._state.registry.primitives.get(skill_id) + return meta is not None and meta.get("type") == "code" def _emit_skill_output(self, result, is_code: bool) -> None: """Surface a successful code skill's output in the chat (never spoken).""" diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/types.py b/ros2_ws/src/brain/brain_client/brain_client/skills/types.py index d7dfe1e73..e287f830f 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/skills/types.py +++ b/ros2_ws/src/brain/brain_client/brain_client/skills/types.py @@ -1,98 +1,203 @@ # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc +import inspect import json import os +import sys import threading import time +import warnings from abc import ABC, abstractmethod +from collections.abc import Callable, Iterator +from dataclasses import dataclass from enum import Enum +from functools import cache from pathlib import Path -from typing import Any +from types import GeneratorType, UnionType # stdlib `types`, not this module +from typing import TYPE_CHECKING, Any, Generic, NoReturn, TypeVar, Union, get_args, get_origin, overload from rclpy.node import Node from std_msgs.msg import String +from typing_extensions import Self +from brain_client.common.dynamic_loader import class_name_to_snake_case from brain_client.common.logging import UniversalLogger +from brain_client.common.script_paths import Source + +if TYPE_CHECKING: + from brain_client.skills.invoker import SkillInvoker + +T = TypeVar("T") +_T_resource = TypeVar("_T_resource") + +# What execute() may return: the result message (SkillOutput to attach a +# structured payload for chaining callers), or None. Failure is self.fail(); +# cancellation is the framework's. Legacy (message, SkillResult[, data]) +# tuples still normalize at runtime but are deprecated. +SkillReturn = Union[None, str, "SkillOutput"] -# brain_client_node plays text published here (see transport/tts.py) and -# reports playback on the status topic, which say(wait=True) watches. TTS_TOPIC = "/brain/tts" TTS_STATUS_TOPIC = "/tts/is_playing" class SkillResult(Enum): - """ - Enum representing the possible results of a skill execution. + SUCCESS = "success" + FAILURE = "failure" + CANCELLED = "cancelled" + + +class SkillFailed(Exception): + """A composed skill reported FAILURE. Raised by Skill.__call__ and fail().""" + + +class SkillCancelled(BaseException): + """A composed skill was cancelled; unwinds the routine. + + BaseException, not Exception — same reasoning as asyncio.CancelledError: + a skill's broad ``except Exception`` (around a Gemini call, file IO, a + chained child) must never swallow a cancellation. Catch it explicitly + only to clean up, and re-raise. """ - SUCCESS = "success" # The skill completed successfully - FAILURE = "failure" # The skill failed to complete - CANCELLED = "cancelled" # The skill was cancelled before completion +# The active run's cancel latch. One skill runs at a time (the server's +# execution slot enforces it), so module state is exact: the server swaps +# this around each execute(), and interface helpers block on it so their +# loops unwind on cancel without any plumbing from the skill. +_run_cancel = threading.Event() -class SkillOutput(str): - """A skill's output message (a plain str), with an optional structured - payload on .data — the third element of an execute() return, if any.""" - data: Any = None +def swap_run_cancel(event: "threading.Event | None") -> threading.Event: + global _run_cancel + previous = _run_cancel + _run_cancel = event if event is not None else threading.Event() + return previous - def __new__(cls, message: str, data: Any = None): - output = super().__new__(cls, message) - output.data = data - return output +def cancellable_sleep(seconds: float) -> None: + """time.sleep that raises SkillCancelled the moment the run is cancelled.""" + if _run_cancel.wait(seconds): + raise SkillCancelled("cancelled") -def normalize_skill_result(result) -> tuple[SkillOutput, "SkillResult"]: - """Turn execute()'s (message, status[, data]) into (SkillOutput, status).""" - if isinstance(result, (tuple, list)) and len(result) == 3: - message, status, data = result - return SkillOutput(message, data), status - message, status = result - return SkillOutput(message), status +class SkillOutput: + """The result of a skill run: the message, the status (a SkillResult, not + a string), and an optional structured payload for chaining callers. -class RobotStateType(Enum): + ``str(output)`` and f-strings give the message; ``output.ok`` is the + success check. Unpacking as the legacy ``(message, status)`` tuple still + works but is deprecated. """ - Enum representing the types of robot state a skill might require. + + __slots__ = ("message", "data", "status") + + def __init__(self, message: str, data: Any = None, status: "SkillResult" = SkillResult.SUCCESS): + self.message = str(message) + self.data = data + self.status = status + + @property + def ok(self) -> bool: + return self.status is SkillResult.SUCCESS + + def __str__(self) -> str: + return self.message + + def __repr__(self) -> str: + data = f", data={self.data!r}" if self.data is not None else "" + return f"SkillOutput({self.message!r}, status={self.status.value}{data})" + + def __iter__(self): + # legacy `message, status = ...` unpacking, from when results were tuples + _warn_once( + "unpack", + "Unpacking a skill result as (message, status) is deprecated — " + "use output.message / output.status / output.ok.", + ) + yield self.message + yield self.status + + +# Deprecation keys already warned about, once per process. +_deprecation_warned: set[str] = set() + + +def _warn_once(key: str, message: str, logger=None) -> None: + if key in _deprecation_warned: + return + _deprecation_warned.add(key) + if logger is not None: + logger.warning(message) + else: + warnings.warn(message, DeprecationWarning, stacklevel=3) + + +def normalize_skill_result(result, skill_name: str = "Skill", logger=None) -> SkillOutput: + """Turn any execute() return into a SkillOutput. + + Legacy (message, SkillResult[, data]) tuples still normalize, with a + once-per-skill deprecation warning; anything else raises SkillFailed so + the author sees the contract as the run's failure message. """ + if result is None: + return SkillOutput(f"{skill_name} completed") + if isinstance(result, SkillOutput): + return result # forwarded child output: keep .data and .status + if isinstance(result, str): + return SkillOutput(result) + if isinstance(result, (tuple, list)) and len(result) in (2, 3) and isinstance(result[1], SkillResult): + _warn_once( + f"return:{skill_name}", + f"{skill_name}.execute() returned a (message, SkillResult) tuple — deprecated. " + "Return the message str on success (SkillOutput(message, data) to attach a payload); " + "call self.fail(message) to fail. Cancellation is the framework's.", + logger, + ) + message, status = result[0], result[1] + data = result[2] if len(result) == 3 else None + return SkillOutput(message, data, status) + raise SkillFailed( + f"{skill_name}.execute() returned {type(result).__name__} — return a str, " + "SkillOutput(message, data), or None; call self.fail(message) to fail." + ) + +class RobotStateType(Enum): LAST_MAIN_CAMERA_IMAGE_B64 = "last_main_camera_image_b64" LAST_WRIST_CAMERA_IMAGE_B64 = "last_wrist_camera_image_b64" + LAST_DEPTH_IMAGE = "last_depth_image" LAST_ODOM = "last_odom" LAST_MAP = "last_map" LAST_HEAD_POSITION = "last_head_position" LAST_JOINT_STATES = "last_joint_states" LAST_BATTERY = "last_battery" + LAST_POSE = "last_pose" + LAST_LIDAR = "last_lidar" + LAST_ARM = "last_arm" class InterfaceType(Enum): - """ - Enum representing the types of interfaces a skill might require. - """ - MANIPULATION = "manipulation" MOBILITY = "mobility" HEAD = "head" class SkillStorage: - """Persistent per-skill key-value store: a JSON file with dict access. - - Values must be JSON-serializable. Loaded lazily; writes are atomic - (tmp file + os.replace). - """ + """Persistent per-skill key-value store: a JSON file with dict access.""" def __init__(self, path: str | Path): self._path = Path(path) - self._data: dict | None = None + self._data: dict[str, Any] | None = None - def _load(self) -> dict: - if self._data is None: + def _load(self) -> dict[str, Any]: + data = self._data + if data is None: try: - self._data = json.loads(self._path.read_text()) + data = json.loads(self._path.read_text()) except (FileNotFoundError, json.JSONDecodeError): - self._data = {} - return self._data + data = {} + self._data = data + return data def _save(self) -> None: self._path.parent.mkdir(parents=True, exist_ok=True) @@ -119,129 +224,638 @@ def __contains__(self, key: str) -> bool: def _storage_dir() -> Path: - # same INNATE_OS_ROOT resolution as common/script_paths.py root = Path(os.environ.get("INNATE_OS_ROOT", Path.home() / "innate-os")) return root / "workspace" / "skill_storage" -class RobotState: - """ - Descriptor for declaring and accessing robot state in skills. +class _Injected: + """Descriptor plumbing shared by RobotState and Interface: the injected + value is stored on the instance under a private name, None until set.""" - Usage: - class MySkill(Skill): - image = RobotState(RobotStateType.LAST_MAIN_CAMERA_IMAGE_B64) - odom = RobotState(RobotStateType.LAST_ODOM) + _attr_prefix = "_injected_" - def execute(self): - if self.image: # Access state directly - ... - """ - - def __init__(self, state_type: RobotStateType): - self.state_type = state_type - self._attr_name: str | None = None + def __init__(self, required: bool = False): + self.required = required + self._attr_name: str = "" # set by __set_name__ before any access def __set_name__(self, owner: type, name: str): - """Called when the descriptor is assigned to a class attribute.""" - self._attr_name = f"_robot_state_{name}" + self._attr_name = f"{self._attr_prefix}{name}" def __get__(self, obj: Any, objtype: type | None = None) -> Any: - """Get the current state value.""" if obj is None: return self return getattr(obj, self._attr_name, None) def __set__(self, obj: Any, value: Any): - """Set the state value.""" setattr(obj, self._attr_name, value) -class Interface: +class RobotState(_Injected): + """Descriptor behind robot-state declarations (``odom: Odometry``). + + A required (non-``| None``) state is guaranteed before execute(); the run + fails if no message arrives. Legacy explicit declarations + (``odom = RobotState(RobotStateType.LAST_ODOM)``) keep the old + tolerate-None behavior. """ - Descriptor for declaring and accessing interfaces in skills. - Usage: - class MySkill(Skill): - mobility = Interface(InterfaceType.MOBILITY) - head = Interface(InterfaceType.HEAD) + _attr_prefix = "_robot_state_" + + def __init__(self, state_type: RobotStateType, required: bool = False): + super().__init__(required) + self.state_type = state_type + - def execute(self): - self.mobility.rotate(0.5) # Use interface directly +class Camera(RobotState): + """Descriptor behind camera-feed declarations (``image: MainImage``). + + Cameras must be declared: the server starts them per run — frame encoding + is too expensive to keep warm. A required camera fails the run if no frame + arrives within the grace; an optional (``| None``) one never delays the + start — wait in execute() (``self.wait_for(lambda: self.image)``). """ - def __init__(self, interface_type: InterfaceType): + def __init__(self, feed: RobotStateType, required: bool = True): + if feed not in _camera_feed_keys(): + raise ValueError( + "Not a camera feed; declare cameras via annotations — image: MainImage / WristImage / DepthMap" + ) + super().__init__(feed, required=required) + + +class Interface(_Injected): + """Descriptor behind interface declarations (``mobility: Mobility``). + + Declaring is requiring: the run fails up front when a declared interface + is unavailable; ``| None`` makes it best effort instead. Legacy explicit + declarations (``head = Interface(InterfaceType.HEAD)``) keep the old + tolerate-None behavior. + """ + + _attr_prefix = "_interface_" + + def __init__(self, interface_type: InterfaceType, required: bool = False): + super().__init__(required) self.interface_type = interface_type - self._attr_name: str | None = None + + +class SubSkill(_Injected): + """Descriptor behind sub-skill declarations (``gripper_open: GripperOpen``). + + Composition runs the class you see: for each run the child is constructed + and wired like a root skill (same run node, interfaces, feeds, invoker, + feedback) and shares the parent's cancel latch, then sits on the attribute + as a callable — ``self.gripper_open(percent=50)`` raises SkillFailed / + SkillCancelled instead of returning a status. Override by subclassing the + parent and re-declaring the attribute with your class — never by name + shadowing. Note: a shared cancel unwinds children via the latch + (``cancelled`` / ``check_cancelled``) and fires ``on_cancel`` hooks down + the wired tree; a child's ``cancel()`` *override* is only invoked when + that child is the run's root skill. + """ + + _attr_prefix = "_subskill_" + + def __init__(self, skill_class: "type[Skill]"): + super().__init__(required=True) + self.skill_class = skill_class + + +class TrainedSkill: + """Base of the generated classes in the ``physical_skills`` package — a + typed handle on one of this robot's physical skills (a trained policy or + recorded demonstration: data, with no code class of its own). + + Never subclass this by hand; the skill catalog regenerates the package + whenever the robot's physical skills change. Reference the generated + class anywhere a skill id goes:: + + from physical_skills import PickSocks + + class TidyAgent(Agent): + def get_skills(self): + return [NavigateToPosition, PickSocks] + + class TidyUp(Skill): + pick: PickSocks # same call shape as a code sub-skill + """ + + skill_id: str = "" + + def __call__(self, *, timeout: float | None = None, **inputs) -> "SkillOutput": + """Call shape for a declared physical skill (``self.pick(timeout=60)``). + + At runtime a ``pick: PickSocks`` attribute is a ``_BoundPhysicalSkill``; + this method exists so type checkers see the same contract as calling + a code sub-skill. Instantiating a ref and calling it directly is not + supported. + """ + raise TypeError( + f"{type(self).__name__} is a typed physical-skill ref — declare it " + f"on a Skill (``attr: {type(self).__name__}``) and call that attribute" + ) + + +class _BoundPhysicalSkill: + """A declared physical skill, bound to the run's invoker at wire time.""" + + def __init__(self, skill_id: str, invoker): + self._skill_id = skill_id + self._invoker = invoker + + def __call__(self, *, timeout: float | None = None, **inputs) -> "SkillOutput": + """Same contract as calling a code sub-skill: output on success, + SkillFailed / SkillCancelled otherwise.""" + output = self._invoker.run(self._skill_id, timeout=timeout, **inputs) + if output.status is SkillResult.CANCELLED: + raise SkillCancelled(output.message) + if not output.ok: + raise SkillFailed(output.message) + return output + + +class PhysicalSkill(_Injected): + """Declaration for a physical skill — a trained or recorded policy + (``pick_socks = PhysicalSkill("pick_socks")``). + + Physical skills are data (metadata.json + a checkpoint), so there is no + class to import and the id is the only handle. Declaring one still puts it + in the dependency block with everything else, gives the same call shape as + a code sub-skill (``self.pick_socks(timeout=60)``), and moves an unknown-id + error to run start instead of mid-routine. + """ + + _attr_prefix = "_physical_skill_" + + def __init__(self, skill_id: "str | type[TrainedSkill]"): + super().__init__(required=True) + if isinstance(skill_id, type): + skill_id = skill_id.skill_id + self.skill_id = skill_id + + +class _Resource(Generic[T]): + """Descriptor behind ``@resource`` — see that decorator for the contract.""" + + def __init__(self, factory: Callable[[Any], Iterator[T] | T]): + self._factory = factory + self._name = getattr(factory, "__name__", "resource") + self.__doc__ = factory.__doc__ def __set_name__(self, owner: type, name: str): - """Called when the descriptor is assigned to a class attribute.""" - self._attr_name = f"_interface_{name}" + self._name = name - def __get__(self, obj: Any, objtype: type | None = None) -> Any: - """Get the interface instance.""" + @property + def _gen_key(self) -> str: + return f"_resource_gen_{self._name}" + + @overload + def __get__(self, obj: None, objtype: type | None = None) -> Self: ... + @overload + def __get__(self, obj: object, objtype: type | None = None) -> T: ... + def __get__(self, obj, objtype=None): if obj is None: return self - return getattr(obj, self._attr_name, None) - - def __set__(self, obj: Any, value: Any): - """Set the interface instance.""" - setattr(obj, self._attr_name, value) + # non-data descriptor: after the first build the instance-dict entry + # wins the lookup (cached_property's trick); release() pops to re-arm + if self._name not in obj.__dict__: + produced = self._factory(obj) + if isinstance(produced, GeneratorType): + value = next(produced) + if value is None: + produced.close() + return None # pyright: ignore[reportReturnType] — factories yielding T | None bind T to the optional + obj.__dict__[self._gen_key] = produced + else: + value = produced + if value is None: + return None # pyright: ignore[reportReturnType] — factories returning T | None bind T to the optional + obj.__dict__[self._name] = value + return obj.__dict__[self._name] + + def release(self, obj, logger) -> None: + """Resume the factory generator past its yield so its teardown runs.""" + value = obj.__dict__.pop(self._name, None) + gen = obj.__dict__.pop(self._gen_key, None) + if value is None or gen is None: + return + try: + next(gen) + except StopIteration: + pass + except (Exception, SkillCancelled) as e: + # SkillCancelled (a BaseException) included: teardown running + # after a cancelled run may trip a cancel check, and that must + # not unwind past the run's finalization (the goal would never + # terminate and the roster would show the skill running forever). + if logger is not None: + logger.error(f"releasing resource '{self._name}' failed: {e}") + else: + gen.close() + if logger is not None: + logger.error(f"resource '{self._name}': factory yields more than once") + + +@overload +def resource(factory: Callable[[Any], Iterator[_T_resource]]) -> _Resource[_T_resource]: ... +@overload +def resource(factory: Callable[[Any], _T_resource]) -> _Resource[_T_resource]: ... +def resource(factory: Callable[[Any], Any]) -> _Resource[Any]: + """An expensive object a skill owns, built on first access and cached for + the run. A generator factory tears down below its ``yield`` at run end: + + @resource + def controller(self) -> Iterator[Nav2Controller]: + c = Nav2Controller(self) + yield c + c.destroy() + + A plain ``return`` declares no teardown. Returning or yielding None means + "unavailable": nothing is cached, and the next access retries. + + Implemented as a function (not a decorator class) so type checkers replace + the factory method's type with ``_Resource[T]`` / ``T`` on access — a class + decorator is easy to miss, leaving ``self.foo`` typed as MethodType. + """ + return _Resource(factory) + + +@dataclass(frozen=True) +class _FeedSpec: + """One declarable feed — THE per-feed table; everything below derives + from these rows. Adding a feed is one row plus its RobotStateType member + and its getter/subscription in RobotStateProvider.""" + + skill_type: type # the annotation type authors declare + descriptor: type # Interface / Camera / RobotState — minted for the annotation + key: "RobotStateType | InterfaceType" + author_name: str # the type name as authors write it (the innate export) + hints: tuple[str, ...] # attribute names that suggest this feed (typo help) + label: str = "" # states: human-readable name in run-failure messages + grace_s: float | None = None # states: required-feed warmup bound; None = default + + +@cache +def _feed_specs() -> "tuple[_FeedSpec, ...]": + # imported lazily: the interface classes pull ROS/Nav2 modules + from brain_client.robot.head import Head + from brain_client.robot.manipulation import Manipulation + from brain_client.robot.mobility import Mobility + from brain_client.state.arm import Arm + from brain_client.state.battery import Battery + from brain_client.state.head import HeadState + from brain_client.state.image import DepthMap, MainImage, WristImage + from brain_client.state.joint_states import JointStates + from brain_client.state.lidar import Lidar + from brain_client.state.map import Map + from brain_client.state.odometry import Odometry + from brain_client.state.pose import Pose + + main = RobotStateType.LAST_MAIN_CAMERA_IMAGE_B64 + wrist = RobotStateType.LAST_WRIST_CAMERA_IMAGE_B64 + return ( + _FeedSpec(Manipulation, Interface, InterfaceType.MANIPULATION, "Manipulation", ("manipulation",)), + _FeedSpec(Mobility, Interface, InterfaceType.MOBILITY, "Mobility", ("mobility",)), + _FeedSpec(Head, Interface, InterfaceType.HEAD, "Head", ("head",)), + # cameras start per run (and sim renders on demand) — longer grace + _FeedSpec(MainImage, Camera, main, "MainImage", ("image", "main_image"), "main camera", grace_s=3.0), + _FeedSpec(WristImage, Camera, wrist, "WristImage", ("wrist_image",), "wrist camera", grace_s=3.0), + _FeedSpec( + DepthMap, + Camera, + RobotStateType.LAST_DEPTH_IMAGE, + "DepthMap", + ("depth", "depth_image"), + "depth camera", + grace_s=3.0, + ), + _FeedSpec(Odometry, RobotState, RobotStateType.LAST_ODOM, "Odometry", ("odom",), "odometry"), + _FeedSpec(Pose, RobotState, RobotStateType.LAST_POSE, "Pose", ("pose",), "map pose"), + # battery publishes at ~0.2 Hz — the default grace would always miss it + _FeedSpec(Battery, RobotState, RobotStateType.LAST_BATTERY, "Battery", ("battery",), "battery", grace_s=6.0), + _FeedSpec(Lidar, RobotState, RobotStateType.LAST_LIDAR, "Lidar", ("lidar",), "lidar"), + _FeedSpec(Arm, RobotState, RobotStateType.LAST_ARM, "Arm", ("arm",), "arm pose"), + _FeedSpec(Map, RobotState, RobotStateType.LAST_MAP, "Map", ("map",), "map"), + _FeedSpec( + JointStates, RobotState, RobotStateType.LAST_JOINT_STATES, "JointStates", ("joint_states",), "joint states" + ), + _FeedSpec( + HeadState, RobotState, RobotStateType.LAST_HEAD_POSITION, "HeadState", ("head_position",), "head position" + ), + ) + + +@cache +def _feed_types() -> dict: + """Annotated type -> (descriptor class, feed enum).""" + return {spec.skill_type: (spec.descriptor, spec.key) for spec in _feed_specs()} + + +@cache +def _feed_attr_hints() -> "dict[str, str]": + """Attribute name -> annotation type name, for typo-help messages.""" + return {hint: spec.author_name for spec in _feed_specs() for hint in spec.hints} + + +@cache +def _state_labels() -> dict: + return {spec.key: spec.label for spec in _feed_specs() if spec.label} + + +@cache +def _camera_feed_keys() -> frozenset: + return frozenset(spec.key for spec in _feed_specs() if spec.descriptor is Camera) + + +# required-feed warmup bound when the spec row has no override +_DEFAULT_STATE_GRACE_S = 2.0 + + +@cache +def _state_grace_s() -> dict: + return {spec.key: spec.grace_s for spec in _feed_specs() if spec.grace_s is not None} + + +def _own_annotations(cls) -> dict: + """The class's own annotations, string forms resolved (PEP 563). + + ``inspect.get_annotations`` evaluates all-or-nothing; retry per name so + one typo costs only its own declaration. Entries that still fail stay + strings and surface through the ``_declaration_issues`` warnings. + """ + try: + return inspect.get_annotations(cls, eval_str=True) + except Exception: + module = sys.modules.get(getattr(cls, "__module__", ""), None) + cls_globals = getattr(module, "__dict__", {}) + cls_locals = dict(vars(cls)) + annotations = {} + for name, annotation in cls.__dict__.get("__annotations__", {}).items(): + if isinstance(annotation, str): + try: + annotation = eval(annotation, cls_globals, cls_locals) # noqa: S307 — mirrors inspect + except Exception: + pass + annotations[name] = annotation + return annotations + + +def _split_optional(annotation): + """(base_type, is_optional) — unwraps ``T | None`` / ``Optional[T]``.""" + if get_origin(annotation) in (Union, UnionType): + args = [a for a in get_args(annotation) if a is not type(None)] + if len(args) == 1: + return args[0], True + return annotation, False + + +def _materialize_feed_annotations(cls) -> None: + """Mint the matching descriptor for each bare feed annotation + (``mobility: Mobility``; ``| None`` = optional). A ``= None`` class + default (``image: MainImage | None = None`` — a natural authoring idiom) + counts as an optional declaration, not as opting out of injection. + Suspicious annotations land on ``cls._declaration_issues`` and the + loader logs them.""" + issues: list[str] = [] + cls._declaration_issues = issues # always this class's own, never inherited + # An annotation with a non-None class value (a legacy explicit descriptor, + # a real default) is not a bare declaration; `= None` still declares. + bare = { + name: annotation + for name, annotation in _own_annotations(cls).items() + if name not in cls.__dict__ or cls.__dict__[name] is None + } + if not bare: + return + feed_types = _feed_types() + for name, annotation in bare.items(): + if isinstance(annotation, str): + issues.append( + f"{cls.__name__}.{name}: annotation {annotation!r} does not resolve — if this is a " + "feed declaration, import the type; nothing will be injected for it." + ) + continue + resolved, optional = _split_optional(annotation) + if isinstance(resolved, type) and issubclass(resolved, Skill) and resolved is not Skill: + if optional: + # The feed rule (`| None` = best effort) does not extend to + # composition: a declared sub-skill is always wired and its + # requirements gate the run. Say so, or the author only finds + # out when the parent fails up front on the child's feeds. + issues.append( + f"{cls.__name__}.{name}: `| None` has no effect on a sub-skill declaration — " + f"{resolved.__name__} is always wired and required. For a best-effort dependency, " + "dispatch by id at run time (self.skills.run(...))." + ) + descriptor = SubSkill(resolved) + setattr(cls, name, descriptor) + descriptor.__set_name__(cls, name) + continue + if isinstance(resolved, type) and issubclass(resolved, TrainedSkill) and resolved is not TrainedSkill: + # a generated physical-skill ref: same declaration shape as a code + # sub-skill, materialized as the PhysicalSkill descriptor + if optional: + issues.append( + f"{cls.__name__}.{name}: `| None` has no effect on a physical-skill declaration — " + f"{resolved.__name__} is always required. For a best-effort dependency, " + "dispatch by id at run time (self.skills.run(...))." + ) + descriptor = PhysicalSkill(resolved.skill_id) + setattr(cls, name, descriptor) + descriptor.__set_name__(cls, name) + continue + if resolved is Skill: + issues.append( + f"{cls.__name__}.{name}: annotate with a concrete Skill subclass to compose it " + "(`gripper_open: GripperOpen`); bare `Skill` declares nothing." + ) + continue + entry = feed_types.get(resolved) if isinstance(resolved, type) else None + if entry is None: + hint = _feed_attr_hints().get(name) + if hint is not None: + resolved_name = getattr(resolved, "__name__", repr(resolved)) + issues.append( + f"{cls.__name__}.{name}: annotated with {resolved_name}, which is not a feed " + f"type — did you mean `{name}: {hint}`? Nothing will be injected for it." + ) + continue + descriptor_cls, feed = entry + # a `= None` default reads as "may be None" even without `| None` + descriptor = descriptor_cls(feed, required=not optional and name not in cls.__dict__) + setattr(cls, name, descriptor) + # setattr after class creation skips the implicit __set_name__ hook + descriptor.__set_name__(cls, name) + + +def _index_feed_declarations(cls) -> None: + """Precompute the class's ``{name: descriptor}`` maps (first definition + in MRO wins) — update_robot_state runs at 50 Hz, so the MRO walk happens + once here and the hot path reads a dict.""" + states: dict[str, RobotState] = {} + interfaces: dict[str, Interface] = {} + subskills: dict[str, SubSkill] = {} + physicals: dict[str, PhysicalSkill] = {} + for klass in cls.__mro__: + for name, attr in vars(klass).items(): + if isinstance(attr, RobotState) and name not in states: + states[name] = attr + elif isinstance(attr, Interface) and name not in interfaces: + interfaces[name] = attr + elif isinstance(attr, SubSkill) and name not in subskills: + subskills[name] = attr + elif isinstance(attr, PhysicalSkill) and name not in physicals: + physicals[name] = attr + cls._feed_states = states + cls._feed_interfaces = interfaces + cls._feed_subskills = subskills + cls._feed_physical_skills = physicals class Skill(ABC): # Stamped by the loader to "shipped" or "user" based on origin directory. - source: str = "user" + source: Source = "user" + + # Every subclass registers itself here at definition time, PyTorch-style: + # defining a Skill is what makes the robot know it — no file scanning. + # Keyed by (module, qualname) so a re-import of the same module replaces + # its own entries. Abstract/private classes are filtered at *collect* + # time (registered_skills in workspace_import.py), not here — + # __abstractmethods__ is not populated yet when __init_subclass__ runs. + _registry: "dict[tuple[str, str], type[Skill]]" = {} + + # Precomputed by __init_subclass__ (see _index_feed_declarations); the + # base class itself declares nothing. + _feed_states: "dict[str, RobotState]" = {} + _feed_interfaces: "dict[str, Interface]" = {} + _feed_subskills: "dict[str, SubSkill]" = {} + _feed_physical_skills: "dict[str, PhysicalSkill]" = {} def __init__(self, logger): self.logger = UniversalLogger(enabled=True, wrapped_logger=logger) self.node: Node | None = None self._feedback_callback = None self._cancel_latch() - # SkillInvoker for running other skills from execute(); injected by the - # skills server before each run (see invoker.py and innate/skills.py). - self.skills = None + # injected by the server before each run (see invoker.py) + self.skills: SkillInvoker | None = None self._say_publisher = None self._tts_status_sub = None - self._tts_playing = None # last /tts/is_playing value ("true"/"false") + self._tts_playing = None # last /tts/is_playing value self._storage = None + def __init_subclass__(cls, **kwargs): + super().__init_subclass__(**kwargs) + _materialize_feed_annotations(cls) + _index_feed_declarations(cls) + Skill._registry[(cls.__module__, cls.__qualname__)] = cls + + # hidden from type checkers on purpose: a visible __getattr__ makes every + # attribute legal, so typos would only fail on the robot + if not TYPE_CHECKING: + + def __getattr__(self, name: str): + hint = _feed_attr_hints().get(name) + if hint is not None: + raise AttributeError( + f"'{type(self).__name__}' has no '{name}': declare it on the class with a type " + f"annotation — `{name}: {hint}` (append ` | None` to tolerate it being unavailable)." + ) + raise AttributeError(f"'{type(self).__name__}' object has no attribute {name!r}") + @property + def name(self) -> str: + """The snake_case class name, which must equal the filename stem. + Override only when the two legitimately differ.""" + return class_name_to_snake_case(type(self).__name__) + @abstractmethod - def name(self): - """ - The name of the skill. - Must be defined by every subclass. - """ + def execute(self, *args, **kwargs) -> SkillReturn: pass - @abstractmethod - def execute(self, *args, **kwargs): + def __call__(self, **inputs) -> SkillOutput: + """Run this skill as a step of the caller: composition's call form. + + PyTorch's __call__/forward split: authors implement execute(); calling + the instance runs it with the invoker's contract — success returns the + SkillOutput, FAILURE raises SkillFailed, CANCELLED raises + SkillCancelled. Only meaningful on a wired instance (a declared + sub-skill, or the run root the server built). """ - Execute the skill. + # on_cancel hooks registered during this call expire with it: a wired + # sub-skill instance is reused across calls, so without the truncation + # every execute()'s `self.on_cancel(self._stop)` would stack up — N + # calls, N firings on one cancel — and a hook capturing per-call state + # (a goal handle) would fire long after its run ended. + hooks = vars(self).setdefault("_cancel_hooks", []) + registered_before_call = len(hooks) + try: + result = self.execute(**inputs) + finally: + del hooks[registered_before_call:] + output = normalize_skill_result(result, self.name, logger=self.logger) + if output.status is SkillResult.CANCELLED: + raise SkillCancelled(output.message) + if not output.ok: + raise SkillFailed(output.message) + return output + + def wire_subskills(self, wire_child: "Callable[[type[Skill]], Skill]", _seen: frozenset = frozenset()) -> None: + """Construct and attach every declared sub-skill, recursively. - Subclasses must implement this method. - Returns (result_message, result_status) where result_status is a - SkillResult enum value; an optional third element is a structured - payload chaining callers receive as ``.data`` (see SkillOutput). + ``wire_child`` builds a fully wired instance of a class (run node, + interfaces, invoker, feedback) — the server supplies it. This method + owns what composition adds: sharing the parent's cancel latch so one + cancel unwinds the whole tree, recursion, and the cycle guard. + Cycles can't normally be declared (an annotation needs the class + object, which a circular import prevents), so the guard is defensive. """ - pass + for name, physical in self._feed_physical_skills.items(): + if self.skills is None: + raise RuntimeError( + f"{type(self).__name__}.{name} declares physical skill " + f"'{physical.skill_id}' but no invoker is available" + ) + if self.skills.find(physical.skill_id) is None: + raise RuntimeError( + f"{type(self).__name__}.{name}: no skill with id '{physical.skill_id}' — " + "check the name, or the policy may still be in training" + ) + setattr(self, name, _BoundPhysicalSkill(physical.skill_id, self.skills)) + seen = _seen | {type(self)} + for name, descriptor in self._feed_subskills.items(): + if descriptor.skill_class in seen: + raise RuntimeError( + f"Skill composition cycle: {descriptor.skill_class.__name__} is declared by its own descendant" + ) + child = wire_child(descriptor.skill_class) + vars(child)["_cancel_event"] = self._cancel_latch() + child.wire_subskills(wire_child, seen) + setattr(self, name, child) + + def _wired_children(self) -> "list[Skill]": + return [child for name in self._feed_subskills if (child := getattr(self, name, None)) is not None] + + def fail(self, message: str) -> NoReturn: + """End the run as a FAILURE with ``message``.""" + raise SkillFailed(message) def _cancel_latch(self) -> threading.Event: - """The cancel event, created lazily — some skills skip super().__init__().""" - latch = self.__dict__.get("_cancel_event") + # created lazily — some skills skip super().__init__() + latch = vars(self).get("_cancel_event") if latch is None: - latch = self.__dict__.setdefault("_cancel_event", threading.Event()) + latch = vars(self).setdefault("_cancel_event", threading.Event()) return latch @property - def _cancelled(self) -> bool: - """Whether cancellation was requested for the current run. + def cancelled(self) -> bool: + """True once cancellation was requested for the run.""" + return self._cancel_latch().is_set() - Latches True and ignores False: skills reset the flag at execute() - entry, which would wipe a cancel that raced goal startup. Only the - server re-arms it between runs (_begin_run). - """ + # Legacy write-shim only — read via `cancelled`. It latches True and + # ignores False: fleet skills reset `self._cancelled = False` at execute() + # entry, which would wipe a cancel that raced goal startup. + @property + def _cancelled(self) -> bool: return self._cancel_latch().is_set() @_cancelled.setter @@ -249,61 +863,127 @@ def _cancelled(self, value: bool): if value: self._cancel_latch().set() + def sleep(self, seconds: float) -> None: + """time.sleep for skill code: wakes and raises SkillCancelled the + moment a cancel lands. Sleeping is the only cancel point a loop + needs — write the loop as if cancel didn't exist.""" + if self._cancel_latch().wait(seconds): + raise SkillCancelled(f"{self.name} cancelled") + + def check_cancelled(self) -> None: + """Raise SkillCancelled if a cancel landed — for the rare checkpoint + that has no sleep (e.g. right before an irreversible commit).""" + if self._cancelled: + raise SkillCancelled(f"{self.name} cancelled") + + def wait_for(self, read: Callable[[], T | None], timeout: float = 2.0, poll: float = 0.02) -> T | None: + """Block until ``read()`` returns non-None (or ``timeout`` passes -> + None). Raises SkillCancelled if the run is cancelled while waiting.""" + deadline = time.monotonic() + timeout + while True: + value = read() + if value is not None: + return value + if time.monotonic() >= deadline: + return None + self.sleep(poll) + + def on_cancel(self, callback) -> None: + """Register a zero-arg hook fired (on the cancelling thread) the + moment a cancel lands. Rarely needed: braking the base is automatic + (see ``_halt_interfaces``) — use this only to forward the cancel to + an external goal (``self.on_cancel(handle.cancel_goal_async)``). + A hook registered inside a composed call (``self.child(...)``) lives + only for that call (see ``__call__``); on a run root it lives for + the run.""" + vars(self).setdefault("_cancel_hooks", []).append(callback) + def _begin_run(self, goal_handle=None): - """Server hook: re-arm the latch for a fresh run, recovering a cancel - that already landed from the goal's persistent cancel status.""" - latch = self._cancel_latch() - latch.clear() + """Server hook: latch a cancel that landed before the run started.""" try: if goal_handle is not None and goal_handle.is_cancel_requested: - latch.set() + self._cancel_latch().set() except Exception: pass # duck-typed handles without cancel status - def cancel(self): - """ - Cancel the execution of the skill. Safe to call at any time; returns - a message describing the result. - - The default latches self._cancelled and stops the child skill running - via self.skills. Override it to stop work of your own — and if you - also chain children, call self.skills.cancel() too. - """ + def cancel(self) -> None: + """Latch self.cancelled, fire the on_cancel hooks, halt motion, and + stop any running child. Override only when a hook can't express the + teardown — and if you chain children, call self.skills.cancel() too.""" self._cancel_latch().set() - if getattr(self, "skills", None) is not None: - return self.skills.cancel() - return "Cancellation requested" - - def shutdown(self): # noqa: B027 - """Release resources this instance owns. Called when the server retires - it — reloads replace instances, and a retired one is never used again. - - Override to destroy ROS nodes/entities the skill itself created (e.g. - Nav2 BasicNavigator nodes): a dropped instance is cyclic garbage whose - graph entities otherwise linger until an eventual gen-2 GC pass, so - every reload leaks subscriptions and memory. Leave entities created on - the shared server node alone — destroying entities under a spinning - executor is unsafe (see #497). - """ + self._fire_cancel_hooks() + self._halt_interfaces() + skills = getattr(self, "skills", None) + if skills is not None: + skills.cancel() + + def _halt_interfaces(self) -> None: + """Call halt() on every injected interface that has one, children + included — the framework brake: the server fires this on cancel and + again at run end, so commanded motion never outlives a run and + skills don't write brake hooks or trailing stops.""" + for name in self._feed_interfaces: + halt = getattr(getattr(self, name, None), "halt", None) + if halt is None: + continue + try: + halt() + except Exception as e: + self.logger.error(f"[{self.name}] halting {name} failed: {e}") + for child in self._wired_children(): + child._halt_interfaces() + + def _fire_cancel_hooks(self) -> None: + """Fire this instance's on_cancel hooks, then the wired children's — + a composed child registers its brake hook (`self.on_cancel(self._stop)`) + on ITSELF, and only the root's cancel() is ever invoked, so without + the walk a child mid-`self.move(...)` would coast until its next + cancelled poll instead of braking immediately.""" + for hook in list(vars(self).get("_cancel_hooks", [])): + try: + hook() + except (Exception, SkillCancelled) as e: + # SkillCancelled is a BaseException, so it must be named here: + # a hook that calls a composed child during a cancel raises it + # (the shared latch is already set), and it must not abort the + # remaining hooks or unwind the server's cancel dispatch. + self.logger.error(f"[{self.name}] on_cancel hook failed: {e}") + for child in self._wired_children(): + child._fire_cancel_hooks() + + def shutdown(self): + """Release every ``@resource`` this instance built, at run end. + Entities on ``self.node`` need no cleanup here — the run's throwaway + node is destroyed wholesale right after this returns (#497).""" + for cls in type(self).__mro__: + for attr in vars(cls).values(): + if isinstance(attr, _Resource): + attr.release(self, getattr(self, "logger", None)) + for child in self._wired_children(): + child.shutdown() @property def storage(self) -> SkillStorage: - """Persistent per-skill key-value store (survives restarts), backed by - workspace/skill_storage/.json.""" + """Persistent per-skill key-value store (survives restarts).""" if self._storage is None: self._storage = SkillStorage(_storage_dir() / f"{self.name}.json") return self._storage def say(self, text: str, wait: bool = False) -> None: - """ - Speak text through the robot's voice. Fire-and-forget by default; - with ``wait=True`` it blocks until playback ends (best effort — it - watches the TTS playback status). No-op if speech isn't available. - """ + """Speak through the robot's voice; ``wait=True`` blocks until + playback ends (best effort). No-op if speech isn't available.""" if not text or self.node is None: return if self._say_publisher is None: self._say_publisher = self.node.create_publisher(String, TTS_TOPIC, 10) + # fresh publisher every run — wait briefly for the TTS engine to + # match, or the run's first utterance is dropped. A cancel skips + # the wait: dropped speech beats a delayed Stop. + deadline = time.time() + 1.0 + while self._say_publisher.get_subscription_count() == 0 and time.time() < deadline: + if self.cancelled: + break + time.sleep(0.02) if wait and self._tts_status_sub is None: self._tts_status_sub = self.node.create_subscription(String, TTS_STATUS_TOPIC, self._on_tts_status, 10) self._say_publisher.publish(String(data=text)) @@ -314,105 +994,106 @@ def _on_tts_status(self, msg: String) -> None: self._tts_playing = msg.data def _wait_for_speech_end(self, text: str) -> None: - # wait for playback to start; if it never does (TTS off, muted) - # don't hang the skill - deadline = time.time() + 15.0 + # A cancel abandons the wait (best effort, like the rest of say(): + # no raise — teardown paths speak after the latch is set). Without + # the check a Stop would ride out the full budget below. + # if playback never starts (TTS off, muted), don't hang the skill + deadline = time.monotonic() + 15.0 while self._tts_playing != "true": - if time.time() > deadline: + if self.cancelled or time.monotonic() > deadline: return time.sleep(0.05) - # then wait for it to finish; budget scales with utterance length - deadline = time.time() + max(30.0, 0.1 * len(text)) - while self._tts_playing == "true" and time.time() < deadline: + # finish budget scales with utterance length + deadline = time.monotonic() + max(30.0, 0.1 * len(text)) + while self._tts_playing == "true" and time.monotonic() < deadline: + if self.cancelled: + return time.sleep(0.05) def update_robot_state(self, **kwargs): - """ - Update the skill with the latest robot state. - Automatically populates RobotState descriptors defined on the class. - Subclasses can override this to add custom handling. - """ - # Auto-populate RobotState descriptors - for name, descriptor in self._get_robot_state_descriptors().items(): + for name, descriptor in self._feed_states.items(): state_key = descriptor.state_type.value if state_key in kwargs: setattr(self, name, kwargs[state_key]) + for child in self._wired_children(): + child.update_robot_state(**kwargs) - def clear_robot_state(self): - """ - Reset all RobotState descriptors to None. - - Skill instances are singletons, so values from a previous run would - otherwise read as fresh sensor data on the next one. The skills - server calls this before each run. - """ - for name in self._get_robot_state_descriptors(): - setattr(self, name, None) - - def get_required_robot_states(self) -> list[RobotStateType]: - """ - Declare the robot states required by this skill. - Automatically collects from RobotState descriptors defined on the class. - """ - return [desc.state_type for desc in self._get_robot_state_descriptors().values()] - - def _get_robot_state_descriptors(self) -> dict[str, "RobotState"]: - """Collect all RobotState descriptors from the class.""" - descriptors = {} - for cls in type(self).__mro__: - for name, attr in vars(cls).items(): - if isinstance(attr, RobotState) and name not in descriptors: - descriptors[name] = attr - return descriptors + def declared_robot_state_types(self) -> list[RobotStateType]: + """Every declared state feed, required or optional, sub-skills included.""" + types = [desc.state_type for desc in self._feed_states.values()] + for child in self._wired_children(): + types.extend(child.declared_robot_state_types()) + return list(dict.fromkeys(types)) - def get_required_interfaces(self) -> list[InterfaceType]: - """ - Declare the interfaces required by this skill. - Automatically collects from Interface descriptors defined on the class. - """ - return [desc.interface_type for desc in self._get_interface_descriptors().values()] - - def _get_interface_descriptors(self) -> dict[str, "Interface"]: - """Collect all Interface descriptors from the class.""" - descriptors = {} - for cls in type(self).__mro__: - for name, attr in vars(cls).items(): - if isinstance(attr, Interface) and name not in descriptors: - descriptors[name] = attr - return descriptors + def declared_interface_types(self) -> list[InterfaceType]: + """Every declared interface, required or optional.""" + return [desc.interface_type for desc in self._feed_interfaces.values()] def inject_interface(self, interface_type: InterfaceType, interface_instance): - """Inject an interface instance into the skill.""" - for name, descriptor in self._get_interface_descriptors().items(): + for name, descriptor in self._feed_interfaces.items(): if descriptor.interface_type == interface_type: setattr(self, name, interface_instance) return True return False - def guidelines(self): - """ - Optionally provide guidelines for this skill. - Subclasses may override this method if guidelines are available. - """ + def missing_required_interfaces(self) -> list[str]: + """Declared required interfaces still None — the server fails the run + up front when non-empty.""" + missing = [ + descriptor.interface_type.value + for name, descriptor in self._feed_interfaces.items() + if descriptor.required and getattr(self, name) is None + ] + for child in self._wired_children(): + missing.extend(child.missing_required_interfaces()) + return list(dict.fromkeys(missing)) + + def required_robot_state_types(self) -> "list[RobotStateType]": + types = [d.state_type for d in self._feed_states.values() if d.required] + for child in self._wired_children(): + types.extend(child.required_robot_state_types()) + return list(dict.fromkeys(types)) + + def missing_required_robot_states(self) -> list[str]: + """Labels of declared required states still None — the server fails + the run up front when non-empty after the warmup wait.""" + missing = [ + _state_labels().get(descriptor.state_type, descriptor.state_type.value) + for name, descriptor in self._feed_states.items() + if descriptor.required and getattr(self, name) is None + ] + for child in self._wired_children(): + missing.extend(child.missing_required_robot_states()) + return list(dict.fromkeys(missing)) + + def describe_feeds(self) -> str: + """Every declared feed for load-time logs, ``?`` marking optional: + ``mobility, image, battery?``.""" + parts = [ + name + ("" if descriptor.required else "?") + for group in (self._feed_interfaces, self._feed_states) + for name, descriptor in group.items() + ] + return ", ".join(parts) + + def guidelines(self) -> str | None: + """What the agent reads to decide when to call this skill; defaults + to the class docstring.""" + doc = type(self).__dict__.get("__doc__") + return inspect.cleandoc(doc) if doc else None + + def guidelines_when_running(self) -> str | None: return None - def guidelines_when_running(self): - """ - Optionally provide guidelines for this skill when it is running. - Subclasses may override this method if guidelines are available. - """ - return None - - def set_feedback_callback(self, callback): - """Sets the feedback callback function.""" - self._feedback_callback = callback - self.logger.debug(f"Feedback callback set for skill {self.name}.") - - def _send_feedback(self, message: str, image_b64: str = None): - """Sends feedback if the callback is set, optionally with an image.""" + def feedback(self, message: str, image_b64: str | None = None) -> None: + """Stream a progress update to whoever launched the skill.""" self.logger.info(f"Skill feedback [{self.name}]: {message}") if self._feedback_callback: try: self._feedback_callback(message, image_b64) except Exception as e: self.logger.error(f"Error sending feedback for skill {self.name}: {e}") + + def set_feedback_callback(self, callback: Callable[[str, str | None], None]) -> None: + self._feedback_callback = callback + self.logger.debug(f"Feedback callback set for skill {self.name}.") diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/workspace_import.py b/ros2_ws/src/brain/brain_client/brain_client/skills/workspace_import.py new file mode 100644 index 000000000..4507fdb5e --- /dev/null +++ b/ros2_ws/src/brain/brain_client/brain_client/skills/workspace_import.py @@ -0,0 +1,190 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +"""Import-based skill discovery: skills register by existing, not by being found. + +The model: every directory under ``workspace/`` not claimed by other +machinery is a Python package. Discovery is a *real import* — +``workspace/`` goes on ``sys.path``, every module in every package is imported +with ``importlib.import_module``, and defining a ``Skill`` subclass registers +it (``Skill.__init_subclass__``), the way defining an ``nn.Module`` is all +PyTorch needs. No file execing, no filename-derived identity, no guessing +whether a file "meant" to be a skill: + +- a module that imports cleanly and registers nothing **is** a helper; +- a module that raises **is** broken, keyed by its real module name with a + real traceback; +- a module that registers classes contributes one skill per class — + ``id = /``. + +Because these are ordinary imports, ordinary Python works: relative imports, +multi-skill files, helpers anywhere, packages importing each other by bare +name, and pip-installed packs registering on import (the torchvision pattern). + +Hot reload = evict every cached module under ``workspace/`` (see +``evict_modules_under``), then import again. +""" + +from __future__ import annotations + +import importlib +import inspect +import sys +from pathlib import Path + +from brain_client.common.dynamic_loader import class_name_to_snake_case +from brain_client.common.script_paths import ( + get_custom_skills_dir, + get_innate_os_root, + get_innate_skills_dir, + get_workspace_dir, + get_workspace_package_dirs, +) +from brain_client.skills.physical import has_physical_metadata +from brain_client.skills.types import Skill + +# workspace package directory name -> skill-id namespace. The two standard +# dirs keep their historical prefixes so every persisted id keeps resolving; +# any other package namespaces by its own directory name. +_NAMESPACE_BY_PACKAGE = {"innate_skills": "innate-os", "custom_skills": "local"} + + +def ensure_import_roots() -> None: + """Put the repo root and workspace/ on sys.path (idempotent). + + The root makes ``workspace.*`` imports resolve (compat with existing + skills); workspace/ itself makes packages importable by bare name + (``from innate_skills import move_straight``, ``import custom_skills.geometry``). + """ + for path in (str(get_innate_os_root()), str(get_workspace_dir())): + if path not in sys.path: + sys.path.insert(0, path) + + +def _iter_module_names(directory: Path, prefix: str): + """Dotted module names for every importable module under ``directory``. + + Hidden and ``_``-prefixed names are skipped from *proactive* import (they + still import fine transitively — same convention as pytest); a directory + with a non-empty ``metadata.json`` is a physical skill's data, not a + package (see ``has_physical_metadata`` for why empties don't count). + """ + for entry in sorted(directory.iterdir()): + if entry.name.startswith((".", "_")): + continue + if entry.is_file() and entry.suffix == ".py": + yield f"{prefix}.{entry.stem}" + elif entry.is_dir() and not has_physical_metadata(entry): + yield f"{prefix}.{entry.name}" + yield from _iter_module_names(entry, f"{prefix}.{entry.name}") + + +def import_workspace_packages(logger) -> dict[str, str]: + """Import every module in every workspace package. + + Returns ``{module_name: error}`` for modules that failed — the catalog + rosters these as broken so nothing silently vanishes. Modules already in + ``sys.modules`` are cheap no-ops; a reload evicts first. + """ + ensure_import_roots() + errors: dict[str, str] = {} + package_dirs = [get_innate_skills_dir(), get_custom_skills_dir(), *get_workspace_package_dirs()] + for package_dir in package_dirs: + if not package_dir.is_dir(): + continue + for module_name in (package_dir.name, *_iter_module_names(package_dir, package_dir.name)): + try: + importlib.import_module(module_name) + except Exception as e: # noqa: BLE001 — a broken user module must not stop discovery + errors[module_name] = f"{type(e).__name__}: {e}" + logger.warning(f"Module {module_name} failed to import: {type(e).__name__}: {e}") + return errors + + +def skill_id_for_class(cls: type) -> str: + """``/`` — identity from the class, not the file.""" + top = cls.__module__.split(".", 1)[0] + namespace = _NAMESPACE_BY_PACKAGE.get(top, top) + return f"{namespace}/{class_name_to_snake_case(cls.__name__)}" + + +def module_skill_id(module_name: str) -> str: + """A roster id for a *module* (used for broken modules, which have no class). + + ``custom_skills.pick_socks`` -> ``local/pick_socks``; + ``john_skills.boards.chess`` -> ``john_skills/boards.chess``. + """ + top, _, rest = module_name.partition(".") + namespace = _NAMESPACE_BY_PACKAGE.get(top, top) + return f"{namespace}/{rest or top}" + + +def _live_class(module, qualname: str): + """The object ``qualname`` currently denotes in ``module``, or None. + + Walks the full dotted path so a Skill nested in a class namespace resolves + to itself, not to its enclosing class (which would silently fail the + identity check below and prune a live skill). Function-local classes + (```` in the qualname) are unreachable from the module by design. + """ + obj = module + for part in qualname.split("."): + if part == "": + return None + obj = getattr(obj, part, None) + if obj is None: + return None + return obj + + +def registered_workspace_skills(logger) -> dict[str, tuple[str, type[Skill], Path]]: + """Live skills from the registry: ``{skill_id: (class_name, cls, source_path)}``. + + Prunes stale registrations as it goes: an entry whose module is no longer + in ``sys.modules`` (evicted for reload) or no longer binds this exact + class object (module re-imported, file edited to remove the class) is + dead. Abstract and ``_``-prefixed classes are helper bases, never skills. + """ + skills: dict[str, tuple[str, type[Skill], Path]] = {} + for (module_name, qualname), cls in list(Skill._registry.items()): + module = sys.modules.get(module_name) + bound = _live_class(module, qualname) if module is not None else None + if bound is not cls: + # A live module with a function-local Skill is an authoring + # mistake, not staleness — pruning it silently would be the exact + # vanishing this import model exists to eliminate. + if module is not None and "" in qualname: + logger.warning( + f"Skill {qualname} in {module_name} is defined inside a function and cannot be " + "loaded; define it at module level." + ) + del Skill._registry[(module_name, qualname)] + continue + if cls.__name__.startswith("_"): + continue # helper base by convention, deliberately not a skill + if inspect.isabstract(cls): + # Usually an unimplemented abstract method (e.g. a misspelled + # execute()) — the class silently vanishing from the roster was + # undiagnosable for the author (same warning the old file-exec + # loader carried; it must not regress here). + missing = ", ".join(sorted(getattr(cls, "__abstractmethods__", ()))) + logger.warning( + f"Skipping abstract class {cls.__name__} in {module_name} (unimplemented: {missing}); " + "implement the missing methods, or prefix the class with '_' if it is a helper base." + ) + continue + if module_name.startswith("workspace."): + # The same file imported via the compat `workspace.` path is a + # *second* module object; the bare-name import above already owns + # the registration, so skip the double to avoid duplicate ids. + continue + source_file = getattr(module, "__file__", None) + if source_file is None: + continue + skill_id = skill_id_for_class(cls) + if skill_id in skills: + logger.warning( + f"Skill id conflict: '{skill_id}' defined by both " + f"{skills[skill_id][2]} and {source_file}. Using the latter." + ) + skills[skill_id] = (cls.__name__, cls, Path(source_file)) + return skills diff --git a/ros2_ws/src/brain/brain_client/brain_client/state/__init__.py b/ros2_ws/src/brain/brain_client/brain_client/state/__init__.py new file mode 100644 index 000000000..8af398be6 --- /dev/null +++ b/ros2_ws/src/brain/brain_client/brain_client/state/__init__.py @@ -0,0 +1,12 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +"""Skill-facing robot-state snapshots — the types behind ``odom: Odometry``, +``pose: Pose``, ``battery: Battery`` and friends. + +Every module here is ROS-free on purpose: these are plain dataclasses (plus +the ``Image`` str subclass) converted per-message from the live feeds, so +they import anywhere — tests, type checkers, dev laptops without ROS. The +framework that injects them lives in ``brain_client.skills``; the actuator +interfaces (``Mobility``, ``Manipulation``, ``Head``) live in +``brain_client.robot``. +""" diff --git a/ros2_ws/src/brain/brain_client/brain_client/state/arm.py b/ros2_ws/src/brain/brain_client/brain_client/state/arm.py new file mode 100644 index 000000000..71e4df676 --- /dev/null +++ b/ros2_ws/src/brain/brain_client/brain_client/state/arm.py @@ -0,0 +1,40 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +"""Skill-facing arm state. ROS-free on purpose.""" + +from dataclasses import dataclass + + +@dataclass(frozen=True) +class Arm: + """An end-effector snapshot, read via ``self.arm`` in skills. + + The pose is the arm's live forward kinematics (/fk_pose); ``gripper`` is + the claw servo's joint position (j6), the same value skills previously + dug out of ``joint_states["position"][5]``. + """ + + x: float + """End-effector X in meters, arm base frame.""" + y: float + """End-effector Y in meters, arm base frame.""" + z: float + """End-effector Z in meters, arm base frame.""" + qx: float + qy: float + qz: float + qw: float + """End-effector orientation quaternion.""" + gripper: float | None = None + """Gripper joint (j6) position in radians; None if joint states are missing.""" + frame_id: str = "" + + @property + def position(self) -> tuple[float, float, float]: + """(x, y, z) in meters, arm base frame.""" + return (self.x, self.y, self.z) + + @property + def orientation(self) -> tuple[float, float, float, float]: + """(qx, qy, qz, qw).""" + return (self.qx, self.qy, self.qz, self.qw) diff --git a/ros2_ws/src/brain/brain_client/brain_client/state/battery.py b/ros2_ws/src/brain/brain_client/brain_client/state/battery.py new file mode 100644 index 000000000..2d8fdf7c9 --- /dev/null +++ b/ros2_ws/src/brain/brain_client/brain_client/state/battery.py @@ -0,0 +1,37 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +"""Skill-facing battery state. ROS-free on purpose.""" + +from dataclasses import dataclass + +from brain_client.state.dictcompat import LegacyMapping + + +@dataclass(frozen=True) +class Battery(LegacyMapping): + """A battery snapshot, read via ``self.battery`` in skills.""" + + percentage: float + """State of charge, 0.0-1.0.""" + voltage: float + """Pack voltage in volts.""" + current: float + """Pack current in amps.""" + charging: bool + """True while the charger reports charging.""" + + # --- legacy dict compatibility --------------------------------------- + # LAST_BATTERY injected this dict through 0.6.x; the LegacyMapping mixin + # keeps that access working (see dictcompat.py). Do not delete. + + _legacy_hint = "the Battery attributes (battery.percentage, battery.charging, ...)" + + @property + def _legacy_dict(self) -> dict: + """Exactly the 0.3.0-0.6.x injected shape — the same four fields.""" + return { + "percentage": self.percentage, + "voltage": self.voltage, + "current": self.current, + "charging": self.charging, + } diff --git a/ros2_ws/src/brain/brain_client/brain_client/state/dictcompat.py b/ros2_ws/src/brain/brain_client/brain_client/state/dictcompat.py new file mode 100644 index 000000000..34f37bc63 --- /dev/null +++ b/ros2_ws/src/brain/brain_client/brain_client/state/dictcompat.py @@ -0,0 +1,52 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +"""Read-only dict compatibility for typed skill-state values. + +Old skills use dict-style access (``value["key"]``, ``.get()``, etc.). Typed +values mix this in so that code keeps working. Soft-deprecated via warning; +subclasses define ``_legacy_dict`` and ``_legacy_hint``. +""" + +import warnings +from typing import Any + + +class LegacyMapping: + _legacy_hint = "the attributes" + + def __getitem__(self, key): + return self._legacy_mapping()[key] + + def __iter__(self): + return iter(self._legacy_mapping()) + + def __len__(self) -> int: + return len(self._legacy_mapping()) + + def __bool__(self) -> bool: + # Avoid __len__ truthiness so `if self.:` does not warn. + return True + + def get(self, key, default=None): + return self._legacy_mapping().get(key, default) + + def __contains__(self, key) -> bool: + return key in self._legacy_mapping() + + def keys(self): + return self._legacy_mapping().keys() + + def items(self): + return self._legacy_mapping().items() + + def values(self): + return self._legacy_mapping().values() + + def _legacy_mapping(self) -> dict[str, Any]: + # FutureWarning (not DeprecationWarning) so skill authors see it on-robot. + warnings.warn( + f"dict-style access is deprecated; use {self._legacy_hint}", + FutureWarning, + stacklevel=3, + ) + return self._legacy_dict # pyright: ignore[reportAttributeAccessIssue] — see class body diff --git a/ros2_ws/src/brain/brain_client/brain_client/state/head.py b/ros2_ws/src/brain/brain_client/brain_client/state/head.py new file mode 100644 index 000000000..cef2af23d --- /dev/null +++ b/ros2_ws/src/brain/brain_client/brain_client/state/head.py @@ -0,0 +1,42 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +"""Skill-facing head state. ROS-free on purpose.""" + +from dataclasses import dataclass, field +from typing import Any + +from brain_client.state.dictcompat import LegacyMapping + + +@dataclass(frozen=True) +class HeadState(LegacyMapping): + """A head snapshot, read via ``self.head_position`` in skills. + + MARS's head is a single pitch axis; negative pitch looks down. + """ + + pitch_degrees: float + """Current head pitch in degrees (the topic's ``current_position``).""" + min_degrees: float | None = None + """Lowest commandable pitch, if the driver reports it.""" + max_degrees: float | None = None + """Highest commandable pitch, if the driver reports it.""" + default_degrees: float | None = None + """The driver's neutral pitch, if it reports one.""" + raw_source: Any = field(default=None, repr=False, compare=False) + """The original topic payload (a dict) — nothing the driver publishes + is lost, even keys this snapshot doesn't model. Excluded from ==/hash.""" + + # LAST_HEAD_POSITION injected the topic payload dict before ambient state + _legacy_hint = "the Head attributes (head_position.pitch_degrees, ...)" + + @property + def _legacy_dict(self) -> dict: + if self.raw_source is not None: + return self.raw_source + return { + "current_position": self.pitch_degrees, + "min_angle": self.min_degrees, + "max_angle": self.max_degrees, + "default_angle": self.default_degrees, + } diff --git a/ros2_ws/src/brain/brain_client/brain_client/state/image.py b/ros2_ws/src/brain/brain_client/brain_client/state/image.py new file mode 100644 index 000000000..2027de44d --- /dev/null +++ b/ros2_ws/src/brain/brain_client/brain_client/state/image.py @@ -0,0 +1,44 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +"""Skill-facing camera frames. ROS-free on purpose.""" + +import base64 +from typing import TypeVar + +import numpy as np + +_ImageT = TypeVar("_ImageT", bound="Image") + + +class Image(str): + """A camera frame: the value IS the JPEG as base64 text, so anything that + treated frames as base64 strings keeps working. ``.jpeg`` is the decoded + bytes.""" + + @classmethod + def from_jpeg(cls: "type[_ImageT]", data: bytes) -> "_ImageT": + image = cls(base64.b64encode(data).decode("ascii")) + image._jpeg = data + return image + + @property + def jpeg(self) -> bytes: + """The frame as raw JPEG bytes (decoded once, then cached).""" + cached = self.__dict__.get("_jpeg") + if cached is None: + cached = base64.b64decode(self) + self._jpeg = cached + return cached + + +class MainImage(Image): + """A main-camera frame, declared via ``image: MainImage``.""" + + +class WristImage(Image): + """A wrist-camera frame, declared via ``image: WristImage``.""" + + +class DepthMap(np.ndarray): + """A (height, width) depth array (uint16 mm or float32 m), declared via + ``depth: DepthMap``. A view-cast of the decoded frame; no state of its own.""" diff --git a/ros2_ws/src/brain/brain_client/brain_client/state/joint_states.py b/ros2_ws/src/brain/brain_client/brain_client/state/joint_states.py new file mode 100644 index 000000000..eae1b9abf --- /dev/null +++ b/ros2_ws/src/brain/brain_client/brain_client/state/joint_states.py @@ -0,0 +1,59 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +"""Skill-facing arm joint state. ROS-free on purpose.""" + +from dataclasses import dataclass +from functools import cached_property + +from brain_client.state.dictcompat import LegacyMapping + + +@dataclass(frozen=True) +class JointStates(LegacyMapping): + """An arm joint-state snapshot, read via ``self.joint_states`` in skills. + + The four tuples are parallel: index i of each describes the same joint + (the gripper claw is index 5, aka j6 — ``self.arm.gripper`` reads it + directly). ``of(name)`` looks a joint up by its published name. + """ + + name: tuple + """Joint names, e.g. ("j1", ..., "j6").""" + position: tuple + """Joint positions in radians.""" + velocity: tuple + """Joint velocities in rad/s.""" + effort: tuple + """Joint efforts/loads as published by the servos.""" + + def of(self, name: str) -> "tuple[float | None, float | None, float | None] | None": + """(position, velocity, effort) for the named joint, or None if the + name is unknown. Missing per-joint entries (a driver may publish + fewer velocities/efforts than names) read as None.""" + try: + i = self.name.index(name) + except ValueError: + return None + + def at(values): + return values[i] if i < len(values) else None + + return (at(self.position), at(self.velocity), at(self.effort)) + + # --- legacy dict compatibility --------------------------------------- + # LAST_JOINT_STATES injected {"name", "position", ...} of lists through + # 0.6.x; the LegacyMapping mixin keeps that access working (see + # dictcompat.py). Do not delete. + + _legacy_hint = "the JointStates attributes (joint_states.position, joint_states.of(name), ...)" + + @cached_property + def _legacy_dict(self) -> dict: + """Exactly the 0.3.0-0.6.x injected shape: the four parallel tuples + as lists.""" + return { + "name": list(self.name), + "position": list(self.position), + "velocity": list(self.velocity), + "effort": list(self.effort), + } diff --git a/ros2_ws/src/brain/brain_client/brain_client/state/lidar.py b/ros2_ws/src/brain/brain_client/brain_client/state/lidar.py new file mode 100644 index 000000000..df02da38b --- /dev/null +++ b/ros2_ws/src/brain/brain_client/brain_client/state/lidar.py @@ -0,0 +1,56 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +"""Skill-facing lidar state. ROS-free on purpose.""" + +import math +from dataclasses import dataclass + + +@dataclass(frozen=True) +class Lidar: + """One lidar sweep, read via ``self.lidar`` in skills. + + Beam ``i`` points at ``angle_min + i * angle_increment`` radians in the + scan frame (counter-clockwise positive, 0 = the lidar's forward axis). + Invalid returns appear as inf/0 in ``ranges``; use :meth:`min_range`, + which filters them. + """ + + ranges: tuple + """Measured distances in meters, one per beam.""" + angle_min: float + """Angle of beam 0 in radians.""" + angle_increment: float + """Angle between consecutive beams in radians.""" + range_min: float + """Sensor minimum valid distance in meters.""" + range_max: float + """Sensor maximum valid distance in meters.""" + stamp: float = 0.0 + """Sensor timestamp in seconds (ROS time).""" + frame_id: str = "" + + def min_range(self, angle_from_deg: float | None = None, angle_to_deg: float | None = None) -> float | None: + """Closest valid return in meters, or None if the sweep has none. + + Optionally restrict to the sector from ``angle_from_deg`` to + ``angle_to_deg`` (degrees, scan frame, counter-clockwise). The pair + may wrap through +-180: ``min_range(150, -150)`` looks behind the + robot, ``min_range(-30, 30)`` ahead. + """ + lo = -180.0 if angle_from_deg is None else angle_from_deg + hi = 180.0 if angle_to_deg is None else angle_to_deg + best = None + for i, r in enumerate(self.ranges): + if not math.isfinite(r) or not self.range_min <= r <= self.range_max: + continue + angle = _wrap_deg(math.degrees(self.angle_min + i * self.angle_increment)) + in_sector = lo <= angle <= hi if lo <= hi else (angle >= lo or angle <= hi) + if in_sector and (best is None or r < best): + best = r + return best + + +def _wrap_deg(angle: float) -> float: + """Wrap an angle in degrees to [-180, 180).""" + return (angle + 180.0) % 360.0 - 180.0 diff --git a/ros2_ws/src/brain/brain_client/brain_client/state/map.py b/ros2_ws/src/brain/brain_client/brain_client/state/map.py new file mode 100644 index 000000000..b08d84ec7 --- /dev/null +++ b/ros2_ws/src/brain/brain_client/brain_client/state/map.py @@ -0,0 +1,88 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +"""Skill-facing occupancy-grid map. ROS-free on purpose (numpy only).""" + +import base64 +import math +from dataclasses import dataclass, field +from functools import cached_property +from typing import Any + +import numpy as np + +from brain_client.state.dictcompat import LegacyMapping + + +@dataclass(frozen=True) +class Map(LegacyMapping): + """The occupancy-grid map, read via ``self.map`` in skills. + + ``grid`` is the useful part: a (height, width) int8 array where -1 is + unknown, 0 free and 100 occupied. Cell (row, col) covers the world + point ``(origin_x + col * resolution, origin_y + row * resolution)`` + (map frame, before origin rotation, which is 0 for MARS maps). + """ + + resolution: float + """Cell edge length in meters.""" + width: int + """Grid width in cells.""" + height: int + """Grid height in cells.""" + origin_x: float + """World X of cell (0, 0)'s corner, map frame.""" + origin_y: float + """World Y of cell (0, 0)'s corner, map frame.""" + origin_theta: float = 0.0 + """Grid rotation in radians (0 for MARS maps).""" + stamp: float = 0.0 + """Map timestamp in seconds (ROS time).""" + frame_id: str = "map" + raw_source: Any = field(default=None, repr=False, compare=False) + """The nav_msgs/OccupancyGrid message this was built from; provenance + for the lazy ``grid``/legacy views. Excluded from ==/hash.""" + + @cached_property + def grid(self) -> "np.ndarray | None": + """(height, width) int8 occupancy values: -1 unknown, 0 free, + 100 occupied. Built lazily so map reads cost nothing until a skill + actually looks at the cells.""" + if self.raw_source is None: + return None + return np.array(self.raw_source.data, dtype=np.int8).reshape((self.height, self.width)) + + # --- legacy dict compatibility --------------------------------------- + # LAST_MAP injected a header/info/data_b64 dict before ambient state. + + _legacy_hint = "the Map attributes (map.grid, map.resolution, map.origin_x, ...)" + + @cached_property + def _legacy_dict(self) -> dict: + sec = int(self.stamp) + source = self.raw_source + load_time = ( + {"sec": source.info.map_load_time.sec, "nanosec": source.info.map_load_time.nanosec} + if source is not None + else {"sec": 0, "nanosec": 0} + ) + origin_z = source.info.origin.position.z if source is not None else 0.0 + half = self.origin_theta / 2.0 + grid = self.grid + return { + "header": { + "stamp": {"sec": sec, "nanosec": int(round((self.stamp - sec) * 1e9))}, + "frame_id": self.frame_id, + }, + "info": { + "map_load_time": load_time, + "resolution": self.resolution, + "width": self.width, + "height": self.height, + "origin": { + "position": {"x": self.origin_x, "y": self.origin_y, "z": origin_z}, + "orientation": {"x": 0.0, "y": 0.0, "z": math.sin(half), "w": math.cos(half)}, + "yaw_degrees": math.degrees(self.origin_theta), + }, + }, + "data_b64": base64.b64encode(grid.tobytes()).decode("utf-8") if grid is not None else "", + } diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/odometry.py b/ros2_ws/src/brain/brain_client/brain_client/state/odometry.py similarity index 70% rename from ros2_ws/src/brain/brain_client/brain_client/skills/odometry.py rename to ros2_ws/src/brain/brain_client/brain_client/state/odometry.py index cbc210cc8..7233bb011 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/skills/odometry.py +++ b/ros2_ws/src/brain/brain_client/brain_client/state/odometry.py @@ -1,28 +1,23 @@ # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc -"""Skill-facing odometry state. - -MARS is a differential-drive base on flat ground, so its pose is fully -described by (x, y, yaw) — skills get that directly instead of the raw ROS -Odometry message with its quaternion orientation. This module is ROS-free on -purpose; contexts without rclpy should import it from here directly (the -`innate` namespace, where it is also exported, pulls in ROS-dependent skill -types). -""" +"""Skill-facing odometry state. ROS-free on purpose. MARS is a differential- +drive base on flat ground, so its pose is fully (x, y, yaw).""" import math -import warnings from dataclasses import dataclass, field from functools import cached_property from typing import Any +from brain_client.state.dictcompat import LegacyMapping + @dataclass(frozen=True) -class Odometry: +class Odometry(LegacyMapping): """A 2D odometry snapshot: pose in the odom frame plus body velocities. - Injected for ``RobotState(RobotStateType.LAST_ODOM)`` and refreshed at - 50 Hz while a skill runs. + Read ambiently via ``self.odom`` while a skill runs — each read converts + the newest /odom message. Also injected for legacy ``RobotState`` + descriptors. """ x: float @@ -77,52 +72,11 @@ def position(self) -> tuple[float, float]: return (self.x, self.y) # --- legacy dict compatibility --------------------------------------- - # LAST_ODOM injected a raw-message dict from 0.3.0 through 0.6.x, so old - # skill files use dict-style access: odom["theta_degrees"], .get(), `in`, - # iteration, .keys()/.items()/.values(). The full read-only mapping - # protocol is provided so that code behaves exactly as it did on the real - # dict. Soft-deprecated (each call warns to nudge authors to the - # attributes) but kept as a permanent compatibility layer -- there is no - # scheduled removal, old skills keep working indefinitely. Do not delete. - - def __getitem__(self, key): - return self._legacy_mapping()[key] - - def __iter__(self): - return iter(self._legacy_mapping()) - - def __len__(self) -> int: - return len(self._legacy_mapping()) - - def __bool__(self) -> bool: - # without this, __len__ would define truthiness -- making the - # documented `if self.odom:` None-check fire the deprecation warning - return True - - def get(self, key, default=None): - return self._legacy_mapping().get(key, default) - - def __contains__(self, key) -> bool: - return key in self._legacy_mapping() - - def keys(self): - return self._legacy_mapping().keys() - - def items(self): - return self._legacy_mapping().items() - - def values(self): - return self._legacy_mapping().values() - - def _legacy_mapping(self) -> dict: - warnings.warn( - "dict-style odometry access is deprecated; use the Odometry " - "attributes instead (odom.x, odom.theta_degrees, ...) or odom.raw " - "for the full message", - DeprecationWarning, - stacklevel=3, # past the dunder/method that called us, at user code - ) - return self._legacy_dict + # LAST_ODOM injected a raw-message dict from 0.3.0 through 0.6.x; the + # LegacyMapping mixin keeps that access working (see dictcompat.py). + # Do not delete. + + _legacy_hint = "the Odometry attributes (odom.x, odom.theta_degrees, ...) or odom.raw for the full message" @cached_property def _legacy_dict(self) -> dict: diff --git a/ros2_ws/src/brain/brain_client/brain_client/state/pose.py b/ros2_ws/src/brain/brain_client/brain_client/state/pose.py new file mode 100644 index 000000000..185701b1a --- /dev/null +++ b/ros2_ws/src/brain/brain_client/brain_client/state/pose.py @@ -0,0 +1,44 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +"""Skill-facing map-frame pose. ROS-free on purpose.""" + +import math +from dataclasses import dataclass + + +@dataclass(frozen=True) +class Pose: + """The robot's pose on the map, read via ``self.pose`` in skills. + + Unlike ``self.odom`` (odom frame: smooth but drifts and resets every + boot), this is the localizer's estimate in the persistent map frame — + the coordinates ``navigate_to_position`` targets live in. ``self.pose`` + reads None until the robot is localized. + """ + + x: float + """Position along X in meters, map frame.""" + y: float + """Position along Y in meters, map frame.""" + theta: float + """Yaw in radians, counter-clockwise positive, wrapped to [-pi, pi].""" + stamp: float = 0.0 + """Sensor timestamp in seconds (ROS time).""" + frame_id: str = "map" + + def __post_init__(self): + # same wrapped-theta contract as Odometry, enforced for hand-built + # instances too + if not -math.pi <= self.theta <= math.pi: + # frozen dataclass: object.__setattr__ bypasses the immutability guard + object.__setattr__(self, "theta", math.atan2(math.sin(self.theta), math.cos(self.theta))) + + @property + def theta_degrees(self) -> float: + """Yaw in degrees, counter-clockwise positive.""" + return math.degrees(self.theta) + + @property + def position(self) -> tuple[float, float]: + """(x, y) in meters, map frame.""" + return (self.x, self.y) diff --git a/ros2_ws/src/brain/brain_client/brain_client/transport/tts.py b/ros2_ws/src/brain/brain_client/brain_client/transport/tts.py index 3dd7ba6b1..40f44832b 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/transport/tts.py +++ b/ros2_ws/src/brain/brain_client/brain_client/transport/tts.py @@ -14,11 +14,10 @@ import time from typing import Any +from brain_client.common.logging import UniversalLogger from innate_proxy import ProxyClient from innate_proxy.adapters.cartesia import ProxyCartesiaClient -from brain_client.common.logging import UniversalLogger - class TTSHandler: """ diff --git a/ros2_ws/src/brain/brain_client/innate/__init__.py b/ros2_ws/src/brain/brain_client/innate/__init__.py index 2e6a9f4f5..2f22e72d0 100644 --- a/ros2_ws/src/brain/brain_client/innate/__init__.py +++ b/ros2_ws/src/brain/brain_client/innate/__init__.py @@ -4,31 +4,120 @@ Everything a skill file needs, under one import: - from innate import Skill, SkillResult - from innate.skills import head_emotion, navigate_to_position + from innate import MainImage, Mobility, Skill + from innate_skills.gripper_open import GripperOpen + + class WaveAtCamera(Skill): + \"\"\"Wave the arm at whoever the camera sees.\"\"\" + + mobility: Mobility + image: MainImage + + def execute(self): + ... + +The class docstring is the agent-facing guidelines, the class name is the +skill name (snake_cased), and everything the skill consumes is declared with +a bare type annotation — the type identifies the feed. + +One rule covers interfaces, cameras and robot state: annotate what you read. +``battery: Battery``, ``odom: Odometry``, ``pose: Pose``, ``lidar: Lidar``, +``arm: Arm``, ``map: Map``, ``joint_states: JointStates``, +``head_position: HeadState``, ``image: MainImage`` / ``WristImage`` / +``DepthMap``, ``mobility: Mobility``, ``head: Head``. A plain annotation is +guaranteed inside execute() — the server waits for the first value and fails +the run up front if none arrives — so no None guards are needed; ``| None`` +(``head: Head | None``) makes it best effort instead, injected when available +and None otherwise. Reading an undeclared feed raises, and your editor flags +it before you ship. + +execute() returns the run's result: the message str, or +``SkillOutput(message, data)`` to attach a structured payload for chaining +callers, or None. Call ``self.fail(message)`` to end the run as a failure. +Callers of other skills get that SkillOutput back — ``out = self.turn(...)`` +then ``out.message`` / ``out.data`` / ``out.ok``, with ``out.status`` a +SkillResult enum, never a bare string. (Legacy ``(message, SkillResult)`` +tuple returns still work but are deprecated.) + +Cancellation is the framework's job, not yours. Use ``self.sleep(seconds)`` +instead of ``time.sleep`` and write loops as if cancel didn't exist: every +blocking framework call (``self.sleep``, ``self.wait_for``, sub-skill calls, +interface helpers) raises SkillCancelled the moment a Stop lands, the base +is braked automatically, and the run reports CANCELLED. ``try/finally`` in +execute() is your cleanup hook; ``self.on_cancel(...)`` exists only to +forward a cancel to an external action goal. """ -from brain_client.skills.odometry import Odometry +from typing import TYPE_CHECKING + from brain_client.skills.types import ( - Interface, - InterfaceType, - RobotState, - RobotStateType, + PhysicalSkill, Skill, + SkillCancelled, + SkillFailed, SkillOutput, SkillResult, + SkillReturn, + TrainedSkill, + resource, ) -from innate.skills import SkillCancelled, SkillFailed +from brain_client.state.arm import Arm +from brain_client.state.battery import Battery +from brain_client.state.head import HeadState +from brain_client.state.image import DepthMap, Image, MainImage, WristImage +from brain_client.state.joint_states import JointStates +from brain_client.state.lidar import Lidar +from brain_client.state.map import Map +from brain_client.state.odometry import Odometry +from brain_client.state.pose import Pose __all__ = [ - "Interface", - "InterfaceType", + "Arm", + "PhysicalSkill", + "Battery", + "DepthMap", + "Head", + "HeadState", + "Image", + "JointStates", + "Lidar", + "MainImage", + "Manipulation", + "Map", + "Mobility", "Odometry", - "RobotState", - "RobotStateType", + "Pose", "Skill", "SkillCancelled", "SkillFailed", "SkillOutput", "SkillResult", + "SkillReturn", + "TrainedSkill", + "WristImage", + "resource", ] + +# The interface classes pull ROS/Nav2 modules, so they resolve lazily +# (PEP 562): `from innate import Mobility` imports them on first use only. +# Type checkers can't follow __getattr__, so they read the imports below. +if TYPE_CHECKING: + from brain_client.robot.head import Head + from brain_client.robot.manipulation import Manipulation + from brain_client.robot.mobility import Mobility + +_LAZY_INTERFACES = { + "Mobility": ("brain_client.robot.mobility", "Mobility"), + "Manipulation": ("brain_client.robot.manipulation", "Manipulation"), + "Head": ("brain_client.robot.head", "Head"), +} + + +def __getattr__(name: str): + target = _LAZY_INTERFACES.get(name) + if target is None: + raise AttributeError(f"module 'innate' has no attribute {name!r}") + module_name, class_name = target + import importlib + + return getattr(importlib.import_module(module_name), class_name) diff --git a/ros2_ws/src/brain/brain_client/innate/gemini.py b/ros2_ws/src/brain/brain_client/innate/gemini.py new file mode 100644 index 000000000..f4857685d --- /dev/null +++ b/ros2_ws/src/brain/brain_client/innate/gemini.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python3 +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +"""Gemini vision via Innate proxy (/v1/chat/completions). Service key needs +"gemini" access or the proxy returns 403. Import as ``from innate import gemini``. +""" + +import json + +from brain_client.skills.types import cancellable_sleep +from innate_proxy import ProxyClient + +SERVICE = "gemini" +ENDPOINT = "/v1/chat/completions" +MODEL = "gemini-3.5-flash" + + +def make_client(): + """ProxyClient, or None if credentials missing.""" + client = ProxyClient() + return client if client.is_available() else None + + +def ask_image(client, images_b64, question, logger=None, retries=3): + """JPEG(s) + question -> reply text. None if no client / all retries fail. + images_b64: one base64 string or a list of them — sent in order, so the + question can refer to them as image 1, image 2, ... Frames go inline as + data URLs (640x480 JPEGs, at most two per call). Raises SkillCancelled + between attempts if the run is cancelled.""" + if client is None: + return None + if isinstance(images_b64, str): + images_b64 = [images_b64] + content = [{"type": "text", "text": question}] + content += [{"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{b}"}} for b in images_b64] + body = { + "model": MODEL, + "temperature": 0.0, + "messages": [{"role": "user", "content": content}], + } + for attempt in range(retries): + cancellable_sleep(0) + try: + with client.request_stream( + SERVICE, + ENDPOINT, + method="POST", + json=body, + ) as resp: + resp.raise_for_status() + data = json.loads(resp.read()) + return data["choices"][0]["message"]["content"] or "" + except Exception as e: # noqa: BLE001 + if logger: + logger.warning(f"[gemini] vision call failed (try {attempt + 1}/{retries}): {e}") + if attempt < retries - 1: + cancellable_sleep(2.0 * (attempt + 1)) + return None diff --git a/ros2_ws/src/brain/brain_client/innate/geometry.py b/ros2_ws/src/brain/brain_client/innate/geometry.py new file mode 100644 index 000000000..20c6cca42 --- /dev/null +++ b/ros2_ws/src/brain/brain_client/innate/geometry.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python3 +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +"""Head-camera <-> floor (z=0) via pinhole + URDF. + +Import as ``from innate import geometry`` (moved from workspace/skill_lib).""" + +import math + +HEAD_ORIGIN = (-0.040751, -0.0002, 0.25882) # base_link -> head joint (URDF) +CAM_IN_HEAD = (0.04327, 0.0297, -0.000275) # head -> left camera optical +IMG_W, IMG_H = 640, 480 +HFOV_DEG = 70.0 + +FX = IMG_W / (2.0 * math.tan(math.radians(HFOV_DEG) / 2.0)) +CX, CY = IMG_W / 2.0, IMG_H / 2.0 + + +def _head_rot(tilt_rad): + c, s = math.cos(tilt_rad), math.sin(tilt_rad) + return ((c, 0.0, -s), (0.0, 1.0, 0.0), (s, 0.0, c)) + + +def _rot(R, v): + return tuple(sum(R[i][k] * v[k] for k in range(3)) for i in range(3)) + + +def _cam_pose(head_tilt_deg): + """Camera origin + (fwd, right, down) axes in base_link for a head tilt.""" + R = _head_rot(math.radians(head_tilt_deg)) + off = _rot(R, CAM_IN_HEAD) + cam = tuple(HEAD_ORIGIN[i] + off[i] for i in range(3)) + return cam, _rot(R, (1, 0, 0)), _rot(R, (0, -1, 0)), _rot(R, (0, 0, -1)) + + +def pixel_to_floor(u, v, head_tilt_deg): + """Pixel (u,v) -> floor (x,y) in base_link, or None.""" + cam, fwd, right, down = _cam_pose(head_tilt_deg) + xo, yo = (u - CX) / FX, (v - CY) / FX + d = tuple(fwd[i] + xo * right[i] + yo * down[i] for i in range(3)) + if d[2] >= -1e-6: + return None + t = -cam[2] / d[2] + x, y = cam[0] + t * d[0], cam[1] + t * d[1] + return (x, y) if x > 0 else None + + +def floor_to_pixel(x, y, head_tilt_deg): + """Floor (x,y) -> pixel, or None. Inverse of pixel_to_floor.""" + cam, fwd, right, down = _cam_pose(head_tilt_deg) + D = (x - cam[0], y - cam[1], -cam[2]) + a = sum(D[i] * fwd[i] for i in range(3)) + if a <= 1e-6: + return None + b = sum(D[i] * right[i] for i in range(3)) + c = sum(D[i] * down[i] for i in range(3)) + return (CX + (b / a) * FX, CY + (c / a) * FX) diff --git a/ros2_ws/src/brain/brain_client/innate/skills.py b/ros2_ws/src/brain/brain_client/innate/skills.py deleted file mode 100644 index cbfbbc2c5..000000000 --- a/ros2_ws/src/brain/brain_client/innate/skills.py +++ /dev/null @@ -1,74 +0,0 @@ -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -"""Skills as plain functions: import them, call them. - - from innate.skills import head_emotion, navigate_to_position - - head_emotion(emotion="excited") - navigate_to_position(x=0.3, y=0.0, theta_degrees=0.0) - -Module __getattr__ (PEP 562) returns a proxy that hands the skill name to -whichever SkillInvoker is currently executing a skill (a contextvar the skills -server sets around every execute(); see use_invoker). Calls block until the -child finishes and raise instead of returning a status: success returns the -child's output message (a SkillOutput — ``.data`` carries any structured -payload), failure raises SkillFailed, cancellation raises SkillCancelled. -Every call accepts a reserved ``timeout=`` seconds kwarg. The tuple-returning -form, self.skills.run(id, **inputs), remains for dynamic skill ids. -""" - -from __future__ import annotations - -from contextlib import contextmanager -from contextvars import ContextVar - -from brain_client.skills.types import SkillResult - - -class SkillFailed(Exception): - """A skill called through innate.skills reported FAILURE.""" - - -class SkillCancelled(Exception): - """A skill called through innate.skills was cancelled; unwinds the routine.""" - - -# The invoker driving the currently-executing skill; None outside a skill run. -_current_invoker: ContextVar = ContextVar("innate_skills_invoker", default=None) - - -@contextmanager -def use_invoker(invoker): - """Make ``invoker`` the ambient invoker for a ``with`` block. - - The skills server wraps every execute() in this. Doubles as the test - fixture: any fake exposing ``run(skill_id, **inputs)`` works. - """ - token = _current_invoker.set(invoker) - try: - yield - finally: - _current_invoker.reset(token) - - -def __getattr__(name: str): - """``from innate.skills import anything`` -> a proxy that runs that skill.""" - if name.startswith("_"): # dunder/private lookups are never skills - raise AttributeError(name) - - def run_skill(**inputs): - invoker = _current_invoker.get() - if invoker is None: - raise RuntimeError( - f"innate.skills.{name}() can only be called while a skill is " - "executing — from inside execute(), or under use_invoker() in tests." - ) - message, status = invoker.run(name, **inputs) - if status is SkillResult.CANCELLED: - raise SkillCancelled(message) - if status is not SkillResult.SUCCESS: - raise SkillFailed(message) - return message - - run_skill.__name__ = run_skill.__qualname__ = name - return run_skill diff --git a/ros2_ws/src/brain/brain_client/innate/vision.py b/ros2_ws/src/brain/brain_client/innate/vision.py new file mode 100644 index 000000000..cd98bc863 --- /dev/null +++ b/ros2_ws/src/brain/brain_client/innate/vision.py @@ -0,0 +1,170 @@ +#!/usr/bin/env python3 +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +"""Stateless vision helpers: decode, Gemini parse, LK track, color seg. + +Import as ``from innate import vision`` (moved from workspace/skill_lib).""" + +import base64 +import json +import math +import re +from typing import Any + +import cv2 +import numpy as np + +from innate.geometry import IMG_H, IMG_W + + +def b64_to_gray(image_b64): + """base64 JPEG -> gray ndarray, or None.""" + try: + arr = np.frombuffer(base64.b64decode(image_b64), np.uint8) + return cv2.imdecode(arr, cv2.IMREAD_GRAYSCALE) + except Exception: # noqa: BLE001 — a bad frame just skips a cycle + return None + + +def b64_to_hsv(image_b64): + """base64 JPEG -> HSV ndarray, or None.""" + try: + arr = np.frombuffer(base64.b64decode(image_b64), np.uint8) + bgr = cv2.imdecode(arr, cv2.IMREAD_COLOR) + except Exception: # noqa: BLE001 + return None + return cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV) if bgr is not None else None + + +def parse_dets(text): + """Parse Gemini JSON list of detection dicts.""" + if not text: + return [] + text = re.sub(r"```(?:json)?", "", text) + m = re.search(r"\[.*\]", text, re.S) + if not m: + return [] + try: + dets = json.loads(m.group(0)) + except json.JSONDecodeError: + return [] + return [d for d in dets if isinstance(d, dict)] + + +def _norm1k(v): + """Gemini's normalized 0-1000 coords drift slightly out of range; clamp so + downstream slicing/CamShift never sees negative or off-image pixels.""" + return min(1000.0, max(0.0, float(v))) + + +def _box_corners_px(det): + """box_2d [ymin,xmin,ymax,xmax] 0-1000 -> (x0,y0,x1,y1) px.""" + b = det.get("box_2d") + if not b or len(b) < 4: + return None + y0, x0, y1, x1 = [_norm1k(v) for v in b[:4]] + y0, y1 = sorted((y0, y1)) + x0, x1 = sorted((x0, x1)) + return (x0 / 1000.0 * IMG_W, y0 / 1000.0 * IMG_H, x1 / 1000.0 * IMG_W, y1 / 1000.0 * IMG_H) + + +def parse_det_px(text): + """Best (u, v) from a Gemini detection reply.""" + for det in parse_dets(text): + gp = det.get("grasp_point") + if gp and len(gp) >= 2: + return (_norm1k(gp[1]) / 1000.0 * IMG_W, _norm1k(gp[0]) / 1000.0 * IMG_H) + c = _box_corners_px(det) + if c: + return ((c[0] + c[2]) / 2.0, (c[1] + c[3]) / 2.0) + return None + + +def parse_det_grip(text): + """grip_strength (float) from the best detection, or None.""" + for det in parse_dets(text): + g = det.get("grip_strength") + if isinstance(g, (int, float)) and not isinstance(g, bool) and math.isfinite(g): + return float(g) + return None + + +def parse_det_box(text): + """Best (x, y, w, h) from a Gemini detection reply.""" + for det in parse_dets(text): + c = _box_corners_px(det) + if not c: + continue + x, y = int(c[0]), int(c[1]) + w, h = int(c[2]) - x, int(c[3]) - y + if w >= 8 and h >= 8: + return (x, y, w, h) + return None + + +LK_PARAMS: dict[str, Any] = dict( + winSize=(21, 21), + maxLevel=3, + criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 20, 0.03), +) + + +def grid_pts(u, v, step=12, n=2): + """(2n+1)^2 LK feature patch around (u,v).""" + pts = [[u + dx * step, v + dy * step] for dx in range(-n, n + 1) for dy in range(-n, n + 1)] + return np.array(pts, dtype=np.float32).reshape(-1, 1, 2) + + +def track_point(prev_gray, gray, grid): + """One LK step -> median pixel, or None if lost.""" + # nextPts=None is the standard cv2 idiom; the stubs demand an array. + nxt, status, _ = cv2.calcOpticalFlowPyrLK( + prev_gray, + gray, + grid, + None, # pyright: ignore[reportCallIssue, reportArgumentType] + **LK_PARAMS, + ) + if nxt is None or status is None: + return None + good = nxt[status.flatten() == 1].reshape(-1, 2) + if len(good) < 3: + return None + return float(np.median(good[:, 0])), float(np.median(good[:, 1])) + + +# Color seg for growing/deforming objects (LK slides off fabric during descent). +_SEG_BINS = [16, 8, 8] +_SEG_RANGES = [0, 180, 0, 256, 0, 256] + + +def seg_model(hsv, box): + """Object/floor hist-ratio LUT for back-projection, or None.""" + x, y, w, h = box + obj = hsv[y : y + h, x : x + w] + rx0, ry0 = max(0, x - w // 2), max(0, y - h // 2) + ring = hsv[ry0 : y + h + h // 2, rx0 : x + w + w // 2] + h_obj = cv2.calcHist([obj], [0, 1, 2], None, _SEG_BINS, _SEG_RANGES) + h_ring = cv2.calcHist([ring], [0, 1, 2], None, _SEG_BINS, _SEG_RANGES) + ratio = h_obj / (np.maximum(h_ring - h_obj, 0.0) + 1.0) + if ratio.max() <= 0: + return None + return (255.0 * ratio / ratio.max()).astype(np.uint8) + + +def seg_track(hsv, model, window, min_score=25.0): + """Back-project + CamShift -> (center|None, window, score). + Numpy bin lookup (cv2.calcBackProject broken for 3-D hist here).""" + ih = (hsv[:, :, 0].astype(np.int32) * _SEG_BINS[0]) // 180 + i_s = (hsv[:, :, 1].astype(np.int32) * _SEG_BINS[1]) // 256 + iv = (hsv[:, :, 2].astype(np.int32) * _SEG_BINS[2]) // 256 + bp = model[np.clip(ih, 0, _SEG_BINS[0] - 1), i_s, iv] + crit = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 1) + _rot, window = cv2.CamShift(bp, window, crit) + x, y, w, h = window + if w < 4 or h < 4 or w * h > 0.4 * IMG_W * IMG_H: + return None, window, 0.0 + score = float(bp[y : y + h, x : x + w].mean()) + if score < min_score: + return None, window, score + return (x + w / 2.0, y + h / 2.0), window, score diff --git a/ros2_ws/src/brain/brain_client/test/test_backwards_compat.py b/ros2_ws/src/brain/brain_client/test/test_backwards_compat.py deleted file mode 100644 index 21b6b43e1..000000000 --- a/ros2_ws/src/brain/brain_client/test/test_backwards_compat.py +++ /dev/null @@ -1,187 +0,0 @@ -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -"""Backwards-compatibility guards for agent / skill / input discovery. - -These pin the historical contract that the concept-folder refactor broke, so it -cannot silently break again: - -1. **Locations** — through release 0.5.x, agents/skills/inputs loaded from - ``$INNATE_OS_ROOT/{agents,skills,inputs}`` and ``~/{agents,skills}``. The - refactor moved shipped/user content under ``workspace/`` and dropped the - legacy roots. They must still be scanned (in place) so content deployed - against an older release keeps loading. -2. **Import paths** — pre-refactor user files import their base class from - ``brain_client.{skill,agent,input}_types`` and ``brain_client.logging_config``. - Those module paths must still resolve to the *same* classes via the shims. - -The end-to-end tests combine both: a file written to a legacy location AND -importing via a legacy module path (exactly what an old user file looks like) -must still be discovered by the loader. - -Part of the fast (no-ROS) pytest bucket in ci/run_integration_tests.sh. Pure -Python + the ROS-free loaders; no rclpy.init / DDS needed. -""" - -import logging -import textwrap -from pathlib import Path - -import pytest - -from brain_client.common import script_paths - -LOGGER = logging.getLogger("backwards_compat_test") - -# Old user file = old import path (the compat shim) + old on-disk location. -AGENT_TEMPLATE = textwrap.dedent(""" - from {import_path} import Agent - - class CompatAgent(Agent): - @property - def id(self): - return "{name}" - - @property - def display_name(self): - return "Compat" - - def get_skills(self): - return [] - - def get_prompt(self): - return "compat" -""") - -SKILL_TEMPLATE = textwrap.dedent(""" - from {import_path} import Skill, SkillResult - - class CompatSkill(Skill): - @property - def name(self): - return "{name}" - - def execute(self, *args, **kwargs): - return "ok", SkillResult.SUCCESS - - def cancel(self): - pass -""") - - -@pytest.fixture -def fake_root(tmp_path, monkeypatch): - """Point INNATE_OS_ROOT and HOME at isolated temp dirs (read per-call).""" - root = tmp_path / "innate-os" - home = tmp_path / "home" - root.mkdir() - home.mkdir() - monkeypatch.setenv("INNATE_OS_ROOT", str(root)) - monkeypatch.setenv("HOME", str(home)) # Path.home() follows $HOME on POSIX - return root, home - - -def _write(directory: Path, filename: str, content: str) -> None: - directory.mkdir(parents=True, exist_ok=True) - (directory / filename).write_text(content) - - -# -------------------------------------------------------------------------- -# 1. Directory contract: legacy + home locations are scanned when present. -# -------------------------------------------------------------------------- -def test_agent_dirs_include_legacy_and_home(fake_root): - root, home = fake_root - (root / "agents").mkdir() - (home / "agents").mkdir() - dirs = [str(p) for p in script_paths.get_agent_directories()] - assert str(root / "agents") in dirs # legacy $INNATE_OS_ROOT/agents - assert str(home / "agents") in dirs # ~/agents - assert str(root / "workspace" / "innate_agents") in dirs - assert str(root / "workspace" / "custom_agents") in dirs - - -def test_skill_dirs_include_legacy_and_home(fake_root): - root, home = fake_root - (root / "skills").mkdir() - (home / "skills").mkdir() - dirs = [str(p) for p in script_paths.get_skill_directories()] - assert str(root / "skills") in dirs # legacy $INNATE_OS_ROOT/skills - assert str(home / "skills") in dirs # ~/skills - assert str(root / "workspace" / "innate_skills") in dirs - assert str(root / "workspace" / "custom_skills") in dirs - - -def test_input_dirs_include_legacy(fake_root): - root, _ = fake_root - (root / "inputs").mkdir() - dirs = [str(p) for p in script_paths.get_input_directories()] - assert str(root / "inputs") in dirs # legacy $INNATE_OS_ROOT/inputs - assert str(root / "workspace" / "inputs") in dirs - - -def test_optional_dirs_absent_when_missing(fake_root): - """Legacy/home dirs are never fabricated — only scanned if they exist.""" - root, home = fake_root - dirs = [str(p) for p in script_paths.get_agent_directories()] - assert str(root / "agents") not in dirs - assert str(home / "agents") not in dirs - - -# -------------------------------------------------------------------------- -# 2. End-to-end discovery from every historical location (old import + old path). -# -------------------------------------------------------------------------- -@pytest.mark.parametrize("location", ["root", "home"]) -def test_agent_discovered_from_legacy_location(fake_root, location): - from brain_client.agents.loader import AgentLoader - - root, home = fake_root - target = (root / "agents") if location == "root" else (home / "agents") - name = f"compat_{location}_agent" - _write(target, "compat_agent.py", AGENT_TEMPLATE.format(import_path="brain_client.agent_types", name=name)) - - discovered = AgentLoader(LOGGER).load_from_directories([str(p) for p in script_paths.get_agent_directories()]) - assert name in discovered - - -@pytest.mark.parametrize("location", ["root", "home"]) -def test_skill_discovered_from_legacy_location(fake_root, location): - from brain_client.skills.loader import SkillLoader - - root, home = fake_root - target = (root / "skills") if location == "root" else (home / "skills") - name = f"compat_{location}_skill" - _write(target, "compat_skill.py", SKILL_TEMPLATE.format(import_path="brain_client.skill_types", name=name)) - - discovered = SkillLoader(LOGGER).load_from_directories([str(p) for p in script_paths.get_skill_directories()]) - assert name in discovered - - -# -------------------------------------------------------------------------- -# 3. Import-path shims resolve to the SAME classes as the new module paths. -# -------------------------------------------------------------------------- -def test_skill_types_shim(): - import brain_client.skill_types as shim - from brain_client.skills.types import Skill, SkillResult - - assert shim.Skill is Skill - assert shim.SkillResult is SkillResult - - -def test_agent_types_shim(): - import brain_client.agent_types as shim - from brain_client.agents.types import Agent - - assert shim.Agent is Agent - - -def test_input_types_shim(): - import brain_client.input_types as shim - from brain_client.inputs.types import InputDevice - - assert shim.InputDevice is InputDevice - - -def test_logging_config_shim(): - import brain_client.logging_config as shim - from brain_client.common.logging import UniversalLogger - - assert shim.UniversalLogger is UniversalLogger diff --git a/ros2_ws/src/brain/brain_client/test/test_cancel_latch.py b/ros2_ws/src/brain/brain_client/test/test_cancel_latch.py index 7bc044321..4f3a3ef0e 100644 --- a/ros2_ws/src/brain/brain_client/test/test_cancel_latch.py +++ b/ros2_ws/src/brain/brain_client/test/test_cancel_latch.py @@ -8,9 +8,21 @@ """ import logging +import threading +import time from types import SimpleNamespace -from brain_client.skills.types import Skill, SkillResult +import pytest + +from brain_client.skills.types import ( + Interface, + InterfaceType, + Skill, + SkillCancelled, + SkillResult, + cancellable_sleep, + swap_run_cancel, +) class LegacyPatternSkill(Skill): @@ -48,18 +60,10 @@ def test_cancel_before_execute_survives_reset(): assert skill._cancelled is True -def test_begin_run_clears_stale_latch_from_previous_run(): +def test_begin_run_latches_cancel_from_goal_handle(): + # cancel landed before _begin_run (before the instance even existed): the + # goal's persistent cancel status latches it at run start. skill = LegacyPatternSkill() - skill.cancel() # previous run was cancelled - skill._begin_run(_goal_handle(False)) - assert skill._cancelled is False - - -def test_begin_run_recovers_cancel_from_goal_handle(): - # cancel landed before _begin_run: the latch was set then cleared, but the - # goal's persistent cancel status re-latches it. - skill = LegacyPatternSkill() - skill.cancel() skill._begin_run(_goal_handle(True)) assert skill._cancelled is True @@ -103,5 +107,66 @@ def test_latch_works_without_super_init(): skill._begin_run(_goal_handle(False)) Skill.cancel(skill) assert skill._cancelled is True - skill._begin_run(_goal_handle(False)) - assert skill._cancelled is False + + +class SleepySkill(Skill): + """The new authoring shape: no cancel code, just self.sleep in loops.""" + + def execute(self): + while True: + self.sleep(0.05) + + @property + def name(self): + return "sleepy" + + +def test_sleep_raises_immediately_once_cancelled(): + skill = SleepySkill(logging.getLogger("test")) + skill.cancel() + start = time.monotonic() + with pytest.raises(SkillCancelled): + skill.sleep(10.0) + assert time.monotonic() - start < 1.0 + + +def test_sleep_wakes_mid_wait_on_cancel(): + skill = SleepySkill(logging.getLogger("test")) + threading.Timer(0.1, skill.cancel).start() + start = time.monotonic() + with pytest.raises(SkillCancelled): + skill.execute() + assert time.monotonic() - start < 5.0 + + +def test_cancel_halts_injected_interfaces(): + class Braked(Skill): + mobility = Interface(InterfaceType.MOBILITY) + + def execute(self): + pass + + class FakeMobility: + def __init__(self): + self.halted = False + + def halt(self): + self.halted = True + + skill = Braked(logging.getLogger("test")) + base = FakeMobility() + skill.inject_interface(InterfaceType.MOBILITY, base) + skill.cancel() + assert base.halted + + +def test_cancellable_sleep_follows_the_bound_run_latch(): + latch = threading.Event() + previous = swap_run_cancel(latch) + try: + cancellable_sleep(0) # not cancelled: returns + latch.set() + with pytest.raises(SkillCancelled): + cancellable_sleep(10.0) + finally: + swap_run_cancel(previous) diff --git a/ros2_ws/src/brain/brain_messages/msg/SkillInfo.msg b/ros2_ws/src/brain/brain_messages/msg/SkillInfo.msg index bef8df061..d5c602738 100644 --- a/ros2_ws/src/brain/brain_messages/msg/SkillInfo.msg +++ b/ros2_ws/src/brain/brain_messages/msg/SkillInfo.msg @@ -2,6 +2,7 @@ # Information about a single skill (code or physical) string id # Deterministic identifier: "innate-os/" or "local/" string name # Cosmetic display name (also used as LLM tool name — must be unique) +string group # Folder path inside the skill package ("" = root, "chess", "chess/openings") — for UI grouping only string type # "code", "learned", "replay", "poses" string guidelines # Agent guidelines for when to use this skill string guidelines_when_running # Agent guidelines while the skill is running @@ -10,3 +11,4 @@ bool in_training # True if skill is still in training (missing ch int32 episode_count # Number of recorded episodes (0 for code skills) string directory # Absolute path to skill directory (empty for code skills) bool wheeled # Replay skills: True if the base moved during the recording +string load_error # Non-empty: the file failed to load and this is the error; the skill is not runnable diff --git a/ros2_ws/src/cloud/clients/proxy-client/proxy_demos/bench_tts.py b/ros2_ws/src/cloud/clients/proxy-client/proxy_demos/bench_tts.py index 684e027e2..2f1f0a19a 100644 --- a/ros2_ws/src/cloud/clients/proxy-client/proxy_demos/bench_tts.py +++ b/ros2_ws/src/cloud/clients/proxy-client/proxy_demos/bench_tts.py @@ -22,6 +22,7 @@ import httpx from dotenv import load_dotenv + from innate_proxy import ProxyClient load_dotenv() diff --git a/ros2_ws/src/cloud/clients/training-client/training_client/src/skill_manager.py b/ros2_ws/src/cloud/clients/training-client/training_client/src/skill_manager.py index 17caadbe9..b09a04b55 100644 --- a/ros2_ws/src/cloud/clients/training-client/training_client/src/skill_manager.py +++ b/ros2_ws/src/cloud/clients/training-client/training_client/src/skill_manager.py @@ -128,6 +128,15 @@ def read_skill_id(skill_dir: str | Path) -> str | None: Automatically migrates from the legacy server-skill.json format first. """ skill_dir = Path(skill_dir) + # A read must not create the file it is asking about. This runs in bulk + # scans over custom_skills/ (node._register_local_skill_dirs), where + # _locked_metadata's touch would stamp a 0-byte metadata.json into every + # subdirectory — which the 0.7 catalog and workspace import classify by: + # phantom "broken" physical skills, and code packages silently dropped + # from import. No metadata and no legacy file means there is nothing to + # lock, migrate, or read. + if not (skill_dir / METADATA_JSON).exists() and not (skill_dir / SKILL_JSON).is_file(): + return None with _locked_metadata(skill_dir) as meta_path: _migrate_skill_id(skill_dir) data = _read_meta(meta_path) diff --git a/ros2_ws/src/mars_bot/mars_arm/config/arm_config.yaml b/ros2_ws/src/mars_bot/mars_arm/config/arm_config.yaml index bf416854d..e2cfdf28f 100644 --- a/ros2_ws/src/mars_bot/mars_arm/config/arm_config.yaml +++ b/ros2_ws/src/mars_bot/mars_arm/config/arm_config.yaml @@ -101,8 +101,28 @@ motor_type: "XC330-M288" position_limits: [-0.8727, 0.3491] pwm_limit: 885 - current_limit: 100 - control_mode: 4 + # Mode 5 = current-based position control: the gripper drives toward its + # goal position with output capped at goal_current, so a deep close + # squeezes any object at constant safe force — no overload trips, no + # software force loops. + # + # goal_current is the REAL torque cap (RAM addr 102). current_limit + # (EEPROM addr 38) only bounds what goal_current may be and sets the + # overload-detection threshold, so it must stay comfortably ABOVE the + # grip current or the servo trips overload while gripping — that is what + # the old current_limit: 100 did in mode 4, where nothing capped output + # and a grip drew 1724 mA against a 100 mA trip threshold. + # + # 700 mA: the metal-robot pick that gripped well ran ~930-1110 mA before + # tripping, and the health warning fires at 800, so this grips firmly + # with headroom. Raised from 600 after a rigid figurine slipped out + # during the verify backup (2026-07-23); 800 is the ceiling here. + current_limit: 1300 + goal_current: 700 + control_mode: 5 + # Close gently — the current cap means it presses in rather than impacts. + profile_velocity: 60 + profile_acceleration: 30 gains_near: [400, 0, 0, 0, 0] gains_teleop: [400, 0, 0, 0, 0] # November teleop gains diff --git a/ros2_ws/src/mars_bot/mars_arm/include/mars_arm/arm_node.hpp b/ros2_ws/src/mars_bot/mars_arm/include/mars_arm/arm_node.hpp index 129e5e784..ef3602c6d 100644 --- a/ros2_ws/src/mars_bot/mars_arm/include/mars_arm/arm_node.hpp +++ b/ros2_ws/src/mars_bot/mars_arm/include/mars_arm/arm_node.hpp @@ -16,6 +16,7 @@ #include "mars_msgs/srv/goto_js.hpp" #include "mars_msgs/srv/goto_js_trajectory.hpp" #include "mars_msgs/msg/arm_status.hpp" +#include #include #include #include @@ -45,6 +46,9 @@ class MarsArmNode : public rclcpp::Node { template void retryServoOp(int servo_id, const char* op_name, Func&& fn, int max_retries = 3); void configureServoByIdLocked(int servo_id, bool enable_torque = true); + // Re-write Goal Current after any torque enable (it is RAM and a + // torque-enable clears it). No-op for joints without a goal_current. + void reapplyGoalCurrentLocked(int servo_id); void configureServosLocked(bool enable_torque = true); void syncTargetToMotorPositions(); @@ -163,8 +167,26 @@ class MarsArmNode : public rclcpp::Node { std::array gs_teleop_; std::array gs_last_applied_; int gs_cycle_counter_ = 0; - GainMode gain_mode_{GainMode::TELEOP}; + // Written by the trajectory and service threads, read (and decayed) by + // the control loop — atomic so those cross-thread accesses are defined. + // The decay's check-then-write can still interleave with a trajectory + // start; the waypoint loop re-asserts the mode each step to bound that. + std::atomic gain_mode_{GainMode::TELEOP}; GainMode last_applied_gain_mode_{GainMode::TELEOP}; + // When the last trajectory finished. Scheduled gains hold the arm firmly + // between the closely-spaced moves a skill sends (dropping to the soft + // teleop gains in the gaps made the arm sag and snap back), but holding + // them while idle cooks the shoulder — joint 2 reached 70 C. So the hold + // decays back to teleop gains after kScheduledHoldTimeout of quiet. + // Atomic for the same reason as gain_mode_: stamped by the trajectory + // threads (HoldGuard), read by the control loop. + std::atomic last_trajectory_end_{std::chrono::steady_clock::time_point{}}; + // True while a trajectory is streaming waypoints. The idle decay must + // never fire mid-trajectory: last_trajectory_end_ is stale during + // execution, and the decay once flipped a 3 s carry move onto soft + // teleop gains 3 ms after it started — the shaken grip dropped the + // object it was carrying. + std::atomic trajectory_executing_{false}; // Control loop timing instrumentation std::array timing_stats_{{TimingAccumulator{"total"}, TimingAccumulator{"lock_wait"}, diff --git a/ros2_ws/src/mars_bot/mars_arm/include/mars_arm/arm_types.hpp b/ros2_ws/src/mars_bot/mars_arm/include/mars_arm/arm_types.hpp index 8b9ee9c23..6c5eb4ea7 100644 --- a/ros2_ws/src/mars_bot/mars_arm/include/mars_arm/arm_types.hpp +++ b/ros2_ws/src/mars_bot/mars_arm/include/mars_arm/arm_types.hpp @@ -17,6 +17,15 @@ static constexpr int kX330MaxCurrentLimit = 1750; static constexpr int kLoadWarningThreshold = 800; // ~80% load (0.1% units) static constexpr int kTemperatureWarningC = 70; static constexpr int kGainScheduleInterval = 20; // control cycles between updates +// How long scheduled (stiff) gains hold after a trajectory before decaying to +// teleop gains. Long enough to span the gaps between a skill's stepped moves, +// short enough that an idle arm is not held stiff and overheating. +static constexpr double kScheduledHoldTimeoutS = 5.0; +// The decay additionally requires shoulder+elbow present load below this +// (0.1% units, so 100 = 10%): a gain drop under real load sags the arm, and +// that jolt shook a carried object out of the gripper. At the folded rest +// pose — the long-idle case the decay exists for — these loads are ~0. +static constexpr int kDecayMaxLoad = 100; inline bool isX330(const std::string& motor_type) { return motor_type.find("330") != std::string::npos; @@ -29,6 +38,10 @@ struct JointConfig { double max_pos_rad; int pwm_limit; int current_limit = 0; + // Mode 5 (current-based position) torque cap, mA. 0 = leave at the servo + // default, which is near-zero — a mode-5 joint MUST set this or it stalls + // under its own friction. + int goal_current = 0; int homing_offset = 0; int control_mode; int kp, ki, kd; diff --git a/ros2_ws/src/mars_bot/mars_arm/include/mars_arm/dynamixel.hpp b/ros2_ws/src/mars_bot/mars_arm/include/mars_arm/dynamixel.hpp index ddfb7bf23..585920e8b 100644 --- a/ros2_ws/src/mars_bot/mars_arm/include/mars_arm/dynamixel.hpp +++ b/ros2_ws/src/mars_bot/mars_arm/include/mars_arm/dynamixel.hpp @@ -51,6 +51,10 @@ class Dynamixel { void setMaxPositionLimit(int motor_id, int max_position); void setPwmLimit(int motor_id, int limit); void setCurrentLimit(int motor_id, int current_limit); + // Goal Current (RAM): in mode 5 this is what actually caps output torque. + // Current Limit (addr 38) only bounds what may be written here and sets + // the overload-detection threshold — it does NOT throttle the motor. + void setGoalCurrent(int motor_id, int goal_current); void setP(int motor_id, int p); void setI(int motor_id, int i); void setD(int motor_id, int d); @@ -111,6 +115,7 @@ class Dynamixel { static constexpr int ADDR_MIN_POSITION_LIMIT = 52; static constexpr int ADDR_MAX_POSITION_LIMIT = 48; static constexpr int ADDR_CURRENT_LIMIT = 38; + static constexpr int ADDR_GOAL_CURRENT = 102; // RAM; the real torque cap in modes 0/5 static constexpr int ADDR_PRESENT_POSITION = 132; static constexpr int ADDR_PRESENT_VELOCITY = 128; static constexpr int ADDR_HOMING_OFFSET = 20; diff --git a/ros2_ws/src/mars_bot/mars_arm/mars_arm/arm_config.cpp b/ros2_ws/src/mars_bot/mars_arm/mars_arm/arm_config.cpp index 60ba1a692..e7909d8c0 100644 --- a/ros2_ws/src/mars_bot/mars_arm/mars_arm/arm_config.cpp +++ b/ros2_ws/src/mars_bot/mars_arm/mars_arm/arm_config.cpp @@ -19,6 +19,7 @@ void MarsArmNode::loadJointConfigs(const std::vector& joint_names) this->declare_parameter(jn + ".control_mode", 3); this->declare_parameter(jn + ".motor_type", std::string("")); this->declare_parameter(jn + ".current_limit", 0); + this->declare_parameter(jn + ".goal_current", 0); this->declare_parameter(jn + ".homing_offset", 0); this->declare_parameter(jn + ".profile_velocity", 0); this->declare_parameter(jn + ".profile_acceleration", 0); @@ -57,6 +58,25 @@ void MarsArmNode::loadJointConfigs(const std::vector& joint_names) "): current_limit not supported — no current control hw"); } + // Goal current: the mode-5 torque cap. Must stay under current_limit, + // which doubles as the overload-detection threshold — a servo gripping + // AT its current_limit trips overload. + config.goal_current = static_cast(this->get_parameter(jn + ".goal_current").as_int()); + if (config.goal_current != 0) { + if (!isX330(config.motor_type)) { + throw std::runtime_error(jn + " (" + config.motor_type + + "): goal_current not supported — no current control hw"); + } + if (config.goal_current < 0 || config.goal_current > config.current_limit) { + throw std::runtime_error( + jn + ": goal_current out of range [0, current_limit=" + std::to_string(config.current_limit) + "]"); + } + } else if (config.control_mode == 0 || config.control_mode == 5) { + throw std::runtime_error(jn + ": control_mode " + std::to_string(config.control_mode) + + " needs a non-zero goal_current — the servo default is near-zero " + "torque and the joint would stall under its own friction"); + } + config.homing_offset = static_cast(this->get_parameter(jn + ".homing_offset").as_int()); config.profile_velocity = static_cast(this->get_parameter(jn + ".profile_velocity").as_int()); config.profile_acceleration = static_cast(this->get_parameter(jn + ".profile_acceleration").as_int()); diff --git a/ros2_ws/src/mars_bot/mars_arm/mars_arm/arm_control.cpp b/ros2_ws/src/mars_bot/mars_arm/mars_arm/arm_control.cpp index 1678a7b54..c20494d97 100644 --- a/ros2_ws/src/mars_bot/mars_arm/mars_arm/arm_control.cpp +++ b/ros2_ws/src/mars_bot/mars_arm/mars_arm/arm_control.cpp @@ -109,6 +109,36 @@ void MarsArmNode::controlTimerCallback() { if (++gs_cycle_counter_ >= kGainScheduleInterval) { gs_cycle_counter_ = 0; + // Decay the post-trajectory stiff hold once the arm has been quiet + // (see last_trajectory_end_): holding scheduled gains indefinitely + // ran joint 2 up to 70 C. Never while a trajectory is executing — + // its end stamp is stale mid-flight, and decaying then flipped a + // carry move onto soft gains 3 ms in and shook the object loose. + // And only while the shoulder/elbow bear ~no load: soft gains under + // real load sag the arm, and that jolt at the end of a pick shook + // the carried object out of the gripper. A loaded pose (carrying) + // stays stiff instead; the temperature warning covers the rare + // carry-for-hours case. + const auto trajectory_end = last_trajectory_end_.load(); + if (gain_mode_ == GainMode::SCHEDULED && !trajectory_executing_ && + trajectory_end.time_since_epoch().count() != 0) { + double idle_s = + std::chrono::duration(std::chrono::steady_clock::now() - trajectory_end).count(); + bool unloaded = + loads.size() > 2 && std::abs(loads[1]) < kDecayMaxLoad && std::abs(loads[2]) < kDecayMaxLoad; + if (idle_s > kScheduledHoldTimeoutS && unloaded) { + gain_mode_ = GainMode::TELEOP; + RCLCPP_INFO(this->get_logger(), "Gain mode -> TELEOP (idle %.1fs, arm unloaded)", idle_s); + } else if (idle_s > kScheduledHoldTimeoutS && loads.size() <= 2) { + // A short state read leaves the load unknown; staying stiff is + // the safe call, but say so — silently holding scheduled gains + // is exactly what ran joint 2 up to 70 C. + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 30000, + "Idle gain decay blocked: no shoulder/elbow load reading — holding " + "scheduled (stiff) gains"); + } + } + bool gs_changed = false; std::vector> gs_pid_data; diff --git a/ros2_ws/src/mars_bot/mars_arm/mars_arm/arm_services.cpp b/ros2_ws/src/mars_bot/mars_arm/mars_arm/arm_services.cpp index 3f1fe25a7..8cd308b8e 100644 --- a/ros2_ws/src/mars_bot/mars_arm/mars_arm/arm_services.cpp +++ b/ros2_ws/src/mars_bot/mars_arm/mars_arm/arm_services.cpp @@ -14,6 +14,24 @@ void MarsArmNode::initializeServos() { configureServosLocked(); } +void MarsArmNode::reapplyGoalCurrentLocked(int servo_id) { + for (const auto& c : joint_configs_) { + if (c.servo_id != servo_id || c.goal_current <= 0) { + continue; + } + try { + dynamixel_->setGoalCurrent(servo_id, c.goal_current); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + RCLCPP_INFO(this->get_logger(), " Servo %d goal current = %d mA (mode %d torque cap)", servo_id, + c.goal_current, c.control_mode); + } catch (const std::exception& e) { + // Left unset the gripper holds with ~no force, so this is loud. + RCLCPP_ERROR(this->get_logger(), "Failed to set goal current on servo %d: %s", servo_id, e.what()); + } + return; + } +} + void MarsArmNode::configureServoByIdLocked(int servo_id, bool enable_torque) { // Find the config for this servo const JointConfig* config_ptr = nullptr; @@ -106,6 +124,8 @@ void MarsArmNode::configureServoByIdLocked(int servo_id, bool enable_torque) { RCLCPP_DEBUG(this->get_logger(), " Enabling torque on servo %d", config.servo_id); retryServoOp(config.servo_id, "enableTorque", [&] { dynamixel_->enableTorque(config.servo_id); }); std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + reapplyGoalCurrentLocked(config.servo_id); } RCLCPP_DEBUG(this->get_logger(), "Servo %d configured and torque %s", config.servo_id, @@ -300,6 +320,7 @@ void MarsArmNode::armTorqueOnCallback(const std::shared_ptrget_logger(), " Enabling torque on servo %d", id); dynamixel_->enableTorque(id); std::this_thread::sleep_for(std::chrono::milliseconds(100)); + reapplyGoalCurrentLocked(id); } try { syncTargetToMotorPositions(); diff --git a/ros2_ws/src/mars_bot/mars_arm/mars_arm/arm_trajectory.cpp b/ros2_ws/src/mars_bot/mars_arm/mars_arm/arm_trajectory.cpp index 3b4f0a396..b0b54f1ad 100644 --- a/ros2_ws/src/mars_bot/mars_arm/mars_arm/arm_trajectory.cpp +++ b/ros2_ws/src/mars_bot/mars_arm/mars_arm/arm_trajectory.cpp @@ -58,6 +58,18 @@ std::vector> MarsArmNode::computeCubicSplineTrajectory(const bool MarsArmNode::planAndExecuteTrajectory(const std::vector& target_positions, double trajectory_time, GainMode trajectory_gain_mode) { + // Block the idle gain decay for the whole call, and stamp the quiet + // period's start on every exit path (validation failures included — + // harmless, it just delays the decay one timeout). + trajectory_executing_ = true; + struct HoldGuard { + MarsArmNode* n; + ~HoldGuard() { + n->last_trajectory_end_ = std::chrono::steady_clock::now(); + n->trajectory_executing_ = false; + } + } hold_guard{this}; + // Switch gains for trajectory execution if (gain_mode_ != trajectory_gain_mode) { gain_mode_ = trajectory_gain_mode; @@ -92,6 +104,18 @@ bool MarsArmNode::planAndExecuteTrajectory(const std::vector& target_pos return false; } + // Gripper (servo 6, current-based position control): spline from the last + // COMMANDED goal, not the measured position. When gripping, the servo + // stalls short of its goal and that standing position error IS the grip + // force — re-seeding the goal at the measured position zeroes the error + // and drops the object at the start of every arm move. + { + std::lock_guard arm_lock(arm_command_mutex_); + if (has_target_) { + current_positions[5] = latest_target_[5]; + } + } + // Use simple cubic spline planning (fast, smooth trajectory) RCLCPP_INFO(this->get_logger(), "Planning with cubic spline for 6-DOF arm (including gripper)..."); const double dt = 1.0 / this->get_parameter("trajectory_rate_hz").as_double(); @@ -118,6 +142,12 @@ bool MarsArmNode::planAndExecuteTrajectory(const std::vector& target_pos for (size_t i = 0; i < interpolated_trajectory.size(); ++i) { const auto& point = interpolated_trajectory[i]; + // Re-assert per waypoint: the idle decay's check-then-write can pass + // its checks just before trajectory_executing_ went true and then + // stomp the mode after the switch above — without this the whole + // trajectory would run on soft teleop gains. + gain_mode_ = trajectory_gain_mode; + // Send command via the control loop's pass-through path { std::lock_guard arm_lock(arm_command_mutex_); @@ -135,17 +165,28 @@ bool MarsArmNode::planAndExecuteTrajectory(const std::vector& target_pos RCLCPP_INFO(this->get_logger(), "Trajectory execution complete"); - // Restore teleop gains after trajectory completes - if (gain_mode_ != GainMode::TELEOP) { - gain_mode_ = GainMode::TELEOP; - RCLCPP_INFO(this->get_logger(), "Gain mode -> TELEOP (trajectory finished)"); - } - + // Deliberately KEEP the trajectory's gain mode for the hold: dropping to + // the soft teleop gains here made the arm sag under gravity between the + // stepped moves a skill sends (visible limp-snap-limp). The control loop + // decays it back to teleop after kScheduledHoldTimeoutS of quiet (the + // HoldGuard above stamps the quiet period) so an idle arm is not held + // stiff — that overheated joint 2 — and teleop also reclaims its gains + // immediately via armCommandCallback. return true; } bool MarsArmNode::planAndExecuteMultiWaypointTrajectory(const std::vector>& waypoints, const std::vector& segment_durations) { + // See planAndExecuteTrajectory: block the idle gain decay while executing. + trajectory_executing_ = true; + struct HoldGuard { + MarsArmNode* n; + ~HoldGuard() { + n->last_trajectory_end_ = std::chrono::steady_clock::now(); + n->trajectory_executing_ = false; + } + } hold_guard{this}; + // Switch to scheduled gains for planned trajectories if (gain_mode_ != GainMode::SCHEDULED) { gain_mode_ = GainMode::SCHEDULED; @@ -205,6 +246,9 @@ bool MarsArmNode::planAndExecuteMultiWaypointTrajectory(const std::vector arm_lock(arm_command_mutex_); for (size_t j = 0; j < 6 && j < point.size(); ++j) { @@ -220,12 +264,8 @@ bool MarsArmNode::planAndExecuteMultiWaypointTrajectory(const std::vectorget_logger(), "Multi-waypoint trajectory execution complete"); - // Restore teleop gains after trajectory completes - if (gain_mode_ != GainMode::TELEOP) { - gain_mode_ = GainMode::TELEOP; - RCLCPP_INFO(this->get_logger(), "Gain mode -> TELEOP (multi-waypoint trajectory finished)"); - } - + // Deliberately KEEP scheduled gains for the hold (see the single-target + // version above) — the control loop decays them after a quiet period. return true; } @@ -261,7 +301,17 @@ void MarsArmNode::armGotoJSTrajectoryCallback(const std::shared_ptr lock(joint_state_mutex_); if (!latest_joint_positions_.empty()) { - waypoints.insert(waypoints.begin(), latest_joint_positions_); + std::vector start = latest_joint_positions_; + // Gripper starts from the last COMMANDED goal (see + // planAndExecuteTrajectory): seeding it at the measured stall + // position would zero the grip preload. + { + std::lock_guard arm_lock(arm_command_mutex_); + if (has_target_ && start.size() >= 6) { + start[5] = latest_target_[5]; + } + } + waypoints.insert(waypoints.begin(), start); if (!durations.empty()) { durations.insert(durations.begin(), durations[0]); } else { diff --git a/ros2_ws/src/mars_bot/mars_arm/mars_arm/dynamixel.cpp b/ros2_ws/src/mars_bot/mars_arm/mars_arm/dynamixel.cpp index 021e2b41c..3b26bd75d 100644 --- a/ros2_ws/src/mars_bot/mars_arm/mars_arm/dynamixel.cpp +++ b/ros2_ws/src/mars_bot/mars_arm/mars_arm/dynamixel.cpp @@ -110,6 +110,10 @@ void Dynamixel::setCurrentLimit(int motor_id, int current_limit) { writeRegister(motor_id, ADDR_CURRENT_LIMIT, current_limit, 2, "set current limit"); } +void Dynamixel::setGoalCurrent(int motor_id, int goal_current) { + writeRegister(motor_id, ADDR_GOAL_CURRENT, goal_current, 2, "set goal current"); +} + void Dynamixel::setP(int motor_id, int p) { writeRegister(motor_id, POSITION_P, p, 2, "set P gain"); } diff --git a/ros2_ws/src/mars_bot/mars_bringup/mars_bringup/config_loader.py b/ros2_ws/src/mars_bot/mars_bringup/mars_bringup/config_loader.py index 2f652c360..e32008c4f 100644 --- a/ros2_ws/src/mars_bot/mars_bringup/mars_bringup/config_loader.py +++ b/ros2_ws/src/mars_bot/mars_bringup/mars_bringup/config_loader.py @@ -198,13 +198,11 @@ def _validate_settings_param_types(data: dict) -> None: ) -def _load_settings_yaml(validate: bool = True) -> dict: +def _load_settings_yaml() -> dict: """Parse config/settings.yaml. Returns ``{}`` when missing, empty, or unreadable (unreadable is warned via :func:`_warn_settings_unreadable`, never silent). - - With ``validate=True`` an int written for a double-typed key raises (see - :func:`_validate_settings_param_types`). Runtime callers that only read the non-ROS - ``script_paths`` block pass ``validate=False`` so the launch-time guard can't crash a reload.""" + An int written for a double-typed key raises (see + :func:`_validate_settings_param_types`).""" path = _settings_yaml_path() if not path.exists(): return {} @@ -215,8 +213,7 @@ def _load_settings_yaml(validate: bool = True) -> dict: return {} if not isinstance(data, dict): return {} - if validate: - _validate_settings_param_types(data) + _validate_settings_param_types(data) return data @@ -228,24 +225,6 @@ def settings_params() -> list: return [str(_settings_yaml_path())] -def load_extra_script_dirs(key: str) -> list[str]: - """Extra agent/skill scan dirs from settings.yaml's ``script_paths`` block. - - ``key`` is ``"extra_agent_dirs"`` or ``"extra_skill_dirs"``. Accepts a YAML list or an - ``os.pathsep``-joined string; returns entries verbatim (caller expands ``~`` / ``$VARS``), - or ``[]`` when unset. - """ - # validate=False: runs in the hot-reload watcher, where the launch-time guard must not crash. - section = _load_settings_yaml(validate=False).get("script_paths", {}) - params = section.get("ros__parameters", {}) if isinstance(section, dict) else {} - value = params.get(key) if isinstance(params, dict) else None - if isinstance(value, str): - return value.split(os.pathsep) - if isinstance(value, list): - return [str(part) for part in value] - return [] - - def _settings_global_params() -> dict: """Flatten settings.yaml's ``/**`` ros__parameters block to dotted keys for the nav remap.""" glob = _load_settings_yaml().get("/**", {}) diff --git a/ros2_ws/src/mars_bot/mars_cam/mars_cam/filters/simple_filters.cpp b/ros2_ws/src/mars_bot/mars_cam/mars_cam/filters/simple_filters.cpp index aae63637b..5b49257ba 100644 --- a/ros2_ws/src/mars_bot/mars_cam/mars_cam/filters/simple_filters.cpp +++ b/ros2_ws/src/mars_bot/mars_cam/mars_cam/filters/simple_filters.cpp @@ -143,11 +143,12 @@ void StereoDepthEstimator::clampByDepth(cv::Mat& img, float f, float t) { } } } - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "depth_clamp: f=%.2f t=%.5f | min_depth=%.3fm -> max_d=%.1f | max_depth=%.1fm -> min_d=%.1f | " - "disp_range=[%.2f, %.2f] | killed: %d near + %d far / %d valid", - f, abs_t, min_depth_meters_, max_d, max_depth_meters_, min_d, d_min_seen, d_max_seen, - killed_near, killed_far, total_valid); + RCLCPP_DEBUG_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "depth_clamp: f=%.2f t=%.5f | min_depth=%.3fm -> max_d=%.1f | max_depth=%.1fm -> min_d=%.1f | " + "disp_range=[%.2f, %.2f] | killed: %d near + %d far / %d valid", + f, abs_t, min_depth_meters_, max_d, max_depth_meters_, min_d, d_min_seen, d_max_seen, killed_near, killed_far, + total_valid); } // ============================================================================= diff --git a/ros2_ws/src/mars_bot/mars_cam/mars_cam/stereo_depth_estimator.cpp b/ros2_ws/src/mars_bot/mars_cam/mars_cam/stereo_depth_estimator.cpp index b6500fabe..851ae9f92 100644 --- a/ros2_ws/src/mars_bot/mars_cam/mars_cam/stereo_depth_estimator.cpp +++ b/ros2_ws/src/mars_bot/mars_cam/mars_cam/stereo_depth_estimator.cpp @@ -322,11 +322,13 @@ void StereoDepthEstimator::syncCallback(const sensor_msgs::msg::Image::ConstShar try { processFrame(left_frame, right_frame, rclcpp::Time(left_msg->header.stamp)); frame_count_++; - if (frame_count_ % 100 == 0) { + // Throughput heartbeat — rare enough to stay readable in the live log (~2 min at 8 FPS). + constexpr int kStatsEveryNFrames = 1000; + if (frame_count_ % kStatsEveryNFrames == 0) { auto now = this->now(); double elapsed = (now - last_stats_time_).seconds(); - RCLCPP_INFO(this->get_logger(), "Depth estimation: %.1f FPS, %d frames processed", 100.0 / elapsed, - frame_count_); + RCLCPP_INFO(this->get_logger(), "Depth estimation: %.1f FPS, %d frames processed", + kStatsEveryNFrames / elapsed, frame_count_); last_stats_time_ = now; } } catch (const std::exception& e) { @@ -469,26 +471,26 @@ void StereoDepthEstimator::processFrame(const cv::Mat& left_input, const cv::Mat return std::chrono::duration(d).count(); }; - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, - "Pipeline %.1fms | sub %.1f | scale %.1f | rectify %.1f | sgm_submit %.1f | " - "color_remap %.1f | mono_pub %.1f | color_pub %.1f | footprint %.1f | sgm_sync %.1f | " - "extract %.1f | unfilt %.1f | filter %.1f | depth %.1f | pc %.1f | " - "subs[L:%d R:%d C:%d J:%d PC:%d PCC:%d D:%d Di:%d Du:%d FP:%d FM:%d FC:%d col:%d]", - ms(t_end - t_start), ms(t_sub - t_start), ms(t_scale - t_sub), ms(t_rect - t_scale), - ms(t_submit - t_rect), ms(t_color_remap - t_submit), ms(t_mono_pub - t_color_remap), - ms(t_color_pub - t_mono_pub), ms(t_footprint - t_color_pub), ms(t_sgm - t_footprint), - ms(t_extract - t_sgm), ms(t_unfilt - t_extract), ms(t_filter - t_unfilt), - ms(t_depth - t_filter), ms(t_pc - t_depth), (int)pub_left_rect, (int)pub_right_rect, - (int)pub_left_color, (int)pub_left_compressed, (int)pub_pointcloud, (int)pub_pointcloud_color, - (int)pub_depth, (int)pub_disparity, (int)pub_unfiltered, (int)pub_footprint_overlay, - (int)pub_footprint_mask, (int)pub_footprint_cutout, (int)has_color); - - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, - "Filter detail %.1fms | fp_mask %.1f | down %.1f | clamp %.1f | domain %.1f | speckle %.1f | " - "edge %.1f | median %.1f | bilateral %.1f | hole %.1f | temporal %.1f | up %.1f", - ms(t_filter - t_unfilt), ft.footprint_mask_ms, ft.downsample_ms, ft.depth_clamp_ms, - ft.domain_transform_ms, ft.speckle_ms, ft.edge_inv_ms, ft.median_ms, ft.bilateral_ms, - ft.hole_fill_ms, ft.temporal_ms, ft.upsample_ms); + RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 5000, + "Pipeline %.1fms | sub %.1f | scale %.1f | rectify %.1f | sgm_submit %.1f | " + "color_remap %.1f | mono_pub %.1f | color_pub %.1f | footprint %.1f | sgm_sync %.1f | " + "extract %.1f | unfilt %.1f | filter %.1f | depth %.1f | pc %.1f | " + "subs[L:%d R:%d C:%d J:%d PC:%d PCC:%d D:%d Di:%d Du:%d FP:%d FM:%d FC:%d col:%d]", + ms(t_end - t_start), ms(t_sub - t_start), ms(t_scale - t_sub), ms(t_rect - t_scale), + ms(t_submit - t_rect), ms(t_color_remap - t_submit), ms(t_mono_pub - t_color_remap), + ms(t_color_pub - t_mono_pub), ms(t_footprint - t_color_pub), ms(t_sgm - t_footprint), + ms(t_extract - t_sgm), ms(t_unfilt - t_extract), ms(t_filter - t_unfilt), + ms(t_depth - t_filter), ms(t_pc - t_depth), (int)pub_left_rect, (int)pub_right_rect, + (int)pub_left_color, (int)pub_left_compressed, (int)pub_pointcloud, (int)pub_pointcloud_color, + (int)pub_depth, (int)pub_disparity, (int)pub_unfiltered, (int)pub_footprint_overlay, + (int)pub_footprint_mask, (int)pub_footprint_cutout, (int)has_color); + + RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 5000, + "Filter detail %.1fms | fp_mask %.1f | down %.1f | clamp %.1f | domain %.1f | speckle %.1f | " + "edge %.1f | median %.1f | bilateral %.1f | hole %.1f | temporal %.1f | up %.1f", + ms(t_filter - t_unfilt), ft.footprint_mask_ms, ft.downsample_ms, ft.depth_clamp_ms, + ft.domain_transform_ms, ft.speckle_ms, ft.edge_inv_ms, ft.median_ms, ft.bilateral_ms, + ft.hole_fill_ms, ft.temporal_ms, ft.upsample_ms); } } // namespace mars_cam diff --git a/scripts/innate b/scripts/innate index 51d06e2b9..39f43d983 100755 --- a/scripts/innate +++ b/scripts/innate @@ -1445,6 +1445,8 @@ def _cached_skill_contracts(): def _skill_status(skill_info): + if getattr(skill_info, "type", "") == "broken": + return "error" return "training" if getattr(skill_info, "in_training", False) else "ready" diff --git a/scripts/update/migrate_user_data.sh b/scripts/update/migrate_user_data.sh index b8eb7c1ab..d801a06bc 100755 --- a/scripts/update/migrate_user_data.sh +++ b/scripts/update/migrate_user_data.sh @@ -15,16 +15,36 @@ # /skills/* -> /workspace/custom_skills/ # /primitives/**/*.{h5,pt,pth} -> /workspace/innate_skills/ # /inputs/* -> /workspace/inputs/ +# ~/agents/* -> /workspace/custom_agents/ +# ~/skills/* -> /workspace/custom_skills/ # /maps/* -> /data/maps/ # /.last_mode -> /data/.last_mode # /.last_map -> /data/.last_map +# settings.yaml extra_skill_dirs entries -> /workspace/custom_skills/ (symlink) +# settings.yaml extra_agent_dirs entries -> /workspace/custom_agents/* (symlinks) # -# Home-directory skills/agents (~/skills, ~/agents) are NOT handled here; they -# are migrated at brain startup by -# brain_client.script_paths.migrate_legacy_home_directories(). +# The home-dir lanes (~/skills, ~/agents) were scanned in place through 0.6.x; +# 0.7 loads only from workspace/, so they are migrated here rather than left to +# stop loading silently. custom_skills/custom_agents keeps their ids unchanged +# (local/). +# +# The extra-dirs lanes (config/settings.yaml script_paths.extra_skill_dirs / +# extra_agent_dirs, shipped 0.6.0 through 0.7.0-rc1) pointed the loaders at +# directories anywhere on the machine; the setting is gone in 0.7. Configured +# dirs are symlinked into workspace/ so their content keeps loading — never +# moved, since an extra dir may be shared state (e.g. /opt/team/skills). +# A skill dir becomes a custom_skills/ *subpackage* rather than a +# workspace-root pack: that keeps the local/ skill ids its skills had +# (agents and anything persisted reference those), where a root pack would +# re-namespace them /. Agent dirs have no pack equivalent +# (agents load from custom_agents/ top-level files only), so their top-level +# entries are linked into custom_agents/ one by one — non-Python assets too, +# because display icons resolve relative to the agent file's directory. # # Optional env: MIGRATE_CHOWN_USER — when set and running as root, moved files # are chowned to this user (best-effort). Unset (e.g. in CI) => no chown. +# Optional env: MIGRATE_HOME — home dir to migrate the ~/ lanes from; defaults +# to $HOME. post_update.sh passes ACTUAL_HOME so sudo doesn't scan /root. # Log via the host script's log() *function* if defined (post_update.sh), else # stdout. Uses `declare -F` so we don't accidentally match an external `log` @@ -72,11 +92,10 @@ _mig_move() { fi } -# Move every child of /$old_rel into /workspace/$new_rel. -_migrate_dir_into_workspace() { - local repo="$1" old_rel="$2" new_rel="$3" - local old_path="$repo/$old_rel" - local new_path="$repo/workspace/$new_rel" +# Move every child of $old_path into $new_path. $old_label is what the logs +# call the source ("skills/", "~/skills/"). +_migrate_path_into_workspace() { + local old_path="$1" new_path="$2" old_label="$3" new_rel="$4" [ -d "$old_path" ] || return 0 shopt -s dotglob nullglob @@ -84,7 +103,7 @@ _migrate_dir_into_workspace() { shopt -u dotglob nullglob if [ ${#items[@]} -gt 0 ]; then - _mig_log "Migrating $old_rel/ -> workspace/$new_rel/" + _mig_log "Migrating $old_label/ -> workspace/$new_rel/" _mig_mkdir "$new_path" local item name for item in "${items[@]}"; do @@ -93,11 +112,32 @@ _migrate_dir_into_workspace() { rm -rf "$item" continue fi - _mig_move "$item" "$new_path/$name" "$old_rel/$name -> workspace/$new_rel/$name" + _mig_move "$item" "$new_path/$name" "$old_label/$name -> workspace/$new_rel/$name" done fi - rmdir "$old_path" 2>/dev/null && _mig_log "Removed empty $old_rel/" || true + rmdir "$old_path" 2>/dev/null && _mig_log "Removed empty $old_label/" || true +} + +# Move every child of /$old_rel into /workspace/$new_rel. +_migrate_dir_into_workspace() { + local repo="$1" old_rel="$2" new_rel="$3" + _migrate_path_into_workspace "$repo/$old_rel" "$repo/workspace/$new_rel" "$old_rel" "$new_rel" +} + +# Same, for the home-dir lanes (~/skills, ~/agents). These were scanned in +# place through 0.6.x and are not scanned at all as of 0.7, so without this +# their content would silently stop loading. MIGRATE_HOME is the *invoking +# user's* home (post_update.sh passes ACTUAL_HOME): under sudo, $HOME is +# /root and the real content would be missed. +_migrate_home_dir_into_workspace() { + local repo="$1" home_rel="$2" new_rel="$3" + local home="${MIGRATE_HOME:-${HOME:-}}" + [ -n "$home" ] || return 0 + # INNATE_OS_ROOT=~ collapses ~/skills onto /skills, already migrated + # above by the repo-relative pass; skip so it isn't reported twice. + [ "$home" != "$repo" ] || return 0 + _migrate_path_into_workspace "$home/$home_rel" "$repo/workspace/$new_rel" "~/$home_rel" "$new_rel" } # Move trained-model files out of the legacy primitives/ tree into innate_skills, @@ -159,12 +199,180 @@ _migrate_nav_state() { done } +# Symlink $src to $dst unless $dst exists. A symlink already pointing at $src +# is the previous run's work — silent no-op, keeping re-runs idempotent. +_mig_symlink() { + local src="$1" dst="$2" label="$3" + if [ -L "$dst" ] && [ "$(readlink "$dst")" = "$src" ]; then + return 0 + fi + if [ -e "$dst" ] || [ -L "$dst" ]; then + _mig_log " Kept $label (already exists at destination — reconcile manually)" + return 0 + fi + _mig_mkdir "$(dirname "$dst")" + ln -s "$src" "$dst" + if [ -n "${MIGRATE_CHOWN_USER:-}" ] && [ "$(id -u)" -eq 0 ]; then + chown -h "$MIGRATE_CHOWN_USER:$MIGRATE_CHOWN_USER" "$dst" 2>/dev/null || true + fi + _mig_log " Linked $label" +} + +# True if $1 has any visible entry besides __pycache__ (i.e. is worth linking). +_dir_has_content() { + [ -n "$(find "$1" -mindepth 1 -maxdepth 1 ! -name '.*' ! -name '__pycache__' -print -quit 2>/dev/null)" ] +} + +# Print "\t\t" per configured extra dir, kind in +# {skill, agent}. Needs python3 + PyYAML (both ship with the robot's ROS +# stack): exit 3 = no PyYAML, 4 = settings.yaml unparseable. ``~`` expands +# against MIGRATE_HOME, not $HOME — under sudo, $HOME is /root. Pack names +# must be Python identifiers (a custom_skills subpackage is imported); the +# old exec-based scanner had no such constraint, so sanitize. +_extra_dirs_plan() { + MIGRATE_HOME="${MIGRATE_HOME:-${HOME:-}}" python3 - "$1" <<'PY' +import os +import re +import sys + +try: + import yaml +except ImportError: + sys.exit(3) + +home = os.environ.get("MIGRATE_HOME") or os.path.expanduser("~") +try: + with open(sys.argv[1]) as f: + data = yaml.safe_load(f) or {} +except Exception: + sys.exit(4) + +section = data.get("script_paths") +params = section.get("ros__parameters") if isinstance(section, dict) else None +params = params if isinstance(params, dict) else {} + + +def dirs(key): + # Same shapes load_extra_script_dirs() accepted: YAML list or + # os.pathsep-joined string, blanks dropped, ~ and $VARS expanded. + value = params.get(key) + if isinstance(value, str): + parts = value.split(os.pathsep) + elif isinstance(value, list): + parts = [str(p) for p in value] + else: + parts = [] + out = [] + for part in parts: + part = part.strip() + if not part: + continue + if part == "~" or part.startswith("~/"): + part = home + part[1:] + part = os.path.abspath(os.path.expandvars(os.path.expanduser(part))) + if part not in out: + out.append(part) + return out + + +def pack_name(src): + name = re.sub(r"[^0-9A-Za-z_]", "_", os.path.basename(src.rstrip("/"))) + if not name or name[0].isdigit() or name.startswith("_"): + # _-prefixed packages are skipped by discovery; digits can't start one. + name = "pack_" + name.lstrip("_") + return name + + +for src in dirs("extra_skill_dirs"): + print(f"skill\t{src}\t{pack_name(src)}") +for src in dirs("extra_agent_dirs"): + print(f"agent\t{src}\t-") +PY +} + +# Symlink the dirs configured in settings.yaml script_paths (see header) into +# workspace/. Must run AFTER the move passes: an extra dir pointing at a moved +# lane (~/skills, /skills) is empty or gone by the time this runs, and +# is skipped here rather than left as a hollow link. +_migrate_extra_script_dirs() { + local repo="$1" + local settings="$repo/config/settings.yaml" + [ -f "$settings" ] || return 0 + # Cheap pre-filter: nothing to do (and no parser needed) unless the + # setting appears outside a comment. + grep -Eq '^[^#]*extra_(skill|agent)_dirs' "$settings" || return 0 + + local plan + if ! plan=$(_extra_dirs_plan "$settings"); then + _mig_log "WARNING: settings.yaml sets script_paths.extra_*_dirs but the section could not be parsed (python3/PyYAML missing, or malformed YAML)." + _mig_log "WARNING: 0.7 no longer scans extra dirs — symlink them into workspace/ by hand (see config/settings.yaml.template)." + return 0 + fi + [ -n "$plan" ] || return 0 + + local home="${MIGRATE_HOME:-${HOME:-}}" + # Canonicalize the paths compared against below, so a symlinked repo or + # home still matches. _canon leaves a non-existent path as-is. + _canon() { readlink -f "$1" 2>/dev/null || echo "$1"; } + local ws_resolved repo_resolved home_resolved + ws_resolved=$(_canon "$repo/workspace") + repo_resolved=$(_canon "$repo") + home_resolved=$(_canon "${home:-//nonexistent}") + + _mig_log "Migrating settings.yaml extra script dirs -> workspace/ symlinks" + local kind src name resolved entry base + while IFS=$'\t' read -r kind src name; do + [ -n "$kind" ] || continue + if [ ! -d "$src" ]; then + _mig_log " Skipped $kind dir $src (not a directory — moved by a pass above, or never existed)" + continue + fi + resolved=$(_canon "$src") + case "$resolved" in + "$ws_resolved" | "$ws_resolved"/*) + _mig_log " Skipped $kind dir $src (already inside workspace/ — loads without migration)" + continue + ;; + esac + # Lanes the move passes above own; their contents already moved (any + # leftovers are name clashes those passes logged for manual review). + case "$resolved" in + "$repo_resolved"/skills | "$repo_resolved"/agents | "$repo_resolved"/directives | \ + "$home_resolved"/skills | "$home_resolved"/agents) + _mig_log " Skipped $kind dir $src (contents moved by the migration above)" + continue + ;; + esac + if ! _dir_has_content "$src"; then + _mig_log " Skipped $kind dir $src (empty)" + continue + fi + if [ "$kind" = "skill" ]; then + _mig_symlink "$src" "$repo/workspace/custom_skills/$name" \ + "extra skill dir $src -> workspace/custom_skills/$name" + else + # Per-entry links: agents load from custom_agents/ itself, and a + # linked subdirectory would not be scanned. + for entry in "$src"/*; do + [ -e "$entry" ] || continue + base=$(basename "$entry") + [ "$base" = "__pycache__" ] && continue + _mig_symlink "$entry" "$repo/workspace/custom_agents/$base" \ + "extra agent entry $src/$base -> workspace/custom_agents/$base" + done + fi + done <<< "$plan" +} + run_user_data_migrations() { local repo="${1:?run_user_data_migrations: repo dir required}" _migrate_dir_into_workspace "$repo" agents custom_agents _migrate_dir_into_workspace "$repo" directives custom_agents _migrate_dir_into_workspace "$repo" skills custom_skills _migrate_dir_into_workspace "$repo" inputs inputs + _migrate_home_dir_into_workspace "$repo" agents custom_agents + _migrate_home_dir_into_workspace "$repo" skills custom_skills + _migrate_extra_script_dirs "$repo" _migrate_primitives_models "$repo" _migrate_nav_state "$repo" } diff --git a/scripts/update/post_update.sh b/scripts/update/post_update.sh index 7bc0764f7..bbee1297f 100755 --- a/scripts/update/post_update.sh +++ b/scripts/update/post_update.sh @@ -256,16 +256,22 @@ fi # Repair the mode outside the seeding branch above: a robot whose .env lost its key — the # very case this fallback exists for — or one flashed with a pre-seeded /etc/innate.env # never reaches that branch, so gating the repair on it leaves the file 0600 root:root, -# unreadable by the non-root launch readers. Skipped when ACTUAL_USER resolves to root -# (run from a root shell, no sudo): "root:root 640" would revoke the launch readers' -# group access on a file that may currently be correct. +# unreadable by the non-root launch readers (print_runtime_env.py then treats it as +# absent and the service key silently drops out — proxy "not configured"). +# Idempotent; matches the seeded state above. Contents are never touched. if [ -f "$SYSTEM_ENV_FILE" ]; then - if [ "$ACTUAL_USER" != "root" ]; then - chown "root:$ACTUAL_USER" "$SYSTEM_ENV_FILE" - chmod 640 "$SYSTEM_ENV_FILE" - log "Set $SYSTEM_ENV_FILE to 640 root:$ACTUAL_USER" - else + if [ "$ACTUAL_USER" = "root" ]; then + # Run from a root shell, no sudo: "root:root 640" would revoke the launch + # readers' group access on a file that may currently be correct. log "Skipping $SYSTEM_ENV_FILE permission repair (no non-root user; re-run via sudo)" + elif [ "$(stat -c '%U:%G %a' "$SYSTEM_ENV_FILE")" != "root:$ACTUAL_USER 640" ]; then + # Best-effort under set -e: a user without a same-named group would fail + # the chown, and that must not abort the rest of the update. + if chown "root:$ACTUAL_USER" "$SYSTEM_ENV_FILE" && chmod 640 "$SYSTEM_ENV_FILE"; then + log "Set $SYSTEM_ENV_FILE to 640 root:$ACTUAL_USER so launch readers can read the service key" + else + log "WARNING: could not fix $SYSTEM_ENV_FILE ownership/mode (group '$ACTUAL_USER' missing?); continuing" + fi fi fi @@ -276,12 +282,12 @@ fi # shipped files; this step preserves any user-created *untracked* content # (custom skills/agents/inputs, trained models, SLAM maps, last mode/map). # Idempotent, never overwrites, only rmdir's empty dirs. -# Home-dir ~/agents and ~/skills are migrated separately at brain startup -# (brain_client.script_paths.migrate_legacy_home_directories). +# Home-dir ~/agents and ~/skills are migrated too: 0.7 loads only from +# workspace/, so leaving them in place would silently stop loading them. # ----------------------------------------------------------------------------- # shellcheck source=scripts/update/migrate_user_data.sh source "$SCRIPT_DIR/migrate_user_data.sh" -MIGRATE_CHOWN_USER="$ACTUAL_USER" run_user_data_migrations "$REPO_DIR" +MIGRATE_CHOWN_USER="$ACTUAL_USER" MIGRATE_HOME="$ACTUAL_HOME" run_user_data_migrations "$REPO_DIR" # ----------------------------------------------------------------------------- # 0a2. Create config/settings.yaml from template if missing. diff --git a/tests/test_settings_hot_reload_validation.py b/tests/test_settings_hot_reload_validation.py index 78b30049b..190a24f25 100644 --- a/tests/test_settings_hot_reload_validation.py +++ b/tests/test_settings_hot_reload_validation.py @@ -1,10 +1,10 @@ # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc -"""The int-vs-double settings guard must fail fast at launch but never crash a reload. +"""The int-vs-double settings guard must fail fast at launch. -Locks in the split: launch-time ``settings_params`` raises on an int-typed double, while the -runtime ``load_extra_script_dirs`` read tolerates the same file and still returns its dirs. -Pure Python — no ROS or colcon build required. +Locks in that launch-time ``settings_params`` raises on an int-typed double +(motion_control.max_speed: 4, not 4.0) instead of letting ROS reject the +param at node startup. Pure Python — no ROS or colcon build required. """ from __future__ import annotations @@ -19,17 +19,13 @@ from mars_bringup import config_loader # noqa: E402 -# An int written where ROS declares a double (motion_control.max_speed: 4, not 4.0) — -# the case _validate_settings_param_types rejects — alongside a valid script_paths block. +# An int written where ROS declares a double — the case +# _validate_settings_param_types rejects. _SETTINGS_INT_DOUBLE = """\ /**: ros__parameters: motion_control: max_speed: 4 -script_paths: - ros__parameters: - extra_agent_dirs: ["/opt/team/agents"] - extra_skill_dirs: ["/opt/team/skills"] """ @@ -40,14 +36,7 @@ def settings_with_int_double(tmp_path, monkeypatch): monkeypatch.setenv("INNATE_OS_ROOT", str(tmp_path)) -def test_launch_path_still_fails_fast(settings_with_int_double): - """settings_params() (the ROS-param path) must still reject the int-typed double.""" +def test_launch_path_fails_fast(settings_with_int_double): + """settings_params() (the ROS-param path) must reject the int-typed double.""" with pytest.raises(ValueError, match="decimals"): config_loader.settings_params() - - -def test_hot_reload_path_does_not_raise(settings_with_int_double): - """load_extra_script_dirs() (the runtime hot-reload path) must read the script_paths - block without re-raising the launch-time int-vs-double guard.""" - assert config_loader.load_extra_script_dirs("extra_agent_dirs") == ["/opt/team/agents"] - assert config_loader.load_extra_script_dirs("extra_skill_dirs") == ["/opt/team/skills"] diff --git a/webapp/css/app.css b/webapp/css/app.css index 1754b6e41..d472f7ffc 100644 --- a/webapp/css/app.css +++ b/webapp/css/app.css @@ -4471,6 +4471,40 @@ button { flex-direction: column; } +/* Folder section header (SkillInfo.group): a full-width toggle row, visually + quieter than skill rows so sections read as structure, not entries. */ +.skills-pop-group { + display: flex; + align-items: center; + justify-content: space-between; + gap: 10px; + width: 100%; + margin-top: 6px; + padding: 6px 12px; + text-align: left; + font-size: 11px; + letter-spacing: 0.04em; + text-transform: uppercase; + color: var(--muted); + background: transparent; + border: none; + border-top: 1px solid var(--hairline); + border-radius: 0; + cursor: pointer; + transition: background 150ms ease; +} + +.skills-pop-group:hover { + background: rgb(255 255 255 / 5%); +} + +.skills-pop-group-name { + min-width: 0; + overflow: hidden; + text-overflow: ellipsis; + white-space: nowrap; +} + .skills-pop-row.expanded { background: rgb(255 255 255 / 4%); border-radius: 10px; @@ -4531,6 +4565,11 @@ button { background: var(--type-digital); } +/* A skill file that failed to load — row is disabled, error shown inline. */ +.skills-pop-type-dot.broken { + background: var(--danger); +} + .skills-pop-tail { flex: 0 0 auto; font-size: 13px; diff --git a/webapp/js/settings/catalog.js b/webapp/js/settings/catalog.js index 678a8c7ac..06e5bbd31 100644 --- a/webapp/js/settings/catalog.js +++ b/webapp/js/settings/catalog.js @@ -168,12 +168,4 @@ export const CATALOG = [ { path: ["uninavid_node", P, "poll_period_sec"], label: "Poll period", default: 0.02, type: "float", unit: "s", doc: "Action-loop poll interval" }, ], }, - { - section: "Extra agent / skill directories", - note: "Scan agents/skills from extra absolute paths, on top of the built-in workspace dirs. Paths are scanned in place (never created); in a Docker/sim setup they must also be mounted into the container.", - knobs: [ - { path: ["script_paths", P, "extra_agent_dirs"], label: "Extra agent dirs", default: [], type: "list", doc: "Absolute paths scanned for agents" }, - { path: ["script_paths", P, "extra_skill_dirs"], label: "Extra skill dirs", default: [], type: "list", doc: "Absolute paths scanned for skills" }, - ], - }, ]; diff --git a/webapp/js/teleop/joystick.js b/webapp/js/teleop/joystick.js index 93c2fcb36..2a45eee35 100644 --- a/webapp/js/teleop/joystick.js +++ b/webapp/js/teleop/joystick.js @@ -17,6 +17,7 @@ const OUTER_R = 84; const KNOB_R = 13; const MAX_DIST = OUTER_R - KNOB_R; const SVG_NS = "http://www.w3.org/2000/svg"; +const TOGGLE_KEY = "KeyJ"; // press j to hide/show the on-screen joystick /** * @param {string} tag @@ -147,6 +148,26 @@ export function createJoystick(parent, driveController) { svg.addEventListener("pointercancel", release); svg.addEventListener("lostpointercapture", release); + // Hide/show with the j key. Releasing on hide clears any latched command so + // the robot never keeps driving from a joystick the operator can't see. + let hidden = false; + function toggleHidden() { + hidden = !hidden; + if (hidden) release(); + svg.style.display = hidden ? "none" : ""; + } + + /** @param {KeyboardEvent} e */ + function onKeyDown(e) { + if (e.code !== TOGGLE_KEY || e.repeat || e.metaKey || e.ctrlKey || e.altKey) return; + const el = document.activeElement; + if (el instanceof HTMLElement && (el.tagName === "INPUT" || el.tagName === "TEXTAREA" || el.isContentEditable)) { + return; + } + toggleHidden(); + } + window.addEventListener("keydown", onKeyDown); + // Mirror keyboard drive so the knob always shows what the robot was told. const unsubActive = driveController.onActiveChange((state) => { if (pointerEngaged) return; @@ -163,6 +184,7 @@ export function createJoystick(parent, driveController) { return { destroy() { release(); + window.removeEventListener("keydown", onKeyDown); unsubActive(); svg.remove(); }, diff --git a/webapp/js/teleop/skillsMenu.js b/webapp/js/teleop/skillsMenu.js index 177615f03..d61588a9a 100644 --- a/webapp/js/teleop/skillsMenu.js +++ b/webapp/js/teleop/skillsMenu.js @@ -62,6 +62,8 @@ export function createSkillsMenu(parent, rosClient) { let signature = ""; /** @type {string | null} */ let expandedId = null; + /** Folder sections the user collapsed (by group path). @type {Set} */ + const collapsedGroups = new Set(); /** Per-skill, per-param string values, kept across re-renders. @type {Map>} */ const inputValues = new Map(); /** Last/in-flight run. `done` marks the terminal state. @type {{ skillId: string, cancel: () => void, text: string, error: boolean, canceling: boolean, done: boolean } | null} */ @@ -321,7 +323,17 @@ export function createSkillsMenu(parent, rosClient) { // A run started elsewhere (agent, CLI, another tab) has no local cancel // handle — offer a Stop that goes through /brain/cancel_skill instead. if (topicActiveName && !(run && !run.done)) frag.appendChild(renderExternRow()); - for (const skill of skills) frag.appendChild(renderRow(skill)); + // Root skills flat first (pinned order preserved), then one collapsible + // section per folder (SkillInfo.group), folders alphabetical. + for (const skill of skills) { + if (!skill.group) frag.appendChild(renderRow(skill)); + } + for (const [group, members] of groupedSkills()) { + frag.appendChild(renderGroupHeader(group, members.length)); + if (!collapsedGroups.has(group)) { + for (const skill of members) frag.appendChild(renderRow(skill)); + } + } if (skills.length === 0) { const empty = document.createElement("p"); empty.className = "skills-pop-empty"; @@ -331,6 +343,42 @@ export function createSkillsMenu(parent, rosClient) { listEl.replaceChildren(frag); } + /** Grouped skills as [group, members][] with folders alphabetical; members + * keep the pinned/roster order of `skills`. @returns {[string, any[]][]} */ + function groupedSkills() { + /** @type {Map} */ + const groups = new Map(); + for (const skill of skills) { + const group = typeof skill.group === "string" ? skill.group : ""; + if (!group) continue; + const members = groups.get(group) ?? []; + members.push(skill); + groups.set(group, members); + } + return [...groups.entries()].sort(([a], [b]) => a.localeCompare(b)); + } + + /** Section header for one folder: click toggles collapse. @param {string} group @param {number} count */ + function renderGroupHeader(group, count) { + const collapsed = collapsedGroups.has(group); + const head = document.createElement("button"); + head.type = "button"; + head.className = "skills-pop-group"; + const name = document.createElement("span"); + name.className = "skills-pop-group-name"; + name.textContent = prettify(group); + const tail = document.createElement("span"); + tail.className = "skills-pop-tail mono"; + tail.textContent = collapsed ? `${count} ›` : "▾"; + head.append(name, tail); + head.addEventListener("click", () => { + if (collapsed) collapsedGroups.delete(group); + else collapsedGroups.add(group); + render(); + }); + return head; + } + /** Banner row for the externally-started run: name + Stop. */ function renderExternRow() { const row = document.createElement("div"); @@ -350,8 +398,34 @@ export function createSkillsMenu(parent, rosClient) { return row; } + /** A skill whose file failed to load: not runnable, shows the error instead. @param {any} skill */ + function renderBrokenRow(skill) { + const row = document.createElement("div"); + row.className = "skills-pop-row"; + const head = document.createElement("button"); + head.type = "button"; + head.className = "skills-pop-item"; + head.disabled = true; + head.title = skill.load_error; + const dot = document.createElement("span"); + dot.className = "skills-pop-type-dot broken"; + dot.title = "Failed to load"; + const name = document.createElement("span"); + name.className = "skills-pop-name"; + name.textContent = formatName(skill); + head.append(dot, name); + const status = document.createElement("div"); + status.className = "skills-pop-status error"; + const txt = document.createElement("span"); + txt.textContent = skill.load_error; + status.appendChild(txt); + row.append(head, status); + return row; + } + /** @param {any} skill */ function renderRow(skill) { + if (skill.load_error) return renderBrokenRow(skill); const expandable = hasParams(skill); const isExpanded = expandable && expandedId === skill.id; const running = !!run && run.skillId === skill.id && !run.done; @@ -549,7 +623,9 @@ export function createSkillsMenu(parent, rosClient) { .map((entry) => entry.s); // The roster is latched and republishes on any change; avoid a re-render // (which would steal focus mid-typing) unless the set actually changed. - const sig = next.map((s) => s.id).join("|"); + // load_error and group are part of the signature: a skill breaking (or + // moving folders) must repaint even though the id set is identical. + const sig = next.map((s) => s.id + ":" + (s.group || "") + (s.load_error ? `!${s.load_error}` : "")).join("|"); if (sig === signature) return; signature = sig; skills = next; diff --git a/webapp/js/types.d.ts b/webapp/js/types.d.ts index 1d6d3c976..55b4566a1 100644 --- a/webapp/js/types.d.ts +++ b/webapp/js/types.d.ts @@ -84,6 +84,8 @@ interface Skill { id: string; name: string; type: "code" | "learned" | "replay" | "poses" | "eval"; + /** Folder path inside the skill package ("" = root, e.g. "chess"). */ + group?: string; episode_count: number; /** Dataset directory on the robot; absent for code-only skills. */ directory?: string; diff --git a/webapp/proxy/media_routes.py b/webapp/proxy/media_routes.py index 52cfd3f21..29eac41d7 100644 --- a/webapp/proxy/media_routes.py +++ b/webapp/proxy/media_routes.py @@ -20,21 +20,11 @@ # blob (the log viewer wants text, not gigabytes). MAX_LOG_BYTES = 8 * 1024 * 1024 -# Roots the /episode* routes may serve from: workspace/custom_skills plus the -# legacy in-place locations the brain still scans ($INNATE_OS_ROOT/skills, -# ~/skills; used through 0.5.x). Deduped after resolve so INNATE_OS_ROOT=~ (which -# collapses two of them) doesn't double-check. +# Roots the /episode* routes may serve from. Only workspace/custom_skills: +# the pre-0.6 in-place locations ($INNATE_OS_ROOT/skills, ~/skills) are no +# longer scanned by the brain, so serving from them was dead surface. _INNATE_OS_ROOT = os.environ.get("INNATE_OS_ROOT", os.path.expanduser("~/innate-os")) -SKILLS_ROOTS = tuple( - dict.fromkeys( - p.resolve() - for p in ( - Path(_INNATE_OS_ROOT) / "workspace" / "custom_skills", - Path(_INNATE_OS_ROOT) / "skills", - Path(os.path.expanduser("~")) / "skills", - ) - ) -) +SKILLS_ROOTS = ((Path(_INNATE_OS_ROOT) / "workspace" / "custom_skills").resolve(),) def _under_skills_root(p: Path) -> bool: diff --git a/workspace/README.md b/workspace/README.md index f1f9f1a7e..a51b25c0f 100644 --- a/workspace/README.md +++ b/workspace/README.md @@ -1,14 +1,44 @@ # workspace -Where agents and skills live on disk. +Where agents and skills live on disk. Every skills directory is an ordinary +Python package — imported, not scanned. ``` innate_agents/ Shipped agents. Tracked in git, updated by `git pull`. custom_agents/ Your agents. Gitignored, stays on your machine. innate_skills/ Shipped skills. Tracked in git. custom_skills/ Your skills (code and physical). Gitignored. +/ A skill package: someone's skills + helpers, installed by dropping the folder in. ``` -Drop a `.py` file (or, for physical skills, a directory with `metadata.json`) into the matching folder — it auto-loads on the next brain_client restart, and edits trigger hot reload. +A skill is a class: define a `Skill` subclass anywhere in a package and the +robot knows it — defining it is the registration (like a PyTorch `nn.Module`). +The class name is the identity (`class PickSocks` → `pick_socks`), so files +organize however you like: several skills in one file, a skill split across a +subpackage with relative imports, helpers next to it. A `.py` that defines no +`Skill` is just a module you import. Physical skills stay data: a directory +with `metadata.json`. The catalog drops a generated `__init__.py` ref next to +that metadata, so `from innate_skills.wave import Wave` imports a typed handle +to the recording (the same class as `from physical_skills import Wave`). -Skill IDs reflect origin: `innate-os/` for shipped, `local/` for custom. +Everything auto-loads on brain_client start, edits hot-reload on save, and a +module that fails to import shows up in the web app marked broken with its +error (and clears when you fix it) instead of vanishing. + +Skill IDs are namespaced by package: `innate-os/` for shipped, +`local/` for yours, `/` for dropped-in packs. Packages +import each other by bare name (`from innate_skills import arm_utils`). See +the README at the repo root. + +A pack that lives elsewhere on disk (a team checkout, a mounted volume) is +symlinked in rather than copied — it then behaves exactly like a dropped-in +folder: discovered at boot, hot-reloaded on edit, ids namespaced by the link +name (`team_skills/`): + +```bash +ln -s /opt/team/skills ~/innate-os/workspace/team_skills +``` + +(This replaces the 0.6.x `extra_skill_dirs` / `extra_agent_dirs` setting. In +the sim/Docker setup the link target must also be mounted into the container, +or it dangles there and the pack is skipped.) diff --git a/workspace/innate_agents/basic_agent.py b/workspace/innate_agents/basic_agent.py index d242be4d3..a1a30e65c 100644 --- a/workspace/innate_agents/basic_agent.py +++ b/workspace/innate_agents/basic_agent.py @@ -1,6 +1,10 @@ # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc -from brain_client.agents.types import Agent +from innate_skills.navigate_to_position import NavigateToPosition +from innate_skills.navigate_with_vision import NavigateWithVision +from inputs.micro_input import MicroInput + +from brain_client.agents.types import Agent, InputRef, SkillRef class BasicAgent(Agent): @@ -17,13 +21,14 @@ def id(self) -> str: def display_name(self) -> str: return "Basic Navigation" - def get_skills(self) -> list[str]: - """Return the list of skill IDs this directive can use""" - return ["innate-os/navigate_to_position", "innate-os/navigate_with_vision"] + def get_skills(self) -> list[SkillRef]: + """The skills this directive can use — classes for code skills; + physical skills (no class) stay id strings like "local/pick_socks".""" + return [NavigateToPosition, NavigateWithVision] - def get_inputs(self) -> list[str]: + def get_inputs(self) -> list[InputRef]: """Enable microphone input to hear user""" - return ["micro"] + return [MicroInput] def get_prompt(self) -> str: return "" diff --git a/workspace/innate_agents/board_calibration_agent.py b/workspace/innate_agents/board_calibration_agent.py index 4f59dce3a..65da0eef9 100644 --- a/workspace/innate_agents/board_calibration_agent.py +++ b/workspace/innate_agents/board_calibration_agent.py @@ -1,6 +1,9 @@ # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc -from brain_client.agents.types import Agent +from innate_skills.chess.record_position import RecordPosition +from inputs.micro_input import MicroInput + +from brain_client.agents.types import Agent, InputRef, SkillRef class BoardCalibrationAgent(Agent): @@ -16,13 +19,13 @@ def id(self) -> str: def display_name(self) -> str: return "Board Calibration Agent" - def get_skills(self) -> list[str]: + def get_skills(self) -> list[SkillRef]: """Return position recording skill.""" - return ["innate-os/record_position"] + return [RecordPosition] - def get_inputs(self) -> list[str]: + def get_inputs(self) -> list[InputRef]: """Enable microphone input.""" - return ["micro"] + return [MicroInput] def get_prompt(self) -> str: """Return the calibration workflow prompt.""" diff --git a/workspace/innate_agents/chess_agent.py b/workspace/innate_agents/chess_agent.py index ea411e971..5c054f3ca 100644 --- a/workspace/innate_agents/chess_agent.py +++ b/workspace/innate_agents/chess_agent.py @@ -1,6 +1,15 @@ # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc -from brain_client.agents.types import Agent +from innate_skills.arm.arm_utils import ArmUtils +from innate_skills.chess.detect_opponent_move import DetectOpponentMove +from innate_skills.chess.pick_up_piece_simple import PickUpPieceSimple +from innate_skills.chess.recalibrate_manual import RecalibrateManual +from innate_skills.chess.reset_chess_game import ResetChessGame +from innate_skills.chess.update_chess_state import UpdateChessState +from innate_skills.head_emotion import HeadEmotion +from inputs.micro_input import MicroInput + +from brain_client.agents.types import Agent, InputRef, SkillRef class ChessAgent(Agent): @@ -16,21 +25,21 @@ def id(self) -> str: def display_name(self) -> str: return "(BETA) Chess Agent" - def get_skills(self) -> list[str]: + def get_skills(self) -> list[SkillRef]: """Return piece manipulation skills.""" return [ - "innate-os/pick_up_piece_simple", - "innate-os/detect_opponent_move", - "innate-os/update_chess_state", - "innate-os/recalibrate_manual", - "innate-os/arm_utils", - "innate-os/reset_chess_game", - "innate-os/head_emotion", + PickUpPieceSimple, + DetectOpponentMove, + UpdateChessState, + RecalibrateManual, + ArmUtils, + ResetChessGame, + HeadEmotion, ] - def get_inputs(self) -> list[str]: + def get_inputs(self) -> list[InputRef]: """Enable microphone input.""" - return ["micro"] + return [MicroInput] def get_prompt(self) -> str: """Return the chess piece manipulation prompt.""" diff --git a/workspace/innate_agents/demo_agent.py b/workspace/innate_agents/demo_agent.py index b326b0da6..90fcb0cfa 100644 --- a/workspace/innate_agents/demo_agent.py +++ b/workspace/innate_agents/demo_agent.py @@ -1,6 +1,12 @@ # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc -from brain_client.agents.types import Agent +from innate_skills.navigate_to_position import NavigateToPosition +from innate_skills.navigate_with_vision import NavigateWithVision +from innate_skills.pick_any_object import PickAnyObject +from innate_skills.wave import Wave +from inputs.micro_input import MicroInput + +from brain_client.agents.types import Agent, InputRef, SkillRef class DemoAgent(Agent): @@ -16,13 +22,14 @@ def id(self) -> str: def display_name(self) -> str: return "Demo Agent" - def get_skills(self) -> list[str]: - """Return skill IDs for navigation and waving.""" - return ["innate-os/navigate_to_position", "innate-os/wave", "innate-os/navigate_with_vision"] + def get_skills(self) -> list[SkillRef]: + """Navigation code skills plus the recorded wave — Wave is the typed + ref generated inside the recording folder (see skills/physical_refs.py).""" + return [NavigateToPosition, Wave, NavigateWithVision, PickAnyObject] - def get_inputs(self) -> list[str]: + def get_inputs(self) -> list[InputRef]: """Enable microphone input to hear user""" - return ["micro"] + return [MicroInput] def get_prompt(self) -> str: """Return the prompt that defines the robot's personality and behavior""" diff --git a/workspace/innate_agents/empty_directive.py b/workspace/innate_agents/empty_directive.py index 7d18f1063..667aef231 100644 --- a/workspace/innate_agents/empty_directive.py +++ b/workspace/innate_agents/empty_directive.py @@ -1,6 +1,6 @@ # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc -from brain_client.agent_types import Agent +from brain_client.agents.types import Agent, SkillRef class EmptyDirective(Agent): @@ -14,7 +14,7 @@ def id(self) -> str: def display_name(self) -> str: return "No Prompt" - def get_skills(self) -> list[str]: + def get_skills(self) -> list[SkillRef]: return [] def get_prompt(self) -> str: diff --git a/workspace/innate_agents/j3so_directive.py b/workspace/innate_agents/j3so_directive.py index f99b7f450..e940a4065 100644 --- a/workspace/innate_agents/j3so_directive.py +++ b/workspace/innate_agents/j3so_directive.py @@ -1,6 +1,9 @@ # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc -from brain_client.agents.types import Agent +from innate_skills.navigate_to_position import NavigateToPosition +from inputs.micro_input import MicroInput + +from brain_client.agents.types import Agent, InputRef, SkillRef class J3SOAgent(Agent): @@ -21,15 +24,13 @@ def display_name(self) -> str: def display_icon(self) -> str: return "assets/j3so.png" - def get_skills(self) -> list[str]: - """Return the list of skill IDs this directive can use""" - return [ - "innate-os/navigate_to_position", - ] + def get_skills(self) -> list[SkillRef]: + """Return the skills this directive can use""" + return [NavigateToPosition] - def get_inputs(self) -> list[str]: + def get_inputs(self) -> list[InputRef]: """This directive needs microphone input to hear user""" - return ["micro"] + return [MicroInput] def get_prompt(self) -> str: """Return the prompt that defines the robot's personality and behavior""" diff --git a/workspace/innate_agents/routine_demo_agent.py b/workspace/innate_agents/routine_demo_agent.py deleted file mode 100644 index 83d5ce33f..000000000 --- a/workspace/innate_agents/routine_demo_agent.py +++ /dev/null @@ -1,28 +0,0 @@ -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -from brain_client.agents.types import Agent - - -class RoutineDemoAgent(Agent): - """Demo directive showing how one skill can chain several skills in order.""" - - @property - def id(self) -> str: - return "routine_demo_agent" - - @property - def display_name(self) -> str: - return "(Demo) Skill Routine" - - def get_skills(self) -> list[str]: - return ["innate-os/run_routine_demo"] - - def get_inputs(self) -> list[str]: - return ["micro"] - - def get_prompt(self) -> str: - return ( - "You can run a fixed demo routine that chains several skills in a " - "predictable order. When the user asks to run the routine, call " - "run_routine_demo." - ) diff --git a/workspace/innate_agents/security_guard_agent.py b/workspace/innate_agents/security_guard_agent.py index 2450fca32..abbdf308d 100644 --- a/workspace/innate_agents/security_guard_agent.py +++ b/workspace/innate_agents/security_guard_agent.py @@ -1,6 +1,10 @@ # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc -from brain_client.agents.types import Agent +from innate_skills.email.send_email import SendEmail +from innate_skills.navigate_to_position import NavigateToPosition +from inputs.micro_input import MicroInput + +from brain_client.agents.types import Agent, InputRef, SkillRef class SecurityGuardAgent(Agent): @@ -21,16 +25,13 @@ def display_name(self) -> str: def display_icon(self) -> str: return "assets/security_guard.png" - def get_skills(self) -> list[str]: - """Return the list of skill IDs this directive can use""" - return [ - "innate-os/navigate_to_position", - "innate-os/send_email", - ] + def get_skills(self) -> list[SkillRef]: + """Return the skills this directive can use""" + return [NavigateToPosition, SendEmail] - def get_inputs(self) -> list[str]: + def get_inputs(self) -> list[InputRef]: """Enable microphone input to hear user""" - return ["micro"] + return [MicroInput] def get_prompt(self) -> str: return """You are a security guard robot tasked with patrolling the house to detect potential intruders. You have a vigilant and professional personality. diff --git a/workspace/innate_skills/arm/arm_circle_motion.py b/workspace/innate_skills/arm/arm_circle_motion.py new file mode 100644 index 000000000..703529cb0 --- /dev/null +++ b/workspace/innate_skills/arm/arm_circle_motion.py @@ -0,0 +1,56 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +import math + +from innate import Manipulation, Skill, SkillReturn + + +class ArmCircleMotion(Skill): + """Move the arm in a circular motion pattern. The circle is traced in the YZ + plane (vertical) while maintaining a constant X position. You can specify the + center position, radius, number of loops, and speed. A good default center + position is x=0.2, y=-0.05, z=0.2 (roughly in front of the robot with arm + extended).""" + + manipulation: Manipulation + + def execute( + self, + center_x: float = 0.2, + center_y: float = -0.05, + center_z: float = 0.2, + radius: float = 0.1, + num_loops: int = 1, + points_per_loop: int = 16, + duration_per_point: float = 0.5, + ) -> SkillReturn: + orientation = self.manipulation.get_current_orientation_rpy() + if orientation is None: + self.logger.warning("Could not get current orientation, using defaults") + roll, pitch, yaw = 0.0, 0.0, 0.0 + else: + roll, pitch, yaw = orientation["roll"], orientation["pitch"], orientation["yaw"] + + self.logger.info( + f"Circular motion: center=({center_x}, {center_y}, {center_z}), radius={radius}m, loops={num_loops}" + ) + if not self.manipulation.move_to_cartesian_pose( + x=center_x, y=center_y, z=center_z + radius, roll=roll, pitch=pitch, yaw=yaw, duration=1.0 + ): + self.fail("Failed to move to start position") + self.sleep(1.2) + + for loop in range(num_loops): + self.logger.info(f"Starting loop {loop + 1}/{num_loops}") + for i in range(points_per_loop): + angle = (2 * math.pi * i) / points_per_loop + target_y = center_y + radius * math.sin(angle) + target_z = center_z + radius * math.cos(angle) + if not self.manipulation.move_to_cartesian_pose( + x=center_x, y=target_y, z=target_z, roll=0.0, pitch=0.0, yaw=0.0, duration=duration_per_point + ): + self.logger.warning(f"IK failed at point {i + 1}, skipping") + continue + self.sleep(duration_per_point) + + return f"Completed {num_loops} circular loop(s)" diff --git a/workspace/innate_skills/arm/arm_move_to_xyz.py b/workspace/innate_skills/arm/arm_move_to_xyz.py new file mode 100644 index 000000000..761bb0a08 --- /dev/null +++ b/workspace/innate_skills/arm/arm_move_to_xyz.py @@ -0,0 +1,29 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +from innate import Manipulation, Skill, SkillReturn + + +class ArmMoveToXYZ(Skill): + """Move the arm end-effector to a target position in Cartesian space (x, y, z + in meters). Coordinates are relative to the robot base_link. If x, y, z are + omitted the arm moves to its home/resting pose. Optionally specify roll, + pitch, yaw orientation in radians.""" + + manipulation: Manipulation + + def execute( + self, + x: float = 0.15, + y: float = 0.1, + z: float = 0.1, + roll: float = 0.0, + pitch: float = 0.0, + yaw: float = 0.0, + duration: int = 3, + ) -> SkillReturn: + self.logger.info(f"Moving arm to XYZ ({x}, {y}, {z}) with RPY ({roll}, {pitch}, {yaw}) over {duration}s") + if not self.manipulation.move_to_cartesian_pose( + x=x, y=y, z=z, roll=roll, pitch=pitch, yaw=yaw, duration=duration, blocking=True + ): + self.fail("Failed to solve IK or send arm command") + return f"Arm moved to ({x}, {y}, {z})" diff --git a/workspace/innate_skills/arm/arm_utils.py b/workspace/innate_skills/arm/arm_utils.py new file mode 100644 index 000000000..eca0ddbda --- /dev/null +++ b/workspace/innate_skills/arm/arm_utils.py @@ -0,0 +1,37 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +from typing import Literal, cast + +from innate import Manipulation, Skill, SkillReturn + +VALID_COMMANDS = ("torque_on", "torque_off", "reboot_arm") +ArmCommand = Literal["torque_on", "torque_off", "reboot_arm"] + + +class ArmUtils(Skill): + """Utility skill for low-level arm commands. Requires 'command' parameter: + 'torque_on', 'torque_off', or 'reboot_arm'. torque_on enables motor torque + so the arm holds position. torque_off disables torque so the arm goes limp + (for manual positioning). reboot_arm reboots all Dynamixel servos to clear + hardware errors.""" + + manipulation: Manipulation + + def execute(self, command: ArmCommand) -> SkillReturn: + command = cast(ArmCommand, command.strip().lower()) + if command == "torque_on": + if not self.manipulation.torque_on(): + self.fail("Failed to enable arm torque") + return "Arm torque enabled" + if command == "torque_off": + if not self.manipulation.torque_off(): + self.fail("Failed to disable arm torque") + return "Arm torque disabled (arm is limp)" + if command == "reboot_arm": + if not self.manipulation.reboot_servos(): + self.fail("Failed to reboot arm servos") + return "Arm servos rebooted and reinitialized; torque is disabled. Run torque_on before moving." + # Unreachable per the Literal type, but agents pass arbitrary strings at + # runtime and nothing upstream enforces the enum — without this an + # invalid command silently succeeds. + self.fail(f"Invalid command '{command}'. Must be one of: {', '.join(VALID_COMMANDS)}.") diff --git a/workspace/innate_skills/arm/arm_zero_position.py b/workspace/innate_skills/arm/arm_zero_position.py new file mode 100644 index 000000000..4a5b78230 --- /dev/null +++ b/workspace/innate_skills/arm/arm_zero_position.py @@ -0,0 +1,21 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +from innate import JointStates, Manipulation, Skill, SkillReturn + + +class ArmZeroPosition(Skill): + """Use this to move the arm to its zero/home position where all joints are + at 0 radians. Safe while holding an object: the gripper keeps its current + closure unless keep_gripper=False.""" + + manipulation: Manipulation + joint_states: JointStates + + def execute(self, duration: int = 3, keep_gripper: bool = True) -> SkillReturn: + joints = list[float](self.manipulation.ZERO) + if keep_gripper: + # j6 zero is *less* closed than a gripping pose, so blindly + # zeroing it drops whatever the claw holds. + joints = self.manipulation.with_gripper(joints, self.manipulation.gripper_j6(self.joint_states)) + self.manipulation.go(joints, duration=duration, logger=self.logger) + return "Arm moved to zero position" diff --git a/workspace/innate_skills/arm_circle_motion.py b/workspace/innate_skills/arm_circle_motion.py deleted file mode 100644 index 600eb1ef6..000000000 --- a/workspace/innate_skills/arm_circle_motion.py +++ /dev/null @@ -1,136 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -""" -Arm Circle Motion Skill - Move arm in a circular pattern. -""" - -import math -import time - -from brain_client.skills.types import Interface, InterfaceType, Skill, SkillResult - - -class ArmCircleMotion(Skill): - """Move the arm in a circular motion pattern.""" - - manipulation = Interface(InterfaceType.MANIPULATION) - - def __init__(self, logger): - super().__init__(logger) - self._cancelled = False - - @property - def name(self): - return "arm_circle_motion" - - def guidelines(self): - return ( - "Move the arm in a circular motion pattern. The circle is traced in the YZ plane " - "(vertical) while maintaining a constant X position. You can specify the center position, " - "radius, number of loops, and speed. A good default center position is x=0.2, y=-0.05, z=0.2 " - "(roughly in front of the robot with arm extended)." - ) - - def execute( - self, - center_x: float = 0.2, - center_y: float = -0.05, - center_z: float = 0.2, - radius: float = 0.1, - num_loops: int = 1, - points_per_loop: int = 16, - duration_per_point: float = 0.5, - ): - """ - Move arm in a circular pattern. - - Args: - center_x: X coordinate of circle center (forward from base), default 0.2m - center_y: Y coordinate of circle center (left from base), default -0.5m - center_z: Z height to maintain during circle, default 0.2m - radius: Radius of the circle in meters, default 0.1m - num_loops: Number of complete circles to trace, default 1 - points_per_loop: Number of waypoints per circle (more = smoother), default 16 - duration_per_point: Time to move between each waypoint in seconds, default 0.5s - """ - self._cancelled = False - - if self.manipulation is None: - return "Manipulation interface not available", SkillResult.FAILURE - - # Get current orientation to maintain during circle motion - current_orientation = self.manipulation.get_current_orientation_rpy() - if current_orientation is None: - self.logger.warning("Could not get current orientation, using defaults") - roll, pitch, yaw = 0.0, 0.0, 0.0 - else: - roll, pitch, yaw = current_orientation["roll"], current_orientation["pitch"], current_orientation["yaw"] - self.logger.info( - f"Current orientation: roll={math.degrees(roll):.1f}°, pitch={math.degrees(pitch):.1f}°, yaw={math.degrees(yaw):.1f}°" - ) - - total_points = num_loops * points_per_loop - self.logger.info( - f"Starting circular motion: center=({center_x}, {center_y}, {center_z}), " - f"radius={radius}m, loops={num_loops}, points={total_points}" - ) - - # First move to starting position (top of circle: center_y, center_z + radius) - start_y = center_y - start_z = center_z + radius - - self.logger.info(f"Moving to start position: ({center_x}, {start_y}, {start_z})") - success = self.manipulation.move_to_cartesian_pose( - x=center_x, y=start_y, z=start_z, roll=roll, pitch=pitch, yaw=yaw, duration=1.0 - ) - - if not success: - return "Failed to move to start position", SkillResult.FAILURE - - # Wait for initial move - time.sleep(1.2) - - if self._cancelled: - return "Circle motion cancelled", SkillResult.CANCELLED - - # Trace the circle(s) - for loop in range(num_loops): - self.logger.info(f"Starting loop {loop + 1}/{num_loops}") - - for i in range(points_per_loop): - if self._cancelled: - return "Circle motion cancelled", SkillResult.CANCELLED - - # Calculate angle for this point (start from top, go clockwise) - # Angle 0 = top (positive Y direction from center) - angle = (2 * math.pi * i) / points_per_loop - - # Calculate YZ position on circle (X stays constant) - # Using standard circle parametrization, rotated so 0 starts at top - target_y = center_y + radius * math.sin(angle) - target_z = center_z + radius * math.cos(angle) - - self.logger.debug( - f"Point {i + 1}/{points_per_loop}: angle={math.degrees(angle):.1f}°, " - f"pos=({center_x:.3f}, {target_y:.3f}, {target_z:.3f})" - ) - - success = self.manipulation.move_to_cartesian_pose( - x=center_x, y=target_y, z=target_z, roll=0.0, pitch=0.0, yaw=0.0, duration=duration_per_point - ) - - if not success: - self.logger.warning(f"IK failed at point {i + 1}, skipping") - continue - - # Wait for motion - time.sleep(duration_per_point) - - self.logger.info("Circle motion completed successfully") - return f"Completed {num_loops} circular loop(s)", SkillResult.SUCCESS - - def cancel(self): - """Cancel the circular motion.""" - self._cancelled = True - return "Circle motion cancelled" diff --git a/workspace/innate_skills/arm_move_to_xyz.py b/workspace/innate_skills/arm_move_to_xyz.py deleted file mode 100644 index be9d8a892..000000000 --- a/workspace/innate_skills/arm_move_to_xyz.py +++ /dev/null @@ -1,82 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -""" -Arm Move To XYZ Skill - Move arm to a Cartesian position using IK. -""" - -import time - -from brain_client.skills.types import Interface, InterfaceType, Skill, SkillResult - - -class ArmMoveToXYZ(Skill): - """Move the arm to a Cartesian position using inverse kinematics.""" - - manipulation = Interface(InterfaceType.MANIPULATION) - - def __init__(self, logger): - super().__init__(logger) - self._cancelled = False - - @property - def name(self): - return "arm_move_to_xyz" - - def guidelines(self): - return ( - "Move the arm end-effector to a target position in Cartesian space (x, y, z in meters). " - "Coordinates are relative to the robot base_link. If x, y, z are omitted the arm moves to " - "its home/resting pose. Optionally specify roll, pitch, yaw orientation in radians." - ) - - def execute( - self, - x: float = 0.15, - y: float = 0.1, - z: float = 0.1, - roll: float = 0.0, - pitch: float = 0.0, - yaw: float = 0.0, - duration: int = 3, - ): - """ - Move arm to Cartesian pose using IK. - - Args: - x: Target x position in meters (forward from base). Defaults to the - home/resting pose (0.15, 0.1, 0.1) when x/y/z are omitted. - y: Target y position in meters (left from base) - z: Target z position in meters (up from base) - roll: Target roll orientation in radians - pitch: Target pitch orientation in radians - yaw: Target yaw orientation in radians - duration: Motion duration in seconds - """ - self._cancelled = False - - if self.manipulation is None: - return "Manipulation interface not available", SkillResult.FAILURE - - self.logger.info(f"Moving arm to XYZ ({x}, {y}, {z}) with RPY ({roll}, {pitch}, {yaw}) over {duration}s") - - success = self.manipulation.move_to_cartesian_pose( - x=x, y=y, z=z, roll=roll, pitch=pitch, yaw=yaw, duration=duration - ) - - if not success: - return "Failed to solve IK or send arm command", SkillResult.FAILURE - - # Wait for motion to complete (with cancellation check) - start_time = time.time() - while time.time() - start_time < duration: - if self._cancelled: - return "Arm motion cancelled", SkillResult.CANCELLED - time.sleep(0.1) - - return f"Arm moved to ({x}, {y}, {z})", SkillResult.SUCCESS - - def cancel(self): - """Cancel the arm movement.""" - self._cancelled = True - return "Arm motion cancelled" diff --git a/workspace/innate_skills/arm_primitives_notes.md b/workspace/innate_skills/arm_primitives_notes.md deleted file mode 100644 index 91e9ee508..000000000 --- a/workspace/innate_skills/arm_primitives_notes.md +++ /dev/null @@ -1,70 +0,0 @@ -# Arm Primitive Execution Notes - -This file captures a few *practical* rules we learned while debugging `move_arm_to_pose` and the arm manipulation stack. - -## 1. Avoid blocking the executor inside action callbacks - -When the `ExecutePrimitive` action callback runs in `PrimitiveExecutionActionServer`, **do not**: - -- Call `rclpy.spin()` or `rclpy.spin_once()` on the same node. -- Call `rclpy.spin_until_future_complete()` on service/action futures. -- Call `wait_for_service()` with a long timeout. -- Sleep for long periods while also holding up all other callbacks. - -These patterns can starve the executor and cause symptoms like: - -- First primitive works, second primitive goal is never accepted/processed. -- `ros2 action send_goal` hangs after printing `Sending goal:`. - -## 2. IK (`solve_ik`) should not spin the node - -`ManipulationInterface.solve_ik` used to do this in a loop: - -```python -rclpy.spin_once(self.node, timeout_sec=0.01) -``` - -This was called **from inside** the action server’s execute callback. - -Instead, we now: - -- Publish the IK target (`ik_delta`). -- Poll for `_ik_solution` with a short `time.sleep(0.01)`. -- Let the main executor thread deliver subscription callbacks. - -This keeps IK polling local and avoids nested spinning. - -## 3. Joint-space motion (GotoJS) must be non-blocking - -For the arm joint motion service (`/mars/arm/goto_js`): - -- Create the `GotoJS` client once in `ManipulationInterface.__init__`. -- Use `client.service_is_ready()` as a **non-blocking** readiness check. -- Use `client.call_async(request)` in a **fire-and-forget** style. -- Do **not** call `wait_for_service()` or `spin_until_future_complete()` from inside primitive execution. - -At this level, `move_to_joint_positions` should: - -- Return `True` as soon as the request has been dispatched (or `False` if the client/service is clearly unavailable). -- Leave the lower-level GotoJS server responsible for handling motion timing and detailed errors. - -## 4. "Primitive succeeded" vs. "motion finished" - -With the non-blocking pattern: - -- `move_arm_to_pose` reports **success** once: - - IK has produced a valid joint configuration, and - - The GotoJS request has been sent successfully. -- It does **not** wait for the physical motion to complete. - -If a primitive **must** block until motion completion, prefer: - -- A higher-level action or topic signaling motion completion, or -- A separate node that owns the blocking waits, so the brain/action server executor stays responsive. - -## 5. General guideline - -When in doubt: treat the `PrimitiveExecutionActionServer` execute callback as a place to **orchestrate** short, non-blocking operations, not a place to perform long or nested spins. Long-running work should either: - -- Be delegated to other actions/services and handled asynchronously, or -- Run in separate nodes/threads that do their own spinning. diff --git a/workspace/innate_skills/arm_rest_position.py b/workspace/innate_skills/arm_rest_position.py new file mode 100644 index 000000000..d0fe6ad15 --- /dev/null +++ b/workspace/innate_skills/arm_rest_position.py @@ -0,0 +1,20 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +from innate import JointStates, Manipulation, Skill, SkillReturn + + +class ArmRestPosition(Skill): + """Use this to move the arm to its resting position (folded against the + body, servos unloaded). Safe while holding an object: the gripper keeps + its current closure unless keep_gripper=False.""" + + manipulation: Manipulation + joint_states: JointStates + + def execute(self, duration: int = 3, keep_gripper: bool = True) -> SkillReturn: + self.manipulation.go( + self.manipulation.rest_joints(self.joint_states, keep_gripper), + duration=duration, + logger=self.logger, + ) + return "Arm moved to rest position" diff --git a/workspace/innate_skills/arm_utils.py b/workspace/innate_skills/arm_utils.py deleted file mode 100644 index 894977abb..000000000 --- a/workspace/innate_skills/arm_utils.py +++ /dev/null @@ -1,76 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -""" -Arm Utils Skill - Torque on, torque off, or reboot the arm servos. -""" - -from typing import Literal - -from brain_client.skills.types import Interface, InterfaceType, Skill, SkillResult - -VALID_COMMANDS = ("torque_on", "torque_off", "reboot_arm") -ArmCommand = Literal["torque_on", "torque_off", "reboot_arm"] - - -class ArmUtils(Skill): - """Utility commands for the arm: enable/disable torque or reboot servos.""" - - manipulation = Interface(InterfaceType.MANIPULATION) - - def __init__(self, logger): - super().__init__(logger) - - @property - def name(self): - return "arm_utils" - - def guidelines(self): - return ( - "Utility skill for low-level arm commands. " - "Requires 'command' parameter: 'torque_on', 'torque_off', or 'reboot_arm'. " - "torque_on enables motor torque so the arm holds position. " - "torque_off disables torque so the arm goes limp (for manual positioning). " - "reboot_arm reboots all Dynamixel servos to clear hardware errors." - ) - - def execute(self, command: ArmCommand): - """ - Execute an arm utility command. - - Args: - command: 'torque_on', 'torque_off', or 'reboot_arm' - """ - if self.manipulation is None: - return "Manipulation interface not available", SkillResult.FAILURE - - command = command.strip().lower() - if command not in VALID_COMMANDS: - return ( - f"Invalid command '{command}'. Must be one of: {', '.join(VALID_COMMANDS)}.", - SkillResult.FAILURE, - ) - - if command == "torque_on": - success = self.manipulation.torque_on() - if success: - return "Arm torque enabled", SkillResult.SUCCESS - return "Failed to enable arm torque", SkillResult.FAILURE - - if command == "torque_off": - success = self.manipulation.torque_off() - if success: - return "Arm torque disabled (arm is limp)", SkillResult.SUCCESS - return "Failed to disable arm torque", SkillResult.FAILURE - - # reboot_arm - success = self.manipulation.reboot_servos() - if success: - return ( - "Arm servos rebooted and reinitialized; torque is disabled. Run torque_on before moving.", - SkillResult.SUCCESS, - ) - return "Failed to reboot arm servos", SkillResult.FAILURE - - def cancel(self): - return "Arm utils cannot be cancelled" diff --git a/workspace/innate_skills/arm_zero_position.py b/workspace/innate_skills/arm_zero_position.py deleted file mode 100644 index 1f43cb310..000000000 --- a/workspace/innate_skills/arm_zero_position.py +++ /dev/null @@ -1,59 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -""" -Arm Zero Position Skill - Move arm to all-zeros joint position. -""" - -import time - -from brain_client.skills.types import Interface, InterfaceType, Skill, SkillResult - - -class ArmZeroPosition(Skill): - """Move the arm to the zero position (all joints at 0 radians).""" - - manipulation = Interface(InterfaceType.MANIPULATION) - - def __init__(self, logger): - super().__init__(logger) - self._cancelled = False - - @property - def name(self): - return "arm_zero_position" - - def guidelines(self): - return "Use this to move the arm to its zero/home position where all joints are at 0 radians." - - def execute(self, duration: int = 3): - """Execute the arm movement to zero position.""" - self._cancelled = False - - if self.manipulation is None: - return "Manipulation interface not available", SkillResult.FAILURE - - self.logger.info(f"Moving arm to zero position [0,0,0,0,0,0] over {duration}s") - - joint_positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - success = self.manipulation.move_to_joint_positions( - joint_positions=joint_positions, duration=duration, blocking=False - ) - - if not success: - return "Failed to send arm command", SkillResult.FAILURE - - # Wait for motion to complete (with cancellation check) - start_time = time.time() - while time.time() - start_time < duration: - if self._cancelled: - return "Arm motion cancelled", SkillResult.CANCELLED - time.sleep(0.1) - - return "Arm moved to zero position", SkillResult.SUCCESS - - def cancel(self): - """Cancel the arm movement.""" - self._cancelled = True - return "Arm motion cancelled" diff --git a/workspace/innate_skills/chess/detect_opponent_move.py b/workspace/innate_skills/chess/detect_opponent_move.py new file mode 100644 index 000000000..1a3c4106b --- /dev/null +++ b/workspace/innate_skills/chess/detect_opponent_move.py @@ -0,0 +1,306 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +import json +import time +from pathlib import Path +from typing import Any, Literal, cast + +import chess +from google import genai +from google.genai import types + +from innate import Head, Image, MainImage, Manipulation, Skill, SkillReturn, WristImage, resource + +GAME_STATE_FILE = Path.home() / "chess_game_state.json" +CALIBRATION_FILE = Path.home() / "board_calibration.json" +DATA_DIR = Path.home() / "innate-os/data/detect_move" + +# Handicap: White starts without the a1 rook (no queenside castling) +HANDICAP_FEN = "rnbqkbnr/pppppppp/8/8/8/8/PPPPPPPP/1NBQKBNR w Kkq - 0 1" +ROBOT_COLORS = ("white", "black") +RobotColor = Literal["white", "black"] + + +def _load_env_file(env_path: Path) -> dict: + env_vars = {} + if env_path.exists(): + for line in env_path.read_text().splitlines(): + line = line.strip() + if line and not line.startswith("#") and "=" in line: + key, value = line.split("=", 1) + env_vars[key.strip()] = value.strip() + return env_vars + + +def _fen_to_ascii(fen: str) -> str: + board = chess.Board(fen) + lines = [] + for rank in range(7, -1, -1): + row = [(p.symbol() if (p := board.piece_at(chess.square(file, rank))) else ".") for file in range(8)] + lines.append(f" {rank + 1} {' '.join(row)}") + lines.append(" a b c d e f g h") + return "\n".join(lines) + + +class DetectOpponentMove(Skill): + """Detect the opponent's last chess move. The robot positions its wrist + camera above the board, captures images from both the main and wrist + cameras, then asks Gemini which legal move was played. The board state + (FEN) is stored in ~/chess_game_state.json. Optionally pass + robot_color='white' or 'black' to set perspective.""" + + manipulation: Manipulation + head: Head | None + # `| None`: detection degrades to whichever camera is live; execute() + # fails only when BOTH are absent. + main_image: MainImage | None + wrist_image: WristImage | None + + OBS_Z = 0.18 + OBS_PITCH = 1.37 + OBS_YAW = 0.0 + FIXED_ROLL = 0.0 + HEAD_TILT_DOWN = -20 + HEAD_TILT_NEUTRAL = 0 + CONFIDENCE_THRESHOLD = 0.7 + + # Board centre X/Y, loaded from calibration at run start; None until calibrated. + obs_x: float | None = None + obs_y: float | None = None + + @resource + def gemini_client(self): + env_vars = _load_env_file(Path(__file__).parents[1] / ".env.scan") + api_key = env_vars.get("GEMINI_API_KEY", "") + if not api_key or api_key == "your_gemini_api_key_here": + self.logger.warning("[DetectOpponentMove] GEMINI_API_KEY not set in .env.scan") + return None + return genai.Client(api_key=api_key) + + def _load_board_center(self): + """Board centre X/Y from calibration corners; None until calibrated.""" + self.obs_x = None + self.obs_y = None + if not CALIBRATION_FILE.exists(): + self.logger.warning("[DetectOpponentMove] No board calibration yet — run board calibration before use") + return + try: + cal = json.loads(CALIBRATION_FILE.read_text()) + corners = [cal[k] for k in ("top_left", "top_right", "bottom_left", "bottom_right")] + self.obs_x = sum(c["x"] for c in corners) / 4.0 + self.obs_y = sum(c["y"] for c in corners) / 4.0 + except Exception as e: + self.logger.warning(f"[DetectOpponentMove] Unreadable board calibration ({e}) — re-run board calibration") + + def _load_game_state(self) -> dict | None: + if not GAME_STATE_FILE.exists(): + return None + try: + return json.loads(GAME_STATE_FILE.read_text()) + except Exception as e: + self.logger.error(f"[DetectOpponentMove] Failed to load game state: {e}") + return None + + def _init_game_state(self, robot_color: str) -> dict: + state = { + "fen": HANDICAP_FEN, + "move_history": [], + "last_detected_move": None, + "turn": "white", + "robot_color": robot_color, + } + GAME_STATE_FILE.write_text(json.dumps(state, indent=2)) + return state + + def _move_to_observation_pose(self) -> bool: + if self.obs_x is None or self.obs_y is None: + self.logger.error("[DetectOpponentMove] No board calibration — cannot position arm") + return False + self.manipulation.open_gripper(100) + self.sleep(0.5) + if self.head: + self.head.set_position(self.HEAD_TILT_DOWN) + self.sleep(0.5) + self.feedback("Moving arm to observation pose...") + success = self.manipulation.move_to_cartesian_pose( + x=self.obs_x, + y=self.obs_y, + z=self.OBS_Z, + roll=self.FIXED_ROLL, + pitch=self.OBS_PITCH, + yaw=self.OBS_YAW, + duration=2.0, + blocking=True, + ) + if success: + self.sleep(0.5) # let camera auto-exposure settle + return success + + def _go_to_safe_pose(self): + self.manipulation.move_to_cartesian_pose( + x=0.15, + y=0.1, + z=0.1, + roll=self.FIXED_ROLL, + pitch=self.OBS_PITCH, + yaw=self.OBS_YAW, + duration=2.0, + blocking=True, + ) + if self.head: + self.head.set_position(self.HEAD_TILT_NEUTRAL) + + def _capture_images(self) -> tuple[MainImage | None, WristImage | None]: + self.sleep(0.3) + main_b64 = self.main_image + wrist_b64 = self.wrist_image + self._save_debug("capture", None, main_b64, wrist_b64) + return main_b64, wrist_b64 + + def _build_board_context(self, fen: str) -> tuple: + board = chess.Board(fen) + legal_moves = [m.uci() for m in board.legal_moves] + turn = "White" if board.turn == chess.WHITE else "Black" + context = ( + f"CURRENT BOARD STATE (FEN): {fen}\n\n" + f"ASCII board:\n{_fen_to_ascii(fen)}\n\n" + f"It is {turn}'s turn.\n" + f"Legal moves ({len(legal_moves)}): {', '.join(legal_moves)}\n" + ) + return context, legal_moves, board + + def _save_debug(self, label: str, text: str | None, main_b64: Image | None, wrist_b64: Image | None) -> None: + try: + DATA_DIR.mkdir(parents=True, exist_ok=True) + ts = int(time.time()) + if text: + (DATA_DIR / f"{label}_{ts}.txt").write_text(text) + if main_b64: + (DATA_DIR / f"{label}_main_{ts}.jpg").write_bytes(main_b64.jpeg) + if wrist_b64: + (DATA_DIR / f"{label}_wrist_{ts}.jpg").write_bytes(wrist_b64.jpeg) + except Exception as e: + self.logger.warning(f"[DetectOpponentMove] Failed to save {label} debug files: {e}") + + def _ask_gemini(self, label: str, model: str, thinking_budget: int, prompt: str, wrist_b64) -> dict | None: + """Ask for {move_uci, confidence, reasoning}; None on failure.""" + client = self.gemini_client + if client is None: # execute() fails before this can happen; keeps the deref safe + return None + contents: list[Any] = [prompt] + if wrist_b64: + contents.append(types.Part.from_bytes(data=wrist_b64.jpeg, mime_type="image/jpeg")) + self._save_debug(f"{label}_prompt", prompt, None, wrist_b64) + try: + response = client.models.generate_content( + model=model, + contents=contents, + config=types.GenerateContentConfig( + response_mime_type="application/json", + thinking_config=types.ThinkingConfig(thinking_budget=thinking_budget), + ), + ) + result = json.loads((response.text or "").strip()) + self.logger.info(f"[DetectOpponentMove] {label} result: {result}") + return result + except Exception as e: + self.logger.error(f"[DetectOpponentMove] Gemini {label} failed: {e}") + return None + + def _detect_prompt(self, board_context: str) -> str: + return ( + "You are analyzing a physical chess board to detect the opponent's last move.\n\n" + f"{board_context}\n" + "I am providing a close-up overhead wrist camera image of the board.\n\n" + "Compare the CURRENT FEN state (the state BEFORE the opponent moved) " + "with what you see in the images (the state AFTER the opponent moved).\n" + "Identify which piece moved and where it went.\n\n" + "IMPORTANT: The move MUST be one of the legal moves listed above.\n\n" + "Return ONLY JSON:\n" + '{"move_uci": "", "confidence": 0.0-1.0, ' + '"reasoning": "brief explanation"}' + ) + + def _confirm_prompt(self, board_context: str, candidates: list) -> str: + return ( + "You are analyzing a physical chess board to confirm which move was played.\n\n" + f"{board_context}\n" + f"The most likely moves are: {', '.join(candidates)}\n\n" + "I am providing a close-up overhead wrist camera image of the board.\n" + "Look carefully at which squares changed and which piece is now where.\n\n" + "Return ONLY JSON:\n" + '{"move_uci": "", "confidence": 0.0-1.0, ' + '"reasoning": "brief explanation"}' + ) + + def execute(self, robot_color: RobotColor = "white") -> SkillReturn: + robot_color = cast(RobotColor, robot_color.strip().lower()) + if robot_color not in ROBOT_COLORS: + self.fail(f"Invalid robot_color '{robot_color}'. Must be 'white' or 'black'.") + if self.gemini_client is None: + self.fail("Gemini not configured (check .env.scan)") + self._load_board_center() + + state = self._load_game_state() + if state is None: + self.feedback("No game state found — initialising new game.") + state = self._init_game_state(robot_color) + fen = state["fen"] + self.feedback(f"Current board state loaded. Turn: {state.get('turn', '?')}") + + if not self._move_to_observation_pose(): + self.fail("Failed to move arm to observation pose") + try: + return self._detect(fen) + finally: + self._go_to_safe_pose() + + def _detect(self, fen: str) -> str: + self.feedback("Capturing images...") + main_b64, wrist_b64 = self._capture_images() + if not main_b64 and not wrist_b64: + self.fail("No camera images available") + + board_context, legal_moves, board = self._build_board_context(fen) + if not legal_moves: + return "No legal moves — game may be over" + + self.feedback(f"Asking Gemini to identify the move ({len(legal_moves)} legal moves)...") + result = self._ask_gemini( + "stage1", "gemini-3.1-pro-preview", 1024, self._detect_prompt(board_context), wrist_b64 + ) + if result is None: + self.fail("Gemini failed to analyse the board") + + move_uci = result.get("move_uci", "") + confidence = float(result.get("confidence", 0.0)) + reasoning = result.get("reasoning", "") + + if confidence < self.CONFIDENCE_THRESHOLD and move_uci in legal_moves: + self.feedback(f"Low confidence ({confidence:.0%}) on {move_uci}. Running confirmation...") + candidates = [move_uci] + candidates += [ + m for m in legal_moves if m != move_uci and (m[:2] == move_uci[:2] or m[2:4] == move_uci[2:4]) + ] + candidates += [m for m in legal_moves if m not in candidates] + result2 = self._ask_gemini( + "stage2", "gemini-3-flash-preview", 512, self._confirm_prompt(board_context, candidates[:8]), wrist_b64 + ) + if result2: + move2 = result2.get("move_uci", "") + conf2 = float(result2.get("confidence", 0.0)) + if conf2 > confidence and move2 in legal_moves: + move_uci, confidence = move2, conf2 + reasoning = result2.get("reasoning", reasoning) + + if move_uci not in legal_moves: + self.fail(f"Detected move {move_uci} is not legal. Legal moves: {legal_moves}") + + san = board.san(chess.Move.from_uci(move_uci)) + msg = ( + f"Detected opponent move: {san} ({move_uci}). " + f"Confidence: {confidence:.0%}. {reasoning}. " + f'Call update_chess_state(move_uci="{move_uci}") to apply it.' + ) + self.feedback(msg) + return msg diff --git a/workspace/innate_skills/chess/pick_up_piece_simple.py b/workspace/innate_skills/chess/pick_up_piece_simple.py new file mode 100644 index 000000000..ea4d8c562 --- /dev/null +++ b/workspace/innate_skills/chess/pick_up_piece_simple.py @@ -0,0 +1,397 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +"""Pick up and place chess pieces using only calibration data and arm +orientation, without Gemini vision or base driving. + +Orientation strategy by rank/column: + Ranks 4-6: pitch straight down, yaw centered + Ranks 7-8: pitch tilted, yaw centered + Ranks 1-3: pitch straight down, yaw left (cols A-D) or right (cols E-H) + +Ranks 5-6 can reach ranks 7-8 directly with tilted orientation; any other +move across the rank 6/7 boundary relays through rank 5. +""" + +import json +from pathlib import Path +from typing import Any, Literal + +from innate import Manipulation, Skill, SkillReturn + +CALIBRATION_FILE = Path.home() / "board_calibration.json" +PieceType = Literal["king", "queen", "rook", "bishop", "knight", "pawn"] + + +class PickUpPieceSimple(Skill): + """Pick up a piece from one square and place it on another without using + Gemini vision or base driving. Uses arm orientation changes to reach all + ranks. Parameters: square (source, e.g. 'E2'), place_square (target, + e.g. 'E4'), piece (str, e.g. 'pawn'), is_capture (bool, True if capturing + an opponent piece), speed (float).""" + + manipulation: Manipulation + + FIXED_ROLL = 0.0 + PITCH_DOWN = 1.45 # nearly straight down (compensates for slight arm slouch) + PITCH_TILTED = 1.57 - 0.48 # tilted for far ranks (7-8) + YAW_CENTER = 0.0 + YAW_LEFT = 1.57 + YAW_RIGHT = -1.57 + + HEIGHT_SAFE_CLEARANCE = 0.25 # minimum z before any lateral move + HEIGHT_SAFE = 0.15 + HEIGHT_PICK_TALL = 0.06 # king, queen + HEIGHT_PICK_SHORT = 0.04 + + GRIPPER_OPEN_PERCENT = 50 + GRIPPER_CLOSE_STRENGTH = 0.4 + GRIPPER_MIN_WAIT = 1.0 + + VERTICAL_STEPS = 4 + + # Phase speed multipliers (> 1.0 = faster, on top of speed) + PHASE_AIR = 1.5 + PHASE_LIFT = 1.3 + PHASE_DESCENT_FAR = 1.2 + PHASE_DESCENT_NEAR = 0.6 + + TILTED_SPEED_FACTOR = 0.75 + HEIGHT_SAFE_TILTED = 0.18 + + RELAY_RANK = 5 + + # Discard zone: right of column H for captured pieces + DISCARD_SQUARES_RIGHT = 2 + DISCARD_RANK = 4.5 + + TALL_PIECES = {"king", "queen"} + + def _load_calibration(self): + if not CALIBRATION_FILE.exists(): + return None + try: + return json.loads(CALIBRATION_FILE.read_text()) + except Exception: + return None + + def _square_to_position(self, square, calibration): + """Chess notation (e.g. 'E4') -> (x, y, z) by bilinear interpolation + of the four calibrated corners.""" + if len(square) != 2: + return None + file_char, rank_char = square[0].upper(), square[1] + if file_char not in "ABCDEFGH" or rank_char not in "12345678": + return None + u = (ord(file_char) - ord("A")) / 7.0 + v = (int(rank_char) - 1) / 7.0 + + tl, tr = calibration.get("top_left"), calibration.get("top_right") + bl, br = calibration.get("bottom_left"), calibration.get("bottom_right") + if not all([tl, tr, bl, br]): + return None + + def lerp(axis): + return ( + (1 - u) * (1 - v) * bl.get(axis, 0) + + u * (1 - v) * br.get(axis, 0) + + (1 - u) * v * tl.get(axis, 0) + + u * v * tr.get(axis, 0) + ) + + return lerp("x"), lerp("y"), lerp("z") + + def _orientation_for_square(self, square): + rank = int(square[1]) + col = square[0].upper() + if rank >= 7: + return self.PITCH_TILTED, self.YAW_CENTER + if rank >= 4: + return self.PITCH_DOWN, self.YAW_CENTER + return self.PITCH_DOWN, (self.YAW_LEFT if col in "ABCD" else self.YAW_RIGHT) + + def _d(self, seconds: float) -> float: + return seconds / self._speed + + def _gripper_wait(self, seconds: float): + self.sleep(max(seconds / self._speed, self.GRIPPER_MIN_WAIT)) + + def _move_arm(self, x, y, z, pitch, yaw, duration, gripper_position=None) -> bool: + kwargs: dict[str, Any] = dict( + x=x, y=y, z=z, roll=self.FIXED_ROLL, pitch=pitch, yaw=yaw, duration=self._d(duration), blocking=True + ) + if gripper_position is not None: + kwargs["gripper_position"] = gripper_position + return self.manipulation.move_to_cartesian_pose(**kwargs) + + def _vertical_move(self, x, y, from_z, to_z, pitch, yaw, gripper_position=None, caution=1.0): + """Move vertically in VERTICAL_STEPS increments with fixed X, Y. + Descent is fast at the top and slow near the board; lift is uniformly + fast. Tries the smooth trajectory service, falls back to step moves.""" + descending = to_z < from_z + direction = "descending" if descending else "lifting" + n = self.VERTICAL_STEPS + + seg_durs = [] + for i in range(n): + if descending: + frac = (i + 0.5) / n + phase = self.PHASE_DESCENT_FAR + (self.PHASE_DESCENT_NEAR - self.PHASE_DESCENT_FAR) * frac + else: + phase = self.PHASE_LIFT + seg_durs.append(1.0 / (self._speed * phase * caution)) + + poses = [ + dict(x=x, y=y, z=from_z + (to_z - from_z) * i / n, roll=self.FIXED_ROLL, pitch=pitch, yaw=yaw) + for i in range(n + 1) + ] + try: + if self.manipulation.move_cartesian_trajectory( + poses, segment_durations=seg_durs, gripper_position=gripper_position + ): + return + self.logger.warning("[PickUpPieceSimple] Trajectory service failed, falling back to step-by-step") + except Exception as e: + self.logger.warning(f"[PickUpPieceSimple] Trajectory not available ({e}), falling back") + + for i in range(1, n + 1): + z = from_z + (to_z - from_z) * i / n + kwargs: dict[str, Any] = dict( + x=x, y=y, z=z, roll=self.FIXED_ROLL, pitch=pitch, yaw=yaw, duration=seg_durs[i - 1] + ) + if gripper_position is not None: + kwargs["gripper_position"] = gripper_position + if not self.manipulation.move_to_cartesian_pose(**kwargs): + self.fail(f"Failed at {direction} step {i}/{n} z={z:.3f}m") + self.sleep(seg_durs[i - 1]) + + def _go_to_safe_pose(self): + """Lift straight up first (no lateral move over the board), then fold + to the resting safe pose.""" + ee = self.manipulation.get_current_end_effector_pose() + if ee is not None and ee["position"]["z"] < self.HEIGHT_SAFE_CLEARANCE: + rpy = self.manipulation.get_current_orientation_rpy() + self._move_arm( + ee["position"]["x"], + ee["position"]["y"], + self.HEIGHT_SAFE_CLEARANCE, + rpy["pitch"] if rpy else 0.0, + rpy["yaw"] if rpy else 0.0, + 2.0, + ) + self._move_arm(0.05, 0.08, 0.3, 0.0, 1.57, 2.0) + return self._move_arm(0.05, 0.08, 0.11, 0.0, 1.57, 1.0) + + def _needs_relay(self, src_square, dst_square): + src_rank, dst_rank = int(src_square[1]), int(dst_square[1]) + if src_rank in (5, 6) and dst_rank >= 7: + return False + if dst_rank in (5, 6) and src_rank >= 7: + return False + return (src_rank <= 6) != (dst_rank <= 6) + + def _discard_position(self, calibration): + tl, tr = calibration.get("top_left"), calibration.get("top_right") + bl, br = calibration.get("bottom_left"), calibration.get("bottom_right") + if not all([tl, tr, bl, br]): + return None + sq_y = ((bl["y"] - br["y"]) + (tl["y"] - tr["y"])) / 2.0 / 7.0 + v = (self.DISCARD_RANK - 1) / 7.0 + x = (1 - v) * br["x"] + v * tr["x"] + y = (1 - v) * br["y"] + v * tr["y"] - sq_y * self.DISCARD_SQUARES_RIGHT + z = (1 - v) * br.get("z", 0) + v * tr.get("z", 0) + return x, y, z + + def _relay_position(self, src_square, dst_square, calibration): + """A relay square on rank 5, on the file of whichever square is in + ranks 1-6 so the relay stays on the reachable side.""" + file_char = (src_square if int(src_square[1]) <= 6 else dst_square)[0].upper() + relay_square = f"{file_char}{self.RELAY_RANK}" + return relay_square, self._square_to_position(relay_square, calibration) + + def _do_pick_place( + self, + src_x, + src_y, + dst_x, + dst_y, + pick_height, + src_pitch, + src_yaw, + dst_pitch, + dst_yaw, + src_label, + dst_label, + safe_height=None, + caution=1.0, + ): + """One pick-and-place cycle: pick from src, place at dst. Raises + SkillFailed on any arm failure. Every trajectory command carries an + explicit gripper target to avoid stale _arm_state reads.""" + if safe_height is None: + safe_height = self.HEIGHT_SAFE + open_grip = self.manipulation.GRIPPER_CLOSED + ( + self.manipulation.GRIPPER_OPEN - self.manipulation.GRIPPER_CLOSED + ) * (self.GRIPPER_OPEN_PERCENT / 100.0) + closed_grip = self.manipulation.GRIPPER_CLOSED - self.GRIPPER_CLOSE_STRENGTH + air_dur = 2.0 / (self.PHASE_AIR * caution) + + self.feedback(f"Moving above {src_label}...") + if not self._move_arm(src_x, src_y, safe_height, src_pitch, src_yaw, air_dur): + self.fail(f"Failed to move above {src_label}") + + self.feedback("Opening gripper...") + self.manipulation.open_gripper(self.GRIPPER_OPEN_PERCENT) + self._gripper_wait(1.5) + + self.feedback(f"Descending to pick from {src_label}...") + self._vertical_move(src_x, src_y, safe_height, pick_height, src_pitch, src_yaw, open_grip, caution) + + self.feedback("Grabbing piece...") + self.manipulation.close_gripper(strength=self.GRIPPER_CLOSE_STRENGTH, blocking=True) + self._gripper_wait(2.0) + + self.feedback("Lifting piece...") + self._vertical_move(src_x, src_y, pick_height, safe_height, src_pitch, src_yaw, closed_grip, caution) + + self.feedback(f"Moving above {dst_label}...") + if not self._move_arm(dst_x, dst_y, safe_height, dst_pitch, dst_yaw, air_dur, gripper_position=closed_grip): + self.fail(f"Failed to move above {dst_label}") + + self.feedback(f"Descending to place on {dst_label}...") + self._vertical_move(dst_x, dst_y, safe_height, pick_height, dst_pitch, dst_yaw, closed_grip, caution) + + self.feedback("Releasing piece...") + self.manipulation.open_gripper(self.GRIPPER_OPEN_PERCENT) + self._gripper_wait(1.5) + + self.feedback("Lifting after place...") + self._vertical_move(dst_x, dst_y, pick_height, safe_height, dst_pitch, dst_yaw, open_grip, caution) + + def execute( + self, + square: str, + place_square: str, + piece: PieceType = "pawn", + is_capture: bool = False, + speed: float = 1.0, + ) -> SkillReturn: + self._speed = max(0.1, min(speed, 3.0)) + self._go_to_safe_pose() + + calibration = self._load_calibration() + if calibration is None: + self.fail("No calibration data found. Run board calibration first.") + src_pos = self._square_to_position(square, calibration) + if src_pos is None: + self.fail(f"Invalid source square '{square}'") + dst_pos = self._square_to_position(place_square, calibration) + if dst_pos is None: + self.fail(f"Invalid target square '{place_square}'") + + try: + self._move_piece(square, place_square, piece, is_capture, calibration, src_pos, dst_pos) + finally: + self.feedback("Returning to safe pose...") + if not self._go_to_safe_pose(): + self.logger.warning("[PickUpPieceSimple] Failed to reach safe pose after move") + + msg = f"Moved piece from {square} to {place_square}" + self.feedback(msg) + return msg + + def _move_piece(self, square, place_square, piece, is_capture, calibration, src_pos, dst_pos): + src_x, src_y, src_board_z = src_pos + dst_x, dst_y, dst_board_z = dst_pos + base_pick_height = ( + self.HEIGHT_PICK_TALL if piece.strip().lower() in self.TALL_PIECES else self.HEIGHT_PICK_SHORT + ) + src_pitch, src_yaw = self._orientation_for_square(square) + dst_pitch, dst_yaw = self._orientation_for_square(place_square) + src_rank, dst_rank = int(square[1]), int(place_square[1]) + + if is_capture: + discard_pos = self._discard_position(calibration) + if discard_pos is None: + self.fail("Failed to compute discard position") + disc_x, disc_y, _disc_z = discard_pos + cap_tilted = dst_rank >= 7 + self.feedback(f"Capturing: removing piece from {place_square}...") + self._do_pick_place( + dst_x, + dst_y, + disc_x, + disc_y, + self.HEIGHT_PICK_SHORT + dst_board_z, + dst_pitch, + dst_yaw, + self.PITCH_DOWN, + self.YAW_CENTER, + place_square, + "discard", + safe_height=self.HEIGHT_SAFE_TILTED if cap_tilted else self.HEIGHT_SAFE, + caution=self.TILTED_SPEED_FACTOR if cap_tilted else 1.0, + ) + + if self._needs_relay(square, place_square): + relay_sq, relay_pos = self._relay_position(square, place_square, calibration) + if relay_pos is None: + self.fail("Failed to compute relay position") + relay_x, relay_y, relay_board_z = relay_pos + + leg1_tilted = src_rank >= 7 + self.feedback(f"Relay leg 1: {square} -> {relay_sq}") + self._do_pick_place( + src_x, + src_y, + relay_x, + relay_y, + base_pick_height + src_board_z, + src_pitch, + src_yaw, + src_pitch, + src_yaw, + square, + f"relay {relay_sq}", + safe_height=self.HEIGHT_SAFE_TILTED if leg1_tilted else None, + caution=self.TILTED_SPEED_FACTOR if leg1_tilted else 1.0, + ) + + leg2_tilted = dst_rank >= 7 + self.feedback(f"Relay leg 2: {relay_sq} -> {place_square}") + self._do_pick_place( + relay_x, + relay_y, + dst_x, + dst_y, + base_pick_height + relay_board_z, + dst_pitch, + dst_yaw, + dst_pitch, + dst_yaw, + f"relay {relay_sq}", + place_square, + safe_height=self.HEIGHT_SAFE_TILTED if leg2_tilted else None, + caution=self.TILTED_SPEED_FACTOR if leg2_tilted else 1.0, + ) + return + + cross_56_78 = (src_rank in (5, 6) and dst_rank >= 7) or (dst_rank in (5, 6) and src_rank >= 7) + if cross_56_78: + src_pitch, src_yaw = self.PITCH_TILTED, self.YAW_CENTER + dst_pitch, dst_yaw = self.PITCH_TILTED, self.YAW_CENTER + any_tilted = src_rank >= 7 or dst_rank >= 7 or cross_56_78 + self._do_pick_place( + src_x, + src_y, + dst_x, + dst_y, + base_pick_height + src_board_z, + src_pitch, + src_yaw, + dst_pitch, + dst_yaw, + square, + place_square, + safe_height=self.HEIGHT_SAFE_TILTED if any_tilted else None, + caution=self.TILTED_SPEED_FACTOR if any_tilted else 1.0, + ) diff --git a/workspace/innate_skills/chess/recalibrate_manual.py b/workspace/innate_skills/chess/recalibrate_manual.py new file mode 100644 index 000000000..7f2e22314 --- /dev/null +++ b/workspace/innate_skills/chess/recalibrate_manual.py @@ -0,0 +1,105 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +import json +import math +from datetime import datetime +from pathlib import Path +from typing import Literal, cast + +from innate import Manipulation, Skill, SkillReturn, WristImage + +CALIBRATION_FILE = Path.home() / "board_calibration.json" +CAPTURES_DIR = Path.home() / "innate-os/captures/corners" +CalibrationCorner = Literal["A8", "H8"] + + +class RecalibrateManual(Skill): + """Manually recalibrate one top corner of the chessboard. The human + positions the arm above the center of A8 or H8, then this skill records + the position and recomputes the full board calibration using square + geometry. Requires 'corner' parameter: 'A8' or 'H8'.""" + + manipulation: Manipulation + image: WristImage | None # debug snapshot only; missing frame must not abort + + def _load_calibration(self): + if not CALIBRATION_FILE.exists(): + return {} + try: + return json.loads(CALIBRATION_FILE.read_text()) + except Exception: + return {} + + def _recompute_from_top_corners(self, new_a8, new_h8, z): + """The board is a square: the bottom edge is the top edge (A8->H8) + rotated 90 deg clockwise, toward the robot.""" + side_x = new_h8[0] - new_a8[0] + side_y = new_h8[1] - new_a8[1] + down_x, down_y = side_y, -side_x + return { + "top_left": {"x": new_a8[0], "y": new_a8[1], "z": z}, + "top_right": {"x": new_h8[0], "y": new_h8[1], "z": z}, + "bottom_left": {"x": new_a8[0] + down_x, "y": new_a8[1] + down_y, "z": z}, + "bottom_right": {"x": new_h8[0] + down_x, "y": new_h8[1] + down_y, "z": z}, + } + + def _save_corner_image(self, corner: str): + if not self.image: + self.logger.warning("[RecalibrateManual] No wrist camera image available to save") + return + try: + CAPTURES_DIR.mkdir(parents=True, exist_ok=True) + ts = datetime.now().strftime("%Y%m%d_%H%M%S") + (CAPTURES_DIR / f"manual_{corner}_{ts}.jpg").write_bytes(self.image.jpeg) + except Exception as e: + self.logger.warning(f"[RecalibrateManual] Failed to save corner image: {e}") + + def _save_calibration(self, calibration): + try: + CALIBRATION_FILE.write_text(json.dumps(calibration, indent=2)) + except Exception as e: + self.fail(f"Failed to save calibration: {e}") + + def execute(self, corner: CalibrationCorner) -> SkillReturn: + corner = cast(CalibrationCorner, corner.upper().strip()) + if corner not in ("A8", "H8"): + self.fail(f"Invalid corner '{corner}'. Must be 'A8' or 'H8'.") + + this_key, other_key, other_name = ( + ("top_left", "top_right", "H8") if corner == "A8" else ("top_right", "top_left", "A8") + ) + calibration = self._load_calibration() + + fk_pose = self.manipulation.get_current_end_effector_pose() + if not fk_pose: + self.fail("Could not get current arm position") + pos = fk_pose["position"] + position_str = f"X={pos['x']:.4f}, Y={pos['y']:.4f}, Z={pos['z']:.4f}" + self.feedback(f"Recording {corner} at {position_str}") + self._save_corner_image(corner) + + if other_key not in calibration: + calibration[this_key] = {"x": pos["x"], "y": pos["y"], "z": pos["z"]} + self._save_calibration(calibration) + msg = ( + f"Recorded {corner} at {position_str}. " + f"Other corner {other_name} not yet recorded — record it to compute full board." + ) + self.feedback(msg) + return msg + + other = calibration[other_key] + new_pos = (pos["x"], pos["y"]) + other_pos = (other["x"], other["y"]) + new_a8, new_h8 = (new_pos, other_pos) if corner == "A8" else (other_pos, new_pos) + cal_z = calibration.get("top_right", calibration.get("top_left", {})).get("z", pos["z"]) + self._save_calibration(self._recompute_from_top_corners(new_a8, new_h8, cal_z)) + + side_len = math.hypot(new_h8[0] - new_a8[0], new_h8[1] - new_a8[1]) + msg = ( + f"Recorded {corner} at {position_str}. " + f"Recomputed full board from {corner} (new) + {other_name} (fixed). " + f"Board side={side_len * 100:.1f}cm." + ) + self.feedback(msg) + return msg diff --git a/workspace/innate_skills/chess/record_position.py b/workspace/innate_skills/chess/record_position.py new file mode 100644 index 000000000..0f6438fe2 --- /dev/null +++ b/workspace/innate_skills/chess/record_position.py @@ -0,0 +1,58 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +import json +from datetime import datetime +from pathlib import Path +from typing import Literal, cast + +from innate import Manipulation, Skill, SkillReturn, WristImage + +CALIBRATION_FILE = Path.home() / "board_calibration.json" +CORNER_CAPTURES_DIR = Path("/home/jetson1/innate-os/captures/corners") +BoardCorner = Literal["top_left", "top_right", "bottom_right", "bottom_left"] +VALID_CORNERS = ("top_left", "top_right", "bottom_right", "bottom_left") + + +class RecordPosition(Skill): + """Record the current arm position for a board corner. Requires 'corner' + parameter: 'top_left', 'top_right', 'bottom_right', or 'bottom_left'. + Saves to calibration file and returns coordinates.""" + + manipulation: Manipulation + image: WristImage | None # debug snapshot only; missing frame must not abort + + def execute(self, corner: BoardCorner) -> SkillReturn: + corner = cast(BoardCorner, corner.lower().replace("-", "_").replace(" ", "_")) + if corner not in VALID_CORNERS: + self.fail(f"Invalid corner '{corner}'. Must be one of: {VALID_CORNERS}") + + fk_pose = self.manipulation.get_current_end_effector_pose() + if not fk_pose: + self.fail("Could not get current position") + pos = fk_pose["position"] + + calibration = {} + if CALIBRATION_FILE.exists(): + try: + calibration = json.loads(CALIBRATION_FILE.read_text()) + except Exception: + calibration = {} + calibration[corner] = {"x": pos["x"], "y": pos["y"], "z": pos["z"]} + CALIBRATION_FILE.write_text(json.dumps(calibration, indent=2)) + + self._save_corner_image(corner) + + position_str = f"X={pos['x']:.4f}, Y={pos['y']:.4f}, Z={pos['z']:.4f}" + self.feedback(f"RECORDED {corner.upper()}: {position_str}") + return f"{corner} recorded: {position_str}" + + def _save_corner_image(self, corner: str): + if not self.image: + self.logger.warning("No wrist camera image available to save") + return + try: + CORNER_CAPTURES_DIR.mkdir(parents=True, exist_ok=True) + ts = datetime.now().strftime("%Y%m%d_%H%M%S") + (CORNER_CAPTURES_DIR / f"corner_{corner}_{ts}.jpg").write_bytes(self.image.jpeg) + except Exception as e: + self.logger.warning(f"Failed to save corner image: {e}") diff --git a/workspace/innate_skills/chess/reset_chess_game.py b/workspace/innate_skills/chess/reset_chess_game.py new file mode 100644 index 000000000..6150cece4 --- /dev/null +++ b/workspace/innate_skills/chess/reset_chess_game.py @@ -0,0 +1,56 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +import json +from pathlib import Path +from typing import Literal, cast + +from innate import Skill, SkillReturn + +GAME_STATE_FILE = Path.home() / "chess_game_state.json" +CALIBRATION_FILE = Path.home() / "board_calibration.json" +REQUIRED_CORNERS = ("top_left", "top_right", "bottom_right", "bottom_left") +ROBOT_COLORS = ("white", "black") +RobotColor = Literal["white", "black"] + +# Handicap: White starts without the a1 rook (no queenside castling) +HANDICAP_FEN = "rnbqkbnr/pppppppp/8/8/8/8/PPPPPPPP/1NBQKBNR w Kkq - 0 1" + + +class ResetChessGame(Skill): + """Reset the chess game to the starting position. Requires board + calibration to be present first. Clears the move history and sets the + board to the initial FEN. Optionally pass robot_color ('white' or 'black') + to set which side the robot plays.""" + + def _is_calibrated(self) -> bool: + try: + calibration = json.loads(CALIBRATION_FILE.read_text()) + for corner in REQUIRED_CORNERS: + for axis in "xyz": + float(calibration[corner][axis]) + return True + except Exception: + return False + + def execute(self, robot_color: RobotColor = "white") -> SkillReturn: + robot_color = cast(RobotColor, robot_color.strip().lower()) + if robot_color not in ROBOT_COLORS: + self.fail(f"Invalid robot_color '{robot_color}'. Must be 'white' or 'black'.") + if not self._is_calibrated(): + self.fail("Board is not calibrated. Run board calibration first, then reset the chess game.") + + state = { + "fen": HANDICAP_FEN, + "move_history": [], + "last_detected_move": None, + "turn": "white", + "robot_color": robot_color, + } + try: + GAME_STATE_FILE.write_text(json.dumps(state, indent=2)) + except Exception as e: + self.fail(f"Failed to write game state: {e}") + + msg = f"Game reset to starting position. Robot plays {robot_color}." + self.feedback(msg) + return msg diff --git a/workspace/innate_skills/chess/update_chess_state.py b/workspace/innate_skills/chess/update_chess_state.py new file mode 100644 index 000000000..4a64a5e3f --- /dev/null +++ b/workspace/innate_skills/chess/update_chess_state.py @@ -0,0 +1,63 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +import json +from pathlib import Path + +import chess + +from innate import Skill, SkillReturn + +GAME_STATE_FILE = Path.home() / "chess_game_state.json" + + +class UpdateChessState(Skill): + """Apply a chess move (UCI notation, e.g. 'e2e4') to the game state. + Validates the move is legal, updates the FEN and move history in + ~/chess_game_state.json. Call this after every move — both the robot's + own moves and detected opponent moves.""" + + def execute(self, move_uci: str) -> SkillReturn: + move_uci = move_uci.strip().lower() + + if not GAME_STATE_FILE.exists(): + self.fail("No game state found. Call reset_chess_game first.") + try: + state = json.loads(GAME_STATE_FILE.read_text()) + except Exception as e: + self.fail(f"Failed to load game state: {e}") + + board = chess.Board(state.get("fen", chess.STARTING_FEN)) + try: + move = chess.Move.from_uci(move_uci) + except ValueError: + self.fail(f"Invalid UCI notation: '{move_uci}'") + if move not in board.legal_moves: + self.fail(f"Move {move_uci} is not legal. Legal moves: {[m.uci() for m in board.legal_moves]}") + + san = board.san(move) + board.push(move) + turn = "white" if board.turn == chess.WHITE else "black" + new_state = { + "fen": board.fen(), + "move_history": [*state.get("move_history", []), move_uci], + "last_move": move_uci, + "turn": turn, + "robot_color": state.get("robot_color", "white"), + } + try: + GAME_STATE_FILE.write_text(json.dumps(new_state, indent=2)) + except Exception as e: + self.fail(f"Failed to save game state: {e}") + + status = "" + if board.is_checkmate(): + winner = "Black" if board.turn == chess.WHITE else "White" + status = f" Checkmate — {winner} wins!" + elif board.is_stalemate(): + status = " Stalemate — draw!" + elif board.is_check(): + status = " Check!" + + msg = f"Applied {san} ({move_uci}). Turn: {turn}. FEN: {board.fen()}{status}" + self.feedback(msg) + return msg diff --git a/workspace/innate_skills/detect_opponent_move.py b/workspace/innate_skills/detect_opponent_move.py deleted file mode 100644 index 11ec731b1..000000000 --- a/workspace/innate_skills/detect_opponent_move.py +++ /dev/null @@ -1,498 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -""" -Skill that detects the opponent's last chess move by comparing the known -board state (FEN) against what the cameras currently see. - -Uses two images (main camera wide view + wrist camera overhead close-up) -and asks Gemini to identify which legal move was played. The board state -is persisted in ~/chess_game_state.json. -""" - -import base64 -import json -import time -from pathlib import Path -from typing import Literal - -import chess -from google import genai -from google.genai import types - -from brain_client.skills.types import ( - Interface, - InterfaceType, - RobotState, - RobotStateType, - Skill, - SkillResult, -) - -# ── Paths ───────────────────────────────────────────────────────────── -GAME_STATE_FILE = Path.home() / "chess_game_state.json" - -# Handicap: White starts without the a1 rook (no queenside castling) -HANDICAP_FEN = "rnbqkbnr/pppppppp/8/8/8/8/PPPPPPPP/1NBQKBNR w Kkq - 0 1" -CALIBRATION_FILE = Path.home() / "board_calibration.json" -DATA_DIR = Path.home() / "innate-os/data/detect_move" -ROBOT_COLORS = ("white", "black") -RobotColor = Literal["white", "black"] - - -def _load_env_file(env_path: Path) -> dict: - env_vars = {} - if env_path.exists(): - with open(env_path) as f: - for line in f: - line = line.strip() - if line and not line.startswith("#") and "=" in line: - key, value = line.split("=", 1) - env_vars[key.strip()] = value.strip() - return env_vars - - -# ── ASCII board renderer ────────────────────────────────────────────── - - -def _fen_to_ascii(fen: str) -> str: - """Render a FEN string as a labelled ASCII board (rank 8 at top).""" - board = chess.Board(fen) - lines = [] - for rank in range(7, -1, -1): # 8 down to 1 - row = [] - for file in range(8): # a to h - piece = board.piece_at(chess.square(file, rank)) - row.append(piece.symbol() if piece else ".") - lines.append(f" {rank + 1} {' '.join(row)}") - lines.append(" a b c d e f g h") - return "\n".join(lines) - - -class DetectOpponentMove(Skill): - """Detect the opponent's last move using Gemini vision + FEN state.""" - - manipulation = Interface(InterfaceType.MANIPULATION) - head = Interface(InterfaceType.HEAD) - main_image = RobotState(RobotStateType.LAST_MAIN_CAMERA_IMAGE_B64) - wrist_image = RobotState(RobotStateType.LAST_WRIST_CAMERA_IMAGE_B64) - - # Observation pose: X/Y come from board calibration; Z and angles are fixed - OBS_Z = 0.18 - OBS_PITCH = 1.37 # straight down - OBS_YAW = 0.0 - FIXED_ROLL = 0.0 - - # Head tilt for looking at the board (degrees, negative = down) - HEAD_TILT_DOWN = -20 - HEAD_TILT_NEUTRAL = 0 - - # Confidence threshold: below this triggers a second Gemini call - CONFIDENCE_THRESHOLD = 0.7 - - def __init__(self, logger): - super().__init__(logger) - self._cancelled = False - self._init_gemini() - self._load_board_center() - - def _init_gemini(self): - env_vars = _load_env_file(Path(__file__).parent / ".env.scan") - api_key = env_vars.get("GEMINI_API_KEY", "") - self.gemini_client = None - if api_key and api_key != "your_gemini_api_key_here": - self.gemini_client = genai.Client(api_key=api_key) - self.logger.info("[DetectOpponentMove] Gemini configured") - else: - self.logger.warning("[DetectOpponentMove] GEMINI_API_KEY not set in .env.scan") - - def _load_board_center(self): - """Compute board centre X/Y from calibration corners. - - Loaded at skill discovery (boot), so an absent or partial calibration is - not an error here — just "not calibrated yet". execute() errors when the - pose is actually needed. - """ - self.obs_x = None - self.obs_y = None - if not CALIBRATION_FILE.exists(): - self.logger.warning("[DetectOpponentMove] No board calibration yet — run board calibration before use") - return - try: - cal = json.loads(CALIBRATION_FILE.read_text()) - corner_keys = ("top_left", "top_right", "bottom_left", "bottom_right") - missing = [k for k in corner_keys if k not in cal] - if missing: - self.logger.warning( - f"[DetectOpponentMove] Board calibration incomplete (missing {', '.join(missing)}) — " - "re-run board calibration" - ) - return - corners = [cal[k] for k in corner_keys] - self.obs_x = sum(c["x"] for c in corners) / 4.0 - self.obs_y = sum(c["y"] for c in corners) / 4.0 - self.logger.info( - f"[DetectOpponentMove] Board center from calibration: x={self.obs_x:.4f}, y={self.obs_y:.4f}" - ) - except Exception as e: - self.logger.warning(f"[DetectOpponentMove] Unreadable board calibration ({e}) — re-run board calibration") - - # ── Metadata ────────────────────────────────────────────────────── - - @property - def name(self): - return "detect_opponent_move" - - def guidelines(self): - return ( - "Detect the opponent's last chess move. The robot positions its " - "wrist camera above the board, captures images from both the main " - "and wrist cameras, then asks Gemini which legal move was played. " - "The board state (FEN) is stored in ~/chess_game_state.json. " - "Optionally pass robot_color='white' or 'black' to set perspective." - ) - - # ── State persistence ───────────────────────────────────────────── - - def _load_game_state(self) -> dict | None: - """Load game state from JSON. Returns dict or None.""" - if not GAME_STATE_FILE.exists(): - return None - try: - return json.loads(GAME_STATE_FILE.read_text()) - except Exception as e: - self.logger.error(f"[DetectOpponentMove] Failed to load game state: {e}") - return None - - def _init_game_state(self, robot_color: str) -> dict: - """Create a fresh game state with the handicap starting position.""" - state = { - "fen": HANDICAP_FEN, - "move_history": [], - "last_detected_move": None, - "turn": "white", - "robot_color": robot_color, - } - GAME_STATE_FILE.write_text(json.dumps(state, indent=2)) - self.logger.info("[DetectOpponentMove] Initialised new game state") - return state - - # ── Arm positioning ─────────────────────────────────────────────── - - def _move_to_observation_pose(self) -> bool: - """Move arm above the board centre and tilt head down.""" - if self.obs_x is None or self.obs_y is None: - self.logger.error("[DetectOpponentMove] No board calibration — cannot position arm") - return False - - # Open gripper fully so it doesn't occlude the wrist camera - self.manipulation.open_gripper(100) - time.sleep(0.5) - - # Tilt head down to look at the board - if self.head: - self.head.set_position(self.HEAD_TILT_DOWN) - time.sleep(0.5) - - self._send_feedback("Moving arm to observation pose...") - success = self.manipulation.move_to_cartesian_pose( - x=self.obs_x, - y=self.obs_y, - z=self.OBS_Z, - roll=self.FIXED_ROLL, - pitch=self.OBS_PITCH, - yaw=self.OBS_YAW, - duration=2.0, - blocking=True, - ) - if success: - time.sleep(0.5) # let camera auto-exposure settle - return success - - def _go_to_safe_pose(self): - """Return arm to resting safe pose and head to neutral.""" - self.manipulation.move_to_cartesian_pose( - x=0.15, - y=0.1, - z=0.1, - roll=self.FIXED_ROLL, - pitch=self.OBS_PITCH, - yaw=self.OBS_YAW, - duration=2.0, - ) - if self.head: - self.head.set_position(self.HEAD_TILT_NEUTRAL) - time.sleep(1.5) - - # ── Image capture ───────────────────────────────────────────────── - - def _capture_images(self) -> tuple: - """Grab latest main + wrist camera images. - - Returns (main_b64, wrist_b64). Either may be None. - """ - # Give the camera a moment to refresh after arm settles - time.sleep(0.3) - main_b64 = self.main_image - wrist_b64 = self.wrist_image - - # Save captures for debugging - try: - DATA_DIR.mkdir(parents=True, exist_ok=True) - ts = int(time.time()) - if main_b64: - (DATA_DIR / f"main_{ts}.jpg").write_bytes(base64.b64decode(main_b64)) - if wrist_b64: - (DATA_DIR / f"wrist_{ts}.jpg").write_bytes(base64.b64decode(wrist_b64)) - except Exception as e: - self.logger.warning(f"[DetectOpponentMove] Failed to save captures: {e}") - - return main_b64, wrist_b64 - - # ── Board context builder ───────────────────────────────────────── - - def _build_board_context(self, fen: str) -> tuple: - """Build text context from the current FEN. - - Returns (context_str, legal_moves_uci_list, board). - """ - board = chess.Board(fen) - ascii_board = _fen_to_ascii(fen) - legal_moves = [m.uci() for m in board.legal_moves] - turn = "White" if board.turn == chess.WHITE else "Black" - - context = ( - f"CURRENT BOARD STATE (FEN): {fen}\n\n" - f"ASCII board:\n{ascii_board}\n\n" - f"It is {turn}'s turn.\n" - f"Legal moves ({len(legal_moves)}): {', '.join(legal_moves)}\n" - ) - return context, legal_moves, board - - # ── Debug helpers ────────────────────────────────────────────────── - - def _save_gemini_inputs(self, label: str, prompt: str, main_b64: str | None, wrist_b64: str | None): - """Save the prompt and images sent to Gemini for debugging.""" - try: - DATA_DIR.mkdir(parents=True, exist_ok=True) - ts = int(time.time()) - (DATA_DIR / f"{label}_prompt_{ts}.txt").write_text(prompt) - if main_b64: - (DATA_DIR / f"{label}_main_{ts}.jpg").write_bytes(base64.b64decode(main_b64)) - if wrist_b64: - (DATA_DIR / f"{label}_wrist_{ts}.jpg").write_bytes(base64.b64decode(wrist_b64)) - self.logger.info(f"[DetectOpponentMove] Saved {label} inputs to {DATA_DIR}") - except Exception as e: - self.logger.warning(f"[DetectOpponentMove] Failed to save {label} inputs: {e}") - - def _save_gemini_response(self, label: str, result: dict): - """Save the Gemini response JSON for debugging.""" - try: - DATA_DIR.mkdir(parents=True, exist_ok=True) - ts = int(time.time()) - (DATA_DIR / f"{label}_response_{ts}.json").write_text(json.dumps(result, indent=2)) - except Exception as e: - self.logger.warning(f"[DetectOpponentMove] Failed to save {label} response: {e}") - - # ── Gemini calls ────────────────────────────────────────────────── - - def _ask_gemini_detect_move(self, board_context: str, main_b64: str | None, wrist_b64: str | None) -> dict | None: - """Stage 1: Ask Gemini which move was played. - - Returns parsed dict {move_uci, confidence, reasoning} or None. - """ - prompt = ( - "You are analyzing a physical chess board to detect the opponent's last move.\n\n" - f"{board_context}\n" - "I am providing a close-up overhead wrist camera image of the board.\n\n" - "Compare the CURRENT FEN state (the state BEFORE the opponent moved) " - "with what you see in the images (the state AFTER the opponent moved).\n" - "Identify which piece moved and where it went.\n\n" - "IMPORTANT: The move MUST be one of the legal moves listed above.\n\n" - "Return ONLY JSON:\n" - '{"move_uci": "", "confidence": 0.0-1.0, ' - '"reasoning": "brief explanation"}' - ) - - contents = [prompt] - if wrist_b64: - contents.append(types.Part.from_bytes(data=base64.b64decode(wrist_b64), mime_type="image/jpeg")) - - # Save what we're sending to Gemini for debugging - self._save_gemini_inputs("stage1", prompt, None, wrist_b64) - - try: - response = self.gemini_client.models.generate_content( - model="gemini-3.1-pro-preview", - contents=contents, - config=types.GenerateContentConfig( - response_mime_type="application/json", - thinking_config=types.ThinkingConfig(thinking_budget=1024), - ), - ) - result = json.loads(response.text.strip()) - self.logger.info(f"[DetectOpponentMove] Stage 1 result: {result}") - self._save_gemini_response("stage1", result) - return result - except Exception as e: - self.logger.error(f"[DetectOpponentMove] Gemini stage 1 failed: {e}") - return None - - def _ask_gemini_confirm( - self, board_context: str, candidates: list, main_b64: str | None, wrist_b64: str | None - ) -> dict | None: - """Stage 2: Disambiguation when stage 1 had low confidence. - - Narrows the candidate list and asks Gemini to pick one. - """ - prompt = ( - "You are analyzing a physical chess board to confirm which move was played.\n\n" - f"{board_context}\n" - f"The most likely moves are: {', '.join(candidates)}\n\n" - "I am providing a close-up overhead wrist camera image of the board.\n" - "Look carefully at which squares changed and which piece is now where.\n\n" - "Return ONLY JSON:\n" - '{"move_uci": "", "confidence": 0.0-1.0, ' - '"reasoning": "brief explanation"}' - ) - - contents = [prompt] - if wrist_b64: - contents.append(types.Part.from_bytes(data=base64.b64decode(wrist_b64), mime_type="image/jpeg")) - - # Save what we're sending to Gemini for debugging - self._save_gemini_inputs("stage2", prompt, None, wrist_b64) - - try: - response = self.gemini_client.models.generate_content( - model="gemini-3-flash-preview", - contents=contents, - config=types.GenerateContentConfig( - response_mime_type="application/json", - thinking_config=types.ThinkingConfig(thinking_budget=512), - ), - ) - result = json.loads(response.text.strip()) - self.logger.info(f"[DetectOpponentMove] Stage 2 result: {result}") - self._save_gemini_response("stage2", result) - return result - except Exception as e: - self.logger.error(f"[DetectOpponentMove] Gemini stage 2 failed: {e}") - return None - - # ── Main execute ────────────────────────────────────────────────── - - def execute(self, robot_color: RobotColor = "white"): - """ - Detect the opponent's last move. - - Args: - robot_color: 'white' or 'black' — which side the robot plays. - On first call, initialises the game state. - """ - self._cancelled = False - robot_color = robot_color.strip().lower() - if robot_color not in ROBOT_COLORS: - return f"Invalid robot_color '{robot_color}'. Must be 'white' or 'black'.", SkillResult.FAILURE - - if self.manipulation is None: - return "Manipulation interface not available", SkillResult.FAILURE - if self.gemini_client is None: - return "Gemini not configured (check .env.scan)", SkillResult.FAILURE - - # ── 1. Load or initialise game state ── - state = self._load_game_state() - if state is None: - self._send_feedback("No game state found — initialising new game.") - state = self._init_game_state(robot_color) - - fen = state["fen"] - robot_color = state.get("robot_color", robot_color) - - self.logger.info(f"[DetectOpponentMove] Current FEN: {fen}") - self._send_feedback(f"Current board state loaded. Turn: {state.get('turn', '?')}") - - # ── 2. Move arm to observation pose ── - if not self._move_to_observation_pose(): - return "Failed to move arm to observation pose", SkillResult.FAILURE - if self._cancelled: - return "Cancelled", SkillResult.CANCELLED - - # ── 3. Capture images ── - self._send_feedback("Capturing images...") - main_b64, wrist_b64 = self._capture_images() - - if not main_b64 and not wrist_b64: - self._go_to_safe_pose() - return "No camera images available", SkillResult.FAILURE - - # ── 4. Build board context ── - board_context, legal_moves, board = self._build_board_context(fen) - - if not legal_moves: - self._go_to_safe_pose() - return "No legal moves — game may be over", SkillResult.SUCCESS - - # ── 5. Stage 1: Ask Gemini ── - self._send_feedback(f"Asking Gemini to identify the move ({len(legal_moves)} legal moves)...") - result = self._ask_gemini_detect_move(board_context, main_b64, wrist_b64) - - if result is None: - self._go_to_safe_pose() - return "Gemini failed to analyse the board", SkillResult.FAILURE - - move_uci = result.get("move_uci", "") - confidence = float(result.get("confidence", 0.0)) - reasoning = result.get("reasoning", "") - - self.logger.info(f"[DetectOpponentMove] Stage 1: move={move_uci} conf={confidence:.2f} reason={reasoning}") - - # ── 6. Stage 2: Confirm if low confidence ── - if confidence < self.CONFIDENCE_THRESHOLD and move_uci in legal_moves: - self._send_feedback(f"Low confidence ({confidence:.0%}) on {move_uci}. Running confirmation...") - # Build candidate list: the detected move + a few neighbours - candidates = [move_uci] - for m in legal_moves: - if m != move_uci and len(candidates) < 8: - # Prefer moves from or to the same squares - if m[:2] == move_uci[:2] or m[2:4] == move_uci[2:4]: - candidates.append(m) - # Pad with random legal moves if too few - for m in legal_moves: - if m not in candidates and len(candidates) < 8: - candidates.append(m) - - result2 = self._ask_gemini_confirm(board_context, candidates, main_b64, wrist_b64) - if result2: - move2 = result2.get("move_uci", "") - conf2 = float(result2.get("confidence", 0.0)) - if conf2 > confidence and move2 in legal_moves: - move_uci = move2 - confidence = conf2 - reasoning = result2.get("reasoning", reasoning) - self.logger.info(f"[DetectOpponentMove] Stage 2 override: move={move_uci} conf={confidence:.2f}") - - # ── 7. Validate move is legal (but don't apply — agent calls update_chess_state) ── - if move_uci not in legal_moves: - self._go_to_safe_pose() - return ( - f"Detected move {move_uci} is not legal. Legal moves: {legal_moves}", - SkillResult.FAILURE, - ) - - san = board.san(chess.Move.from_uci(move_uci)) - - # ── 8. Return to safe pose ── - self._go_to_safe_pose() - - msg = ( - f"Detected opponent move: {san} ({move_uci}). " - f"Confidence: {confidence:.0%}. {reasoning}. " - f'Call update_chess_state(move_uci="{move_uci}") to apply it.' - ) - self._send_feedback(msg) - return msg, SkillResult.SUCCESS - - def cancel(self): - self._cancelled = True - return "Move detection cancelled" diff --git a/workspace/innate_skills/email/retrieve_emails.py b/workspace/innate_skills/email/retrieve_emails.py new file mode 100644 index 000000000..c5e230888 --- /dev/null +++ b/workspace/innate_skills/email/retrieve_emails.py @@ -0,0 +1,115 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +import email +import imaplib +from email.header import decode_header + +from innate import Skill, SkillFailed, SkillReturn + +IMAP_SERVER = "imap.gmail.com" +IMAP_PORT = 993 +EMAIL_ADDRESS = "" # configure account +EMAIL_PASSWORD = "" # configure app password +MAX_CONTENT_CHARS = 500 + + +def _decode_header_value(value): + if value is None: + return "" + decoded = "" + for part, encoding in decode_header(value): + if isinstance(part, bytes): + try: + decoded += part.decode(encoding or "utf-8") + except (UnicodeDecodeError, LookupError): + decoded += part.decode("utf-8", errors="ignore") + else: + decoded += str(part) + return decoded + + +def _extract_content(msg): + if not msg.is_multipart(): + body = msg.get_payload(decode=True) + return (body.decode("utf-8", errors="ignore") if body else "").strip() + plain, html = "", "" + for part in msg.walk(): + if "attachment" in str(part.get("Content-Disposition")): + continue + body = part.get_payload(decode=True) + if not body: + continue + if part.get_content_type() == "text/plain": + plain += body.decode("utf-8", errors="ignore") + elif part.get_content_type() == "text/html" and not plain: + html += body.decode("utf-8", errors="ignore") + return (plain or html).strip() + + +class RetrieveEmails(Skill): + """Use to retrieve recent emails from the configured email account. + Provide the number of emails to retrieve (default is 5). Returns email + subjects and content. This should be used when you need to check for + recent messages or respond to incoming communications.""" + + def execute(self, count: int = 5) -> SkillReturn: + count = min(max(1, count), 20) + try: + return self._retrieve(count) + except SkillFailed: + raise + except imaplib.IMAP4.error as e: + self.fail(f"IMAP error: {e}") + except Exception as e: + self.fail(f"Failed to retrieve emails: {e}") + + def _retrieve(self, count: int) -> str: + mail = imaplib.IMAP4_SSL(IMAP_SERVER, IMAP_PORT) + mail.login(EMAIL_ADDRESS, EMAIL_PASSWORD) + mail.select("INBOX") + + status, messages = mail.search(None, "ALL") + if status != "OK": + mail.logout() + self.fail("Failed to search emails") + + email_ids = messages[0].split() + if not email_ids: + mail.logout() + return "No emails found in inbox" + + emails = [] + for email_id in reversed(email_ids[-count:]): + status, msg_data = mail.fetch(email_id, "(RFC822)") + payload = msg_data[0] if status == "OK" else None + if not isinstance(payload, tuple): + self.logger.warning(f"Failed to fetch email {email_id}") + continue + msg = email.message_from_bytes(payload[1]) + content = _extract_content(msg) + if len(content) > MAX_CONTENT_CHARS: + content = content[:MAX_CONTENT_CHARS] + "... [truncated]" + emails.append( + { + "subject": _decode_header_value(msg.get("Subject", "No Subject")), + "from": _decode_header_value(msg.get("From", "Unknown Sender")), + "date": msg.get("Date", "Unknown Date"), + "content": content or "[No text content available]", + } + ) + mail.logout() + + if not emails: + self.fail("No emails could be retrieved") + + lines = [f"Retrieved {len(emails)} recent email(s):\n"] + for i, info in enumerate(emails, 1): + lines += [ + f"Email {i}:", + f" Subject: {info['subject']}", + f" From: {info['from']}", + f" Date: {info['date']}", + f" Content: {info['content']}", + "", + ] + return "\n".join(lines) diff --git a/workspace/innate_skills/email/send_email.py b/workspace/innate_skills/email/send_email.py new file mode 100644 index 000000000..de5937d88 --- /dev/null +++ b/workspace/innate_skills/email/send_email.py @@ -0,0 +1,25 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +from innate import Skill, SkillReturn + +DEFAULT_RECIPIENTS = ["axel@innate.bot", "vignesh@innate.bot"] + + +class SendEmail(Skill): + """Use to send an emergency email notification. Provide a subject and + message. You can optionally provide a list of recipients, otherwise it + will be sent to the default list. This should be used when a potential + emergency is detected and assistance might be required.""" + + def execute(self, subject: str, message: str, recipients: list[str] | str | None = None) -> SkillReturn: + if recipients is None: + recipients = DEFAULT_RECIPIENTS + elif isinstance(recipients, str): + recipients = [recipients] + if not recipients: + self.fail("No recipients specified for email.") + + # Demo stub: logs the email instead of talking to an SMTP server. + recipients_str = ", ".join(recipients) + self.logger.info(f"[SendEmail] To: {recipients_str}\nSubject: {subject}\nMessage: {message}") + return f"Email sent to {recipients_str}" diff --git a/workspace/innate_skills/email/send_picture_via_email.py b/workspace/innate_skills/email/send_picture_via_email.py new file mode 100644 index 000000000..2652737f8 --- /dev/null +++ b/workspace/innate_skills/email/send_picture_via_email.py @@ -0,0 +1,41 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +import smtplib +from email.mime.image import MIMEImage +from email.mime.multipart import MIMEMultipart +from email.mime.text import MIMEText + +from innate import MainImage, Skill, SkillReturn + +DEFAULT_RECIPIENT = "axel@innate.bot" +SMTP_SERVER = "smtp.gmail.com" +SMTP_PORT = 587 +SENDER_EMAIL = "axel@innate.bot" +SENDER_PASSWORD = "" # Gmail app password + + +class SendPictureViaEmail(Skill): + """Use to send an email with the latest view from the robot eyes. + Provide a subject and a message body. The view will be automatically + attached.""" + + image: MainImage + + def execute(self, subject: str, message: str, recipient: str | None = None) -> SkillReturn: + recipient = recipient or DEFAULT_RECIPIENT + try: + msg = MIMEMultipart() + msg["From"] = SENDER_EMAIL + msg["To"] = recipient + msg["Subject"] = subject + msg.attach(MIMEText(message, "plain")) + msg.attach(MIMEImage(self.image.jpeg, name="robot_capture.jpg")) + + server = smtplib.SMTP(SMTP_SERVER, SMTP_PORT) + server.starttls() + server.login(SENDER_EMAIL, SENDER_PASSWORD) + server.send_message(msg) + server.quit() + except Exception as e: + self.fail(f"Failed to send email: {e}") + return f"Email with picture sent to {recipient}" diff --git a/workspace/innate_skills/follow_aruco.py b/workspace/innate_skills/follow_aruco.py index 829e30ca7..369429824 100644 --- a/workspace/innate_skills/follow_aruco.py +++ b/workspace/innate_skills/follow_aruco.py @@ -1,100 +1,93 @@ # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc -"""Follow ArUco Skill -- lock onto the first ArUco marker seen through the main -camera and follow it with the base, steering by the marker's pixel position and -keeping distance from its apparent size. Seeing the stop marker (id 4) ends the -follow. No obstacle avoidance.""" - -import base64 import time import cv2 import numpy as np -from innate import Interface, InterfaceType, RobotState, RobotStateType, Skill, SkillResult + +from innate import MainImage, Mobility, Skill, SkillReturn, resource # Must match the dictionary the tags were printed with (ids 0-4, DICT_4X4_50). # The calibration ChArUco boards use DICT_4X4_250 -- a different dictionary. ARUCO_DICT = cv2.aruco.DICT_4X4_50 STOP_ID = 4 # seeing this marker ends the follow -LOCK_IDS = {0, 1, 2, 3} # the printed tag set minus the stop marker +LOCK_IDS = {0, 1, 2, 3} -# Background texture occasionally decodes as a valid marker for a frame, so a -# single detection is not trusted: locking and stopping both require the same -# id in consecutive frames. +# A single detection is not trusted: background texture occasionally decodes +# as a valid marker for one frame. LOCK_CONFIRM_FRAMES = 3 STOP_CONFIRM_FRAMES = 2 -LOOP_PERIOD = 0.1 # control loop (s); camera state refreshes faster than this +LOOP_PERIOD = 0.1 CMD_DURATION = 0.4 # cmd_vel deadman: base stops if the loop dies -MAX_LINEAR = 0.3 # m/s forward -MAX_REVERSE = 0.1 # m/s backward when too close -MAX_ANGULAR = 0.8 # rad/s -TURN_GAIN = 1.5 # rad/s per unit of normalized horizontal offset -LINEAR_GAIN = 0.8 # m/s per unit of relative size error past the deadband +MAX_LINEAR = 0.3 +MAX_REVERSE = 0.1 +MAX_ANGULAR = 0.8 +TURN_GAIN = 1.5 +LINEAR_GAIN = 0.8 # Follow distance is held by keeping the marker's apparent side length at this # fraction of image width (~0.35 m for an 8 cm tag on the main camera). TARGET_SIZE_FRAC = 0.16 -SIZE_DEADBAND = 0.15 # relative size error below which we don't drive +SIZE_DEADBAND = 0.15 # Smoothing: per-frame corner jitter would otherwise feed straight into the # command, and step changes in cmd_vel jerk the base. -MEAS_SMOOTHING = 0.35 # EMA weight of the newest measurement, (0, 1] -LINEAR_SLEW = 0.5 # m/s^2 max change in commanded linear velocity -ANGULAR_SLEW = 2.0 # rad/s^2 max change in commanded angular velocity -LOST_GRACE_FRAMES = 5 # ramp down this many missed frames before a hard stop +MEAS_SMOOTHING = 0.35 +LINEAR_SLEW = 0.5 # m/s^2 +ANGULAR_SLEW = 2.0 # rad/s^2 +LOST_GRACE_FRAMES = 5 class FollowAruco(Skill): - """Follow the first ArUco marker seen until the stop marker appears.""" - - mobility = Interface(InterfaceType.MOBILITY) - image = RobotState(RobotStateType.LAST_MAIN_CAMERA_IMAGE_B64) - - def __init__(self, logger): - super().__init__(logger) + """Follow a person or object carrying an ArUco tag.""" + + mobility: Mobility + # `| None`: the required-feed grace (3 s) is too tight for a cold camera; + # execute() waits up to 5 s itself. + image: MainImage | None + + # Per-run tracking state: the class values are the starting point, and + # each run's instance shadows them as it goes. + _frame_width = 1 + _offset_filtered: float | None = None + _size_error_filtered: float | None = None + _cmd_linear = 0.0 + _cmd_angular = 0.0 + _last_cmd_time: float | None = None + + @resource + def _detector(self) -> cv2.aruco.ArucoDetector: dictionary = cv2.aruco.getPredefinedDictionary(ARUCO_DICT) - self._detector = cv2.aruco.ArucoDetector(dictionary, cv2.aruco.DetectorParameters()) - self._frame_width = 1 # real width set on every successful _detect_markers() decode - self._offset_filtered = None - self._size_error_filtered = None - self._cmd_linear = 0.0 - self._cmd_angular = 0.0 - self._last_cmd_time = None + return cv2.aruco.ArucoDetector(dictionary, cv2.aruco.DetectorParameters()) - @property - def name(self): - return "follow_aruco" - - def guidelines(self): + def guidelines(self) -> str: + ids = ", ".join(str(i) for i in sorted(LOCK_IDS)) return ( - "Follow a person or object carrying an ArUco tag (DICT_4X4_50, ids 0-3). " + f"Follow a person or object carrying an ArUco tag (DICT_4X4_50, ids {ids}). " "The robot waits until it sees one, locks onto that id, says 'Locked in', and " "then drives to keep that marker centered and about 0.35m away. Showing " - f"marker id {STOP_ID} stops the follow. Runs until the stop marker is seen " - "or the skill is cancelled. No obstacle avoidance -- use in clear space." + f"marker id {STOP_ID} stops the follow. Runs until the stop marker is seen or the " + "skill is cancelled. No obstacle avoidance -- use in clear space." ) - def execute(self): - if self.mobility is None: - return "Mobility interface not available", SkillResult.FAILURE - if not self._wait_for_frame(): - return "No camera image available", SkillResult.FAILURE - + def execute(self) -> SkillReturn: + if self.wait_for(lambda: self.image, timeout=5.0) is None: + self.fail("No camera image available") locked_id = None lock_candidate = None lock_streak = 0 stop_streak = 0 lost_frames = 0 - while not self._cancelled: + while True: markers = self._detect_markers() stop_streak = stop_streak + 1 if STOP_ID in markers else 0 if stop_streak >= STOP_CONFIRM_FRAMES: self._stop() self.say("Stopping") - return f"Stop marker (id {STOP_ID}) seen, follow ended", SkillResult.SUCCESS + return f"Stop marker (id {STOP_ID}) seen, follow ended" if locked_id is None: seen = next((i for i in markers if i in LOCK_IDS), None) @@ -103,37 +96,26 @@ def execute(self): if seen is not None and lock_streak >= LOCK_CONFIRM_FRAMES: locked_id = seen self.say("Locked in") - self._send_feedback(f"Locked onto marker id {locked_id}") + self.feedback(f"Locked onto marker id {locked_id}") else: - time.sleep(LOOP_PERIOD) + self.sleep(LOOP_PERIOD) continue quad = markers.get(locked_id) if quad is None: lost_frames += 1 - # Drop stale measurements so a re-acquired marker that moved - # during the miss doesn't steer off the old filter state. self._offset_filtered = None self._size_error_filtered = None if lost_frames > LOST_GRACE_FRAMES: - self._stop() # marker out of sight: hold position, keep scanning + self._stop() else: - self._send_cmd(0.0, 0.0) # brief detection miss: ramp down, don't jerk + self._send_cmd(0.0, 0.0) else: lost_frames = 0 self._drive_toward(quad) - time.sleep(LOOP_PERIOD) - - self._stop() - return "Follow cancelled", SkillResult.CANCELLED - - def cancel(self): - self._cancelled = True - self._stop() - return "Follow cancelled" + self.sleep(LOOP_PERIOD) def _detect_markers(self) -> dict: - """Detected markers in the current frame as {id: quad of 4 (x, y) corners}.""" frame = self._current_frame() if frame is None: return {} @@ -144,48 +126,40 @@ def _detect_markers(self) -> dict: return {int(marker_id): quad.reshape(4, 2) for marker_id, quad in zip(ids.flatten(), corners, strict=True)} def _current_frame(self): - b64 = self.image - if not b64: + frame = self.image + if not frame: return None - data = np.frombuffer(base64.b64decode(b64), dtype=np.uint8) + data = np.frombuffer(frame.jpeg, dtype=np.uint8) return cv2.imdecode(data, cv2.IMREAD_GRAYSCALE) def _drive_toward(self, quad) -> None: - # Steer: normalized horizontal offset of the marker center, [-1, 1]. center_x = quad[:, 0].mean() offset = (center_x - self._frame_width / 2) / (self._frame_width / 2) - # Distance: apparent side length vs. target. Too small -> approach, - # too large -> back off. side_frac = np.mean([np.linalg.norm(quad[i] - quad[(i + 1) % 4]) for i in range(4)]) / self._frame_width size_error = 1.0 - side_frac / TARGET_SIZE_FRAC self._offset_filtered = self._smooth(self._offset_filtered, offset) self._size_error_filtered = self._smooth(self._size_error_filtered, size_error) - # Positive angular_z is a left turn; marker right of center needs a right turn. angular = float(np.clip(-TURN_GAIN * self._offset_filtered, -MAX_ANGULAR, MAX_ANGULAR)) linear = 0.0 err = self._size_error_filtered if abs(err) > SIZE_DEADBAND: - # Ramp from zero at the deadband edge instead of jumping to full gain. past_deadband = err - np.copysign(SIZE_DEADBAND, err) linear = float(np.clip(LINEAR_GAIN * past_deadband, -MAX_REVERSE, MAX_LINEAR)) self._send_cmd(linear, angular) @staticmethod - def _smooth(filtered, measurement): + def _smooth(filtered: float | None, measurement: float) -> float: if filtered is None: return measurement return filtered + MEAS_SMOOTHING * (measurement - filtered) def _send_cmd(self, linear, angular) -> None: - # Slew-limit toward the requested velocities so the base accelerates - # and decelerates gradually. The step scales with real elapsed time, so - # a slow loop iteration doesn't silently lower the slew rate. dt is - # capped at CMD_DURATION: past that the deadman has stopped the base, - # and a bigger step would jerk it from standstill. + # Slew-limit toward the requested velocities; dt capped at CMD_DURATION + # because past that the deadman has stopped the base. now = time.monotonic() dt = min(now - self._last_cmd_time, CMD_DURATION) if self._last_cmd_time is not None else LOOP_PERIOD self._last_cmd_time = now @@ -193,19 +167,10 @@ def _send_cmd(self, linear, angular) -> None: self._cmd_angular += float(np.clip(angular - self._cmd_angular, -ANGULAR_SLEW * dt, ANGULAR_SLEW * dt)) self.mobility.send_cmd_vel(linear_x=self._cmd_linear, angular_z=self._cmd_angular, duration=CMD_DURATION) - def _wait_for_frame(self, timeout: float = 5.0) -> bool: - deadline = time.time() + timeout - while self.image is None and not self._cancelled: - if time.time() > deadline: - return False - time.sleep(0.05) - return True - def _stop(self): self._offset_filtered = None self._size_error_filtered = None self._cmd_linear = 0.0 self._cmd_angular = 0.0 self._last_cmd_time = None - if self.mobility is not None: - self.mobility.send_cmd_vel(linear_x=0.0, angular_z=0.0) + self.mobility.stop() diff --git a/workspace/innate_skills/head_emotion.py b/workspace/innate_skills/head_emotion.py index 8f1b313f9..31c6acdd4 100644 --- a/workspace/innate_skills/head_emotion.py +++ b/workspace/innate_skills/head_emotion.py @@ -1,14 +1,8 @@ -#!/usr/bin/env python3 # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc -""" -Head Emotion Skill - Express emotions through vertical head (tilt) movements. -""" +from typing import Literal, cast -import time -from typing import Literal - -from brain_client.skills.types import Interface, InterfaceType, Skill, SkillResult +from innate import Head, Skill, SkillReturn # Each pose is (angle_degrees, duration_seconds). Duration is the time to # interpolate from the previous pose to this one. @@ -88,82 +82,44 @@ "disagreeing", ] +INTERPOLATION_RATE_HZ = 30.0 -class HeadEmotion(Skill): - """Express emotions through vertical head tilt movements.""" - - head = Interface(InterfaceType.HEAD) - def __init__(self, logger): - super().__init__(logger) - self._cancelled = False +class HeadEmotion(Skill): + """Express an emotion through head tilt movements.""" - @property - def name(self): - return "head_emotion" + head: Head - def guidelines(self): - emotion_list = ", ".join(f"'{e}'" for e in EMOTIONS) + def guidelines(self) -> str: return ( - "Express an emotion through head tilt movements. " - f"Requires 'emotion' parameter, one of: {emotion_list}. " + "Express an emotion through head tilt movements. Requires 'emotion' " + f"parameter, one of: {', '.join(repr(name) for name in EMOTIONS)}. " "Optionally pass 'repeat' (int, default 1) to loop the animation." ) - def execute(self, emotion: EmotionName, repeat: int = 1): - """ - Play a head-tilt animation for the given emotion. - - Args: - emotion: One of the supported emotion names. - repeat: Number of times to play the animation (default 1). - """ - self._cancelled = False - - if self.head is None: - return "Head interface not available", SkillResult.FAILURE - - emotion = emotion.strip().lower() + def execute(self, emotion: EmotionName, repeat: int = 1) -> SkillReturn: + emotion = cast(EmotionName, emotion.strip().lower()) if emotion not in EMOTIONS: - available = ", ".join(sorted(EMOTIONS)) - return f"Unknown emotion '{emotion}'. Available: {available}", SkillResult.FAILURE + self.fail(f"Unknown emotion '{emotion}'. Available: {', '.join(sorted(EMOTIONS))}") repeat = max(1, min(int(repeat), 5)) entry = EMOTIONS[emotion] - sequence = entry["sequence"] - - self.logger.info(f"[HeadEmotion] Playing '{emotion}' ({entry['description']}) x{repeat}") - self._send_feedback(f"Expressing: {emotion}") - - interpolation_rate = 30.0 # Hz - dt = 1.0 / interpolation_rate - - for r in range(repeat): - current_angle = 0.0 - for target_angle, duration in sequence: - if self._cancelled: - self.head.set_position(0) - return "Cancelled", SkillResult.CANCELLED - steps = max(1, int(round(duration * interpolation_rate))) - for i in range(1, steps + 1): - if self._cancelled: - self.head.set_position(0) - return "Cancelled", SkillResult.CANCELLED - t = i / steps - interp = current_angle + (target_angle - current_angle) * t - self.head.set_position(int(round(interp))) - time.sleep(dt) - current_angle = float(target_angle) - if r < repeat - 1: - time.sleep(0.2) - - # Return to neutral - self.head.set_position(0) - - msg = f"Expressed '{emotion}' ({entry['description']})" - self.logger.info(f"[HeadEmotion] {msg}") - return msg, SkillResult.SUCCESS - - def cancel(self): - self._cancelled = True - return "Head emotion cancelled" + self.feedback(f"Expressing: {emotion}") + + dt = 1.0 / INTERPOLATION_RATE_HZ + try: + for r in range(repeat): + current_angle = 0.0 + for target_angle, duration in entry["sequence"]: + steps = max(1, int(round(duration * INTERPOLATION_RATE_HZ))) + for i in range(1, steps + 1): + interp = current_angle + (target_angle - current_angle) * i / steps + self.head.set_position(int(round(interp))) + self.sleep(dt) + current_angle = float(target_angle) + if r < repeat - 1: + self.sleep(0.2) + finally: + self.head.set_position(0) + + return f"Expressed '{emotion}' ({entry['description']})" diff --git a/workspace/innate_skills/move_straight.py b/workspace/innate_skills/move_straight.py index c461b0ec2..18accb6da 100644 --- a/workspace/innate_skills/move_straight.py +++ b/workspace/innate_skills/move_straight.py @@ -3,15 +3,14 @@ import math import time -from innate import Interface, InterfaceType, RobotState, RobotStateType, Skill, SkillResult from pydantic import BaseModel +from innate import Mobility, Odometry, Skill, SkillOutput, SkillReturn + # Allowed base speeds (m/s). Slow on purpose: no obstacle avoidance here. MIN_SPEED = 0.05 MAX_SPEED = 0.3 DEFAULT_SPEED = 0.15 -# how long to wait for the first odom message after execute() starts -ODOM_WAIT_SEC = 2.0 class MoveResult(BaseModel): @@ -21,90 +20,30 @@ class MoveResult(BaseModel): class MoveStraight(Skill): - """Move straight for a distance using raw cmd_vel closed on odometry -- - no Nav2, no map, and NO obstacle avoidance. Negative distance moves - backward.""" - - mobility = Interface(InterfaceType.MOBILITY) - odom = RobotState(RobotStateType.LAST_ODOM) - - def __init__(self, logger): - super().__init__(logger) - self._cancelled = False - - @property - def name(self): - return "move_straight" - - def guidelines(self): - return ( - "Move the robot straight forward (positive distance, meters) or backward " - "(negative distance) using odometry only -- no map or path planning, and no " - "obstacle avoidance. Use for short moves in clear space; prefer " - "navigate_to_position when a map position is needed." - ) + """Move the robot straight forward (positive distance, meters) or backward + (negative distance) using odometry only -- no map or path planning, and no + obstacle avoidance. Use for short moves in clear space; prefer + navigate_to_position when a map position is needed.""" - def execute(self, distance: float, speed: float = DEFAULT_SPEED): - try: - return self._execute(distance, speed) - finally: - # reset on exit, not entry: an entry reset would erase a cancel - # delivered while the server was still setting up the goal - self._cancelled = False + mobility: Mobility + odom: Odometry - def _execute(self, distance: float, speed: float): - if self.mobility is None: - return "Mobility interface not available", SkillResult.FAILURE + def execute(self, distance: float, speed: float = DEFAULT_SPEED) -> SkillReturn: if distance == 0.0: - return "Moved 0.0m", SkillResult.SUCCESS, MoveResult(traveled_m=0.0) - start = self._wait_for_position() - if self._cancelled: - return "Move cancelled", SkillResult.CANCELLED - if start is None: - return "No odometry available", SkillResult.FAILURE - + return SkillOutput("Moved 0.0m", MoveResult(traveled_m=0.0)) + start = self.odom.position target = abs(distance) velocity = math.copysign(min(max(abs(speed), MIN_SPEED), MAX_SPEED), distance) - # generous time budget; if stuck, stop commanding motion instead of pushing forever deadline = time.time() + target / abs(velocity) * 3.0 + 2.0 traveled = 0.0 while traveled < target: - if self._cancelled: - self._stop() - return f"Move cancelled after {traveled:.2f}m", SkillResult.CANCELLED if time.time() > deadline: - self._stop() - return f"Stuck: moved only {traveled:.2f}m of {target:.2f}m", SkillResult.FAILURE - # duration acts as a deadman: if this loop dies, the base stops + self.fail(f"Stuck: moved only {traveled:.2f}m of {target:.2f}m") self.mobility.send_cmd_vel(linear_x=velocity, duration=0.5) - time.sleep(0.1) - current = self._position() - if current is not None: - traveled = math.dist(current, start) + self.sleep(0.1) + traveled = math.dist(self.odom.position, start) - self._stop() + self.mobility.stop() direction = "forward" if distance > 0 else "backward" - return f"Moved {traveled:.2f}m {direction}", SkillResult.SUCCESS, MoveResult(traveled_m=round(traveled, 3)) - - def cancel(self): - self._cancelled = True - self._stop() - return "Move cancelled" - - def _position(self): - """Current (x, y) from the odometry robot state, or None if absent.""" - return self.odom.position if self.odom is not None else None - - def _wait_for_position(self): - """(x, y) once odometry arrives, or None after ODOM_WAIT_SEC.""" - deadline = time.time() + ODOM_WAIT_SEC - while True: - position = self._position() - if position is not None or self._cancelled or time.time() > deadline: - return position - time.sleep(0.02) - - def _stop(self): - if self.mobility is not None: - self.mobility.send_cmd_vel(linear_x=0.0, angular_z=0.0) + return SkillOutput(f"Moved {traveled:.2f}m {direction}", MoveResult(traveled_m=round(traveled, 3))) diff --git a/workspace/innate_skills/navigate_to_position.py b/workspace/innate_skills/navigate_to_position.py index e435a7184..cf1278015 100644 --- a/workspace/innate_skills/navigate_to_position.py +++ b/workspace/innate_skills/navigate_to_position.py @@ -1,24 +1,18 @@ # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc import math -import threading import time +from collections.abc import Iterator -import rclpy -from geometry_msgs.msg import PoseStamped, Twist +from geometry_msgs.msg import PoseStamped from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult -from rclpy.duration import Duration from rclpy.qos import DurabilityPolicy, QoSProfile -from rclpy.time import Time -from tf2_ros import TransformException -from tf2_ros.buffer import Buffer -from tf2_ros.transform_listener import TransformListener -from brain_client.common.geometry import quaternion_to_yaw -from brain_client.skills.types import Skill, SkillResult +from innate import Odometry, Skill, SkillCancelled, SkillFailed, SkillReturn, resource # Frame local (robot-relative) goals are resolved into before being sent to -# Nav2. Must match the mapfree costmap's global_frame (mars_nav costmap.yaml). +# Nav2. Must match the mapfree costmap's global_frame (mars_nav costmap.yaml) +# and Odometry.frame_id, which is what resolves them. LOCAL_GOAL_FIXED_FRAME = "odom" @@ -31,224 +25,107 @@ def resolve_local_goal(base_x, base_y, base_yaw, x, y, theta): class Nav2Controller: - def __init__(self, logger, primitive): - """ - Initialize the Nav2Controller by creating a BasicNavigator instance - """ - # Create a BasicNavigator instance to communicate with Nav2. + def __init__(self, skill): + self.skill = skill + self.logger = skill.logger self.navigator = BasicNavigator(namespace="") self.navigator_mapfree = BasicNavigator(namespace="mapfree") self.navigator_navigation = BasicNavigator(namespace="navigation") - self.logger = logger - # Add a cancellation flag - self._cancel_requested = threading.Event() - - # Create a publisher for velocity commands - # self.cmd_vel_pub = self.navigator.create_publisher( - # Twist, '/cmd_vel', 10 - # ) - self._send_feedback = primitive._send_feedback - - # TF listener used to resolve local goals into LOCAL_GOAL_FIXED_FRAME - self.tf_buffer = Buffer() - self.tf_listener = TransformListener(self.tf_buffer, self.navigator) # The exact goal this skill commands, latched so UIs can render the # true target (the replanned path's endpoint wiggles). latched = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) self._commanded_goal_pub = self.navigator.create_publisher(PoseStamped, "/nav/commanded_goal", latched) - self.logger.info("Nav2 position primitive node created") - - def _lookup_fresh_base_pose(self, timeout_sec: float = 2.0, max_age_sec: float = 1.0): - """Latest odom->base_link transform no older than max_age_sec, or None. - - Spins the navigator node so the TF listener actually receives data — - outside Nav2's own blocking helpers nothing services its subscriptions. - """ - navigator = self.navigator - clock = navigator.get_clock() - deadline = clock.now() + Duration(seconds=timeout_sec) - min_stamp = clock.now() - Duration(seconds=max_age_sec) - while clock.now() < deadline: - rclpy.spin_once(navigator, timeout_sec=0.05) - try: - transform = self.tf_buffer.lookup_transform(LOCAL_GOAL_FIXED_FRAME, "base_link", Time()) - except TransformException: - continue - if Time.from_msg(transform.header.stamp) >= min_stamp: - return transform - return None + def _resolve_goal(self, x, y, theta, local_frame): + if not local_frame: + return x, y, theta, "map" + # self.skill.odom is the odom->base_link pose the goal is relative to, + # injected and kept current by the framework — so there is no TF buffer + # to warm up here, which is what made a freshly built controller's + # first local goal racy. + base = self.skill.odom + gx, gy, gyaw = resolve_local_goal(base.x, base.y, base.theta, x, y, theta) + self.logger.info(f"Resolved local goal ({x}, {y}, {theta}) to ({gx:.3f}, {gy:.3f}, {gyaw:.3f})") + return gx, gy, gyaw, LOCAL_GOAL_FIXED_FRAME + + def go_to_position(self, x: float, y: float, theta: float, local_frame: bool) -> None: + """Navigate to the goal, blocking until Nav2 finishes. Raises + SkillFailed with a human-readable reason; a skill cancel unwinds as + SkillCancelled with the Nav2 task cancelled.""" + goal_x, goal_y, goal_yaw, goal_frame = self._resolve_goal(x, y, theta, local_frame) - def go_to_position(self, x: float, y: float, theta: float, local_frame: bool): - """ - Sends a navigation goal to the navigator and waits until navigation ends. - - Args: - x (float): x-coordinate of the target position. - y (float): y-coordinate of the target position. - theta (float): The orientation angle in radians. - - Returns: - (TaskResult, str | None): the navigator's result status, plus a - human-readable detail explaining a failure (None on success/cancel). - """ - navigator = self.navigator - # Reset cancellation flag - self._cancel_requested.clear() - - # Determine behavior tree based on navigation mode - behavior_tree = "mapfree" if local_frame else "navigation" - - # Local goals are resolved into the odom frame up front: Nav2 replans - # the original goal at 1 Hz, and a base_link goal ages out of the - # ~10 s TF buffer, breaking replans on longer navigations. - if local_frame: - base_tf = self._lookup_fresh_base_pose() - if base_tf is None: - self.logger.error( - f"No fresh base_link->{LOCAL_GOAL_FIXED_FRAME} transform available; cannot resolve local goal" - ) - return TaskResult.FAILED, ( - "could not determine the robot's current pose " - f"(no fresh {LOCAL_GOAL_FIXED_FRAME} transform), so the local goal could not be resolved" - ) - base = base_tf.transform - goal_x, goal_y, goal_yaw = resolve_local_goal( - base.translation.x, base.translation.y, quaternion_to_yaw(base.rotation), x, y, theta - ) - goal_frame = LOCAL_GOAL_FIXED_FRAME - self.logger.info( - f"Resolved local goal ({x}, {y}, {theta}) to {goal_frame} frame: " - f"({goal_x:.3f}, {goal_y:.3f}, {goal_yaw:.3f})" - ) - else: - goal_x, goal_y, goal_yaw = x, y, theta - goal_frame = "map" - - # Create a PoseStamped goal. goal_pose = PoseStamped() goal_pose.header.frame_id = goal_frame - goal_pose.header.stamp = navigator.get_clock().now().to_msg() + goal_pose.header.stamp = self.navigator.get_clock().now().to_msg() goal_pose.pose.position.x = goal_x goal_pose.pose.position.y = goal_y - goal_pose.pose.position.z = 0.0 - - goal_pose.pose.orientation.x = 0.0 - goal_pose.pose.orientation.y = 0.0 goal_pose.pose.orientation.z = math.sin(goal_yaw / 2.0) goal_pose.pose.orientation.w = math.cos(goal_yaw / 2.0) - self._commanded_goal_pub.publish(goal_pose) - self.logger.debug(f"Sending goal pose ... behavior_tree: {behavior_tree}") path_navigator = self.navigator_mapfree if local_frame else self.navigator_navigation - path = path_navigator.getPath(goal_pose, goal_pose, use_start=False) - - # If the path is None, we can't navigate to the goal - if path is None: - self.logger.error("Failed to get path to goal") - return TaskResult.FAILED, ( + if path_navigator.getPath(goal_pose, goal_pose, use_start=False) is None: + raise SkillFailed( f"the planner found no path to ({goal_x:.2f}, {goal_y:.2f}) in the {goal_frame} frame " "— the goal may be unreachable, blocked, or outside the map" ) - navigator.goToPose(goal_pose, behavior_tree=behavior_tree) + self.navigator.goToPose(goal_pose, behavior_tree="mapfree" if local_frame else "navigation") - self.logger.debug("Waiting for navigation to complete ...") - - was_canceled = False - initial_distance_remaining = -1.0 # Not set until the first valid feedback - feedback_sent_close_to_goal = False # Flag to track if feedback has been sent - last_progress_log = 0.0 - # Last feedback snapshot, so a failure can say how far the robot got. - last_distance_remaining = -1.0 + initial_distance = -1.0 + last_distance = -1.0 last_recoveries = 0 + last_progress_log = 0.0 + said_close_to_goal = False + while not self.navigator.isTaskComplete(): + try: + self.skill.sleep(0.1) + except SkillCancelled: + self.navigator.cancelTask() + raise - # Modified loop to check for cancellation - while not navigator.isTaskComplete(): - # Check if cancellation was requested - if self._cancel_requested.is_set(): - self.logger.info("Cancellation detected in navigation loop") - was_canceled = True - navigator.cancelTask() - break - - # Small sleep to prevent CPU hogging; also paces the 10 Hz cancel poll - time.sleep(0.1) - - feedback = navigator.getFeedback() + feedback = self.navigator.getFeedback() if not feedback: continue - - # Use Nav2's own distance_remaining: feedback.current_pose is - # in the navigator's global frame while our goal may be in - # another, so computing distances ourselves would be wrong. - distance_remaining = feedback.distance_remaining + # Nav2's own distance_remaining: feedback.current_pose is in the + # navigator's global frame while our goal may be in another. + distance = feedback.distance_remaining last_recoveries = feedback.number_of_recoveries - if distance_remaining > 0.0: - last_distance_remaining = distance_remaining - - if initial_distance_remaining < 0.0 and distance_remaining > 0.0: - initial_distance_remaining = distance_remaining - - path_completion = 0.0 - if initial_distance_remaining > 0.0: - path_completion = max(0.0, min(100.0, (1.0 - distance_remaining / initial_distance_remaining) * 100.0)) + if distance > 0.0: + last_distance = distance + if initial_distance < 0.0: + initial_distance = distance now = time.monotonic() if now - last_progress_log >= 1.0: last_progress_log = now + completion = 100.0 * (1.0 - distance / initial_distance) if initial_distance > 0.0 else 0.0 self.logger.info( - f"Navigation progress: {path_completion:.0f}% ({distance_remaining:.2f}m remaining, " - f"{feedback.number_of_recoveries} recoveries)" + f"Navigation progress: {max(0.0, min(100.0, completion)):.0f}% " + f"({distance:.2f}m remaining, {last_recoveries} recoveries)" ) - if 0.0 < distance_remaining < 0.2 and not feedback_sent_close_to_goal: - self._send_feedback( + if 0.0 < distance < 0.2 and not said_close_to_goal: + said_close_to_goal = True + self.skill.feedback( "I'm almost done with this movement, if I think I should navigate again to pursue this task" ", I should stop the current primitive and start a new navigation movement." ) - feedback_sent_close_to_goal = True - - result = navigator.getResult() - detail = None - - if was_canceled: - self.logger.debug("Goal was canceled!") - # This should not be necessary but somehow the navigator.cancelTask does not make result == TaskResult.CANCELED - result = TaskResult.CANCELED - elif result == TaskResult.SUCCEEDED: - self.logger.debug("Goal succeeded!") - else: - self.logger.debug(f"Goal failed or timed out. result: {result}") - detail = f"Nav2 reported {getattr(result, 'name', result)}" - if last_distance_remaining >= 0.0: - detail += f" with {last_distance_remaining:.2f}m still to go" - if last_recoveries > 0: - detail += f" after {last_recoveries} recovery attempt{'s' if last_recoveries != 1 else ''}" - detail += " — the route may be blocked or the robot may be stuck" - - # Stop the robot by publishing a stop command. - stop_cmd = Twist() - stop_cmd.linear.x = 0.0 - stop_cmd.angular.z = 0.0 - # self.cmd_vel_pub.publish(stop_cmd) - return result, detail - - def cancel_navigation(self): - """ - Cancels the current navigation task. - """ - self.logger.debug("Canceling current navigation task...") - # Set the cancellation flag - self._cancel_requested.set() + result = self.navigator.getResult() + if result == TaskResult.SUCCEEDED: + return + detail = f"Nav2 reported {getattr(result, 'name', result)}" + if last_distance >= 0.0: + detail += f" with {last_distance:.2f}m still to go" + if last_recoveries > 0: + detail += f" after {last_recoveries} recovery attempt{'s' if last_recoveries != 1 else ''}" + raise SkillFailed(detail + " — the route may be blocked or the robot may be stuck") def destroy(self): """Destroy the navigator nodes so their graph entities disappear now, - not at some eventual GC pass. Safe outside go_to_position — nothing - else spins these nodes.""" + not at some eventual GC pass.""" for navigator in (self.navigator, self.navigator_mapfree, self.navigator_navigation): try: # Humble's BasicNavigator.destroy_node() misses this client; its @@ -263,61 +140,35 @@ def destroy(self): class NavigateToPosition(Skill): - def __init__(self, logger): - self.nav2_controller = Nav2Controller(logger, self) - self.logger = logger - - @property - def name(self): - return "navigate_to_position" - - def guidelines(self): - return ( - "Use when you need to navigate the robot to the specified position " - "using provided x, y coordinates (meters), and theta_degrees (yaw) IN DEGREES. " - "If local_frame is set to false, it navigates to a specific point in the map." - "If local_frame is set to true, it navigates locally, where the robot is currently (0,0)" - ) - - def execute(self, x: float, y: float, theta_degrees: float = 0.0, local_frame: bool = False, **legacy): + """Use when you need to navigate the robot to the specified position + using provided x, y coordinates (meters), and theta_degrees (yaw) IN DEGREES. + If local_frame is set to false, it navigates to a specific point in the map. + If local_frame is set to true, it navigates locally, where the robot is currently (0,0)""" + + odom: Odometry + """Resolves local_frame goals; see Nav2Controller._resolve_goal.""" + + @resource + def controller(self) -> Iterator[Nav2Controller]: + controller = Nav2Controller(self) + yield controller + controller.destroy() + + def execute( + self, x: float, y: float, theta_degrees: float = 0.0, local_frame: bool = False, **legacy + ) -> SkillReturn: # The tool schema speaks theta_degrees, but the cloud agent and the - # pose-adjustment pipeline speak `theta` in radians (the same keys the - # robot's pose payload uses). The alias lives in **legacy so schema - # introspection doesn't surface it as a second UI field. + # pose-adjustment pipeline speak `theta` in radians. if legacy.get("theta") is not None: theta = float(legacy["theta"]) theta_degrees = math.degrees(theta) else: theta = math.radians(theta_degrees) - self.logger.info( - f"Initiating navigation to position: x={x}, y={y}, theta_degrees={theta_degrees}, local_frame={local_frame}" - ) - - result, detail = self.nav2_controller.go_to_position(x, y, theta, local_frame) - - # Check if the navigation was canceled - if result == TaskResult.CANCELED: - self.logger.info("Navigation was canceled") - return "Navigation canceled", SkillResult.CANCELLED - elif result == TaskResult.SUCCEEDED: - self.logger.info( - f"Navigation complete. Arrived at position: x={x}, y={y}, theta_degrees={theta_degrees}, local_frame={local_frame}" - ) - return f"Reached position ({x}, {y}, {theta_degrees} deg)", SkillResult.SUCCESS - else: - goal_desc = f"({x}, {y}, {theta_degrees} deg, {'local' if local_frame else 'map'} frame)" - message = f"Navigation to {goal_desc} failed: {detail}" - self.logger.info(message) - return message, SkillResult.FAILURE - - def cancel(self): - """ - Cancels the current navigation task. - """ - self.logger.debug("Canceling navigation task") - self.nav2_controller.cancel_navigation() - return "Navigation canceled" - - def shutdown(self): - """Destroy this instance's navigator nodes when the server retires it.""" - self.nav2_controller.destroy() + self.logger.info(f"Navigating to x={x}, y={y}, theta_degrees={theta_degrees}, local_frame={local_frame}") + + goal_desc = f"({x}, {y}, {theta_degrees} deg, {'local' if local_frame else 'map'} frame)" + try: + self.controller.go_to_position(x, y, theta, local_frame) + except SkillFailed as e: + self.fail(f"Navigation to {goal_desc} failed: {e}") + return f"Reached position ({x}, {y}, {theta_degrees} deg)" diff --git a/workspace/innate_skills/navigate_with_vision.py b/workspace/innate_skills/navigate_with_vision.py index 0325e4321..4235f4e25 100644 --- a/workspace/innate_skills/navigate_with_vision.py +++ b/workspace/innate_skills/navigate_with_vision.py @@ -1,24 +1,13 @@ -#!/usr/bin/env python3 # SPDX-License-Identifier: Apache-2.0 # Copyright (c) 2026 Innate Inc -""" -Navigate With Vision Skill — sends a natural-language navigation instruction -to the UniNavid cloud service and follows the returned action commands until -the goal is reached (or canceled). - -Uses the ``navigate_instruction`` ROS 2 action server exposed by the -``innate_uninavid`` node. -""" - import threading from action_msgs.msg import GoalStatus from innate_cloud_msgs.action import NavigateInstruction from rclpy.action import ActionClient -from brain_client.skills.types import Skill, SkillResult +from innate import Skill, SkillCancelled, SkillReturn, resource -# Human-readable labels for the integer action codes returned by the server. _ACTION_LABELS = { 0: "STOP", 1: "FORWARD", @@ -27,184 +16,92 @@ } +class _NavigateClient: + """The UniNavid action client, built on first use. No explicit teardown: + it lives on the run's throwaway node and dies with it at run end.""" + + def __init__(self, skill: Skill): + self._client = ActionClient(skill.node, NavigateInstruction, "/navigate_instruction") + + def wait_for_server(self, timeout_sec: float) -> bool: + return self._client.wait_for_server(timeout_sec=timeout_sec) + + def send_goal_async(self, goal, feedback_callback=None): + return self._client.send_goal_async(goal, feedback_callback=feedback_callback) + + class NavigateWithVision(Skill): - """Send text navigation instructions to the UniNavid cloud service.""" - - def __init__(self, logger): - super().__init__(logger) - self._action_client: ActionClient | None = None - self._goal_handle = None - self._cancel_requested = threading.Event() - self._last_feedback_action: int | None = None - self._last_feedback_stops: int = 0 - - # ── Skill interface ─────────────────────────────────────────────────────── - - @property - def name(self): - return "navigate_with_vision" - - def guidelines(self): - return ( - "Use when you want the robot to navigate using camera vision and a " - "natural-language instruction (e.g. 'walk to the red chair and stop'). " - "The instruction is sent to the UniNavid cloud service which streams " - "back movement commands until the goal is reached. " - "Requires param 'instruction' (str)." - "Some other examples of instructions are 'move to the nearest sofa and then stop', 'follow the human wearing black pants'." - ) - - # ── Execution ───────────────────────────────────────────────────────────── - - def execute(self, instruction: str): - """Send *instruction* to UniNavid and block until the goal finishes. - - Args: - instruction: A natural-language navigation command, - e.g. ``"walk to the red chair and stop"``. - - Returns: - tuple: ``(result_message, SkillResult)`` - """ - if not self.node: - msg = "Navigation skill has no ROS node and cannot execute." - self.logger.error(msg) - self._send_feedback(msg) - return msg, SkillResult.FAILURE - - self._cancel_requested.clear() - - # Lazily create the action client (same pattern as PhysicalSkill) - if self._action_client is None: - self._action_client = ActionClient(self.node, NavigateInstruction, "/navigate_instruction") + """Use when you want the robot to navigate using camera vision and a + natural-language instruction (e.g. 'walk to the red chair and stop'). + The instruction is sent to the UniNavid cloud service which streams + back movement commands until the goal is reached. + Requires param 'instruction' (str). + Some other examples of instructions are 'move to the nearest sofa and then stop', 'follow the human wearing black pants'.""" + + _last_feedback_action: int | None = None + + @resource + def client(self) -> _NavigateClient: + return _NavigateClient(self) + def execute(self, instruction: str) -> SkillReturn: self.logger.info(f"[NavigateWithVision] Instruction: {instruction!r}") - self._send_feedback(f"Sending instruction: {instruction}") + self.feedback(f"Sending instruction: {instruction}") - # ── Wait for the action server ──────────────────────────────────────── - if not self._action_client.wait_for_server(timeout_sec=10.0): - msg = "Navigation server is not available. Please try again." - self.logger.error(msg) - self._send_feedback(msg) - return msg, SkillResult.FAILURE + if not self.client.wait_for_server(timeout_sec=10.0): + self.fail("Navigation server is not available. Please try again.") - # ── Send goal ───────────────────────────────────────────────────────── goal_msg = NavigateInstruction.Goal() goal_msg.instruction = instruction + goal_future = self.client.send_goal_async(goal_msg, feedback_callback=self._on_feedback) + if not self._wait_for_future(goal_future, timeout_sec=10.0): + self.fail("Navigation goal timed out waiting for acceptance.") - goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self._on_feedback) + goal_handle = goal_future.result() + if goal_handle is None or not goal_handle.accepted: + self.fail("Navigation goal was rejected.") - if not self._wait_for_future(goal_future, timeout_sec=10.0): - msg = "Navigation goal timed out waiting for acceptance." - self.logger.error(msg) - self._send_feedback(msg) - return msg, SkillResult.FAILURE - - self._goal_handle = goal_future.result() - if not self._goal_handle.accepted: - msg = "Navigation goal was rejected." - self.logger.info(msg) - self._send_feedback(msg) - return msg, SkillResult.FAILURE - - self.logger.info("Goal accepted — waiting for result …") - self._send_feedback("Navigation started, waiting for completion …") - - result_future = self._goal_handle.get_result_async() - - # Wait for the result WITHOUT re-spinning self.node — the skills_server's - # dedicated executor already services the result/feedback callbacks. Register - # the done-callback once and poll the Event in short slices so we can react - # to a cancel request; registering per-iteration would pile up closures on a - # long-running goal. Calling rclpy.spin_until_future_complete(self.node, …) - # here would add this node to the global executor and race the dedicated one, - # corrupting the shared wait set (RCLError "wait set index … out of bounds" - # → SIGABRT). + # A cancel must reach the action server immediately, not at the next poll. + self.on_cancel(goal_handle.cancel_goal_async) + self.feedback("Navigation started, waiting for completion ...") + + # The skills_server's dedicated executor services the result callback; + # spinning self.node here would race it and corrupt the wait set. + result_future = goal_handle.get_result_async() result_ready = threading.Event() result_future.add_done_callback(lambda _future: result_ready.set()) - while not result_ready.wait(timeout=0.25): - if self._cancel_requested.is_set(): - self.logger.info("Cancel requested — forwarding to action server") - self._goal_handle.cancel_goal_async() - # Wait for the server to acknowledge the cancel. - result_ready.wait(timeout=10.0) - break - - if not result_future.done(): - msg = "Navigation timed out." - self.logger.error(msg) - self._send_feedback(msg) - self._goal_handle = None - return msg, SkillResult.FAILURE + while not result_ready.is_set(): + self.sleep(0.25) result_response = result_future.result() status = result_response.status - result = result_response.result - - self._goal_handle = None + message = result_response.result.message if status == GoalStatus.STATUS_SUCCEEDED: - msg = result.message or "Navigation completed" - self.logger.info(f"Goal succeeded: {msg}") - self._send_feedback(msg) - return msg, SkillResult.SUCCESS - - if status in (GoalStatus.STATUS_CANCELED, GoalStatus.STATUS_ABORTED): - msg = result.message or "Navigation canceled" - self.logger.info(f"Goal canceled/aborted: {msg}") - self._send_feedback(msg) - return msg, SkillResult.CANCELLED - - msg = result.message or "Navigation ended unexpectedly." - self.logger.warning(msg) - self._send_feedback(msg) - return msg, SkillResult.FAILURE - - # ── Future waiting (no node re-spin) ────────────────────────────────────── + msg = message or "Navigation completed" + self.feedback(msg) + return msg + if status == GoalStatus.STATUS_CANCELED: + msg = message or "Navigation canceled" + self.feedback(msg) + raise SkillCancelled(msg) + if status == GoalStatus.STATUS_ABORTED: + self.fail(message or "Navigation aborted.") + self.fail(message or "Navigation ended unexpectedly.") def _wait_for_future(self, future, timeout_sec=None): - """Block until *future* completes, without spinning self.node. - - The skills_server node is already spun by its dedicated executor, which - services this future's done-callback on another thread. We just wait on - that callback via an Event. Returns True if the future completed, False - on timeout. - """ if future.done(): return True done_event = threading.Event() future.add_done_callback(lambda _future: done_event.set()) return done_event.wait(timeout=timeout_sec) - # ── Feedback callback (called on the executor thread) ───────────────────── - def _on_feedback(self, feedback_msg): - """Relay action feedback to the brain as a human-readable string. - - Only sends when the action changes or every 5th consecutive stop - to avoid flooding the brain logs. - """ fb = feedback_msg.feedback action_label = _ACTION_LABELS.get(fb.latest_action, str(fb.latest_action)) - text = f"Action: {action_label} | Consecutive stops: {fb.consecutive_stops}/{fb.max_consecutive_stops}" - self.logger.debug(f"[NavigateWithVision] feedback: {text}") - - action_changed = ( - fb.latest_action != self._last_feedback_action and fb.latest_action != 0 # don't report individual STOPs - ) - stops_milestone = False # no stop-count feedback + action_changed = fb.latest_action != self._last_feedback_action and fb.latest_action != 0 self._last_feedback_action = fb.latest_action - self._last_feedback_stops = fb.consecutive_stops - - if action_changed or stops_milestone: - self._send_feedback(text) - - # ── Cancellation ────────────────────────────────────────────────────────── - - def cancel(self): - """Request cancellation of the running navigation goal.""" - self.logger.info("[NavigateWithVision] Cancel requested") - self._cancel_requested.set() - if self._goal_handle is not None: - self._goal_handle.cancel_goal_async() - return "Cancellation requested for navigate_with_vision" + if action_changed: + self.feedback( + f"Action: {action_label} | Consecutive stops: {fb.consecutive_stops}/{fb.max_consecutive_stops}" + ) diff --git a/workspace/innate_skills/pick_any_object.py b/workspace/innate_skills/pick_any_object.py new file mode 100644 index 000000000..437482af9 --- /dev/null +++ b/workspace/innate_skills/pick_any_object.py @@ -0,0 +1,800 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +"""Pick an object from the floor by text prompt. + +Metric localize (pixel -> floor -> base_link), drive into a pick box, +grasp (optional wrist visual-servo), verify by backing up. +No depth camera — URDF + pinhole model. +""" + +import math +import re +import time + +from brain_client.robot.manipulation import ArmFailed, ArmUnhealthy +from innate import ( + Head, + JointStates, + MainImage, + Manipulation, + Mobility, + Odometry, + Skill, + SkillFailed, + SkillReturn, + WristImage, + resource, + vision, +) +from innate import gemini as gemlib +from innate.geometry import IMG_H, IMG_W, floor_to_pixel, pixel_to_floor + +GRIPPER_EMPTY_J6 = -0.085 +VERIFY_BACKUP_M = 0.15 +# Clamp for Gemini's per-object grip_strength. Grip force lives in hardware +# (current cap); this only serves as the soft/rigid signal for the fabric twist. +GRIP_STRENGTH_RANGE = (0.30, 0.60) +SOFT_GRIP_MIN = 0.5 + +# Post-pick carry pose (j1-5). j6 comes from close_strength, not this pose. +# j2 = -0.50: the coupling limit at j1~0.05 clamps past it and logs forever. +CARRY_ARM = [0.0537, -0.50, 0.4157, 0.9434, -0.0077] + +# Pick parameters, tuned on hardware. +PARAMS = { + # FIND / LOCALIZE + "tilt_deg": -20.0, + "settle_s": 1.2, + # POSITION. sweet_x ceiling is 0.43 (reach clamp); 0.37 grasps at ~0.32 — + # grasping at the 0.40 reach edge stalls the wrist and overloads servo 2. + "sweet_x": 0.37, + "box_y": 0.0, + "box_half_px": 40.0, + "accept_frac": 0.5, + "box_steps": 6.0, + "bearing_go_deg": 4.0, + "follow_gain_ang": 0.3, + "follow_gain_lin": 0.06, + "rot_tol_deg": 2.5, + "rot_kp": 1.2, + "rot_wz_max": 0.5, + "rot_wz_min": 0.15, + "drive_tol_m": 0.015, + "drive_kp": 0.3, + "drive_v_max": 0.10, + "drive_v_min": 0.04, + # WRIST ALIGN (0 wrist_steps = blind grasp) + "wrist_steps": 2.0, + "wrist_stop_z": 0.05, + "wrist_z_step": 0.01, + "wrist_move_s": 0.5, + "wrist_pitch": 0.82, + "wrist_box_u": 320.0, + # Below image center: the wrist cam sits above the fingertips, so + # mid-frame aims short of them. 350 is the hardware-tuned parallax bias. + "wrist_box_v": 350.0, + "wrist_half_px": 60.0, + "wrist_kx": -0.04, + "wrist_ky": -0.04, + "wrist_step_max": 0.04, + "wrist_settle_s": 0.8, + # GRASP + "grasp_x_off": 0.05, + "hover_z": 0.15, + "hover_s": 2.0, + "descend_z1": 0.10, + "descend_z2": 0.07, + "descend_z3": 0.045, + # ee_link target, not fingertip height. 0.01 dug into carpet and aborted. + "floor_z": 0.03, + "descend_s": 1.2, + "descend_abort_z": 0.12, + "arm_pitch": 1.30, + # close_strength is close depth, not force (servo 6 runs current-based + # position control); above 0.6 the servo trips and needs a reboot. + "close_strength": 0.60, + "close_s": 1.5, + "close_settle_s": 0.8, + # Un-press before closing: the descent parks the fingers pressed into the + # floor; a small lift lets them close around the object, not drag it. + "close_lift_m": 0.01, + "twist_rad": 0.6, + "lift_rad": 0.6, +} + +FOLLOW_TIMEOUT_S = 20.0 +WRIST_ALIGN_TIMEOUT_S = 60.0 +WRIST_MAX_JUMP_PX = 80.0 +WRIST_SEG_MIN_SCORE = 25.0 +WRIST_CAM_ABOVE_EE = 0.07 +WRIST_SEARCH_ARM = [0.1473, -0.0706, -0.4449, 1.3376, -0.0491] + + +def _inside_box(px, cu, cv, half): + return abs(px[0] - cu) <= half and abs(px[1] - cv) <= half + + +class _BlobTracker: + """CamShift color-blob tracker seeded from a Gemini box.""" + + def __init__(self, hsv, box, px): + self.model = vision.seg_model(hsv, box) + self.window = box + self.guess = px + self.misses = 0 + + @property + def ok(self): + return self.model is not None + + def update(self, hsv): + """Blob center, or None on miss (keeps last window for retry).""" + pt, window, _score = vision.seg_track(hsv, self.model, self.window, min_score=WRIST_SEG_MIN_SCORE) + if pt is not None and math.hypot(pt[0] - self.guess[0], pt[1] - self.guess[1]) > WRIST_MAX_JUMP_PX: + pt = None + if pt is None: + self.misses += 1 + return None + self.misses = 0 + self.window = window + self.guess = pt + return pt + + +class PickAnyObject(Skill): + """Pick up an object lying on the floor, described in natural language + (e.g. prompt='the white sock', 'a red cup'). The robot localizes the + object metrically with the head camera, drives above it, grasps, and + verifies the grasp by backing up and checking the floor. The arm is + returned to rest either way.""" + + manipulation: Manipulation + mobility: Mobility + head: Head + # `| None` — best effort, every read is guarded: positioning falls back + # to stepwise re-detection without head frames, the wrist stage degrades + # to the blind grasp without wrist frames. + main_image: MainImage | None + wrist_image: WristImage | None + joint_states: JointStates | None + odom: Odometry | None + + _p = PARAMS + _grip_strength: float | None = None + _holding = False # fingers committed on an object this run + + @resource + def _proxy(self): + return gemlib.make_client() + + def _detect_px(self, prompt): + """Head frame -> best grasp pixel, or None. Also records Gemini's + per-object grip_strength for the close.""" + self.mobility.stop() + self.sleep(self._p["settle_s"]) + img = self.main_image + if not img: + return None + text = gemlib.ask_image( + self._proxy, + img, + f"Find '{prompt}' lying on the floor in this image. Match precisely — " + "not paper/packaging when asked for clothing, and NOT anything held " + "by the robot arm. Return ONLY a JSON list of matches, each " + '{"box_2d":[ymin,xmin,ymax,xmax], "grasp_point":[y,x], ' + '"grip_strength":s} normalized 0-1000, best first. grasp_point is ' + "the CENTER of the object (geometric middle of the visible blob), " + "not an edge or tip. grip_strength is how hard a parallel gripper " + "should squeeze this object, 0.30-0.60: soft/deformable objects " + "(socks, fabric, plush) need 0.60 or they slip out; rigid/hard " + "objects (metal, hard plastic, wood, ceramic) need 0.30-0.40 — " + "squeezing them harder stalls the gripper servo. " + "Empty list if not present.", + logger=self.logger, + ) + px = vision.parse_det_px(text) + grip = vision.parse_det_grip(text) + if px is not None and grip is not None: + lo, hi = GRIP_STRENGTH_RANGE + self._grip_strength = max(lo, min(hi, grip)) + return px + + def _localize_px(self, prompt): + """Detect + back-project -> ((x,y)|None, pixel|None).""" + px = self._detect_px(prompt) + if px is None: + return None, None + xy = pixel_to_floor(px[0], px[1], self._p["tilt_deg"]) + if xy: + self.logger.info(f"[PickAnyObject] px=({px[0]:.0f},{px[1]:.0f}) -> base_link ({xy[0]:.3f},{xy[1]:.3f})") + return xy, px + + def _localize_retry(self, prompt): + """One retry: a single "not visible" is noise, not absence.""" + xy, px = self._localize_px(prompt) + if px is None: + xy, px = self._localize_px(prompt) + return xy, px + + def _odom_xyt(self): + return self.mobility.odom_xyt(self.odom) + + def _rotate_by(self, angle): + return self.mobility.rotate_by( + self._odom_xyt, + angle, + kp=self._p["rot_kp"], + wz_max=self._p["rot_wz_max"], + wz_min=self._p["rot_wz_min"], + tol=math.radians(self._p["rot_tol_deg"]), + logger=self.logger, + ) + + def _drive(self, dist): + return self.mobility.drive( + self._odom_xyt, + dist, + kp=self._p["drive_kp"], + v_max=self._p["drive_v_max"], + v_min=self._p["drive_v_min"], + tol=self._p["drive_tol_m"], + logger=self.logger, + ) + + def _rest_arm(self, keep_grip): + """Best-effort teardown: carry if holding, else fold to rest. Never + raises. REST, not ZERO: after a failed descent the arm can be near the + floor, and the zero posture would sweep the gripper through it.""" + joints = CARRY_ARM + [-self._p["close_strength"]] if keep_grip else list(self.manipulation.REST) + try: + self.manipulation.go(joints, duration=3.0, times=2) + except Exception as e: # noqa: BLE001 — teardown must not mask the run result + self.logger.warning(f"[PickAnyObject] rest-arm failed: {e}") + + def _search(self, prompt): + """Scan: straight, right 30°, left 60°. First hit wins. (+yaw=left)""" + for i, turn in enumerate((0.0, -math.radians(30), math.radians(60))): + if turn: + if i == 1: + self.say("Scanning around for it.") + # Best-effort: a rotate cut short (timeout / odom loss) still + # changed the view, and the localize below measures from + # wherever the base actually ended up. + self._rotate_by(turn) + xy, _px = self._localize_px(prompt) + if xy is not None: + return xy + raise SkillFailed(f"Could not find '{prompt}' on the floor, even after scanning") + + def _sweet_box(self): + """(center_px, outer_half, accept_half). Stop only inside accept.""" + c = floor_to_pixel(self._p["sweet_x"], self._p["box_y"], self._p["tilt_deg"]) + if c is None or not (0 <= c[0] < IMG_W and 0 <= c[1] < IMG_H): + raise SkillFailed("pick box off-image — check tilt_deg/sweet_x") + half = self._p["box_half_px"] + return (c[0], c[1]), half, half * self._p["accept_frac"] + + def _follow_into_box(self, seed_px): + """Optical-flow base servo into pick box. No Gemini. + Returns ('in_box'|'lost'|'timeout'|'noframe', px|None).""" + raw = self.main_image + prev = vision.b64_to_gray(raw) if raw else None + if prev is None: + return "noframe", None + u, v = seed_px + grid = vision.grid_pts(u, v) + in_box = 0 + (cu, cv), _half, accept = self._sweet_box() + t0 = time.monotonic() + while time.monotonic() - t0 < FOLLOW_TIMEOUT_S: + # Only track NEW frames: the camera runs slower than this loop, + # and a stale frame re-tracked would count one observation twice. + # Compare by identity, not content: the provider builds one Image + # per ROS message, and in sim consecutive frames of a static scene + # are byte-identical, so `==` would deadlock waiting for a change. + img = self.main_image + if not img or img is raw: + self.sleep(0.03) + continue + gray = vision.b64_to_gray(img) + raw = img + if gray is None: + self.sleep(0.03) + continue + tracked = vision.track_point(prev, gray, grid) + prev = gray + if tracked is None: + self.mobility.stop() + return "lost", None + u, v = tracked + grid = vision.grid_pts(u, v) + if not (0 <= u < IMG_W and 0 <= v < IMG_H): + self.mobility.stop() + return "lost", None + + if _inside_box((u, v), cu, cv, accept): + in_box += 1 + self.mobility.stop() + if in_box >= 3: + return "in_box", (u, v) + self.sleep(0.03) + continue + in_box = 0 + + # Deadband = accept (inner) box; right -> -wz, too close (low) -> -vx. + wz = self.mobility.servo_vel( + u - cu, self._p["follow_gain_ang"], self._p["rot_wz_min"], self._p["rot_wz_max"], accept + ) + vx = self.mobility.servo_vel( + v - cv, self._p["follow_gain_lin"], self._p["drive_v_min"], self._p["drive_v_max"], accept + ) + self.mobility.send_cmd_vel(vx, wz, 0.15) + self.sleep(0.03) + self.mobility.stop() + return "timeout", None + + def _position_failed(self, prompt): + raise SkillFailed(f"Could not centre '{prompt}' in the pick box") + + def _position_above(self, prompt, xy): + """Flow-follow into pick box; Gemini reseed/confirm. Stepwise if no cam. + Raises SkillFailed if the object cannot be centred.""" + if not self.main_image: + return self._position_stepwise(prompt, xy) + + seed = floor_to_pixel(xy[0], xy[1], self._p["tilt_deg"]) + for _attempt in range(int(self._p["box_steps"])): + if seed is None: + seed = self._detect_px(prompt) or self._detect_px(prompt) + if seed is None: + self._position_failed(prompt) + result, _pt = self._follow_into_box(seed) + if result == "noframe": + return self._position_stepwise(prompt, xy) + if result == "lost": + seed = None + continue + xy2, px2 = self._localize_retry(prompt) + if px2 is None: + self._position_failed(prompt) + (cu, cv), _half, accept = self._sweet_box() + if xy2 is not None and _inside_box(px2, cu, cv, accept): + return xy2 + seed = px2 if xy2 is not None else None + self._position_failed(prompt) + + def _position_stepwise(self, prompt, xy): + """No-camera fallback: turn OR drive, re-detect, repeat. + Raises SkillFailed if the object cannot be centred.""" + target_bearing = math.atan2(self._p["box_y"], self._p["sweet_x"]) + target_range = math.hypot(self._p["sweet_x"], self._p["box_y"]) + px = floor_to_pixel(xy[0], xy[1], self._p["tilt_deg"]) + for _step in range(int(self._p["box_steps"])): + if px is None: + xy, px = self._localize_retry(prompt) + if px is None: + self._position_failed(prompt) + (cu, cv), _half, accept = self._sweet_box() + if xy is not None and _inside_box(px, cu, cv, accept): + return xy + if xy is None: + px = None + continue + bearing_err = math.atan2(xy[1], xy[0]) - target_bearing + if abs(bearing_err) > math.radians(self._p["bearing_go_deg"]): + moved = self._rotate_by(bearing_err) + else: + moved = self._drive(math.hypot(xy[0], xy[1]) - target_range) + if not moved: + # Odom loss or a stuck base: this closed-odometry stepper + # cannot make progress, so burning the remaining steps (a + # Gemini localize each) would just end in a misleading + # "could not centre". + raise SkillFailed("Base positioning failed (odometry lost or motion timed out)") + px = None + self._position_failed(prompt) + + # GRASP: search pose -> seed -> servo down -> blind push -> close/twist/lift + def _wrist_seed(self, prompt): + """Wrist Gemini box -> (center_px, box) or (None, None).""" + self.sleep(self._p["wrist_settle_s"]) + img = self.wrist_image + text = ( + gemlib.ask_image( + self._proxy, + img, + f"Wrist camera on a robot gripper, looking down at the floor. " + f"Find '{prompt}' on the floor. Ignore the gripper fingers " + "themselves. Return ONLY a JSON list of matches, each " + '{"box_2d":[ymin,xmin,ymax,xmax]} normalized 0-1000, best first, ' + "each box TIGHT around its object. Empty list if not visible.", + logger=self.logger, + ) + if img + else None + ) + box = vision.parse_det_box(text) + px = (box[0] + box[2] / 2.0, box[1] + box[3] / 2.0) if box else None + return px, box + + def _next_wrist_hsv(self, last_b64, timeout=1.5): + """Wait for a new wrist frame -> (hsv|None, b64).""" + t0 = time.monotonic() + while time.monotonic() - t0 < timeout: + # Identity, not content: sim frames can be byte-identical (see + # _follow_into_box). + img = self.wrist_image + if img and img is not last_b64: + # Consume the frame even when it won't decode (truncated JPEG + # under load): re-decoding the same buffer every poll would + # burn the whole timeout on ~40 decodes of a known-bad frame. + last_b64 = img + hsv = vision.b64_to_hsv(img) + if hsv is not None: + return hsv, img + self.sleep(0.04) + return None, last_b64 + + def _wrist_done(self, x, y, z, reason): + self.logger.info(f"[PickAnyObject] wrist stage: {reason} (z={z:.3f})") + return x, y, z + + def _wrist_reseed(self, prompt, raw): + """Persistent tracking loss: one Gemini look + a fresh color model, + since the view has changed. -> (tracker|None, raw, fail_reason).""" + px, box = self._wrist_seed(prompt) + if px is None: + return None, raw, "lost track" + hsv, raw = self._next_wrist_hsv(raw) + if hsv is None: + return None, raw, "no wrist frames" + tracker = _BlobTracker(hsv, box, px) + if not tracker.ok: + return None, raw, "lost track" + return tracker, raw, "" + + def _wrist_descend(self, prompt, tx, ty): + """Wrist CamShift servo down to wrist_stop_z: nudge toward the wrist + box, or step down once the object has been seen inside it twice. + A miss gets 2 frames of patience, then a Gemini re-seed (budget = + wrist_steps - 1). Color model, not LK: the object grows/deforms + during the descent and optical flow slides off. + Returns (x,y,z); falls back to (tx,ty) if never seen.""" + p = self._p + ee = self.manipulation.ee_xyz() + z = ee[2] if ee else p["hover_z"] + looks = int(p["wrist_steps"]) - 1 + + px, box = self._wrist_seed(prompt) + if px is None: + return self._wrist_done(tx, ty, z, "not seen") + x, y = (ee[0], ee[1]) if ee else (tx, ty) + + hsv, raw = self._next_wrist_hsv(None) + if hsv is None: + return self._wrist_done(tx, ty, z, "no wrist frames") + tracker = _BlobTracker(hsv, box, px) + if not tracker.ok: + return self._wrist_done(tx, ty, z, "not seen") + + deadline = time.monotonic() + WRIST_ALIGN_TIMEOUT_S + streak = 0 # verified matches since the arm last moved + centered = 0 # consecutive matches INSIDE the box + stalled = 0 # consecutive steps eaten by the reach clamp + reason = "reached stop z" + while z > p["wrist_stop_z"] + 1e-6: + # Explicit cancel point: with a fresh frame already buffered (the + # common case after each blocking move) _next_wrist_hsv returns + # without ever sleeping, so a Stop would ride the whole descent. + self.check_cancelled() + if time.monotonic() > deadline: + reason = "timeout" + break + hsv, raw = self._next_wrist_hsv(raw) + if hsv is None: + reason = "no wrist frames" + break + + px = tracker.update(hsv) + if px is None: + streak = centered = 0 + if tracker.misses < 3: + continue # transient (blur / mid-move frame) — wait + if looks <= 0: + reason = "lost track" + break + looks -= 1 + tracker, raw, fail = self._wrist_reseed(prompt, raw) + if tracker is None: + reason = fail + break + px = tracker.guess + streak += 1 + + err_u = px[0] - p["wrist_box_u"] + err_v = px[1] - p["wrist_box_v"] + inside = _inside_box(px, p["wrist_box_u"], p["wrist_box_v"], p["wrist_half_px"]) + centered = centered + 1 if inside else 0 + if streak < 2: + continue # watch one more frame before trusting it + if inside and centered < 2: + continue # just entered the box — confirm it stays + + stepped_down = centered >= 2 + if stepped_down: + z = max(p["wrist_stop_z"], z - p["wrist_z_step"]) + # Descending IS progress: only consecutive clamped nudges count + # as stalled, or three clamps spread across a long tracking + # descent would abort a servo that is working. + stalled = 0 + else: + # Gains tuned at z=0.15; scale with camera height. + s = (z + WRIST_CAM_ABOVE_EE) / (0.15 + WRIST_CAM_ABOVE_EE) + cap = p["wrist_step_max"] + step_x = max(-cap, min(cap, p["wrist_kx"] / 100.0 * err_v * s)) + step_y = max(-cap, min(cap, p["wrist_ky"] / 100.0 * err_u * s)) + nx, ny = self.manipulation.clamp_reach(x + step_x, y + step_y) + # The reach clamp can eat the whole step (object at the edge of + # the reach box) — hand off to the blind push instead of + # re-commanding the same clamped pose until timeout. + if math.hypot(nx - x, ny - y) < 0.25 * math.hypot(step_x, step_y): + stalled += 1 + if stalled >= 3: + reason = "reach limit" + break + continue + stalled = 0 + x, y = nx, ny + tracker.guess = (p["wrist_box_u"], p["wrist_box_v"]) + self.manipulation.move_checked( + x, y, z, pitch=p["wrist_pitch"], duration=p["wrist_move_s"], logger=self.logger + ) + if stepped_down: + # A pure z-hop barely shifts the view: one fresh confirming + # frame is enough, so hops chain instead of re-earning 2+2. + streak = 1 + else: + streak = 0 + centered = 0 # view shifted — re-confirm centering + + return self._wrist_done(x, y, z, reason) + + def _goto_search_pose(self, bearing): + """WRIST_SEARCH_ARM aimed at bearing; pins IK to elbow-up branch. + + j6 commands GRIPPER_OPEN instead of echoing a live j6 read: right + after the gripper open the joint_states snapshot is one tick + stale, and re-commanding it would close the just-opened gripper.""" + a = WRIST_SEARCH_ARM + pose = [bearing, a[1], a[2], self._p["wrist_pitch"] - a[1] - a[2], a[4], self.manipulation.GRIPPER_OPEN] + self.manipulation.go(pose, duration=self._p["hover_s"], logger=self.logger) + self.sleep(0.3) + + def _open_gripper_checked(self): + """Open gripper (the interface reboots + retries a tripped servo). + False if still shut.""" + self.manipulation.torque_on() + return self.manipulation.open_gripper(duration=1.0, blocking=True) + + def _push_to_floor(self, x, y, z_from): + """Blind descent to floor as ONE multi-waypoint trajectory — the + rung-by-rung version decelerated at every rung and looked choppy. + Contact just stalls the final segments; abort if still high.""" + p = self._p + self.check_cancelled() + rungs = [z for z in (p["descend_z1"], p["descend_z2"], p["descend_z3"], p["floor_z"]) if z < z_from - 1e-6] + if rungs: + if len(rungs) >= 2: # the trajectory service needs >= 2 waypoints + poses = [{"x": x, "y": y, "z": z, "roll": 0.0, "pitch": p["arm_pitch"], "yaw": 0.0} for z in rungs] + ok = self.manipulation.move_cartesian_trajectory( + poses, + segment_duration=p["descend_s"], + gripper_position=self.manipulation.GRIPPER_OPEN, + ) + else: + # gripper_position pinned like the trajectory branch: without + # it j6 is re-seeded from the measured arm state, and a claw + # that drifted shut during the wrist descent stays shut. + ok = self.manipulation.move_to_cartesian_pose( + x=x, + y=y, + z=rungs[0], + roll=0.0, + pitch=p["arm_pitch"], + yaw=0.0, + duration=p["descend_s"], + blocking=True, + gripper_position=self.manipulation.GRIPPER_OPEN, + ) + if not ok: + self.manipulation.recover(self.logger) + # Covers the blind path only: after a wrist align the EE already + # starts below the abort height — a limp arm there is caught by + # _grasp_verified instead. + ee = self.manipulation.ee_xyz() + if ee is not None and ee[2] > p["descend_abort_z"]: + self.manipulation.recover(self.logger) + raise ArmUnhealthy("arm would not descend") + + def _arm_joints(self): + """The 6 current joint positions; raises LookupError when joint + states are missing or short (callers fall back to IK).""" + js = self.joint_states + if js is None or len(js.position) < 6: + raise LookupError("joint states missing or short") + return list(js.position[:6]) + + def _close_twist_lift(self, x, y): + """Close, joint-space twist+lift (IK would unwind j5). Uses time.sleep + on purpose: the fingers have committed, and a cancel must not unwind + mid-grip — the run finishes this and the teardown carries the object. + Closing on air reaches ~GRIPPER_EMPTY_J6, which _grasp_verified's j6 + check catches.""" + p = self._p + if p["close_lift_m"] > 0: + ee = self.manipulation.ee_xyz() + if ee is not None: + self.manipulation.move_to_cartesian_pose( + x=x, + y=y, + z=ee[2] + p["close_lift_m"], + roll=0.0, + pitch=p["arm_pitch"], + yaw=0.0, + duration=0.5, + blocking=True, + ) + if not self.manipulation.close_gripper(strength=p["close_strength"], duration=p["close_s"], blocking=True): + raise ArmUnhealthy("gripper would not close") + # Fingers have committed: from here teardown must fold with the grip + # kept, not open over the floor mid-carry — only a verified miss + # clears the flag. Set here, not after _grasp_at returns: an exception + # in the twist/lift below (e.g. ArmUnhealthy from the LookupError + # fallback) must not release a just-grasped object on the way home. + self._holding = True + time.sleep(p["close_settle_s"]) + + grip = -p["close_strength"] + # The twist winds FABRIC onto the fingers; on a rigid shell it helps + # eject the object. Gemini's grip_strength doubles as the hardness signal. + soft = self._grip_strength is None or self._grip_strength >= SOFT_GRIP_MIN + lifted = False + try: + if soft: + j = self._arm_joints() + j[4] = max(-1.4, min(1.4, j[4] + p["twist_rad"])) + j[5] = grip + self.manipulation.move_to_joint_positions(joint_positions=j, duration=1.0, blocking=True) + time.sleep(0.3) + j = self._arm_joints() + j[1] = max(-1.4, j[1] - p["lift_rad"]) + j[5] = grip + # move_to_joint_positions reports failure by returning False, not + # raising — a silently skipped lift would leave the EE at floor_z + # for _grasp_verified's 0.15 m backup drive. + lifted = self.manipulation.move_to_joint_positions(joint_positions=j, duration=2.0, blocking=True) + time.sleep(0.3) + except LookupError: + pass + if not lifted: + # gripper=grip, never the default: the default re-seeds j6 from the + # measured (stalled) position, zeroing the grip preload — the + # object would drop out mid-lift. move_checked verifies by FK and + # recover-retries, so the arm is confirmed off the floor (or the + # run fails cleanly and teardown carries with the grip kept). + self.manipulation.move_checked( + x, y, 0.22, pitch=p["arm_pitch"], duration=2.0, tol_xy=0.10, gripper=grip, logger=self.logger + ) + + def _grasp_at(self, prompt, xy): + """Full grasp at floor xy (base_link).""" + p = self._p + x, y = self.manipulation.clamp_reach(xy[0] - p["grasp_x_off"], xy[1]) + + if not self._open_gripper_checked(): + raise ArmUnhealthy("gripper would not open") + if p["wrist_steps"] >= 1: + self._goto_search_pose(math.atan2(y, x)) + x, y, z = self._wrist_descend(prompt, x, y) + else: + z = p["hover_z"] + self.manipulation.move_checked(x, y, z, pitch=p["arm_pitch"], duration=p["hover_s"], logger=self.logger) + + self._push_to_floor(x, y, z) + self.check_cancelled() # last exit before the fingers commit + self._close_twist_lift(x, y) + + def _grasp_verified(self, prompt): + """Back up, then check floor clear + gripper not open. Gemini gets both + cameras: the wrist view can show the object in the fingers, so a held + object isn't mistaken for a dropped one.""" + self._drive(-VERIFY_BACKUP_M) + self.sleep(self._p["settle_s"]) + j6 = self.manipulation.gripper_j6(self.joint_states) + main_img, wrist_img = self.main_image, self.wrist_image + images = [img for img in (main_img, wrist_img) if img] + labels = [] + if main_img: + labels.append(f"Image {len(labels) + 1} is the head camera looking at the floor.") + if wrist_img: + labels.append( + f"Image {len(labels) + 1} is the WRIST camera next to the gripper " + "fingers (mirrored) — the object may be visible held in the fingers there." + ) + floor_text = ( + gemlib.ask_image( + self._proxy, + images, + f"Robot just tried to pick up '{prompt}'. {' '.join(labels)} " + f"Is '{prompt}' lying loose on the floor/carpet, OUT of the " + "robot's gripper? An object held between the gripper fingers " + "counts as grabbed even if it is still touching or resting on " + "the floor — answer NO for that, as for anything hanging from " + "the gripper. Answer YES only if the object is on the floor " + "free of the gripper. Answer only YES or NO.", + logger=self.logger, + ) + if images + else None + ) + j6_ok = j6 is not None and j6 > GRIPPER_EMPTY_J6 + 0.02 + # Token scan, not a prefix match: replies like "The object is not on + # the floor." answer correctly without leading with the word, and the + # old anchored match counted them — and an empty reply — as + # floor-not-clear, reporting a demonstrably held object as a miss and + # then releasing it in teardown. \b keeps hedges out: "CANNOT" and + # "NOT SURE" contain no whole-word NO or YES. + verdict = (floor_text or "").upper() + said_no = re.search(r"\bNO\b", verdict) is not None + said_yes = re.search(r"\bYES\b", verdict) is not None + if said_no == said_yes: + # Empty, hedged, or contradictory reply — same as no vision + # verdict at all (no cameras, or the call failed after retries): + # fall back to the gripper evidence alone rather than report a + # demonstrably held object as a missed grasp. + held = j6_ok + else: + held = said_no and j6_ok + self.logger.info( + f"[PickAnyObject] verify: floor={floor_text!r} j6={j6} " + f"({len(images)} cams) -> {'HELD' if held else 'NOT HELD'}" + ) + return held + + def execute(self, prompt: str = "the sock") -> SkillReturn: + """Pick up `prompt` from the floor.""" + if self._proxy is None: + self.fail("Innate proxy not configured (INNATE_SERVICE_KEY)") + + # Per-run reset: don't carry the last run's object or grip rating. + self._grip_strength = None + self._holding = False + try: + self.head.set_position(int(round(self._p["tilt_deg"]))) + # Fold to rest so the arm doesn't occlude the head camera. + self.manipulation.go(self.manipulation.rest_joints(self.joint_states), duration=3.0, logger=self.logger) + + self.say(f"Looking for {prompt}.") + xy = self._search(prompt) + xy = self._position_above(prompt, xy) + self.say("Picking it up.") + self._grasp_at(prompt, xy) + # _close_twist_lift latched self._holding the moment the fingers + # committed — only a verified miss clears it. + if not self._grasp_verified(prompt): + self._holding = False + self.say("I couldn't get a grip on it.") + raise SkillFailed(f"Grasp missed — '{prompt}' is still on the floor (verified after backing up)") + self.say("Got it.") + return f"Picked up '{prompt}' (verified: floor clear after backing up)" + except ArmFailed as e: + # A clean arm give-up is a skill failure, not a crash. SkillFailed + # and SkillCancelled propagate untouched — the framework owns them. + self.fail(str(e)) + except ArmUnhealthy as e: + self.say("My arm isn't responding properly, stopping.") + self.fail(f"Arm servo failure: {e}") + finally: + self.mobility.stop() + self._rest_arm(keep_grip=self._holding) + self.head.set_position(0) diff --git a/workspace/innate_skills/pick_socks/__init__.py b/workspace/innate_skills/pick_socks/__init__.py new file mode 100644 index 000000000..1ef2ae8c8 --- /dev/null +++ b/workspace/innate_skills/pick_socks/__init__.py @@ -0,0 +1,11 @@ +# AUTO-GENERATED physical-skill ref by the skill catalog — do not edit; edits are overwritten. +"""Typed ref to 'innate-os/pick_socks', the recording in this folder. + +Same class either way: + + from physical_skills import PickSocks +""" + +from physical_skills import PickSocks + +__all__ = ["PickSocks"] diff --git a/workspace/innate_skills/pick_up_piece_simple.py b/workspace/innate_skills/pick_up_piece_simple.py deleted file mode 100644 index dca0ee3d9..000000000 --- a/workspace/innate_skills/pick_up_piece_simple.py +++ /dev/null @@ -1,645 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -""" -Pick Up Piece Simple Skill - Pick up and place chess pieces using only -calibration data and arm orientation, without Gemini vision or base driving. - -Orientation strategy by rank/column: - Ranks 4-6: pitch=1.57 (straight down), yaw=0 - Ranks 7-8: pitch=1.09 (tilted, 1.57-0.48), yaw=0 - Ranks 1-3, cols A-D: pitch=1.57, yaw=-1.57 (rotated left) - Ranks 1-3, cols E-H: pitch=1.57, yaw=+1.57 (rotated right) - -Shortcut: ranks 5-6 can reach ranks 7-8 directly with tilted -orientation, skipping the relay handoff. -""" - -import json -import time -from pathlib import Path -from typing import Literal - -from brain_client.skills.types import Interface, InterfaceType, Skill, SkillResult - -CALIBRATION_FILE = Path.home() / "board_calibration.json" -PieceType = Literal["king", "queen", "rook", "bishop", "knight", "pawn"] - - -class PickUpPieceSimple(Skill): - """Pick up a chess piece and place it on another square using calibration - positions only. No vision correction, no base driving.""" - - manipulation = Interface(InterfaceType.MANIPULATION) - - # Orientation constants - FIXED_ROLL = 0.0 - PITCH_DOWN = 1.45 # nearly straight down (compensates for slight arm slouch) - PITCH_TILTED = 1.57 - 0.48 # tilted for far ranks (7-8) - YAW_CENTER = 0.0 - YAW_LEFT = 1.57 # rotated left for ranks 1-3, cols A-D - YAW_RIGHT = -1.57 # rotated right for ranks 1-3, cols E-H - - # Heights in meters - HEIGHT_SAFE_CLEARANCE = 0.25 # minimum z before any lateral move - HEIGHT_SAFE = 0.15 # 20cm safe travel height - HEIGHT_PICK_TALL = 0.06 # 8cm pick height for tall pieces (king, queen) - HEIGHT_PICK_SHORT = 0.04 # 4cm pick height for short pieces (everything else) - - # Gripper parameters - GRIPPER_OPEN_PERCENT = 50 - GRIPPER_CLOSE_STRENGTH = 0.4 - GRIPPER_MIN_WAIT = 1.0 # minimum seconds to wait for gripper to actuate - - # Number of intermediate Z steps for vertical moves - VERTICAL_STEPS = 4 - - # Phase speed multipliers (> 1.0 = faster, < 1.0 = slower, on top of speed) - PHASE_AIR = 1.5 # lateral moves at safe height - PHASE_LIFT = 1.3 # lifting after grab/release - PHASE_DESCENT_FAR = 1.2 # top portion of descent - PHASE_DESCENT_NEAR = 0.6 # near-board portion of descent - - # Ranks 7-8 adjustments - TILTED_SPEED_FACTOR = 0.75 # extra caution multiplier for tilted moves - HEIGHT_SAFE_TILTED = 0.18 # higher safe height to avoid bumps - - # Relay rank for intermediary handoff when crossing rank 6/7 boundary - RELAY_RANK = 5 - - # Discard zone: squares to the right of column H for captured pieces - DISCARD_SQUARES_RIGHT = 2 # how many square-widths right of H - DISCARD_RANK = 4.5 # midpoint between rank 4 and 5 - - def __init__(self, logger): - super().__init__(logger) - self._cancelled = False - self._speed = 1.0 - - @property - def name(self): - return "pick_up_piece_simple" - - def guidelines(self): - return ( - "Pick up a piece from one square and place it on another without " - "using Gemini vision or base driving. Uses arm orientation changes " - "to reach all ranks. Parameters: square (source, e.g. 'E2'), " - "place_square (target, e.g. 'E4'), piece (str, e.g. 'pawn'), " - "is_capture (bool, True if capturing an opponent piece), speed (float)." - ) - - # ── Helpers ─────────────────────────────────────────────────────── - - def _load_calibration(self): - if not CALIBRATION_FILE.exists(): - return None - try: - return json.loads(CALIBRATION_FILE.read_text()) - except Exception: - return None - - def _square_to_position(self, square, calibration): - """Convert chess notation (e.g. 'E4') to (x, y, z) robot coordinates.""" - if len(square) != 2: - return None - file_char = square[0].upper() - rank_char = square[1] - if file_char not in "ABCDEFGH" or rank_char not in "12345678": - return None - - file_idx = ord(file_char) - ord("A") # A=0, H=7 - rank_idx = int(rank_char) - 1 # 1=0, 8=7 - - u = file_idx / 7.0 # 0 at A, 1 at H - v = rank_idx / 7.0 # 0 at rank 1, 1 at rank 8 - - tl = calibration.get("top_left") - tr = calibration.get("top_right") - bl = calibration.get("bottom_left") - br = calibration.get("bottom_right") - if not all([tl, tr, bl, br]): - return None - - x = (1 - u) * (1 - v) * bl["x"] + u * (1 - v) * br["x"] + (1 - u) * v * tl["x"] + u * v * tr["x"] - y = (1 - u) * (1 - v) * bl["y"] + u * (1 - v) * br["y"] + (1 - u) * v * tl["y"] + u * v * tr["y"] - z = ( - (1 - u) * (1 - v) * bl.get("z", 0) - + u * (1 - v) * br.get("z", 0) - + (1 - u) * v * tl.get("z", 0) - + u * v * tr.get("z", 0) - ) - return x, y, z - - def _orientation_for_square(self, square): - """Return (pitch, yaw) for reaching a given square. - - Ranks 4-6: straight down, center yaw - Ranks 7-8: tilted pitch, center yaw - Ranks 1-3: straight down, yaw left (A-D) or right (E-H) - """ - rank = int(square[1]) - col = square[0].upper() - - if rank >= 7: - return self.PITCH_TILTED, self.YAW_CENTER - elif rank >= 4: - return self.PITCH_DOWN, self.YAW_CENTER - else: # ranks 1-3 - if col in "ABCD": - return self.PITCH_DOWN, self.YAW_LEFT - else: - return self.PITCH_DOWN, self.YAW_RIGHT - - def _d(self, seconds: float) -> float: - """Scale a duration by the speed factor.""" - return seconds / self._speed - - def _w(self, seconds: float): - """Sleep for a scaled duration.""" - time.sleep(seconds / self._speed) - - def _gripper_wait(self, seconds: float): - """Wait for a gripper operation. Scales with speed but never below GRIPPER_MIN_WAIT.""" - time.sleep(max(seconds / self._speed, self.GRIPPER_MIN_WAIT)) - - def _move_arm(self, x, y, z, pitch, yaw, duration, blocking=True, gripper_position=None): - """Move arm to pose and optionally wait. Returns True on success.""" - kwargs = dict( - x=x, y=y, z=z, roll=self.FIXED_ROLL, pitch=pitch, yaw=yaw, duration=self._d(duration), blocking=blocking - ) - if gripper_position is not None: - kwargs["gripper_position"] = gripper_position - success = self.manipulation.move_to_cartesian_pose(**kwargs) - return success - - def _vertical_move(self, x, y, from_z, to_z, pitch, yaw, gripper_position=None, caution=1.0): - """Move vertically in VERTICAL_STEPS increments with fixed X, Y. - - Per-segment speed profiling: - - Descent: fast at top (PHASE_DESCENT_FAR), slow near board (PHASE_DESCENT_NEAR) - - Lift: uniformly faster (PHASE_LIFT) - caution: extra multiplier (< 1.0 = more cautious, e.g. for tilted moves). - - Tries the smooth trajectory service first (no stop between steps). - Falls back to individual moves if the service isn't available. - Returns error string or None. - """ - descending = to_z < from_z - direction = "Descending" if descending else "Lifting" - n = self.VERTICAL_STEPS - - # Compute per-segment durations (actual seconds) - seg_durs = [] - for i in range(n): - if descending: - frac = (i + 0.5) / n # midpoint of segment, 0=top 1=bottom - phase = self.PHASE_DESCENT_FAR + (self.PHASE_DESCENT_NEAR - self.PHASE_DESCENT_FAR) * frac - else: - phase = self.PHASE_LIFT - seg_durs.append(1.0 / (self._speed * phase * caution)) - - # Build waypoint poses (including from_z so the trajectory starts there) - poses = [] - for i in range(n + 1): - frac = i / n - z = from_z + (to_z - from_z) * frac - poses.append(dict(x=x, y=y, z=z, roll=self.FIXED_ROLL, pitch=pitch, yaw=yaw)) - - # Try smooth trajectory first - try: - success = self.manipulation.move_cartesian_trajectory( - poses, - segment_durations=seg_durs, - gripper_position=gripper_position, - ) - if success: - self.logger.info( - f"[PickUpPieceSimple] {direction} trajectory complete " - f"({n} segments, durs={[f'{d:.2f}' for d in seg_durs]})" - ) - return None - self.logger.warning("[PickUpPieceSimple] Trajectory service failed, falling back to step-by-step") - except Exception as e: - self.logger.warning(f"[PickUpPieceSimple] Trajectory not available ({e}), falling back") - - # Fallback: individual moves with per-segment durations - for i in range(1, n + 1): - frac = i / n - z = from_z + (to_z - from_z) * frac - dur = seg_durs[i - 1] - self.logger.info(f"[PickUpPieceSimple] {direction} step {i}/{n} -> z={z:.3f}m ({dur:.2f}s)") - kwargs = dict(x=x, y=y, z=z, roll=self.FIXED_ROLL, pitch=pitch, yaw=yaw, duration=dur) - if gripper_position is not None: - kwargs["gripper_position"] = gripper_position - if not self.manipulation.move_to_cartesian_pose(**kwargs): - return f"Failed at {direction.lower()} step {i}/{n} z={z:.3f}m" - time.sleep(dur) - if self._cancelled: - return "Cancelled" - return None - - def _go_to_safe_pose(self): - """Return arm to the resting safe pose. - - First lifts straight up to HEIGHT_SAFE_CLEARANCE from the current - position (no lateral movement) to avoid sweeping the board, then - moves to the final safe pose. - - Returns: - bool: True on success, False otherwise. - """ - ee = self.manipulation.get_current_end_effector_pose() - if ee is not None: - cur_z = ee["position"]["z"] - if cur_z < self.HEIGHT_SAFE_CLEARANCE: - cur_x = ee["position"]["x"] - cur_y = ee["position"]["y"] - rpy = self.manipulation.get_current_orientation_rpy() - cur_pitch = rpy["pitch"] if rpy else 0.0 - cur_yaw = rpy["yaw"] if rpy else 0.0 - self._move_arm(cur_x, cur_y, self.HEIGHT_SAFE_CLEARANCE, cur_pitch, cur_yaw, 2.0) - self._move_arm(0.05, 0.08, 0.3, 0.0, 1.57, 2.0, blocking=True) - return self._move_arm(0.05, 0.08, 0.11, 0.0, 1.57, 1.0, blocking=True) - - def _needs_relay(self, src_square, dst_square): - """Check if move crosses the rank 6/7 boundary requiring a relay. - - Ranks 5-6 can reach 7-8 directly with tilted orientation, - so no relay is needed for that transition. - """ - src_rank = int(src_square[1]) - dst_rank = int(dst_square[1]) - # Ranks 5-6 <-> 7-8 can be done directly with tilted pitch - if src_rank in (5, 6) and dst_rank >= 7: - return False - if dst_rank in (5, 6) and src_rank >= 7: - return False - return (src_rank <= 6) != (dst_rank <= 6) - - def _discard_position(self, calibration): - """Compute the (x, y, z) discard zone for captured pieces. - - Located DISCARD_SQUARES_RIGHT square-widths to the right of column H, - at rank DISCARD_RANK level. Orientation is straight down, center yaw - (same as ranks 4-6). - """ - tr = calibration.get("top_right") - tl = calibration.get("top_left") - br = calibration.get("bottom_right") - bl = calibration.get("bottom_left") - if not all([tl, tr, bl, br]): - return None - - # One square width in the y direction (A->H = 7 squares) - sq_y = ((bl["y"] - br["y"]) + (tl["y"] - tr["y"])) / 2.0 / 7.0 - # Offset: move right (negative y) by DISCARD_SQUARES_RIGHT squares - y_offset = -sq_y * self.DISCARD_SQUARES_RIGHT - - # Interpolate along rank axis: v = (rank - 1) / 7 - v = (self.DISCARD_RANK - 1) / 7.0 - # Use u = 1.0 (column H) then shift by y_offset - x = (1 - v) * br["x"] + v * tr["x"] - y = (1 - v) * br["y"] + v * tr["y"] + y_offset - z = (1 - v) * br.get("z", 0) + v * tr.get("z", 0) - return x, y, z - - def _relay_position(self, src_square, dst_square, calibration): - """Compute a relay square on rank 5 for intermediary handoff. - - Uses the file of whichever square is in ranks 1-6 so the relay - stays close to the reachable side of the board. - Returns (relay_square_str, (x, y, z)) or (relay_square_str, None). - """ - src_rank = int(src_square[1]) - if src_rank <= 6: - file_char = src_square[0].upper() - else: - file_char = dst_square[0].upper() - relay_square = f"{file_char}{self.RELAY_RANK}" - pos = self._square_to_position(relay_square, calibration) - return relay_square, pos - - def _do_pick_place( - self, - src_x, - src_y, - dst_x, - dst_y, - pick_height, - src_pitch, - src_yaw, - dst_pitch, - dst_yaw, - src_label, - dst_label, - safe_height=None, - caution=1.0, - ): - """Single pick-and-place cycle: pick from src, place at dst. - - Uses src orientation for picking, dst orientation for placing. - Phase-aware speeds: air travel is fast, lift is fast, descent - progressively slows near the board. Gripper waits are never rushed. - - Args: - safe_height: Override for HEIGHT_SAFE (e.g. higher for tilted). - caution: Multiplier < 1.0 makes everything more cautious. - Returns error string or None on success. - """ - if safe_height is None: - safe_height = self.HEIGHT_SAFE - - # Compute gripper positions in radians so every trajectory command - # carries an explicit gripper target (avoids stale _arm_state reads). - open_grip = self.manipulation.GRIPPER_CLOSED + ( - self.manipulation.GRIPPER_OPEN - self.manipulation.GRIPPER_CLOSED - ) * (self.GRIPPER_OPEN_PERCENT / 100.0) - closed_grip = self.manipulation.GRIPPER_CLOSED - self.GRIPPER_CLOSE_STRENGTH - - # Phase-adjusted base duration for air moves (before _d scaling) - air_dur = 2.0 / (self.PHASE_AIR * caution) - - # Move above source at safe height (FAST – in the air) - self._send_feedback(f"Moving above {src_label}...") - if not self._move_arm(src_x, src_y, safe_height, src_pitch, src_yaw, air_dur): - return f"Failed to move above {src_label}" - if self._cancelled: - return "Cancelled" - - # Open gripper and wait for it to fully open before descending - self._send_feedback("Opening gripper...") - self.manipulation.open_gripper(self.GRIPPER_OPEN_PERCENT) - self._gripper_wait(1.5) - - # Descend to pick height (PROGRESSIVE – fast top, slow near board) - self._send_feedback(f"Descending to pick from {src_label}...") - err = self._vertical_move( - src_x, src_y, safe_height, pick_height, src_pitch, src_yaw, gripper_position=open_grip, caution=caution - ) - if err: - return f"Pick descent failed: {err}" - - # Grab (NEUTRAL – gripper waits are never rushed) - self._send_feedback("Grabbing piece...") - self.manipulation.close_gripper(strength=self.GRIPPER_CLOSE_STRENGTH, blocking=True) - self._gripper_wait(2.0) - grip_position = closed_grip - - # Lift to safe height (FAST – lifting via PHASE_LIFT) - self._send_feedback("Lifting piece...") - err = self._vertical_move( - src_x, src_y, pick_height, safe_height, src_pitch, src_yaw, gripper_position=grip_position, caution=caution - ) - if err: - return f"Lift failed: {err}" - if self._cancelled: - return "Cancelled" - - # Move above destination at safe height (FAST – in the air) - self._send_feedback(f"Moving above {dst_label}...") - if not self._move_arm(dst_x, dst_y, safe_height, dst_pitch, dst_yaw, air_dur, gripper_position=grip_position): - return f"Failed to move above {dst_label}" - if self._cancelled: - return "Cancelled" - - # Descend to place height (PROGRESSIVE) - self._send_feedback(f"Descending to place on {dst_label}...") - err = self._vertical_move( - dst_x, dst_y, safe_height, pick_height, dst_pitch, dst_yaw, gripper_position=grip_position, caution=caution - ) - if err: - return f"Place descent failed: {err}" - - # Release (NEUTRAL) - self._send_feedback("Releasing piece...") - self.manipulation.open_gripper(self.GRIPPER_OPEN_PERCENT) - self._gripper_wait(1.5) - - # Lift to safe height (FAST) - self._send_feedback("Lifting after place...") - err = self._vertical_move( - dst_x, dst_y, pick_height, safe_height, dst_pitch, dst_yaw, gripper_position=open_grip, caution=caution - ) - if err: - return f"Post-place lift failed: {err}" - - return None - - # ── Main logic ──────────────────────────────────────────────────── - - TALL_PIECES = {"king", "queen"} - - def execute( - self, - square: str, - place_square: str, - piece: PieceType = "pawn", - is_capture: bool = False, - speed: float = 1.0, - ): - """ - Pick up a piece from square and place it on place_square. - - If is_capture is True, first removes the opponent's piece from - place_square to a discard zone (right of column H, near rank 4-5), - then moves our piece from square to place_square. - - When a move crosses the rank 6/7 boundary the arm cannot reach - ranks 7-8 with a vertical gripper, so we relay through an - intermediary square on rank 5: place the piece there, reorient - the gripper (tilted ~0.48 rad for 7-8, vertical for 1-6), then - pick up again and continue to the destination. - - Args: - square: Source square in chess notation (e.g. 'A4') - place_square: Target square (e.g. 'D5') - piece: Piece type ('king', 'queen', 'rook', 'bishop', 'knight', 'pawn') - is_capture: If True, remove opponent piece from place_square first - speed: Speed multiplier (1.0 = normal) - """ - self._speed = max(0.1, min(speed, 3.0)) - self._cancelled = False - - if self.manipulation is None: - return "Manipulation interface not available", SkillResult.FAILURE - - # Safety: ensure arm starts from a safe pose (lifts first if low) - self._go_to_safe_pose() - - calibration = self._load_calibration() - if calibration is None: - return "No calibration data found. Run board calibration first.", SkillResult.FAILURE - - src_pos = self._square_to_position(square, calibration) - if src_pos is None: - return f"Invalid source square '{square}'", SkillResult.FAILURE - dst_pos = self._square_to_position(place_square, calibration) - if dst_pos is None: - return f"Invalid target square '{place_square}'", SkillResult.FAILURE - - src_x, src_y, src_board_z = src_pos - dst_x, dst_y, dst_board_z = dst_pos - is_tall = piece.strip().lower() in self.TALL_PIECES - base_pick_height = self.HEIGHT_PICK_TALL if is_tall else self.HEIGHT_PICK_SHORT - src_pitch, src_yaw = self._orientation_for_square(square) - dst_pitch, dst_yaw = self._orientation_for_square(place_square) - - self.logger.info( - f"[PickUpPieceSimple] Pick {square} ({src_x:.4f},{src_y:.4f},z={src_board_z:.4f}) " - f"pitch={src_pitch:.2f} yaw={src_yaw:.2f} -> " - f"Place {place_square} ({dst_x:.4f},{dst_y:.4f},z={dst_board_z:.4f}) " - f"pitch={dst_pitch:.2f} yaw={dst_yaw:.2f}" - f"{' [CAPTURE]' if is_capture else ''}" - ) - - # ── Capture: remove opponent piece to discard zone first ── - if is_capture: - discard_pos = self._discard_position(calibration) - if discard_pos is None: - return "Failed to compute discard position", SkillResult.FAILURE - disc_x, disc_y, disc_z = discard_pos - # Use short pick height for captured piece (we don't know its type) - cap_pick_height = self.HEIGHT_PICK_SHORT + dst_board_z - cap_pitch, cap_yaw = self._orientation_for_square(place_square) - # Discard zone is at ranks 4-5 level -> straight down, center yaw - disc_pitch, disc_yaw = self.PITCH_DOWN, self.YAW_CENTER - disc_safe = self.HEIGHT_SAFE - - self.logger.info( - f"[PickUpPieceSimple] Capture: removing piece from {place_square} " - f"to discard ({disc_x:.4f},{disc_y:.4f},z={disc_z:.4f})" - ) - self._send_feedback(f"Capturing: removing piece from {place_square}...") - - # If target square is in ranks 7-8, use tilted approach - cap_rank = int(place_square[1]) - cap_tilted = cap_rank >= 7 - err = self._do_pick_place( - dst_x, - dst_y, - disc_x, - disc_y, - cap_pick_height, - cap_pitch, - cap_yaw, - disc_pitch, - disc_yaw, - place_square, - "discard", - safe_height=self.HEIGHT_SAFE_TILTED if cap_tilted else disc_safe, - caution=self.TILTED_SPEED_FACTOR if cap_tilted else 1.0, - ) - if err: - self._go_to_safe_pose() - return f"Capture discard failed: {err}", SkillResult.FAILURE - if self._cancelled: - self._go_to_safe_pose() - return "Cancelled", SkillResult.CANCELLED - - src_rank = int(square[1]) - dst_rank = int(place_square[1]) - - if self._needs_relay(square, place_square): - # ── Two-step relay through intermediary ── - relay_sq, relay_pos = self._relay_position(square, place_square, calibration) - if relay_pos is None: - return "Failed to compute relay position", SkillResult.FAILURE - relay_x, relay_y, relay_board_z = relay_pos - relay_pick_height = base_pick_height + relay_board_z - - self.logger.info( - f"[PickUpPieceSimple] Relay through {relay_sq} ({relay_x:.4f},{relay_y:.4f},z={relay_board_z:.4f})" - ) - - # Leg 1: pick from source, place at relay (keep source orientation) - # Tilted caution if source is rank 7-8 - leg1_tilted = src_rank >= 7 - src_pick_height = base_pick_height + src_board_z - self._send_feedback(f"Relay leg 1: {square} -> {relay_sq}") - err = self._do_pick_place( - src_x, - src_y, - relay_x, - relay_y, - src_pick_height, - src_pitch, - src_yaw, - src_pitch, - src_yaw, - square, - f"relay {relay_sq}", - safe_height=self.HEIGHT_SAFE_TILTED if leg1_tilted else None, - caution=self.TILTED_SPEED_FACTOR if leg1_tilted else 1.0, - ) - if err: - self._go_to_safe_pose() - return f"Relay leg 1 failed: {err}", SkillResult.FAILURE - if self._cancelled: - self._go_to_safe_pose() - return "Cancelled", SkillResult.CANCELLED - - # Leg 2: pick from relay, place at destination (destination orientation) - # Tilted caution if destination is rank 7-8 - leg2_tilted = dst_rank >= 7 - self._send_feedback(f"Relay leg 2: {relay_sq} -> {place_square}") - err = self._do_pick_place( - relay_x, - relay_y, - dst_x, - dst_y, - relay_pick_height, - dst_pitch, - dst_yaw, - dst_pitch, - dst_yaw, - f"relay {relay_sq}", - place_square, - safe_height=self.HEIGHT_SAFE_TILTED if leg2_tilted else None, - caution=self.TILTED_SPEED_FACTOR if leg2_tilted else 1.0, - ) - if err: - self._go_to_safe_pose() - return f"Relay leg 2 failed: {err}", SkillResult.FAILURE - else: - # ── Direct move (no orientation change needed) ── - # Ranks 5-6 <-> 7-8: use tilted orientation for both ends - cross_56_78 = (src_rank in (5, 6) and dst_rank >= 7) or (dst_rank in (5, 6) and src_rank >= 7) - if cross_56_78: - src_pitch, src_yaw = self.PITCH_TILTED, self.YAW_CENTER - dst_pitch, dst_yaw = self.PITCH_TILTED, self.YAW_CENTER - self.logger.info(f"[PickUpPieceSimple] Rank {src_rank}->{dst_rank}: direct tilted (no relay)") - any_tilted = src_rank >= 7 or dst_rank >= 7 or cross_56_78 - pick_height = base_pick_height + src_board_z - err = self._do_pick_place( - src_x, - src_y, - dst_x, - dst_y, - pick_height, - src_pitch, - src_yaw, - dst_pitch, - dst_yaw, - square, - place_square, - safe_height=self.HEIGHT_SAFE_TILTED if any_tilted else None, - caution=self.TILTED_SPEED_FACTOR if any_tilted else 1.0, - ) - if err: - self._go_to_safe_pose() - return f"Move failed: {err}", SkillResult.FAILURE - - # ── Return to safe pose ── - self._send_feedback("Returning to safe pose...") - if not self._go_to_safe_pose(): - self.logger.warning("[PickUpPieceSimple] Failed to reach safe pose after move") - self._send_feedback("Warning: failed to reach safe pose") - - msg = f"Moved piece from {square} to {place_square}" - self._send_feedback(msg) - return msg, SkillResult.SUCCESS - - def cancel(self): - self._cancelled = True - return "Pick up piece simple cancelled" diff --git a/workspace/innate_skills/recalibrate_manual.py b/workspace/innate_skills/recalibrate_manual.py deleted file mode 100644 index ba142f72b..000000000 --- a/workspace/innate_skills/recalibrate_manual.py +++ /dev/null @@ -1,206 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -""" -Recalibrate Manual Skill - Human positions the arm above a top corner square -(A8 or H8), records the position, and recomputes the full board calibration -using square geometry with the other top corner held fixed. -""" - -import base64 -import json -import math -from datetime import datetime -from pathlib import Path -from typing import Literal - -from brain_client.skills.types import Interface, InterfaceType, RobotState, RobotStateType, Skill, SkillResult - -CALIBRATION_FILE = Path.home() / "board_calibration.json" -CAPTURES_DIR = Path.home() / "innate-os/captures/corners" -CalibrationCorner = Literal["A8", "H8"] - - -class RecalibrateManual(Skill): - """Record the arm's current position as a top corner (A8 or H8), - keep the other top corner from existing calibration, and recompute - all four corners using square geometry.""" - - manipulation = Interface(InterfaceType.MANIPULATION) - image = RobotState(RobotStateType.LAST_WRIST_CAMERA_IMAGE_B64) - - def __init__(self, logger): - super().__init__(logger) - - @property - def name(self): - return "recalibrate_manual" - - def guidelines(self): - return ( - "Manually recalibrate one top corner of the chessboard. " - "The human positions the arm above the center of A8 or H8, " - "then this skill records the position and recomputes the full " - "board calibration using square geometry. " - "Requires 'corner' parameter: 'A8' or 'H8'." - ) - - def _load_calibration(self): - if not CALIBRATION_FILE.exists(): - return None - try: - return json.loads(CALIBRATION_FILE.read_text()) - except Exception: - return None - - def _recompute_calibration_from_top_corners(self, new_a8, new_h8, z): - """Given A8 (top_left) and H8 (top_right) center positions, - derive A1 (bottom_left) and H1 (bottom_right) using square geometry. - - The board is a square: the bottom edge is obtained by rotating the - top edge (A8->H8) 90 deg clockwise (toward the robot, i.e. -X direction). - """ - # Top side vector: A8 -> H8 - side_x = new_h8[0] - new_a8[0] - side_y = new_h8[1] - new_a8[1] - - # Perpendicular vector pointing toward bottom of board (-X direction) - # Rotate (side_x, side_y) by -90 deg -> (side_y, -side_x) - down_x = side_y - down_y = -side_x - - # Bottom corners - a1_x = new_a8[0] + down_x - a1_y = new_a8[1] + down_y - h1_x = new_h8[0] + down_x - h1_y = new_h8[1] + down_y - - updated = { - "top_left": {"x": new_a8[0], "y": new_a8[1], "z": z}, - "top_right": {"x": new_h8[0], "y": new_h8[1], "z": z}, - "bottom_left": {"x": a1_x, "y": a1_y, "z": z}, - "bottom_right": {"x": h1_x, "y": h1_y, "z": z}, - } - - side_len = math.sqrt(side_x**2 + side_y**2) - self.logger.info( - f"[RecalibrateManual] Square geometry: side={side_len * 100:.1f}cm " - f"A8=({new_a8[0]:.4f},{new_a8[1]:.4f}) H8=({new_h8[0]:.4f},{new_h8[1]:.4f}) " - f"A1=({a1_x:.4f},{a1_y:.4f}) H1=({h1_x:.4f},{h1_y:.4f})" - ) - return updated - - def _save_corner_image(self, corner: str, pos: dict): - """Save the latest wrist camera frame as a corner snapshot.""" - if not self.image: - self.logger.warning("[RecalibrateManual] No wrist camera image available to save") - return - try: - CAPTURES_DIR.mkdir(parents=True, exist_ok=True) - ts = datetime.now().strftime("%Y%m%d_%H%M%S") - path = CAPTURES_DIR / f"manual_{corner}_{ts}.jpg" - path.write_bytes(base64.b64decode(self.image)) - self.logger.info(f"[RecalibrateManual] Corner image saved: {path}") - except Exception as e: - self.logger.warning(f"[RecalibrateManual] Failed to save corner image: {e}") - - def execute(self, corner: CalibrationCorner): - """ - Record current arm FK position as a top corner and recompute full board. - - Args: - corner: 'A8' (top_left) or 'H8' (top_right) - """ - if self.manipulation is None: - return "Manipulation interface not available", SkillResult.FAILURE - - corner = corner.upper().strip() - if corner not in ("A8", "H8"): - return f"Invalid corner '{corner}'. Must be 'A8' or 'H8'.", SkillResult.FAILURE - - calibration = self._load_calibration() - if calibration is None: - calibration = {} - - # Need the other top corner to exist - if corner == "A8": - this_key = "top_left" - other_key = "top_right" - other_name = "H8" - else: - this_key = "top_right" - other_key = "top_left" - other_name = "A8" - - # Read current arm position - fk_pose = self.manipulation.get_current_end_effector_pose() - if not fk_pose: - return "Could not get current arm position", SkillResult.FAILURE - - pos = fk_pose["position"] - new_pos = (pos["x"], pos["y"]) - z = pos["z"] - - position_str = f"X={pos['x']:.4f}, Y={pos['y']:.4f}, Z={pos['z']:.4f}" - self.logger.info(f"[RecalibrateManual] Recording {corner} at {position_str}") - self._send_feedback(f"Recording {corner} at {position_str}") - - # Save snapshot - self._save_corner_image(corner, pos) - - # If the other top corner is missing, save only this corner - if other_key not in calibration: - calibration[this_key] = {"x": pos["x"], "y": pos["y"], "z": z} - try: - CALIBRATION_FILE.write_text(json.dumps(calibration, indent=2)) - self.logger.info(f"[RecalibrateManual] Calibration saved to {CALIBRATION_FILE}") - except Exception as e: - return f"Failed to save calibration: {e}", SkillResult.FAILURE - - msg = ( - f"Recorded {corner} at {position_str}. " - f"Other corner {other_name} not yet recorded — record it to compute full board." - ) - self.logger.info(f"[RecalibrateManual] {msg}") - self._send_feedback(msg) - return msg, SkillResult.SUCCESS - - # Build the two top corners - other = calibration[other_key] - other_pos = (other["x"], other["y"]) - - if corner == "A8": - new_a8 = new_pos - new_h8 = other_pos - else: - new_a8 = other_pos - new_h8 = new_pos - - # Use z from existing calibration if available, else from current pose - cal_z = calibration.get("top_right", calibration.get("top_left", {})).get("z", z) - - # Recompute full board - updated = self._recompute_calibration_from_top_corners(new_a8, new_h8, cal_z) - - try: - CALIBRATION_FILE.write_text(json.dumps(updated, indent=2)) - self.logger.info(f"[RecalibrateManual] Calibration saved to {CALIBRATION_FILE}") - except Exception as e: - return f"Failed to save calibration: {e}", SkillResult.FAILURE - - # Report - side_x = new_h8[0] - new_a8[0] - side_y = new_h8[1] - new_a8[1] - side_len = math.sqrt(side_x**2 + side_y**2) - - msg = ( - f"Recorded {corner} at {position_str}. " - f"Recomputed full board from {corner} (new) + {other_name} (fixed). " - f"Board side={side_len * 100:.1f}cm." - ) - self.logger.info(f"[RecalibrateManual] {msg}") - self._send_feedback(msg) - return msg, SkillResult.SUCCESS - - def cancel(self): - return "Recalibrate manual cannot be cancelled" diff --git a/workspace/innate_skills/record_position.py b/workspace/innate_skills/record_position.py deleted file mode 100644 index dae03042c..000000000 --- a/workspace/innate_skills/record_position.py +++ /dev/null @@ -1,99 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -""" -Record Position Skill - Record current arm FK position, save to file, and send as feedback. -""" - -import base64 -import json -from datetime import datetime -from pathlib import Path -from typing import Literal - -from brain_client.skills.types import Interface, InterfaceType, RobotState, RobotStateType, Skill, SkillResult - -CALIBRATION_FILE = Path.home() / "board_calibration.json" -CORNER_CAPTURES_DIR = Path("/home/jetson1/innate-os/captures/corners") -BoardCorner = Literal["top_left", "top_right", "bottom_right", "bottom_left"] -VALID_CORNERS = ("top_left", "top_right", "bottom_right", "bottom_left") - - -class RecordPosition(Skill): - """Record current arm position, save to calibration file, and send as feedback.""" - - manipulation = Interface(InterfaceType.MANIPULATION) - image = RobotState(RobotStateType.LAST_WRIST_CAMERA_IMAGE_B64) - - def __init__(self, logger): - super().__init__(logger) - - @property - def name(self): - return "record_position" - - def guidelines(self): - return ( - "Record the current arm position for a board corner. " - "Requires 'corner' parameter: 'top_left', 'top_right', 'bottom_right', or 'bottom_left'. " - "Saves to calibration file and returns coordinates." - ) - - def execute(self, corner: BoardCorner): - """ - Record and save current FK position for a corner. - - Args: - corner: One of 'top_left', 'top_right', 'bottom_right', 'bottom_left' - """ - if self.manipulation is None: - return "Manipulation interface not available", SkillResult.FAILURE - - corner = corner.lower().replace("-", "_").replace(" ", "_") - if corner not in VALID_CORNERS: - return f"Invalid corner '{corner}'. Must be one of: {VALID_CORNERS}", SkillResult.FAILURE - - fk_pose = self.manipulation.get_current_end_effector_pose() - - if not fk_pose: - return "Could not get current position", SkillResult.FAILURE - - pos = fk_pose["position"] - - # Load existing calibration or create new - calibration = {} - if CALIBRATION_FILE.exists(): - try: - calibration = json.loads(CALIBRATION_FILE.read_text()) - except Exception: - calibration = {} - - # Save corner position - calibration[corner] = {"x": pos["x"], "y": pos["y"], "z": pos["z"]} - CALIBRATION_FILE.write_text(json.dumps(calibration, indent=2)) - - self._save_corner_image(corner, pos) - - position_str = f"X={pos['x']:.4f}, Y={pos['y']:.4f}, Z={pos['z']:.4f}" - self._send_feedback(f"RECORDED {corner.upper()}: {position_str}") - self.logger.info(f"Saved {corner} to {CALIBRATION_FILE}") - - return f"{corner} recorded: {position_str}", SkillResult.SUCCESS - - def _save_corner_image(self, corner: str, pos: dict): - """Save the latest wrist camera frame as a corner snapshot.""" - if not self.image: - self.logger.warning("No wrist camera image available to save") - return - try: - CORNER_CAPTURES_DIR.mkdir(parents=True, exist_ok=True) - ts = datetime.now().strftime("%Y%m%d_%H%M%S") - path = CORNER_CAPTURES_DIR / f"corner_{corner}_{ts}.jpg" - path.write_bytes(base64.b64decode(self.image)) - self.logger.info(f"Corner image saved: {path}") - except Exception as e: - self.logger.warning(f"Failed to save corner image: {e}") - - def cancel(self): - """Nothing to cancel.""" - return "Record position cannot be cancelled" diff --git a/workspace/innate_skills/reset_chess_game.py b/workspace/innate_skills/reset_chess_game.py deleted file mode 100644 index 6886829b2..000000000 --- a/workspace/innate_skills/reset_chess_game.py +++ /dev/null @@ -1,103 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -""" -Reset Chess Game Skill - Resets the board state to the starting position. -""" - -import json -from pathlib import Path -from typing import Literal - -from brain_client.skills.types import Skill, SkillResult - -GAME_STATE_FILE = Path.home() / "chess_game_state.json" -CALIBRATION_FILE = Path.home() / "board_calibration.json" -REQUIRED_CORNERS = ("top_left", "top_right", "bottom_right", "bottom_left") -ROBOT_COLORS = ("white", "black") -RobotColor = Literal["white", "black"] - -# Handicap: White starts without the a1 rook (no queenside castling) -HANDICAP_FEN = "rnbqkbnr/pppppppp/8/8/8/8/PPPPPPPP/1NBQKBNR w Kkq - 0 1" - - -class ResetChessGame(Skill): - """Reset the chess game state to the standard starting position.""" - - def __init__(self, logger): - super().__init__(logger) - - @property - def name(self): - return "reset_chess_game" - - def guidelines(self): - return ( - "Reset the chess game to the starting position. " - "Requires board calibration to be present first. " - "Clears the move history and sets the board to the initial FEN. " - "Optionally pass robot_color ('white' or 'black') to set which side the robot plays." - ) - - def _is_calibrated(self) -> bool: - """Return True when board calibration JSON exists and has all four corners.""" - if not CALIBRATION_FILE.exists(): - return False - - try: - calibration = json.loads(CALIBRATION_FILE.read_text()) - except Exception: - return False - - if not isinstance(calibration, dict): - return False - - for corner in REQUIRED_CORNERS: - pos = calibration.get(corner) - if not isinstance(pos, dict): - return False - try: - float(pos["x"]) - float(pos["y"]) - float(pos["z"]) - except Exception: - return False - - return True - - def execute(self, robot_color: RobotColor = "white"): - """ - Reset game state to starting position. - - Args: - robot_color: Which side the robot plays ('white' or 'black'). - """ - robot_color = robot_color.strip().lower() - if robot_color not in ROBOT_COLORS: - return f"Invalid robot_color '{robot_color}'. Must be 'white' or 'black'.", SkillResult.FAILURE - if not self._is_calibrated(): - msg = "Board is not calibrated. Run board calibration first, then reset the chess game." - self.logger.warning(f"[ResetChessGame] {msg}") - self._send_feedback(msg) - return msg, SkillResult.FAILURE - - state = { - "fen": HANDICAP_FEN, - "move_history": [], - "last_detected_move": None, - "turn": "white", - "robot_color": robot_color, - } - - try: - GAME_STATE_FILE.write_text(json.dumps(state, indent=2)) - except Exception as e: - return f"Failed to write game state: {e}", SkillResult.FAILURE - - msg = f"Game reset to starting position. Robot plays {robot_color}." - self.logger.info(f"[ResetChessGame] {msg}") - self._send_feedback(msg) - return msg, SkillResult.SUCCESS - - def cancel(self): - return "Reset cannot be cancelled" diff --git a/workspace/innate_skills/retrieve_emails.py b/workspace/innate_skills/retrieve_emails.py deleted file mode 100644 index c1ea20344..000000000 --- a/workspace/innate_skills/retrieve_emails.py +++ /dev/null @@ -1,240 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -import email -import imaplib -from email.header import decode_header - -from brain_client.skills.types import Skill, SkillResult - - -class RetrieveEmails(Skill): - """ - Primitive for retrieving recent emails from an IMAP server. - This retrieves email titles and content from the configured email account. - """ - - def __init__(self, logger): - self.logger = logger - # Email server configuration - using same credentials as send_email - self.imap_server = "imap.gmail.com" # Gmail IMAP server - self.imap_port = 993 - self.email = "" # Configure email address - self.password = "" # Configure app password - - @property - def name(self): - return "retrieve_emails" - - def guidelines(self): - return ( - "Use to retrieve recent emails from the configured email account. " - "Provide the number of emails to retrieve (default is 5). " - "Returns email subjects and content. This should be used when you need " - "to check for recent messages or respond to incoming communications." - ) - - def _decode_header_value(self, value): - """ - Decode email header values that might be encoded. - - Args: - value: The header value to decode - - Returns: - str: The decoded header value - """ - if value is None: - return "" - - decoded_parts = decode_header(value) - decoded_value = "" - - for part, encoding in decoded_parts: - if isinstance(part, bytes): - if encoding: - try: - decoded_value += part.decode(encoding) - except (UnicodeDecodeError, LookupError): - decoded_value += part.decode("utf-8", errors="ignore") - else: - decoded_value += part.decode("utf-8", errors="ignore") - else: - decoded_value += str(part) - - return decoded_value - - def _extract_email_content(self, msg): - """ - Extract text content from an email message. - - Args: - msg: Email message object - - Returns: - str: The text content of the email - """ - content = "" - - if msg.is_multipart(): - for part in msg.walk(): - content_type = part.get_content_type() - content_disposition = str(part.get("Content-Disposition")) - - # Skip attachments - if "attachment" in content_disposition: - continue - - if content_type == "text/plain": - try: - body = part.get_payload(decode=True) - if body: - content += body.decode("utf-8", errors="ignore") - except Exception as e: - self.logger.warning(f"Could not decode email part: {e}") - continue - elif content_type == "text/html" and not content: - # Only use HTML if no plain text is available - try: - body = part.get_payload(decode=True) - if body: - content += body.decode("utf-8", errors="ignore") - except Exception as e: - self.logger.warning(f"Could not decode HTML email part: {e}") - continue - else: - # Single part message - try: - body = msg.get_payload(decode=True) - if body: - content = body.decode("utf-8", errors="ignore") - except Exception as e: - self.logger.warning(f"Could not decode email body: {e}") - - return content.strip() - - def execute(self, count: int = 5): - """ - Retrieves the most recent emails from the configured IMAP server. - - Args: - count (int): Number of recent emails to retrieve (default: 5, max: 20) - - Returns: - tuple: (result_message, result_status) where result_status is a - PrimitiveResult enum value - """ - # Limit the count to prevent overwhelming responses - count = min(max(1, count), 20) - - self.logger.info(f"\033[96m[BrainClient] Retrieving last {count} emails\033[0m") - - try: - # Connect to the IMAP server - mail = imaplib.IMAP4_SSL(self.imap_server, self.imap_port) - mail.login(self.email, self.password) - - # Select the INBOX - mail.select("INBOX") - - # Search for all emails and get the most recent ones - status, messages = mail.search(None, "ALL") - - if status != "OK": - self.logger.error("Failed to search emails") - mail.logout() - return "Failed to search emails", SkillResult.FAILURE - - # Get message IDs - email_ids = messages[0].split() - - if not email_ids: - self.logger.info("No emails found in inbox") - mail.logout() - return "No emails found in inbox", SkillResult.SUCCESS - - # Get the most recent emails (last 'count' emails) - recent_email_ids = email_ids[-count:] - - emails_info = [] - - for email_id in reversed(recent_email_ids): # Most recent first - try: - # Fetch the email - status, msg_data = mail.fetch(email_id, "(RFC822)") - - if status != "OK": - self.logger.warning(f"Failed to fetch email {email_id}") - continue - - # Parse the email - msg = email.message_from_bytes(msg_data[0][1]) - - # Extract email information - subject = self._decode_header_value(msg.get("Subject", "No Subject")) - sender = self._decode_header_value(msg.get("From", "Unknown Sender")) - date = msg.get("Date", "Unknown Date") - content = self._extract_email_content(msg) - - # Truncate content if it's too long - if len(content) > 500: - content = content[:500] + "... [truncated]" - - email_info = { - "subject": subject, - "from": sender, - "date": date, - "content": (content if content else "[No text content available]"), - } - - emails_info.append(email_info) - - except Exception as e: - self.logger.warning(f"Error processing email {email_id}: {e}") - continue - - mail.logout() - - if not emails_info: - return "No emails could be retrieved", SkillResult.FAILURE - - # Format the result message - result_lines = [f"Retrieved {len(emails_info)} recent email(s):\n"] - - for i, email_info in enumerate(emails_info, 1): - result_lines.append(f"Email {i}:") - result_lines.append(f" Subject: {email_info['subject']}") - result_lines.append(f" From: {email_info['from']}") - result_lines.append(f" Date: {email_info['date']}") - result_lines.append(f" Content: {email_info['content']}") - result_lines.append("") # Empty line between emails - - result_message = "\n".join(result_lines) - - self.logger.info(f"\033[92m[BrainClient] Successfully retrieved {len(emails_info)} emails\033[0m") - - return result_message, SkillResult.SUCCESS - - except imaplib.IMAP4.error as e: - error_msg = f"IMAP error: {str(e)}" - self.logger.error(error_msg) - return error_msg, SkillResult.FAILURE - - except Exception as e: - error_msg = f"Failed to retrieve emails: {str(e)}" - self.logger.error(error_msg) - return error_msg, SkillResult.FAILURE - - def cancel(self): - """ - Cancel the email retrieval operation. - - Since email retrieval is typically a quick operation that completes almost - instantly, this method doesn't do much. It's implemented to satisfy the - Primitive interface. - - Returns: - str: A message describing the cancellation result. - """ - self.logger.info("\033[91m[BrainClient] Email retrieval operation cannot be canceled once started\033[0m") - return "Email retrieval is an atomic operation that cannot be canceled once started" diff --git a/workspace/innate_skills/run_routine_demo.py b/workspace/innate_skills/run_routine_demo.py deleted file mode 100644 index 60647f451..000000000 --- a/workspace/innate_skills/run_routine_demo.py +++ /dev/null @@ -1,48 +0,0 @@ -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -from innate import RobotState, RobotStateType, Skill, SkillFailed, SkillResult -from innate.skills import arm_zero_position, head_emotion, move_straight, pick_socks, turn_in_place - - -class RunRoutineDemo(Skill): - """Demo of a chained routine: skills are imported functions, calls block, - failures raise SkillFailed, and each call is its own step in the app.""" - - battery = RobotState(RobotStateType.LAST_BATTERY) - - @property - def name(self): - return "run_routine_demo" - - def guidelines(self): - return ( - "Run the demo routine: talk, emote, shuffle, turn, and try to pick a " - "sock. Use when the user asks for the demo." - ) - - def execute(self): - runs = self.storage.get("runs", 0) + 1 - self.storage["runs"] = runs - - arm_zero_position() - head_emotion(emotion="excited") - self.say(f"Demo number {runs}. Watch this.", wait=True) - - for distance in (0.2, -0.2): - move_straight(distance=distance) - - turn = turn_in_place(angle_degrees=90) - self.say(f"I turned {turn.data.turned_degrees:.0f} degrees.") - turn_in_place(angle_degrees=-90, timeout=20) - - try: - pick_socks(timeout=60) # learned policy, same call shape - except SkillFailed: - head_emotion(emotion="disappointed") - self.say("No socks today.") - - if self.battery: - self.say(f"Battery at {self.battery['percentage']:.0%}.") - head_emotion(emotion="proud") - self.say("All done!") - return "Demo complete", SkillResult.SUCCESS diff --git a/workspace/innate_skills/send_email.py b/workspace/innate_skills/send_email.py deleted file mode 100644 index 4937b6259..000000000 --- a/workspace/innate_skills/send_email.py +++ /dev/null @@ -1,114 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -import smtplib -from email.mime.multipart import MIMEMultipart -from email.mime.text import MIMEText - -from brain_client.skills.types import Skill, SkillResult - - -class SendEmail(Skill): - """ - Primitive for sending emails for emergency notifications. - This is a simplified version that logs the email content rather than actually - sending. In a production environment, you would configure proper SMTP settings. - """ - - def __init__(self, logger): - self.logger = logger - self.default_recipients = ["axel@innate.bot", "vignesh@innate.bot"] - # Email server configuration - self.smtp_server = "smtp.gmail.com" # Example using Gmail - self.smtp_port = 587 - self.sender_email = "axel@innate.bot" # Replace with robot's email - self.password = "" # Use app password for Gmail - - @property - def name(self): - return "send_email" - - def guidelines(self): - return ( - "Use to send an emergency email notification. Provide a subject and " - "message. You can optionally provide a list of recipients, otherwise " - "it will be sent to the default list. This should be used when a " - "potential emergency is detected and assistance might be required." - ) - - def execute(self, subject: str, message: str, recipients: list[str] | str = None): - """ - Sends an email to the specified recipient(s) (or default list if none provided). - - Args: - subject (str): Email subject line - message (str): Email body content - recipients (list[str] | str, optional): Email recipient or list of recipients. - Defaults to the default list if not specified. - - Returns: - tuple: (result_message, result_status) where result_status is a - PrimitiveResult enum value - """ - current_recipients = [] - if recipients is None: - current_recipients = self.default_recipients - elif isinstance(recipients, str): - current_recipients = [recipients] - else: - current_recipients = recipients - - if not current_recipients: - self.logger.error("No recipients specified for email.") - return "No recipients specified for email.", SkillResult.FAILURE - - recipients_str = ", ".join(current_recipients) - - self.logger.info( - f"\033[96m[BrainClient] Sending emergency email notification\033[0m\n" - f"To: {recipients_str}\n" - f"Subject: {subject}\n" - f"Message: {message}" - ) - - self.logger.info(f"\033[92m[BrainClient] Emergency email sent to {recipients_str}\033[0m") - return f"Email sent to {recipients_str}", SkillResult.SUCCESS - - # Just pretending here it worked for sure. - - try: - # Create message - msg = MIMEMultipart() - msg["From"] = self.sender_email - msg["To"] = recipients_str - msg["Subject"] = subject - msg.attach(MIMEText(message, "plain")) - - # Connect to server and send - server = smtplib.SMTP(self.smtp_server, self.smtp_port) - server.starttls() - server.login(self.sender_email, self.password) - server.send_message(msg) - server.quit() - - # Log success message - self.logger.info(f"\033[92m[BrainClient] Emergency email sent to {recipients_str}\033[0m") - return f"Email sent to {recipients_str}", SkillResult.SUCCESS - - except Exception as e: - self.logger.error(f"Failed to send email: {str(e)}") - return f"Failed to send email: {str(e)}", SkillResult.FAILURE - - def cancel(self): - """ - Cancel the email sending operation. - - Since email sending is typically a quick operation that completes almost - instantly, this method doesn't do much. It's implemented to satisfy the - Primitive interface. - - Returns: - str: A message describing the cancellation result. - """ - self.logger.info("\033[91m[BrainClient] Email sending operation cannot be canceled once started\033[0m") - return "Email sending is an atomic operation that cannot be canceled once started" diff --git a/workspace/innate_skills/send_picture_via_email.py b/workspace/innate_skills/send_picture_via_email.py deleted file mode 100644 index d22ab94be..000000000 --- a/workspace/innate_skills/send_picture_via_email.py +++ /dev/null @@ -1,98 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -import base64 -import smtplib -from email.mime.image import MIMEImage -from email.mime.multipart import MIMEMultipart -from email.mime.text import MIMEText - -from brain_client.skills.types import RobotState, RobotStateType, Skill, SkillResult - - -class SendPictureViaEmail(Skill): - """ - Primitive for sending an email with an attached picture. - """ - - # Declare required robot state using descriptor - image = RobotState(RobotStateType.LAST_MAIN_CAMERA_IMAGE_B64) - - def __init__(self, logger): - super().__init__(logger) - self.default_recipient = "axel@innate.bot" - # Email server configuration (same as SendEmail) - self.smtp_server = "smtp.gmail.com" - self.smtp_port = 587 - self.sender_email = "axel@innate.bot" - self.password = "" # Use app password for Gmail - - @property - def name(self): - return "send_picture_via_email" - - def guidelines(self): - return ( - "Use to send an email with the latest view from the robot eyes. " - "Provide a subject and a message body. " - "The view will be automatically attached." - ) - - def execute(self, subject: str, message: str, recipient: str = None): - """ - Sends an email with the last captured image attached. - - Args: - subject (str): Email subject line. - message (str): Email body content. - recipient (str, optional): Email recipient. Defaults to default_recipient. - - Returns: - tuple: (result_message, result_status) - """ - if not recipient: # Checks for None or empty string - recipient = self.default_recipient - - if not self.image: - self.logger.error("[SendPictureViaEmail] No image available to send.") - return "No image available to send", SkillResult.FAILURE - - self.logger.info(f"\\033[96m[BrainClient] Sending email with picture to {recipient}\\033[0m") - - try: - # Decode the base64 image - image_data = base64.b64decode(self.image) - - # Create message - msg = MIMEMultipart() - msg["From"] = self.sender_email - msg["To"] = recipient - msg["Subject"] = subject - - # Attach the text message - msg.attach(MIMEText(message, "plain")) - - # Attach the image - image = MIMEImage(image_data, name="robot_capture.jpg") - msg.attach(image) - - # Connect to server and send - server = smtplib.SMTP(self.smtp_server, self.smtp_port) - server.starttls() - server.login(self.sender_email, self.password) - server.send_message(msg) - server.quit() - - self.logger.info(f"\\033[92m[BrainClient] Email with picture sent to {recipient}\\033[0m") - return f"Email with picture sent to {recipient}", SkillResult.SUCCESS - - except Exception as e: - self.logger.error(f"[SendPictureViaEmail] Failed to send email: {str(e)}") - return f"Failed to send email: {str(e)}", SkillResult.FAILURE - - def cancel(self): - """ - Cancel the email sending operation (typically quick, so not much to do). - """ - self.logger.info("\\033[91m[BrainClient] Email sending cannot be effectively canceled once started.\\033[0m") - return "Email sending is an atomic operation and cannot be effectively canceled." diff --git a/workspace/innate_skills/turn_in_place.py b/workspace/innate_skills/turn_in_place.py index ff70cdd95..412dacf01 100644 --- a/workspace/innate_skills/turn_in_place.py +++ b/workspace/innate_skills/turn_in_place.py @@ -3,15 +3,14 @@ import math import time -from innate import Interface, InterfaceType, RobotState, RobotStateType, Skill, SkillResult from pydantic import BaseModel +from innate import Mobility, Odometry, Skill, SkillOutput, SkillReturn + # Allowed angular speeds (rad/s). Slow on purpose: no obstacle awareness here. MIN_SPEED = 0.2 MAX_SPEED = 1.0 DEFAULT_SPEED = 0.5 -# how long to wait for the first odom message after execute() starts -ODOM_WAIT_SEC = 2.0 class TurnResult(BaseModel): @@ -21,98 +20,36 @@ class TurnResult(BaseModel): class TurnInPlace(Skill): - """Turn in place by an angle using raw cmd_vel closed on odometry yaw -- - no Nav2, no map. Positive angle turns left (counter-clockwise, ROS - convention), negative turns right.""" - - mobility = Interface(InterfaceType.MOBILITY) - odom = RobotState(RobotStateType.LAST_ODOM) - - def __init__(self, logger): - super().__init__(logger) - self._cancelled = False - - @property - def name(self): - return "turn_in_place" - - def guidelines(self): - return ( - "Turn the robot in place by angle_degrees: positive turns left " - "(counter-clockwise), negative turns right. Uses odometry only -- no map or " - "path planning. E.g. turn right 90 degrees -> angle_degrees=-90." - ) + """Turn the robot in place by angle_degrees: positive turns left + (counter-clockwise), negative turns right. Uses odometry only -- no map or + path planning. E.g. turn right 90 degrees -> angle_degrees=-90.""" - def execute(self, angle_degrees: float, speed: float = DEFAULT_SPEED): - try: - return self._execute(angle_degrees, speed) - finally: - # reset on exit, not entry: an entry reset would erase a cancel - # delivered while the server was still setting up the goal - self._cancelled = False + mobility: Mobility + odom: Odometry - def _execute(self, angle_degrees: float, speed: float): - if self.mobility is None: - return "Mobility interface not available", SkillResult.FAILURE + def execute(self, angle_degrees: float, speed: float = DEFAULT_SPEED) -> SkillReturn: if angle_degrees == 0.0: - return "Turned 0 degrees", SkillResult.SUCCESS, TurnResult(turned_degrees=0.0) - yaw = self._wait_for_yaw() - if self._cancelled: - return "Turn cancelled", SkillResult.CANCELLED - if yaw is None: - return "No odometry available", SkillResult.FAILURE - + return SkillOutput("Turned 0 degrees", TurnResult(turned_degrees=0.0)) target = abs(angle_degrees) sign = 1.0 if angle_degrees > 0 else -1.0 velocity = math.copysign(min(max(abs(speed), MIN_SPEED), MAX_SPEED), angle_degrees) deadline = time.time() + math.radians(target) / abs(velocity) * 3.0 + 2.0 - # accumulate wrapped yaw deltas so multi-turn and the ±180° seam work; - # signed so motion against the commanded direction subtracts + # accumulate wrapped yaw deltas so multi-turn and the ±180° seam work turned = 0.0 - last_yaw = yaw + last_yaw = self.odom.theta_degrees while turned < target: - if self._cancelled: - self._stop() - return f"Turn cancelled after {turned:.0f} degrees", SkillResult.CANCELLED if time.time() > deadline: - self._stop() - return f"Stuck: turned only {turned:.0f} of {target:.0f} degrees", SkillResult.FAILURE - # duration acts as a deadman: if this loop dies, the base stops + self.fail(f"Stuck: turned only {turned:.0f} of {target:.0f} degrees") self.mobility.send_cmd_vel(angular_z=velocity, duration=0.5) - time.sleep(0.05) - yaw = self._yaw() - if yaw is not None: - delta = (yaw - last_yaw + 180.0) % 360.0 - 180.0 - turned += delta * sign - last_yaw = yaw + self.sleep(0.05) + yaw = self.odom.theta_degrees + turned += ((yaw - last_yaw + 180.0) % 360.0 - 180.0) * sign + last_yaw = yaw - self._stop() + self.mobility.stop() direction = "left" if angle_degrees > 0 else "right" - return ( + return SkillOutput( f"Turned {turned:.0f} degrees {direction}", - SkillResult.SUCCESS, TurnResult(turned_degrees=round(turned, 1)), ) - - def cancel(self): - self._cancelled = True - self._stop() - return "Turn cancelled" - - def _yaw(self): - """Current heading in degrees from the odometry state, or None.""" - return self.odom.theta_degrees if self.odom is not None else None - - def _wait_for_yaw(self): - """Heading once odometry arrives, or None after ODOM_WAIT_SEC.""" - deadline = time.time() + ODOM_WAIT_SEC - while True: - yaw = self._yaw() - if yaw is not None or self._cancelled or time.time() > deadline: - return yaw - time.sleep(0.02) - - def _stop(self): - if self.mobility is not None: - self.mobility.send_cmd_vel(linear_x=0.0, angular_z=0.0) diff --git a/workspace/innate_skills/update_chess_state.py b/workspace/innate_skills/update_chess_state.py deleted file mode 100644 index 6bdd13681..000000000 --- a/workspace/innate_skills/update_chess_state.py +++ /dev/null @@ -1,110 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-License-Identifier: Apache-2.0 -# Copyright (c) 2026 Innate Inc -""" -Update Chess State Skill - Validates and applies a UCI move to the persisted -board state in ~/chess_game_state.json. - -Called by the agent after every move (robot's own move or detected opponent move). -""" - -import json -from pathlib import Path - -import chess - -from brain_client.skills.types import Skill, SkillResult - -GAME_STATE_FILE = Path.home() / "chess_game_state.json" - - -class UpdateChessState(Skill): - """Validate a UCI move and apply it to the persisted game state.""" - - def __init__(self, logger): - super().__init__(logger) - - @property - def name(self): - return "update_chess_state" - - def guidelines(self): - return ( - "Apply a chess move (UCI notation, e.g. 'e2e4') to the game state. " - "Validates the move is legal, updates the FEN and move history in " - "~/chess_game_state.json. Call this after every move — both the " - "robot's own moves and detected opponent moves." - ) - - def execute(self, move_uci: str): - """ - Validate and apply a UCI move to the game state. - - Args: - move_uci: Move in UCI notation (e.g. 'e2e4', 'd7d5', 'e1g1'). - """ - move_uci = move_uci.strip().lower() - - # Load current state - if not GAME_STATE_FILE.exists(): - return "No game state found. Call reset_chess_game first.", SkillResult.FAILURE - try: - state = json.loads(GAME_STATE_FILE.read_text()) - except Exception as e: - return f"Failed to load game state: {e}", SkillResult.FAILURE - - fen = state.get("fen", chess.STARTING_FEN) - move_history = list(state.get("move_history", [])) - robot_color = state.get("robot_color", "white") - - # Validate - board = chess.Board(fen) - try: - move = chess.Move.from_uci(move_uci) - except ValueError: - return f"Invalid UCI notation: '{move_uci}'", SkillResult.FAILURE - - if move not in board.legal_moves: - legal = [m.uci() for m in board.legal_moves] - return ( - f"Move {move_uci} is not legal. Legal moves: {legal}", - SkillResult.FAILURE, - ) - - # Apply - san = board.san(move) - board.push(move) - new_fen = board.fen() - move_history.append(move_uci) - - turn = "white" if board.turn == chess.WHITE else "black" - new_state = { - "fen": new_fen, - "move_history": move_history, - "last_move": move_uci, - "turn": turn, - "robot_color": robot_color, - } - - try: - GAME_STATE_FILE.write_text(json.dumps(new_state, indent=2)) - except Exception as e: - return f"Failed to save game state: {e}", SkillResult.FAILURE - - # Check for game-ending conditions - status = "" - if board.is_checkmate(): - winner = "Black" if board.turn == chess.WHITE else "White" - status = f" Checkmate — {winner} wins!" - elif board.is_stalemate(): - status = " Stalemate — draw!" - elif board.is_check(): - status = " Check!" - - msg = f"Applied {san} ({move_uci}). Turn: {turn}. FEN: {new_fen}{status}" - self.logger.info(f"[UpdateChessState] {msg}") - self._send_feedback(msg) - return msg, SkillResult.SUCCESS - - def cancel(self): - return "Update cannot be cancelled" diff --git a/workspace/innate_skills/wave/__init__.py b/workspace/innate_skills/wave/__init__.py new file mode 100644 index 000000000..4a5cbcf80 --- /dev/null +++ b/workspace/innate_skills/wave/__init__.py @@ -0,0 +1,11 @@ +# AUTO-GENERATED physical-skill ref by the skill catalog — do not edit; edits are overwritten. +"""Typed ref to 'innate-os/wave', the recording in this folder. + +Same class either way: + + from physical_skills import Wave +""" + +from physical_skills import Wave + +__all__ = ["Wave"] diff --git a/workspace/inputs/micro_input.py b/workspace/inputs/micro_input.py index 31f0198ee..dd49a23e1 100644 --- a/workspace/inputs/micro_input.py +++ b/workspace/inputs/micro_input.py @@ -18,14 +18,11 @@ import threading import time -import sounddevice as sd - from brain_client.common.logging import UniversalLogger from brain_client.inputs.types import InputDevice DEFAULT_SAMPLE_RATE = 24_000 DEFAULT_CHANNELS = 1 -DTYPE = "int16" CHUNK_DURATION_SEC = 0.02 @@ -495,44 +492,3 @@ def stop(self): self._proc.kill() except Exception: pass - - -class MicStreamer: - """Streams audio via sounddevice (PortAudio).""" - - def __init__(self, logger): - self.queue: queue.Queue[bytes] = queue.Queue(maxsize=50) - self._stream: sd.RawInputStream | None = None - self.logger = logger - self.sample_rate = DEFAULT_SAMPLE_RATE - self.channels = DEFAULT_CHANNELS - - def _callback(self, indata, frames, time_info, status): - if status: - self.logger.warn(f"[PortAudio] {status}") - try: - self.queue.put_nowait(bytes(indata)) - except queue.Full: - pass - - def start( - self, device: str | None = None, sample_rate: int = DEFAULT_SAMPLE_RATE, channels: int = DEFAULT_CHANNELS - ): - self.sample_rate = int(sample_rate) - self.channels = int(channels) - frames_per_chunk = int(self.sample_rate * CHUNK_DURATION_SEC) - kwargs = dict( - samplerate=self.sample_rate, - channels=self.channels, - dtype=DTYPE, - blocksize=frames_per_chunk, - callback=self._callback, - ) - if device: - try: - kwargs["device"] = int(device) if isinstance(device, str) and device.isdigit() else device - except Exception: - kwargs["device"] = device - - self._stream = sd.RawInputStream(**kwargs) - self._stream.start()