feat: generate grant Webots objects

src/iiwa_experement/iiwa_experement/webots_nodes.py

@@ -0,0 +1,350 @@
+"""Turn a validated ScenarioSpec into deterministic Webots ``Solid`` nodes.
+
+The generated nodes are sent one at a time to
+``/Ros2Supervisor/spawn_node_from_string``.  Their order is:
+
+    fixture -> body -> rack -> working tools -> tray -> screws -> cap
+
+All positions are the initial scene only.  The fixture, body, rack and tray are
+fixed.  A loose hatch has physics; an installed hatch is locked.  Screws and
+tools are static in their tray/socket: this package does not implement grasp,
+tool docking or dynamic hatch fixation, so a later skill executor must release
+them before a robot can pick them up.
+"""
+from __future__ import annotations
+
+import json
+import math
+import re
+from pathlib import Path
+
+from .scenario import ScenarioSpec
+
+
+ASSETS = {
+    'fixture': 'stapel.obj',
+    'body': 'base-detail.obj',
+    'rack': 'tool-magizine.obj',
+    'tray': 'lotok.obj',
+    'screw': 'vint.obj',
+    'cap': 'cap.obj',
+    'vacuum': 'vacum-tool.obj',
+    'magnetic_tool': 'magnit-tool.obj',
+    'sealant_tool': 'germetic-tool.obj',
+    'torque_tool': 'wranch-tool.obj',
+}
+
+TOOL_NODE_NAMES = {
+    'vacuum': 'grant_vacuum',
+    'magnetic_tool': 'grant_magnetic_tool',
+    'sealant_tool': 'grant_sealant_tool',
+    'torque_tool': 'grant_torque_tool',
+}
+
+TOOL_SIZE = {
+    'vacuum': (0.0821, 0.14, 0.081),
+    'magnetic_tool': (0.036, 0.14, 0.036),
+    'sealant_tool': (0.05608, 0.14, 0.05608),
+    'torque_tool': (0.049, 0.14, 0.049),
+}
+
+TOOL_APPEARANCE = {
+    'vacuum': '0.16 0.47 0.72',
+    'magnetic_tool': '0.43 0.30 0.65',
+    'sealant_tool': '0.23 0.57 0.31',
+    'torque_tool': '0.80 0.32 0.18',
+}
+
+
+def rotation_matrix(yaw_deg: float) -> tuple[tuple[float, float, float], ...]:
+    """Rz(yaw) @ Rx(90): the OBJ Y axis becomes Webots Z."""
+    angle = math.radians(yaw_deg)
+    cosine, sine = math.cos(angle), math.sin(angle)
+    return (
+        (cosine, 0.0, sine),
+        (sine, 0.0, -cosine),
+        (0.0, 1.0, 0.0),
+    )
+
+
+def rotation_axis_angle(yaw_deg: float) -> tuple[float, float, float, float]:
+    """Return the Webots axis-angle equivalent of :func:`rotation_matrix`."""
+    matrix = rotation_matrix(yaw_deg)
+    trace = matrix[0][0] + matrix[1][1] + matrix[2][2]
+    cosine = max(-1.0, min(1.0, (trace - 1.0) / 2.0))
+    angle = math.acos(cosine)
+    if angle < 1e-12:
+        return 0.0, 0.0, 1.0, 0.0
+    if math.pi - angle < 1e-9:
+        # 180 degrees: recover the axis from the symmetric part.
+        diagonal = [matrix[i][i] for i in range(3)]
+        axis = [math.sqrt(max(0.0, (diagonal[i] + 1.0) / 2.0)) for i in range(3)]
+        return tuple(axis) + (angle,)
+    scale = 2.0 * math.sin(angle)
+    axis = (
+        (matrix[2][1] - matrix[1][2]) / scale,
+        (matrix[0][2] - matrix[2][0]) / scale,
+        (matrix[1][0] - matrix[0][1]) / scale,
+    )
+    return tuple(axis) + (angle,)
+
+
+def _vector(values) -> str:
+    return ' '.join(format(value, '.12g') for value in values)
+
+
+def _asset_path(asset_root: Path, name: str) -> Path:
+    path = asset_root / ASSETS[name]
+    if not path.is_file():
+        raise FileNotFoundError(f'Webots asset does not exist: {path}')
+    source = path.resolve()
+    for line in source.read_text(encoding='utf-8', errors='replace').splitlines():
+        parts = line.split()
+        if parts and parts[0].lower() == 'mtllib':
+            for material in parts[1:]:
+                material_path = source.parent / material
+                if not material_path.is_file():
+                    raise FileNotFoundError(
+                        f'Webots material does not exist: {material_path} '
+                        f'(referenced by {source})'
+                    )
+    return path
+
+
+def _mesh_shape(asset: Path, color: str, scale: float | None = None) -> str:
+    shape = f'''Shape {{
+  appearance PBRAppearance {{
+    baseColor {color}
+    roughness 0.5
+    metalness 0.3
+  }}
+  geometry Mesh {{
+    url [ {json.dumps(str(asset))} ]
+  }}
+}}'''
+    if scale is None:
+        return shape
+    return f'''Transform {{
+  scale {_vector((scale, scale, scale))}
+  children [
+{_indent(shape, 4)}
+  ]
+}}'''
+
+
+def _indent(text: str, spaces: int) -> str:
+    prefix = ' ' * spaces
+    return '\n'.join(prefix + line for line in text.splitlines())
+
+
+def _solid(
+    *,
+    name: str,
+    translation,
+    rotation,
+    children: str,
+    bounding: str,
+    locked: bool,
+    physics_mass: float | None = None,
+) -> str:
+    lines = [
+        'Solid {',
+        f'  translation {_vector(translation)}',
+        f'  rotation {_vector(rotation)}',
+        f'  name "{name}"',
+    ]
+    if locked:
+        lines.append('  locked TRUE')
+    lines.extend([
+        '  children [',
+        _indent(children, 4),
+        '  ]',
+        '  boundingObject ' + bounding,
+    ])
+    if physics_mass is not None:
+        lines.append(f'  physics Physics {{ mass {format(physics_mass, ".12g")} }}')
+    lines.append('}')
+    return '\n'.join(lines)
+
+
+def _fixture_node(spec: ScenarioSpec, asset: Path) -> str:
+    pose = spec.fixture
+    bounding = '''Group {
+  children [
+    Pose {
+      translation 0 -0.005 0
+      children [ Box { size 0.35 0.01 0.22 } ]
+    }
+    Pose {
+      translation -0.104 0.008 0
+      children [ Box { size 0.014 0.016 0.156 } ]
+    }
+    Pose {
+      translation 0.104 0.008 0
+      children [ Box { size 0.014 0.016 0.156 } ]
+    }
+  ]
+}'''
+    return _solid(
+        name='grant_fixture',
+        translation=(pose.x, pose.y, pose.z),
+        rotation=rotation_axis_angle(pose.yaw_deg),
+        children=_mesh_shape(asset, '0.30 0.34 0.39'),
+        bounding=bounding,
+        locked=True,
+    )
+
+
+def _body_node(spec: ScenarioSpec, asset: Path) -> str:
+    pose = spec.body
+    bounding = '''Pose {
+  translation 0 0.000695 0
+  children [ Box { size 0.30 0.00915 0.17 } ]
+}'''
+    return _solid(
+        name='grant_body',
+        translation=(pose.x, pose.y, pose.z),
+        rotation=rotation_axis_angle(pose.yaw_deg),
+        children=_mesh_shape(asset, '0.64 0.68 0.72'),
+        bounding=bounding,
+        locked=True,
+    )
+
+
+def _rack_node(spec: ScenarioSpec, asset: Path) -> str:
+    pose = spec.rack
+    # The magazine model was validated with its local X turned around, so the
+    # tool sockets face the robot. It still maps the OBJ Y axis to Webots Z.
+    rotation = rotation_axis_angle(pose.yaw_deg + 180.0)
+    bounding = '''Pose {
+  translation 0 0.005 0
+  children [ Box { size 0.42 0.01 0.14 } ]
+}'''
+    return _solid(
+        name='grant_rack',
+        translation=(pose.x, pose.y, pose.z),
+        rotation=rotation,
+        children=_mesh_shape(asset, '0.72 0.47 0.14'),
+        bounding=bounding,
+        locked=True,
+    )
+
+
+def _tool_node(tool, asset: Path) -> str:
+    scale = 10.0 if tool.name == 'sealant_tool' else None
+    bounding = f'''Pose {{
+  translation 0 0.07 0
+  children [ Box {{ size {_vector(TOOL_SIZE[tool.name])} }} ]
+}}'''
+    return _solid(
+        name=TOOL_NODE_NAMES[tool.name],
+        translation=(tool.pose.x, tool.pose.y, tool.pose.z),
+        rotation=rotation_axis_angle(tool.pose.yaw_deg + 180.0),
+        children=_mesh_shape(asset, TOOL_APPEARANCE[tool.name], scale=scale),
+        bounding=bounding,
+        locked=True,
+    )
+
+
+def _tray_node(spec: ScenarioSpec, asset: Path) -> str:
+    pose = spec.tray_pose
+    assert pose is not None
+    bounding = '''Group {
+  children [
+    Pose {
+      translation 0 0.0012 0
+      children [ Box { size 0.12 0.0024 0.07 } ]
+    }
+    Pose {
+      translation -0.059 0.005 0
+      children [ Box { size 0.002 0.01 0.07 } ]
+    }
+    Pose {
+      translation 0.059 0.005 0
+      children [ Box { size 0.002 0.01 0.07 } ]
+    }
+    Pose {
+      translation 0 0.005 -0.034
+      children [ Box { size 0.12 0.01 0.002 } ]
+    }
+    Pose {
+      translation 0 0.005 0.034
+      children [ Box { size 0.12 0.01 0.002 } ]
+    }
+  ]
+}'''
+    return _solid(
+        name='grant_tray',
+        translation=(pose.x, pose.y, pose.z),
+        rotation=rotation_axis_angle(pose.yaw_deg),
+        children=_mesh_shape(asset, '0.25 0.25 0.28'),
+        bounding=bounding,
+        locked=True,
+    )
+
+
+def _screw_node(index: int, screw, asset: Path) -> str:
+    match = re.fullmatch(r'screw_(\d+)', screw.name)
+    number = int(match.group(1)) if match else index
+    bounding = '''Pose {
+  translation 0 0.0045 0
+  children [ Box { size 0.0055 0.009 0.0055 } ]
+}'''
+    return _solid(
+        name=f'grant_screw_{number:02d}',
+        translation=(screw.pose.x, screw.pose.y, screw.pose.z),
+        rotation=rotation_axis_angle(screw.pose.yaw_deg),
+        children=_mesh_shape(asset, '0.62 0.64 0.67'),
+        bounding=bounding,
+        locked=True,
+    )
+
+
+def _cap_node(spec: ScenarioSpec, asset: Path) -> str:
+    pose = spec.cap_pose
+    assert pose is not None
+    bounding = '''Pose {
+  translation 0 0.004165 0
+  children [ Box { size 0.1384 0.00367 0.0984 } ]
+}'''
+    installed = spec.cap_state == 'installed'
+    return _solid(
+        name='grant_cap',
+        translation=(pose.x, pose.y, pose.z),
+        rotation=rotation_axis_angle(pose.yaw_deg),
+        children=_mesh_shape(asset, '0.78 0.82 0.86'),
+        bounding=bounding,
+        locked=installed,
+        physics_mass=None if installed else 0.1,
+    )
+
+
+def build_nodes(spec: ScenarioSpec, asset_root: Path | str) -> list[str]:
+    """Return the deterministic Webots node strings for one experiment."""
+    root = Path(asset_root)
+    if not root.is_dir():
+        raise FileNotFoundError(f'Webots asset directory does not exist: {root}')
+
+    required = ['fixture', 'body', 'rack']
+    required.extend(tool.name for tool in spec.tools)
+    if spec.tray_pose is not None:
+        required.extend(['tray', 'screw'])
+    if spec.cap_state != 'absent':
+        required.append('cap')
+    assets = {name: _asset_path(root, name) for name in required}
+
+    nodes = [
+        _fixture_node(spec, assets['fixture']),
+        _body_node(spec, assets['body']),
+        _rack_node(spec, assets['rack']),
+    ]
+    nodes.extend(_tool_node(tool, assets[tool.name]) for tool in spec.tools)
+    if spec.tray_pose is not None:
+        nodes.append(_tray_node(spec, assets['tray']))
+        nodes.extend(
+            _screw_node(index, screw, assets['screw'])
+            for index, screw in enumerate(spec.screws, start=1)
+        )
+    if spec.cap_state != 'absent':
+        nodes.append(_cap_node(spec, assets['cap']))
+    return nodes

