diff --git a/src/iiwa_experement/doc/README.md b/src/iiwa_experement/doc/README.md index 0adf081..b1fa064 100644 --- a/src/iiwa_experement/doc/README.md +++ b/src/iiwa_experement/doc/README.md @@ -11,7 +11,10 @@ передаёт винты виртуальному оператору и выполняет имитацию затяжки затяжным инструментом. Винты не вставляются: после передачи отверстие остаётся пустым, а `torque_tool` подходит к отверстию, поворачивается вокруг своей оси и -отходит. Режим `view` (по умолчанию) оставляет сцену неподвижной. +отходит. Режим `planner` готовит ту же записываемую сцену (supervisor +принимает захваты), но **не** запускает `reference_runner`: готовый граф +передаётся снаружи в `iiwa_llm_plan_executor`. Режим `view` (по умолчанию) +оставляет сцену неподвижной. ## Сборка diff --git a/src/iiwa_llm_plan_executor/config/planner.yaml b/src/iiwa_llm_plan_executor/config/planner.yaml new file mode 100644 index 0000000..d16de00 --- /dev/null +++ b/src/iiwa_llm_plan_executor/config/planner.yaml @@ -0,0 +1,15 @@ +# Defaults for `ros2 launch iiwa_llm_plan_executor planner.launch.py`. +# Qwen3 is called outside this project; this file only configures Webots and +# the two new nodes. +start_delay_sec: 3.0 +scene_max_age_sec: 2.0 +max_steps: 512 +scene_freshness_sec: 2.0 +close_inspection_objects: + - cap +top_camera_topic: /d455_top/image_raw +hand_camera_topic: /camera/color +frame_timeout_sec: 2.0 +trace_dir: ~/.ros/iiwa_llm_plan_executor/runs +planning_group: iiwa_arm +pose_link: tcp diff --git a/src/iiwa_llm_plan_executor/doc/README.md b/src/iiwa_llm_plan_executor/doc/README.md new file mode 100644 index 0000000..3e96874 --- /dev/null +++ b/src/iiwa_llm_plan_executor/doc/README.md @@ -0,0 +1,102 @@ +# Executing external Qwen3 graphs in Webots + +This package accepts a **ready-made** JSON graph. It does not call Qwen3, does +not evaluate the model and does not compare runs with a reference sequence. +The experiment number selects only the initial Webots scene. + +## Start + +```bash +source /opt/ros/jazzy/setup.bash +source install/setup.bash +ros2 launch iiwa_llm_plan_executor planner.launch.py experiment_id:=20 +``` + +The launch starts the standard `iiwa_bringup` simulation, the writable Grant +supervisor (`mode=planner`), the scene context and the graph executor. It +never starts `reference_runner` and never starts a second `iiwa_web` server: +the web server already comes from `iiwa_bringup` when `web.enabled` is true in +`cobot-setting.yaml`. For a ROS-only run set `web.enabled: false` in your +settings before launch. + +Useful arguments: `rviz:=true`, `config:=/path/planner.yaml`. +`simulate` must remain `true`; the physical FRI robot is not connected. + +## Read the scene and submit a graph + +Wait until the launch log shows the scene is ready (the snapshot is only +served after `/iiwa_experement/scene_ready`). + +ROS: + +```bash +ros2 service call /iiwa_scene_context/get_scene std_srvs/srv/Trigger {} +ros2 topic echo /iiwa_scene_context/scene_json --qos-durability transient_local +``` + +Give `scene_json` and your instruction to Qwen3 outside this project. Submit +the answer, the `scene_revision` from the same snapshot and a client-chosen +`run_id`: + +```bash +ros2 service call /iiwa_llm_plan_executor/submit webots_ros2_msgs/srv/SetString \ + "{value: '{\"run_id\":\"demo-1\",\"scene_revision\":1,\"response\":{\"status\":\"ready\",\"entry\":\"detect\",\"plan\":[...]}}'}" + +ros2 topic echo /iiwa_llm_plan_executor/plan --qos-durability transient_local +ros2 topic echo /iiwa_llm_plan_executor/status +``` + +The accepted graph is published **unchanged** in +`/iiwa_llm_plan_executor/plan` before the first movement. A rejected graph +never appears there; read the reason from +`/iiwa_llm_plan_executor/status` or: + +```bash +ros2 service call /iiwa_llm_plan_executor/get_status iiwa_msgs/srv/GetRunStatus \ + "{run_id: demo-1}" +ros2 service call /iiwa_llm_plan_executor/cancel webots_ros2_msgs/srv/SetString \ + "{value: '{\"run_id\":\"demo-1\"}'}" +``` + +HTTP (existing `iiwa_web` server; default port 8007): + +```bash +curl http://localhost:8007/planner/scene +curl -X POST http://localhost:8007/planner/runs \ + -H 'Content-Type: application/json' \ + -d '{"run_id":"demo-1","scene_revision":1,"response":""}' +curl http://localhost:8007/planner/runs/demo-1 +curl -X POST http://localhost:8007/planner/runs/demo-1/cancel +``` + +The same four operations are MCP tools built from the same router +(`get_scene_planner_scene_get`, `submit_run_planner_runs_post`, +`get_run_planner_runs`, `cancel_run_planner_runs`). + +## Nodes and terminals + +Only one graph is active per robot. Repeating the same `run_id` is +idempotent; a different `run_id` while active is rejected (`busy`). + +Status events contain the run id, node id, skill, result event, the chosen +next node, the scene revision, the mounted tool, the held object and, at the +end, the terminal. Terminals: + +* `completed` — the submitted graph and its checked postconditions finished. +* `replan_required` — the graph stopped and a fresh snapshot was emitted; the + external client decides whether to call Qwen3 again. +* `operator_required` — a human decision is needed; the robot is stopped. +* `failed` / `canceled` — safety stop (stale state, limits, collision, cancel, + step limit); the grasp state is preserved in the status. + +The JSONL log with every event is written to +`~/.ros/iiwa_llm_plan_executor/runs/.jsonl`. + +## Safety scope + +The executor checks J7 (±90° around `work`) and the downward TCP tilt (≤10°) +on **every** planned point, seeds IK from current joints, enforces these as +MoveIt path constraints, checks scene/joint freshness and verifies the final +pose. It never falls back to an unconstrained `MoveToPose` goal. This is a +Webots verification, not a physical-robot validation and not a Qwen3 quality +evaluation. diff --git a/src/iiwa_llm_plan_executor/launch/planner.launch.py b/src/iiwa_llm_plan_executor/launch/planner.launch.py new file mode 100644 index 0000000..1d45c28 --- /dev/null +++ b/src/iiwa_llm_plan_executor/launch/planner.launch.py @@ -0,0 +1,209 @@ +"""Launch one Webots scene with the scene context and the graph executor. + +The experiment only selects the initial scene. The reference runner is never +started in this mode; the graph arrives from outside through +``/iiwa_llm_plan_executor/submit``. +""" +from __future__ import annotations + +from pathlib import Path + +import yaml +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from iiwa_experement.runtime_config import ( + validate_experiment_id, + validate_mode, +) + + +EXPERIMENT_LAUNCH = ('launch', 'experiment.launch.py') +PLANNER_MODE = 'planner' +DEFAULT_CONFIG = { + 'start_delay_sec': 3.0, + 'scene_max_age_sec': 2.0, + 'max_steps': 512, + 'scene_freshness_sec': 2.0, + 'close_inspection_objects': ['cap'], + 'top_camera_topic': '/d455_top/image_raw', + 'hand_camera_topic': '/camera/color', + 'frame_timeout_sec': 2.0, + 'trace_dir': '~/.ros/iiwa_llm_plan_executor/runs', + 'planning_group': 'iiwa_arm', + 'pose_link': 'tcp', +} + + +def load_config(path: Path | str | None) -> dict: + data = dict(DEFAULT_CONFIG) + if path is not None: + source = Path(path) + if source.is_file(): + loaded = yaml.safe_load(source.read_text(encoding='utf-8')) or {} + if isinstance(loaded, dict): + data.update(loaded) + return data + + +def validate_arguments( + experiment_id_raw: str, + mode_raw: str, + simulate_raw: str, +) -> tuple[int, str]: + """Validate everything before Webots is started.""" + experiment_id = validate_experiment_id(experiment_id_raw) + mode = validate_mode(mode_raw) + if mode != PLANNER_MODE: + raise ValueError( + f'mode must be {PLANNER_MODE!r} for the graph executor, got {mode!r}' + ) + simulate = str(simulate_raw).strip().lower() + if simulate not in ('true', '1', 'yes'): + raise ValueError( + 'this executor stage only supports Webots (simulate:=true); ' + 'the physical FRI robot is not connected' + ) + return experiment_id, mode + + +def runtime_actions( + *, + experiment_id: int, + scene_config: str, + base_setting: str, + rviz: str, + config: dict, +) -> list: + experiment = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([ + FindPackageShare('iiwa_experement'), *EXPERIMENT_LAUNCH, + ])), + launch_arguments={ + 'experiment_id': str(experiment_id), + 'mode': PLANNER_MODE, + 'scene_config': str(scene_config), + 'base_setting': str(base_setting), + 'rviz': str(rviz), + }.items(), + ) + scene_context = Node( + package='iiwa_scene_context', + executable='scene_context', + name='iiwa_scene_context', + output='screen', + parameters=[{ + 'experiment_id': int(experiment_id), + 'scene_config': str(scene_config), + 'freshness_sec': float(config.get('scene_freshness_sec', 2.0)), + 'use_sim_time': True, + }], + ) + executor = Node( + package='iiwa_llm_plan_executor', + executable='llm_plan_executor', + name='iiwa_llm_plan_executor', + output='screen', + parameters=[{ + 'experiment_id': int(experiment_id), + 'scene_config': str(scene_config), + 'start_delay_sec': float(config.get('start_delay_sec', 3.0)), + 'scene_max_age_sec': float(config.get('scene_max_age_sec', 2.0)), + 'max_steps': int(config.get('max_steps', 512)), + 'close_inspection_objects': list( + config.get('close_inspection_objects', ['cap']) + ), + 'top_camera_topic': str( + config.get('top_camera_topic', '/d455_top/image_raw') + ), + 'hand_camera_topic': str( + config.get('hand_camera_topic', '/camera/color') + ), + 'frame_timeout_sec': float(config.get('frame_timeout_sec', 2.0)), + 'trace_dir': str( + config.get('trace_dir', '~/.ros/iiwa_llm_plan_executor/runs') + ), + 'planning_group': str(config.get('planning_group', 'iiwa_arm')), + 'pose_link': str(config.get('pose_link', 'tcp')), + 'use_sim_time': True, + }], + ) + return [experiment, scene_context, executor] + + +def _setup(context, *args, **kwargs): + del args, kwargs + raw_id = LaunchConfiguration('experiment_id').perform(context) + raw_mode = LaunchConfiguration('mode').perform(context) + raw_simulate = LaunchConfiguration('simulate').perform(context) + try: + experiment_id, _ = validate_arguments(raw_id, raw_mode, raw_simulate) + except ValueError as error: + raise RuntimeError(f'Invalid planner launch arguments: {error}') from error + config_path = LaunchConfiguration('config').perform(context) + config = load_config(config_path) + print( + f'[iiwa_llm_plan_executor] experiment {experiment_id} planner mode: ' + f'no reference sequence; waiting for an external graph' + ) + return runtime_actions( + experiment_id=experiment_id, + scene_config=str(LaunchConfiguration('scene_config').perform(context)), + base_setting=str(LaunchConfiguration('base_setting').perform(context)), + rviz=str(LaunchConfiguration('rviz').perform(context)).lower(), + config=config, + ) + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'experiment_id', + default_value='', + description='Experiment 1..20; selects only the initial scene.', + ), + DeclareLaunchArgument( + 'mode', + default_value='planner', + description='Must remain planner for externally supplied graphs.', + ), + DeclareLaunchArgument( + 'simulate', + default_value='true', + description='Must remain true; the physical robot is not connected.', + ), + DeclareLaunchArgument( + 'rviz', + default_value='false', + description='true = start RViz together with Webots.', + ), + DeclareLaunchArgument( + 'scene_config', + default_value=PathJoinSubstitution([ + FindPackageShare('iiwa_experement'), 'config', 'scenes.yaml', + ]), + description='Scenario manifest used to build the initial scene.', + ), + DeclareLaunchArgument( + 'base_setting', + default_value=PathJoinSubstitution([ + FindPackageShare('iiwa_config'), 'config', 'setting.yaml', + ]), + description='Base settings for the temporary setting.yaml.', + ), + DeclareLaunchArgument( + 'config', + default_value=PathJoinSubstitution([ + FindPackageShare('iiwa_llm_plan_executor'), + 'config', + 'planner.yaml', + ]), + description='Executor defaults: delays, limits, cameras, logging.', + ), + OpaqueFunction(function=_setup), + ]) diff --git a/src/iiwa_llm_plan_executor/test/test_launch.py b/src/iiwa_llm_plan_executor/test/test_launch.py new file mode 100644 index 0000000..8f074e0 --- /dev/null +++ b/src/iiwa_llm_plan_executor/test/test_launch.py @@ -0,0 +1,139 @@ +"""Launch validation and wiring for the planner scene.""" +from __future__ import annotations + +import importlib.util +from pathlib import Path + +import pytest +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch_ros.actions import Node + +from iiwa_experement.runtime_config import EXPERIMENT_IDS, validate_experiment_id + + +PACKAGE = Path(__file__).resolve().parents[1] +LAUNCH = PACKAGE / 'launch' / 'planner.launch.py' +CONFIG = PACKAGE / 'config' / 'planner.yaml' + + +def load_launch_module(): + spec = importlib.util.spec_from_file_location('planner_launch', LAUNCH) + module = importlib.util.module_from_spec(spec) + assert spec.loader is not None + spec.loader.exec_module(module) + return module + + +def flat_parameters(node: Node): + def flatten(value): + if hasattr(value, 'text'): + return [value.text] + if isinstance(value, (list, tuple)): + result = [] + for item in value: + result.extend(flatten(item)) + return result + return [value] + + values = [] + for parameter_set in node._Node__parameters: + for value in parameter_set.values(): + values.extend(flatten(value)) + return values + + +def test_valid_experiment_ids_and_planner_mode(): + module = load_launch_module() + for experiment_id in EXPERIMENT_IDS: + parsed, mode = module.validate_arguments( + str(experiment_id), 'planner', 'true', + ) + assert parsed == experiment_id + assert mode == 'planner' + assert validate_experiment_id(' 20 ') == 20 + + +@pytest.mark.parametrize('raw', ['', '0', '21', 'abc', '1.0']) +def test_invalid_experiment_ids_fail_before_webots(raw): + module = load_launch_module() + with pytest.raises(ValueError): + module.validate_arguments(raw, 'planner', 'true') + + +def test_unknown_mode_or_physical_robot_is_rejected_before_webots(): + module = load_launch_module() + with pytest.raises(ValueError, match='mode'): + module.validate_arguments('20', 'execute', 'true') + with pytest.raises(ValueError, match='simulate'): + module.validate_arguments('20', 'planner', 'false') + + +def test_runtime_actions_start_scene_context_and_executor_only(): + module = load_launch_module() + config = module.load_config(CONFIG) + actions = module.runtime_actions( + experiment_id=20, + scene_config='/tmp/scenes.yaml', + base_setting='/tmp/setting.yaml', + rviz='false', + config=config, + ) + includes = [action for action in actions if isinstance(action, IncludeLaunchDescription)] + nodes = [action for action in actions if isinstance(action, Node)] + assert len(includes) == 1 + assert len(nodes) == 2 + arguments = dict(includes[0].launch_arguments) + assert arguments['experiment_id'] == '20' + assert arguments['mode'] == 'planner' + assert arguments['scene_config'] == '/tmp/scenes.yaml' + assert arguments['base_setting'] == '/tmp/setting.yaml' + + packages = {node.node_package for node in nodes} + executables = {node.node_executable for node in nodes} + assert packages == {'iiwa_scene_context', 'iiwa_llm_plan_executor'} + assert executables == {'scene_context', 'llm_plan_executor'} + assert 'reference_runner' not in executables + assert 'iiwa_web' not in packages + + scene_node = next(node for node in nodes if node.node_executable == 'scene_context') + executor_node = next( + node for node in nodes if node.node_executable == 'llm_plan_executor' + ) + scene_values = flat_parameters(scene_node) + executor_values = flat_parameters(executor_node) + assert 20 in scene_values + assert 20 in executor_values + assert 3.0 in executor_values + assert 512 in executor_values + assert True in executor_values + assert any('iiwa_arm' in str(value) for value in executor_values) + + +def test_config_contains_the_documented_defaults(): + module = load_launch_module() + config = module.load_config(CONFIG) + assert config['start_delay_sec'] == 3.0 + assert config['scene_max_age_sec'] == 2.0 + assert config['max_steps'] == 512 + assert config['close_inspection_objects'] == ['cap'] + assert config['trace_dir'].endswith('iiwa_llm_plan_executor/runs') + + +def test_launch_description_declares_all_arguments_and_validates_in_opaque(): + module = load_launch_module() + description = module.generate_launch_description() + arguments = { + entity.name: entity.default_value + for entity in description.entities + if isinstance(entity, DeclareLaunchArgument) + } + assert set(arguments) == { + 'experiment_id', 'mode', 'simulate', 'rviz', 'scene_config', + 'base_setting', 'config', + } + assert [item.text for item in arguments['experiment_id']] == [''] + assert [item.text for item in arguments['mode']] == ['planner'] + assert [item.text for item in arguments['simulate']] == ['true'] + assert len([ + entity for entity in description.entities if isinstance(entity, OpaqueFunction) + ]) == 1