From e29ae921c4f6608f7492302b8c37a0687cdef24c 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:47:53 +1000 Subject: [PATCH] feat: generate grant Webots objects --- .../iiwa_experement/webots_nodes.py | 350 ++++++++++++++++++ src/iiwa_experement/test/test_webots_nodes.py | 273 ++++++++++++++ 2 files changed, 623 insertions(+) create mode 100644 src/iiwa_experement/iiwa_experement/webots_nodes.py create mode 100644 src/iiwa_experement/test/test_webots_nodes.py diff --git a/src/iiwa_experement/iiwa_experement/webots_nodes.py b/src/iiwa_experement/iiwa_experement/webots_nodes.py new file mode 100644 index 0000000..23afda1 --- /dev/null +++ b/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 diff --git a/src/iiwa_experement/test/test_webots_nodes.py b/src/iiwa_experement/test/test_webots_nodes.py new file mode 100644 index 0000000..88ae080 --- /dev/null +++ b/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)