src/iiwa_experement/test/test_webots_nodes.py

@@ -0,0 +1,273 @@
+"""Deterministic Webots nodes derived from each scenario."""
+import math
+import re
+from pathlib import Path
+
+import pytest
+from ament_index_python.packages import get_package_share_directory
+
+from iiwa_experement.scenario import TOOL_NAMES, load_scenario
+from iiwa_experement.webots_nodes import (
+    ASSETS,
+    TOOL_NODE_NAMES,
+    build_nodes,
+    rotation_axis_angle,
+    rotation_matrix,
+)
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+SCENES = PACKAGE / 'config' / 'scenes.yaml'
+ASSET_ROOT = (
+    Path(get_package_share_directory('iiwa_description')) / 'objects' / 'grant'
+)
+
+NAME_RE = re.compile(r'name "([^"]+)"')
+TRANSLATION_RE = re.compile(r'translation ([^\n]+)')
+ROTATION_RE = re.compile(r'rotation ([^\n]+)')
+URL_RE = re.compile(r'url \[ "([^"]+)" \]')
+
+
+def spec(experiment_id):
+    return load_scenario(experiment_id, SCENES)
+
+
+def nodes(experiment_id):
+    return build_nodes(spec(experiment_id), ASSET_ROOT)
+
+
+def by_name(node_list):
+    result = {}
+    for node in node_list:
+        match = NAME_RE.search(node)
+        assert match, node
+        result[match.group(1)] = node
+    return result
+
+
+def translation(node):
+    return tuple(map(float, TRANSLATION_RE.search(node).group(1).split()))
+
+
+def rotation(node):
+    return tuple(map(float, ROTATION_RE.search(node).group(1).split()))
+
+
+def obj_y_bounds(asset):
+    low, high = math.inf, -math.inf
+    for line in (ASSET_ROOT / asset).read_text(encoding='utf-8', errors='replace').splitlines():
+        if line.startswith('v '):
+            y = float(line.split()[2])
+            low, high = min(low, y), max(high, y)
+    return low, high
+
+
+def test_scenarios_one_six_eleven_sixteen_nineteen_are_complete():
+    expected = {
+        1: ['grant_fixture', 'grant_body', 'grant_rack', 'grant_vacuum', 'grant_cap'],
+        6: ['grant_fixture', 'grant_body', 'grant_rack', 'grant_sealant_tool'],
+        11: [
+            'grant_fixture', 'grant_body', 'grant_rack', 'grant_magnetic_tool',
+            'grant_tray', *[f'grant_screw_{i:02d}' for i in range(1, 11)],
+            'grant_cap',
+        ],
+        16: [
+            'grant_fixture', 'grant_body', 'grant_rack',
+            'grant_vacuum', 'grant_magnetic_tool', 'grant_sealant_tool',
+            'grant_torque_tool', 'grant_tray',
+            *[f'grant_screw_{i:02d}' for i in range(1, 11)], 'grant_cap',
+        ],
+        19: [
+            'grant_fixture', 'grant_body', 'grant_rack',
+            'grant_vacuum', 'grant_magnetic_tool', 'grant_sealant_tool',
+            'grant_torque_tool', 'grant_tray',
+            *[f'grant_screw_{i:02d}' for i in range(1, 11)], 'grant_cap',
+        ],
+    }
+    for experiment_id, names in expected.items():
+        assert [NAME_RE.search(node).group(1) for node in nodes(experiment_id)] == names
+
+
+def test_node_names_are_unique_and_never_duplicate_patron():
+    for experiment_id in range(1, 21):
+        node_list = nodes(experiment_id)
+        names = [NAME_RE.search(node).group(1) for node in node_list]
+        assert len(names) == len(set(names))
+        assert 'patron' not in names
+        assert all('patron' not in node for node in node_list)
+        assert all('GrantToolRack' not in node for node in node_list)
+
+
+def test_all_mesh_urls_are_existing_installed_assets():
+    for experiment_id in range(1, 21):
+        for node in nodes(experiment_id):
+            urls = URL_RE.findall(node)
+            assert len(urls) == 1, node
+            path = Path(urls[0])
+            assert path.is_file(), path
+            assert path.parent == ASSET_ROOT
+            assert 'output' not in path.parts
+
+
+def test_every_required_obj_and_mtl_exists_for_all_twenty_scenarios():
+    for experiment_id in range(1, 21):
+        scenario = spec(experiment_id)
+        required = {'fixture', 'body', 'rack'}
+        required.update(tool.name for tool in scenario.tools)
+        if scenario.tray_pose is not None:
+            required.update({'tray', 'screw'})
+        if scenario.cap_state != 'absent':
+            required.add('cap')
+        for name in required:
+            asset = ASSET_ROOT / ASSETS[name]
+            assert asset.is_file(), (experiment_id, asset)
+            for line in asset.read_text(encoding='utf-8').splitlines():
+                if line.startswith('mtllib'):
+                    assert (ASSET_ROOT / line.split()[1]).is_file()
+
+
+def test_shared_pose_translations_match_the_scenario():
+    for experiment_id in (1, 2, 3, 10, 19):
+        scenario = spec(experiment_id)
+        parsed = by_name(nodes(experiment_id))
+        assert translation(parsed['grant_fixture']) == pytest.approx(
+            (scenario.fixture.x, scenario.fixture.y, scenario.fixture.z)
+        )
+        assert translation(parsed['grant_body']) == pytest.approx(
+            (scenario.body.x, scenario.body.y, scenario.body.z)
+        )
+        assert translation(parsed['grant_rack']) == pytest.approx(
+            (scenario.rack.x, scenario.rack.y, scenario.rack.z)
+        )
+
+
+def test_working_tools_are_in_their_own_sockets():
+    scenario = spec(16)
+    parsed = by_name(nodes(16))
+    for tool in scenario.tools:
+        assert translation(parsed[TOOL_NODE_NAMES[tool.name]]) == pytest.approx(
+            (tool.pose.x, tool.pose.y, tool.pose.z)
+        )
+    positions = {
+        translation(parsed[TOOL_NODE_NAMES[name]]) for name in TOOL_NAMES
+    }
+    assert len(positions) == 4
+
+
+def test_experiment_nineteen_shifts_the_tray_and_all_ten_screws():
+    standard = by_name(nodes(11))
+    shifted = by_name(nodes(19))
+    tray = translation(shifted['grant_tray'])
+    assert tray == pytest.approx((-0.10, 0.36, 0.793))
+    for index in range(1, 11):
+        name = f'grant_screw_{index:02d}'
+        old = translation(standard[name])
+        new = translation(shifted[name])
+        assert new[0] == pytest.approx(old[0] - 0.02)
+        assert new[1] == pytest.approx(old[1] + 0.02)
+    assert len({translation(shifted[f'grant_screw_{i:02d}']) for i in range(1, 11)}) == 10
+
+
+def test_obj_vertical_axis_maps_to_webots_z():
+    axis = (0.0, 1.0, 0.0)
+    for yaw in (0.0, 45.0, 60.0, 135.0, 180.0):
+        matrix = rotation_matrix(yaw)
+        x = sum(matrix[0][i] * axis[i] for i in range(3))
+        y = sum(matrix[1][i] * axis[i] for i in range(3))
+        z = sum(matrix[2][i] * axis[i] for i in range(3))
+        assert (x, y, z) == pytest.approx((0.0, 0.0, 1.0), abs=1e-12)
+        trace = sum(matrix[i][i] for i in range(3))
+        assert math.isclose(trace, math.cos(math.radians(yaw)), abs_tol=1e-12)
+
+
+def test_cap_rotations_match_45_135_and_60_degrees():
+    assert rotation(by_name(nodes(1))['grant_cap']) == pytest.approx(
+        rotation_axis_angle(45), abs=1e-9
+    )
+    assert rotation(by_name(nodes(2))['grant_cap']) == pytest.approx(
+        rotation_axis_angle(135), abs=1e-9
+    )
+    assert rotation(by_name(nodes(19))['grant_cap']) == pytest.approx(
+        rotation_axis_angle(60), abs=1e-9
+    )
+    assert rotation(by_name(nodes(1))['grant_cap'])[3] == pytest.approx(
+        1.717771517458, abs=1e-9
+    )
+
+
+def test_only_the_sealant_mesh_is_scaled_by_ten():
+    for experiment_id in range(1, 21):
+        scenario = spec(experiment_id)
+        parsed = by_name(nodes(experiment_id))
+        count = sum(node.count('scale 10 10 10') for node in parsed.values())
+        assert count == (1 if 'sealant_tool' in scenario.tool_names else 0)
+        if 'sealant_tool' in scenario.tool_names:
+            assert 'scale 10 10 10' in parsed['grant_sealant_tool']
+
+
+def test_loose_cap_touches_the_table_and_installed_cap_touches_the_body():
+    cap_min, _ = obj_y_bounds(ASSETS['cap'])
+    _, body_max = obj_y_bounds(ASSETS['body'])
+    for experiment_id in range(1, 21):
+        scenario = spec(experiment_id)
+        if scenario.cap_state == 'absent':
+            continue
+        cap = by_name(nodes(experiment_id))['grant_cap']
+        cap_z = translation(cap)[2]
+        if scenario.cap_state == 'free':
+            assert cap_z + cap_min == pytest.approx(0.793, abs=1e-9)
+        else:
+            body_z = translation(by_name(nodes(experiment_id))['grant_body'])[2]
+            assert cap_z + cap_min == pytest.approx(body_z + body_max, abs=1e-9)
+
+
+def test_free_cap_has_physics_and_installed_cap_is_locked():
+    free_cap = by_name(nodes(1))['grant_cap']
+    assert 'physics Physics {' in free_cap
+    assert 'locked TRUE' not in free_cap
+    installed_cap = by_name(nodes(11))['grant_cap']
+    assert 'locked TRUE' in installed_cap
+    assert 'physics Physics {' not in installed_cap
+
+
+def test_ten_screws_are_distinct_and_inside_the_tray():
+    for experiment_id in (11, 19):
+        scenario = spec(experiment_id)
+        parsed = by_name(nodes(experiment_id))
+        positions = [
+            translation(parsed[f'grant_screw_{index:02d}']) for index in range(1, 11)
+        ]
+        assert len(set(positions)) == 10
+        tray = scenario.tray_pose
+        assert tray is not None
+        for x, y, z in positions:
+            assert abs(x - tray.x) < 0.06
+            assert abs(y - tray.y) < 0.035
+            assert z == pytest.approx(0.7954)
+
+
+def mirror_assets(tmp_path, missing):
+    for source in ASSET_ROOT.iterdir():
+        if source.name == missing:
+            continue
+        (tmp_path / source.name).symlink_to(source)
+
+
+def test_missing_obj_is_a_hard_error(tmp_path):
+    mirror_assets(tmp_path, ASSETS['screw'])
+    with pytest.raises(FileNotFoundError, match='vint.obj'):
+        build_nodes(spec(11), tmp_path)
+
+
+def test_missing_mtl_is_a_hard_error(tmp_path):
+    for source in ASSET_ROOT.iterdir():
+        target = tmp_path / source.name
+        if source.name in (ASSETS['screw'], 'vint.mtl'):
+            continue
+        target.symlink_to(source)
+    (tmp_path / ASSETS['screw']).write_text(
+        (ASSET_ROOT / ASSETS['screw']).read_text(encoding='utf-8'),
+        encoding='utf-8',
+    )
+    with pytest.raises(FileNotFoundError, match='vint.mtl'):
+        build_nodes(spec(11), tmp_path)