From dd11119176d2a94a18557b773a4b9f11a07c2305 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= Date: Sun, 8 Nov 2026 10:44:39 +1000 Subject: [PATCH] feat: define deterministic grant scenarios --- src/iiwa_experement/config/scenes.yaml | 176 ++++++++ .../iiwa_experement/scenario.py | 406 ++++++++++++++++++ .../test/test_package_layout.py | 1 + src/iiwa_experement/test/test_scenario.py | 281 ++++++++++++ 4 files changed, 864 insertions(+) create mode 100644 src/iiwa_experement/config/scenes.yaml create mode 100644 src/iiwa_experement/iiwa_experement/scenario.py create mode 100644 src/iiwa_experement/test/test_scenario.py diff --git a/src/iiwa_experement/config/scenes.yaml b/src/iiwa_experement/config/scenes.yaml new file mode 100644 index 0000000..adf5e72 --- /dev/null +++ b/src/iiwa_experement/config/scenes.yaml @@ -0,0 +1,176 @@ +# Machine-readable source for the twenty grant experiments. +# +# Units are metres and degrees in the Webots world frame. The robot is spawned +# by iiwa_bringup at (-0.25, 0, 0.79). The tabletop is at Z = 0.793. +# +# The shared poses below are copied from doc/experiment-scenes.md. Experiments +# only list the objects they add and the poses that differ. No value is chosen +# at run time. `patron` is part of the robot URDF in every experiment and is +# therefore not listed as a scene object. + +schema_version: 1 + +poses: + fixture: + x: 0.30 + y: 0.12 + z: 0.803 + yaw_deg: 45 + body: + x: 0.30 + y: 0.12 + z: 0.823 + yaw_deg: 45 + rack: + x: 0.12 + y: -0.42 + z: 0.793 + yaw_deg: 0 + tray: + x: -0.08 + y: 0.34 + z: 0.793 + yaw_deg: 0 + cap_free: + x: 0.35 + y: -0.22 + z: 0.790667 + yaw_deg: 45 + cap_installed: + x: 0.30 + y: 0.12 + z: 0.82594 + yaw_deg: 45 + tools: + vacuum: + x: 0.255 + y: -0.42 + z: 0.803 + yaw_deg: 0 + magnetic_tool: + x: 0.165 + y: -0.42 + z: 0.803 + yaw_deg: 0 + sealant_tool: + x: 0.075 + y: -0.42 + z: 0.803 + yaw_deg: 0 + torque_tool: + x: -0.015 + y: -0.42 + z: 0.803 + yaw_deg: 0 + +# Screw offsets are relative to the tray centre. The order is the screw_1 ... +# screw_10 order from doc/experiment-scenes.md; only the first four are at the +# grid corners. All screws rest with their lower point at Z = 0.7954. +screws: + z: 0.7954 + layout: + - {name: screw_1, dx: -0.040, dy: -0.015} + - {name: screw_2, dx: 0.040, dy: -0.015} + - {name: screw_3, dx: -0.040, dy: 0.015} + - {name: screw_4, dx: 0.040, dy: 0.015} + - {name: screw_5, dx: -0.020, dy: -0.015} + - {name: screw_6, dx: 0.000, dy: -0.015} + - {name: screw_7, dx: 0.020, dy: -0.015} + - {name: screw_8, dx: -0.020, dy: 0.015} + - {name: screw_9, dx: 0.000, dy: 0.015} + - {name: screw_10, dx: 0.020, dy: 0.015} + +experiments: + "1": + cap: free + tools: [vacuum] + "2": + cap: free + cap_pose: {x: 0.35, y: -0.22, z: 0.790667, yaw_deg: 135} + tools: [vacuum] + "3": + fixture: {x: 0.29, y: 0.14, z: 0.803, yaw_deg: 45} + body: {x: 0.29, y: 0.14, z: 0.823, yaw_deg: 45} + cap: free + cap_pose: {x: 0.33, y: -0.22, z: 0.790667, yaw_deg: 45} + tools: [vacuum] + "4": + cap: free + tools: [vacuum] + failures: + - {skill: grab, outcome: grasp_failed, attempt: 1} + "5": + cap: free + tools: [vacuum] + failures: + - skill: verify + outcome: not_ok + target: cap + attempt: 1 + details: {after_first_install_x: 0.303} + "6": + cap: absent + tools: [sealant_tool] + "7": + cap: absent + tools: [sealant_tool] + "8": + cap: absent + tools: [sealant_tool] + "9": + cap: absent + tools: [sealant_tool] + failures: + - {skill: dispense, outcome: interrupted, target: side_3, attempt: 1} + "10": + cap: free + tools: [sealant_tool, vacuum] + "11": + cap: installed + tray: true + tools: [magnetic_tool] + "12": + cap: installed + tray: true + tools: [magnetic_tool] + "13": + cap: installed + tray: true + tools: [magnetic_tool] + "14": + cap: installed + tray: true + tools: [magnetic_tool, torque_tool] + "15": + cap: installed + tray: true + tools: [magnetic_tool, torque_tool] + failures: + - {skill: grab, outcome: grasp_failed, target: third_selected_screw, attempt: 1} + - {skill: tighten, outcome: torque_not_reached, target: hole_4, attempt: 1} + "16": + cap: free + tray: true + tools: [vacuum, magnetic_tool, sealant_tool, torque_tool] + "17": + cap: free + tray: true + tools: [vacuum, magnetic_tool, sealant_tool, torque_tool] + "18": + cap: free + tray: true + tools: [vacuum, magnetic_tool, sealant_tool, torque_tool] + "19": + cap: free + cap_pose: {x: 0.37, y: -0.20, z: 0.790667, yaw_deg: 60} + tray: true + tray_pose: {x: -0.10, y: 0.36, z: 0.793, yaw_deg: 0} + tools: [vacuum, magnetic_tool, sealant_tool, torque_tool] + "20": + cap: free + tray: true + tools: [vacuum, magnetic_tool, sealant_tool, torque_tool] + failures: + - {skill: dispense, outcome: interrupted, attempt: 1} + - {skill: grab, outcome: grasp_failed, attempt: 1} + - {skill: verify, outcome: not_ok, target: cap, attempt: 1} diff --git a/src/iiwa_experement/iiwa_experement/scenario.py b/src/iiwa_experement/iiwa_experement/scenario.py new file mode 100644 index 0000000..360b46e --- /dev/null +++ b/src/iiwa_experement/iiwa_experement/scenario.py @@ -0,0 +1,406 @@ +"""Strict loader for the deterministic grant experiment manifest. + +The manifest in ``config/scenes.yaml`` is the only source of object poses for +the twenty experiments. Everything in this module is pure Python so that the +same validation runs in unit tests and at launch time. +""" +from __future__ import annotations + +import math +from dataclasses import dataclass +from pathlib import Path +from typing import Any, Mapping + +import yaml + + +CAP_STATES = ('absent', 'free', 'installed') +TOOL_NAMES = ('vacuum', 'magnetic_tool', 'sealant_tool', 'torque_tool') +EXPERIMENT_IDS = tuple(range(1, 21)) + +# Bounds of lotok.obj after its local Y axis is mapped to Webots Z. They are +# repeated here so validation does not need a Webots process or the mesh file. +TRAY_HALF_LENGTH = 0.060 +TRAY_HALF_WIDTH = 0.035 +SCREW_RADIUS = 0.00275 + +# Failure metadata is consumed by a future skill executor. It never changes +# the initial pose of an object. +_ALLOWED_FAILURES = { + 'grab': {'grasp_failed'}, + 'verify': {'not_ok'}, + 'dispense': {'interrupted'}, + 'tighten': {'torque_not_reached'}, + 'seat_screw': {'not_seated'}, + 'press': {'failed'}, +} + + +class ScenarioError(ValueError): + """Raised when the manifest or a requested experiment is invalid.""" + + +@dataclass(frozen=True) +class Pose: + x: float + y: float + z: float + yaw_deg: float + + @property + def xy(self) -> tuple[float, float]: + return self.x, self.y + + +@dataclass(frozen=True) +class Tool: + name: str + pose: Pose + + +@dataclass(frozen=True) +class Screw: + name: str + pose: Pose + + +@dataclass(frozen=True) +class Failure: + skill: str + outcome: str + target: str | None = None + attempt: int | None = None + details: tuple[tuple[str, Any], ...] = () + + def as_dict(self) -> dict[str, Any]: + result: dict[str, Any] = {'skill': self.skill, 'outcome': self.outcome} + if self.target is not None: + result['target'] = self.target + if self.attempt is not None: + result['attempt'] = self.attempt + if self.details: + result['details'] = dict(self.details) + return result + + +@dataclass(frozen=True) +class ScenarioSpec: + experiment_id: int + fixture: Pose + body: Pose + rack: Pose + cap_state: str + cap_pose: Pose | None + tools: tuple[Tool, ...] + tray_pose: Pose | None + screws: tuple[Screw, ...] + failures: tuple[Failure, ...] + + @property + def tool_names(self) -> tuple[str, ...]: + return tuple(tool.name for tool in self.tools) + + +def _mapping(value: Any, label: str) -> Mapping[str, Any]: + if not isinstance(value, Mapping): + raise ScenarioError(f'{label} must be a mapping') + return value + + +def _number(value: Any, label: str) -> float: + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise ScenarioError(f'{label} must be a finite number') + result = float(value) + if not math.isfinite(result): + raise ScenarioError(f'{label} must be a finite number') + return result + + +def _pose(raw: Any, label: str) -> Pose: + data = _mapping(raw, label) + for key in ('x', 'y', 'z'): + if key not in data: + raise ScenarioError(f'{label}.{key} is required') + return Pose( + x=_number(data['x'], f'{label}.x'), + y=_number(data['y'], f'{label}.y'), + z=_number(data['z'], f'{label}.z'), + yaw_deg=_number(data.get('yaw_deg', 0.0), f'{label}.yaw_deg'), + ) + + +def _merge_pose(base: Pose, override: Any, label: str) -> Pose: + if override is None: + return base + data = _mapping(override, label) + merged = { + 'x': base.x, + 'y': base.y, + 'z': base.z, + 'yaw_deg': base.yaw_deg, + } + merged.update(data) + return _pose(merged, label) + + +def _same_angle(first: float, second: float) -> bool: + delta = (first - second) % 360.0 + return math.isclose(min(delta, 360.0 - delta), 0.0, abs_tol=1e-9) + + +def _require_mapping(data: Mapping[str, Any], key: str) -> Mapping[str, Any]: + if key not in data: + raise ScenarioError(f"manifest is missing '{key}'") + return _mapping(data[key], key) + + +def _experiment_table(data: Mapping[str, Any]) -> dict[int, Mapping[str, Any]]: + raw = _require_mapping(data, 'experiments') + parsed: dict[int, Mapping[str, Any]] = {} + for raw_id, entry in raw.items(): + text = str(raw_id).strip() + if isinstance(raw_id, bool) or not text.isdigit(): + raise ScenarioError(f'invalid experiment id {raw_id!r} in manifest') + experiment_id = int(text) + if experiment_id not in EXPERIMENT_IDS or text != str(experiment_id): + raise ScenarioError(f'experiment id {raw_id!r} is outside 1..20') + if experiment_id in parsed: + raise ScenarioError(f'duplicate experiment id {experiment_id} in manifest') + parsed[experiment_id] = _mapping(entry, f'experiments.{experiment_id}') + if set(parsed) != set(EXPERIMENT_IDS): + missing = sorted(set(EXPERIMENT_IDS) - set(parsed)) + extra = sorted(set(parsed) - set(EXPERIMENT_IDS)) + raise ScenarioError( + f'manifest must define experiments 1..20; missing={missing}, extra={extra}' + ) + return parsed + + +def _tool_poses(poses: Mapping[str, Any]) -> dict[str, Pose]: + raw = _mapping(poses.get('tools'), 'poses.tools') + missing = [name for name in TOOL_NAMES if name not in raw] + extra = sorted(set(raw) - set(TOOL_NAMES)) + if missing or extra: + raise ScenarioError( + f'poses.tools must define {list(TOOL_NAMES)}; missing={missing}, extra={extra}' + ) + return {name: _pose(raw[name], f'poses.tools.{name}') for name in TOOL_NAMES} + + +def _tools(raw: Any, poses: Mapping[str, Pose]) -> tuple[Tool, ...]: + if not isinstance(raw, list) or not raw: + raise ScenarioError('experiment tools must be a nonempty list') + tools: list[Tool] = [] + seen: set[str] = set() + for index, name in enumerate(raw): + if not isinstance(name, str): + raise ScenarioError(f'tools[{index}] must be a tool name') + if name not in TOOL_NAMES: + raise ScenarioError( + f'unknown tool {name!r}; expected one of {list(TOOL_NAMES)}' + ) + if name in seen: + raise ScenarioError(f'duplicate tool {name!r}') + seen.add(name) + tools.append(Tool(name, poses[name])) + return tuple(tools) + + +def _rotate_offset(dx: float, dy: float, yaw_deg: float) -> tuple[float, float]: + angle = math.radians(yaw_deg) + cosine, sine = math.cos(angle), math.sin(angle) + return dx * cosine - dy * sine, dx * sine + dy * cosine + + +def _screws( + raw_entry: Mapping[str, Any], + raw_manifest: Mapping[str, Any], + experiment_id: int, + tray_pose: Pose | None, +) -> tuple[Screw, ...]: + explicit = raw_entry.get('screws') + if tray_pose is None: + if explicit is not None: + raise ScenarioError( + f'experiment {experiment_id} has screws but no tray' + ) + return () + + if explicit is None: + screw_data = _mapping(raw_manifest.get('screws'), 'screws') + layout = screw_data.get('layout') + if not isinstance(layout, list): + raise ScenarioError('screws.layout must be a list') + z = _number(screw_data.get('z'), 'screws.z') + else: + if not isinstance(explicit, list): + raise ScenarioError(f'experiments.{experiment_id}.screws must be a list') + layout = explicit + screw_data = _mapping(raw_manifest.get('screws'), 'screws') + z = _number(screw_data.get('z'), 'screws.z') + + if len(layout) != 10: + raise ScenarioError( + f'experiment {experiment_id} has {len(layout)} screws; ' + 'a tray scenario must contain exactly 10 screws' + ) + + result: list[Screw] = [] + names: set[str] = set() + for index, raw_screw in enumerate(layout): + label = f'experiments.{experiment_id}.screws[{index}]' + data = _mapping(raw_screw, label) + name = data.get('name') + if not isinstance(name, str) or not name: + raise ScenarioError(f'{label}.name must be a nonempty string') + if name in names: + raise ScenarioError(f'duplicate screw name {name!r}') + names.add(name) + dx = _number(data.get('dx'), f'{label}.dx') + dy = _number(data.get('dy'), f'{label}.dy') + if abs(dx) > TRAY_HALF_LENGTH - SCREW_RADIUS or abs(dy) > TRAY_HALF_WIDTH - SCREW_RADIUS: + raise ScenarioError( + f'screw {name!r} at offset ({dx}, {dy}) is outside the tray' + ) + world_dx, world_dy = _rotate_offset(dx, dy, tray_pose.yaw_deg) + result.append(Screw( + name=name, + pose=Pose(tray_pose.x + world_dx, tray_pose.y + world_dy, z, tray_pose.yaw_deg), + )) + return tuple(result) + + +def _failures(raw: Any, experiment_id: int) -> tuple[Failure, ...]: + if raw is None: + return () + if not isinstance(raw, list): + raise ScenarioError(f'experiments.{experiment_id}.failures must be a list') + result: list[Failure] = [] + for index, entry in enumerate(raw): + label = f'experiments.{experiment_id}.failures[{index}]' + data = _mapping(entry, label) + skill = data.get('skill') + outcome = data.get('outcome') + if skill not in _ALLOWED_FAILURES: + raise ScenarioError(f'{label}.skill must be one of {sorted(_ALLOWED_FAILURES)}') + if outcome not in _ALLOWED_FAILURES[skill]: + raise ScenarioError( + f'{label}.outcome {outcome!r} is not valid for skill {skill!r}' + ) + target = data.get('target') + if target is not None and not isinstance(target, str): + raise ScenarioError(f'{label}.target must be a string') + attempt = data.get('attempt') + if attempt is not None: + if isinstance(attempt, bool) or not isinstance(attempt, int) or attempt < 1: + raise ScenarioError(f'{label}.attempt must be a positive integer') + details_raw = data.get('details', {}) + details = _mapping(details_raw, f'{label}.details') + result.append(Failure( + skill=skill, + outcome=outcome, + target=target, + attempt=attempt, + details=tuple(sorted(details.items())), + )) + return tuple(result) + + +def load_scenario(experiment_id: int, manifest_path: Path | str) -> ScenarioSpec: + """Load and fully validate one experiment from the YAML manifest.""" + if isinstance(experiment_id, bool) or not isinstance(experiment_id, int): + raise ScenarioError(f'experiment id must be an integer in 1..20, got {experiment_id!r}') + if experiment_id not in EXPERIMENT_IDS: + raise ScenarioError(f'experiment id {experiment_id} is outside 1..20') + + path = Path(manifest_path) + if not path.is_file(): + raise FileNotFoundError(f'scenario manifest does not exist: {path}') + try: + raw = yaml.safe_load(path.read_text(encoding='utf-8')) + except yaml.YAMLError as error: + raise ScenarioError(f'invalid YAML in {path}: {error}') from error + data = _mapping(raw, 'manifest') + if data.get('schema_version') != 1: + raise ScenarioError('manifest schema_version must be 1') + + experiments = _experiment_table(data) + entry = experiments[experiment_id] + poses = _require_mapping(data, 'poses') + fixture = _merge_pose( + _pose(poses.get('fixture'), 'poses.fixture'), + entry.get('fixture'), + f'experiments.{experiment_id}.fixture', + ) + body = _merge_pose( + _pose(poses.get('body'), 'poses.body'), + entry.get('body'), + f'experiments.{experiment_id}.body', + ) + if not ( + math.isclose(fixture.x, body.x, abs_tol=1e-9) + and math.isclose(fixture.y, body.y, abs_tol=1e-9) + and _same_angle(fixture.yaw_deg, body.yaw_deg) + ): + raise ScenarioError( + f'experiment {experiment_id}: fixture and body must share X, Y and yaw' + ) + rack = _pose(poses.get('rack'), 'poses.rack') + tool_poses = _tool_poses(poses) + + cap_state = entry.get('cap', 'free') + if cap_state not in CAP_STATES: + raise ScenarioError( + f'experiment {experiment_id}: cap must be one of {list(CAP_STATES)}' + ) + if cap_state == 'absent': + if entry.get('cap_pose') is not None: + raise ScenarioError(f'experiment {experiment_id}: an absent cap cannot have a pose') + cap_pose = None + elif cap_state == 'free': + cap_pose = _merge_pose( + _pose(poses.get('cap_free'), 'poses.cap_free'), + entry.get('cap_pose'), + f'experiments.{experiment_id}.cap_pose', + ) + else: + cap_pose = _merge_pose( + _pose(poses.get('cap_installed'), 'poses.cap_installed'), + entry.get('cap_pose'), + f'experiments.{experiment_id}.cap_pose', + ) + if not ( + math.isclose(cap_pose.x, body.x, abs_tol=1e-9) + and math.isclose(cap_pose.y, body.y, abs_tol=1e-9) + and _same_angle(cap_pose.yaw_deg, body.yaw_deg) + ): + raise ScenarioError( + f'experiment {experiment_id}: an installed cap must share the body pose' + ) + + tray_enabled = entry.get('tray', False) + if not isinstance(tray_enabled, bool): + raise ScenarioError(f'experiments.{experiment_id}.tray must be true or false') + if tray_enabled: + tray_pose = _merge_pose( + _pose(poses.get('tray'), 'poses.tray'), + entry.get('tray_pose'), + f'experiments.{experiment_id}.tray_pose', + ) + else: + if entry.get('tray_pose') is not None: + raise ScenarioError(f'experiment {experiment_id}: tray_pose requires tray: true') + tray_pose = None + + return ScenarioSpec( + experiment_id=experiment_id, + fixture=fixture, + body=body, + rack=rack, + cap_state=cap_state, + cap_pose=cap_pose, + tools=_tools(entry.get('tools'), tool_poses), + tray_pose=tray_pose, + screws=_screws(entry, data, experiment_id, tray_pose), + failures=_failures(entry.get('failures'), experiment_id), + ) diff --git a/src/iiwa_experement/test/test_package_layout.py b/src/iiwa_experement/test/test_package_layout.py index 8848fa6..c356f15 100644 --- a/src/iiwa_experement/test/test_package_layout.py +++ b/src/iiwa_experement/test/test_package_layout.py @@ -43,6 +43,7 @@ def test_expected_resources_are_installed(): 'doc/experements.md', 'doc/experiment-scenes.md', 'worlds/grant/iiwa-grant.wbt', + 'config/scenes.yaml', ): path = share / relative assert path.is_file(), path diff --git a/src/iiwa_experement/test/test_scenario.py b/src/iiwa_experement/test/test_scenario.py new file mode 100644 index 0000000..483af7b --- /dev/null +++ b/src/iiwa_experement/test/test_scenario.py @@ -0,0 +1,281 @@ +"""The twenty experiments have exactly the composition from the specification.""" +import math +from pathlib import Path + +import pytest +import yaml +from ament_index_python.packages import get_package_share_directory + +from iiwa_experement.scenario import ( + CAP_STATES, + TOOL_NAMES, + ScenarioError, + load_scenario, +) + + +PACKAGE = Path(__file__).resolve().parents[1] +SCENES = PACKAGE / 'config' / 'scenes.yaml' + +STANDARD_TOOLS = set(TOOL_NAMES) +STANDARD_SCREWS = { + 'screw_1': (-0.120, 0.325), + 'screw_2': (-0.040, 0.325), + 'screw_3': (-0.120, 0.355), + 'screw_4': (-0.040, 0.355), + 'screw_5': (-0.100, 0.325), + 'screw_6': (-0.080, 0.325), + 'screw_7': (-0.060, 0.325), + 'screw_8': (-0.100, 0.355), + 'screw_9': (-0.080, 0.355), + 'screw_10': (-0.060, 0.355), +} + + +def scenario(experiment_id): + return load_scenario(experiment_id, SCENES) + + +def variant(tmp_path, mutate): + data = yaml.safe_load(SCENES.read_text(encoding='utf-8')) + mutate(data) + path = tmp_path / 'scenes.yaml' + path.write_text(yaml.safe_dump(data, sort_keys=False), encoding='utf-8') + return path + + +def test_all_twenty_experiments_load(): + for experiment_id in range(1, 21): + spec = scenario(experiment_id) + assert spec.experiment_id == experiment_id + assert spec.cap_state in CAP_STATES + assert spec.tool_names + if experiment_id != 3: + assert (spec.fixture.x, spec.fixture.y) == (0.30, 0.12) + assert (spec.body.x, spec.body.y) == (0.30, 0.12) + assert (spec.rack.x, spec.rack.y, spec.rack.z) == (0.12, -0.42, 0.793) + + +def test_cap_only_experiments_one_to_five(): + for experiment_id in range(1, 6): + spec = scenario(experiment_id) + assert spec.cap_state == 'free' + assert spec.tray_pose is None + assert spec.screws == () + assert spec.tool_names == ('vacuum',) + + +def test_sealant_experiments_do_not_start_with_a_hatch_or_tray(): + for experiment_id in range(6, 10): + spec = scenario(experiment_id) + assert spec.cap_state == 'absent' + assert spec.cap_pose is None + assert spec.tray_pose is None + assert spec.screws == () + assert spec.tool_names == ('sealant_tool',) + + +def test_experiment_ten_has_hatch_and_both_tools(): + spec = scenario(10) + assert spec.cap_state == 'free' + assert spec.tray_pose is None + assert set(spec.tool_names) == {'sealant_tool', 'vacuum'} + + +def test_screw_experiments_have_installed_hatch_and_ten_screws(): + for experiment_id in range(11, 16): + spec = scenario(experiment_id) + assert spec.cap_state == 'installed' + assert spec.tray_pose is not None + assert len(spec.screws) == 10 + assert spec.tool_names[0] == 'magnetic_tool' + assert set(scenario(14).tool_names) == {'magnetic_tool', 'torque_tool'} + assert set(scenario(15).tool_names) == {'magnetic_tool', 'torque_tool'} + + +def test_full_assembly_experiments_have_loose_hatch_tray_and_all_tools(): + for experiment_id in range(16, 21): + spec = scenario(experiment_id) + assert spec.cap_state == 'free' + assert spec.tray_pose is not None + assert len(spec.screws) == 10 + assert set(spec.tool_names) == STANDARD_TOOLS + + +def test_standard_screw_layout_matches_the_specification(): + spec = scenario(11) + assert len({screw.name for screw in spec.screws}) == 10 + assert len({screw.pose.xy for screw in spec.screws}) == 10 + for screw in spec.screws: + assert screw.pose.xy == pytest.approx(STANDARD_SCREWS[screw.name], abs=1e-9) + assert screw.pose.z == pytest.approx(0.7954, abs=1e-12) + + +def test_experiment_two_hatch_is_rotated_135_degrees(): + spec = scenario(2) + assert spec.cap_pose is not None + assert spec.cap_pose.xy == (0.35, -0.22) + assert spec.cap_pose.yaw_deg == 135 + + +def test_experiment_three_moves_fixture_body_and_hatch_together(): + spec = scenario(3) + assert (spec.fixture.x, spec.fixture.y, spec.fixture.z) == (0.29, 0.14, 0.803) + assert (spec.body.x, spec.body.y, spec.body.z) == (0.29, 0.14, 0.823) + assert spec.cap_pose is not None + assert (spec.cap_pose.x, spec.cap_pose.y) == (0.33, -0.22) + + +def test_experiment_five_carries_the_misplaced_verification_metadata(): + spec = scenario(5) + assert spec.cap_pose is not None + assert spec.cap_pose.xy == (0.35, -0.22) + assert len(spec.failures) == 1 + failure = spec.failures[0] + assert (failure.skill, failure.outcome, failure.target, failure.attempt) == ( + 'verify', 'not_ok', 'cap', 1, + ) + assert dict(failure.details)['after_first_install_x'] == 0.303 + + +def test_experiment_nineteen_moves_hatch_and_tray_with_all_screws(): + spec = scenario(19) + assert spec.cap_pose is not None + assert spec.cap_pose.xy == (0.37, -0.20) + assert spec.cap_pose.yaw_deg == 60 + assert spec.tray_pose is not None + assert (spec.tray_pose.x, spec.tray_pose.y, spec.tray_pose.z) == (-0.10, 0.36, 0.793) + for screw in spec.screws: + expected = STANDARD_SCREWS[screw.name] + assert screw.pose.xy == pytest.approx( + (expected[0] - 0.02, expected[1] + 0.02), abs=1e-9 + ) + + +def test_failure_metadata_for_experiments_four_nine_fifteen_twenty(): + assert [(f.skill, f.outcome) for f in scenario(4).failures] == [ + ('grab', 'grasp_failed'), + ] + assert [(f.skill, f.outcome, f.target) for f in scenario(9).failures] == [ + ('dispense', 'interrupted', 'side_3'), + ] + fifteen = [(f.skill, f.outcome, f.target) for f in scenario(15).failures] + assert fifteen == [ + ('grab', 'grasp_failed', 'third_selected_screw'), + ('tighten', 'torque_not_reached', 'hole_4'), + ] + twenty = [(f.skill, f.outcome) for f in scenario(20).failures] + assert twenty == [ + ('dispense', 'interrupted'), + ('grab', 'grasp_failed'), + ('verify', 'not_ok'), + ] + + +def test_installed_cap_sits_on_the_body(): + for experiment_id in range(11, 16): + spec = scenario(experiment_id) + assert spec.cap_pose is not None + assert spec.cap_pose.xy == spec.body.xy + assert spec.cap_pose.yaw_deg == spec.body.yaw_deg + + +def test_scenarios_are_read_from_the_installed_manifest(): + installed = ( + Path(get_package_share_directory('iiwa_experement')) / 'config' / 'scenes.yaml' + ) + for experiment_id in range(1, 21): + assert load_scenario(experiment_id, installed).experiment_id == experiment_id + + +@pytest.mark.parametrize('experiment_id', [0, 21, '1', 1.0, True, None]) +def test_invalid_experiment_ids_are_rejected(experiment_id): + with pytest.raises(ScenarioError): + load_scenario(experiment_id, SCENES) + + +def test_unknown_tool_is_rejected(tmp_path): + def mutate(data): + data['experiments']['1']['tools'] = ['vacuum_cleaner'] + + with pytest.raises(ScenarioError, match='unknown tool'): + load_scenario(1, variant(tmp_path, mutate)) + + +@pytest.mark.parametrize('count', [9, 11]) +def test_incorrect_screw_count_is_rejected(tmp_path, count): + entries = [ + {'name': f'screw_{index}', 'dx': -0.04 + index * 0.005, 'dy': 0.0} + for index in range(1, count + 1) + ] + + def mutate(data): + data['experiments']['1']['tray'] = True + data['experiments']['1']['screws'] = entries + + with pytest.raises(ScenarioError, match='exactly 10 screws'): + load_scenario(1, variant(tmp_path, mutate)) + + +def test_duplicate_screw_name_is_rejected(tmp_path): + entries = [ + {'name': 'screw_1', 'dx': -0.04 + index * 0.005, 'dy': 0.0} + for index in range(10) + ] + + def mutate(data): + data['experiments']['1']['tray'] = True + data['experiments']['1']['screws'] = entries + + with pytest.raises(ScenarioError, match='duplicate screw name'): + load_scenario(1, variant(tmp_path, mutate)) + + +def test_screw_outside_the_tray_is_rejected(tmp_path): + entries = [ + {'name': f'screw_{index}', 'dx': -0.04 + index * 0.005, 'dy': 0.0} + for index in range(10) + ] + entries[4]['dx'] = 0.2 + + def mutate(data): + data['experiments']['1']['tray'] = True + data['experiments']['1']['screws'] = entries + + with pytest.raises(ScenarioError, match='outside the tray'): + load_scenario(1, variant(tmp_path, mutate)) + + +def test_fixture_and_body_must_share_xy(tmp_path): + def mutate(data): + data['experiments']['1']['body'] = { + 'x': 0.31, 'y': 0.12, 'z': 0.823, 'yaw_deg': 45, + } + + with pytest.raises(ScenarioError, match='fixture and body'): + load_scenario(1, variant(tmp_path, mutate)) + + +def test_tray_without_screws_and_screws_without_tray_are_rejected(tmp_path): + def no_screws(data): + data['experiments']['1']['screws'] = [ + {'name': f'screw_{index}', 'dx': 0.0, 'dy': 0.0} + for index in range(10) + ] + + with pytest.raises(ScenarioError, match='no tray'): + load_scenario(1, variant(tmp_path, no_screws)) + + def no_tray(data): + data['experiments']['1']['tray'] = False + data['experiments']['1']['tray_pose'] = { + 'x': -0.08, 'y': 0.34, 'z': 0.793, 'yaw_deg': 0, + } + + with pytest.raises(ScenarioError, match='tray_pose requires tray'): + load_scenario(1, variant(tmp_path, no_tray)) + + +def test_missing_manifest_is_reported(tmp_path): + with pytest.raises(FileNotFoundError): + load_scenario(1, tmp_path / 'missing.yaml')