From ba807e710ba5ce66ffc0174cdb9cae1a1b39587d 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: Fri, 13 Nov 2026 15:47:14 +1000 Subject: [PATCH] change validator --- ...2026-11-11-cobot-llm-motion-integration.md | 69 + .../iiwa_experement/planning_scene.py | 70 +- src/iiwa_llm_plan_executor/doc/README.md | 43 +- src/iiwa_llm_plan_executor/doc/webots-qa.md | 93 +- src/iiwa_llm_plan_executor/docs/interfaces.md | 286 +- .../iiwa_llm_plan_executor/engine.py | 197 +- .../iiwa_llm_plan_executor/moveit_backend.py | 87 +- .../iiwa_llm_plan_executor/node.py | 206 +- .../iiwa_llm_plan_executor/validator.py | 12 +- .../test/test_engine.py | 157 +- .../test/test_full_qwen_graph.py | 15 +- src/iiwa_llm_plan_executor/test/test_node.py | 68 +- .../test/test_validator.py | 24 +- src/iiwa_msgs/CMakeLists.txt | 1 + src/iiwa_msgs/srv/SubmitPlan.srv | 12 + src/iiwa_scene_context/docs/interfaces.md | 138 +- .../docs/scene_json_experiment_20.json | 2391 +++++++++++++++++ .../iiwa_scene_context/catalog.py | 291 +- .../iiwa_scene_context/node.py | 21 +- .../iiwa_scene_context/snapshot.py | 309 ++- src/iiwa_scene_context/package.xml | 2 + src/iiwa_scene_context/setup.py | 1 + src/iiwa_scene_context/test/test_node.py | 27 +- src/iiwa_scene_context/test/test_snapshot.py | 194 +- src/iiwa_web/iiwa_web/planner.py | 58 +- src/iiwa_web/test/test_planner.py | 77 +- 26 files changed, 4211 insertions(+), 638 deletions(-) create mode 100644 docs/superpowers/plans/2026-11-11-cobot-llm-motion-integration.md create mode 100644 src/iiwa_msgs/srv/SubmitPlan.srv create mode 100644 src/iiwa_scene_context/docs/scene_json_experiment_20.json diff --git a/docs/superpowers/plans/2026-11-11-cobot-llm-motion-integration.md b/docs/superpowers/plans/2026-11-11-cobot-llm-motion-integration.md new file mode 100644 index 0000000..76ee3b6 --- /dev/null +++ b/docs/superpowers/plans/2026-11-11-cobot-llm-motion-integration.md @@ -0,0 +1,69 @@ +# Интеграция графов LLM в штатный запуск `cobot` + +> Для код-агента. Реализуй задачи по порядку в ветке `experement` по текущей рабочей копии. Здесь описаны изменения и критерии приёмки. + +После `cobot run` робот должен принимать готовый JSON-граф LLM и перемещать установленный оператором инструмент в нужную точку. Обычный Webots использует `digital_twin.webots.world`, а запуск контроллера использует FRI. Опыты Grant по-прежнему запускаются в Webots через существующие launch-файлы с явным номером опыта. Внешний клиент вызывает LLM и отправляет готовый граф. + +| Запуск | Исполнитель | Источник контекста для LLM | +| --- | --- | --- | +| `cobot run local` или `cobot run docker` → «Симулятор Webots» | Базовые перемещения через штатный MoveIt | `iiwa_scene_context` публикует снимок базовой сцены | +| `cobot run local` или `cobot run docker` → «Контроллер» | Те же перемещения через штатный MoveIt и FRI при включении в YAML | Именованные позиции и телеметрия робота через штатные интерфейсы | +| `ros2 launch iiwa_llm_plan_executor planner.launch.py experiment_id:=20` | Существующие навыки Grant | Существующий `scene_json` от Grant Supervisor | + +Для первой версии предлагаю поддержать именованную позицию и явную TCP-позу, включать исполнение графов на физическом роботе отдельной настройкой и покрыть обе среды `cobot run`. Навыки захвата, переноса объектов, стыковки инструмента и технологических операций входят только в профиль Grant. На физическом роботе `scene_json` пока отсутствует. Имя инструмента, его TCP, коллизионная модель и нагрузка берутся из действующей конфигурации робота и контроллера. + +## 1. Подключить узлы к штатному запуску + +- [ ] Добавить в `cobot-setting.yaml` и `src/iiwa_config/config/setting.yaml` одинаковый блок `llm_planner`. В блок входят `enabled: true`, `hardware_enabled: false` и `velocity_scale: 0.10`. Значение масштаба `0.10` служит начальным значением для Webots. Для физического прогона оператор задаёт согласованное значение. В `src/iiwa_utils/iiwa_utils/setting_loader.py` добавить типизированное чтение блока, строгие булевы значения и конечный масштаб в диапазоне `(0, 1]`. Отсутствие блока в старом YAML означает выключенную интеграцию. При выключенном `enabled` штатный запуск работает независимо от `hardware_enabled`. +- [ ] Создать `src/iiwa_llm_plan_executor/launch/cobot.launch.py` с аргументами `setting`, `simulate`, `rviz`. Включить `iiwa_bringup/iiwa.launch.py` один раз с теми же аргументами. При `enabled: true` и `simulate: true` добавить `scene_context` и `llm_plan_executor` в базовом режиме с `use_sim_time: true`. При `simulate: false` и двух включённых флагах добавить только исполнитель с `use_sim_time: false`. В обоих режимах использовать настройки и пути штатного bringup. Проверять конфигурацию до создания процессов. +- [ ] Использовать общий конструктор команды запуска в `cobot/commands/run.py` для `_run_local` и `_run_docker`. Сохранить меню, подключение ROS overlay, передачу `simulate`, монтирование `cobot-setting.yaml` и остановку дочернего дерева. Для устаревшей сборки вывести команду `cobot rebuild --symlink-install` или `cobot docker-setup` и вернуть ошибку запуска. +- [ ] Объявить зависимости нового launch в `src/iiwa_llm_plan_executor/package.xml` и проверить установку через `setup.py`. Оставить направление зависимости от нового launch к `iiwa_bringup`. Обратная зависимость образует цикл `iiwa_bringup → iiwa_llm_plan_executor → iiwa_experement → iiwa_bringup`. Существующие `planner.launch.py` и `experiment.launch.py` продолжают включать низкоуровневый bringup и свои узлы Grant. +- [ ] Тесты в `cobot/test/` и `src/iiwa_llm_plan_executor/test/` покрывают матрицу Webots/FRI × флаги, старый и неверный YAML, аргументы local/Docker и единственный экземпляр контроллера, MoveIt и `iiwa_web`. При запуске обычного Webots отсутствуют процессы `scene_spawner`, `grant_supervisor` и `reference_runner`. Для существующего launch Grant сохранить прежние тесты, включая отказ при `simulate:=false`. + +## 2. Принимать базовые графы без сцены Grant + +- [ ] В `src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/{node,engine,validator,skills}.py` выделить режим `base` с собственными проверками и двумя навыками. Сохранить общий разбор JSON, публикацию оригинала, `run_id`, повторную отправку, один активный запуск, статус, журнал и отмену. Сценозависимые проверки ревизии, владения и готовности оставить в режиме Grant. В `base` определять готовность по актуальному полному `/joint_states`, готовности штатного сервера движения и активному контроллеру. Проверять состояние перед запуском графа и перед каждым шагом. Выход FRI из рабочего состояния, потеря обратной связи или недоступный контроллер прерывают граф и запрещают следующий шаг. Учитывать уже реализованную обработку `COMMANDING_ACTIVE` в `iiwa_controller`. +- [ ] Сохранить формат `status`, `entry`, `plan`, `transitions` и интерфейс `/iiwa_llm_plan_executor/submit_plan` (`iiwa_msgs/srv/SubmitPlan`). Для `move_to_named` поле `target` задаёт имя `group_state` из активного SRDF для `planning.planning_group`. Для `move_to_pose` требовать ровно шесть чисел `x`, `y`, `z` в метрах и `a`, `b`, `c` в радианах в `params`. Углы задаются в порядке ZYX по соглашению KUKA ABC, фрейм фиксирован `base_link`, TCP-линк берётся из `planning.pose_link`. Для обоих навыков `object` пустой. У `move_to_pose` поле `target` также пустое. Имена, числа, допустимые поля и переходы проверить для всех узлов при приёме. Отклонять неизвестные команды, ссылки на объекты Grant, `NaN`/`Inf`, внешние фреймы и попытки переопределить масштаб скорости. Публиковать результат шага `success` или `motion_failed` по результату сервера. Для `motion_failed` разрешать только переход в `operator_required` или `replan_required`. При отсутствии такого перехода завершать граф как `failed` с причиной. +- [ ] Передавать TCP-цель в штатный action `cobot/move_to_pose` (`iiwa_msgs/action/MoveToPose`). Для именованного движения добавить отменяемый `cobot/move_to_named_pose` (`iiwa_msgs/action/MoveToNamed`) в `src/iiwa_planning/scripts/move_to_pose_server.py` и `src/iiwa_msgs`. Вынести существующее планирование именованной цели из `cobot/move_to_named` в общий участок, чтобы оба интерфейса использовали SRDF, Pilz PTP, лимиты и `MotionSerialiser`. Штатный сервис сохраняет контракт. Для TCP-цели применять настроенный планировщик и масштаб, согласованные с MoveIt. Успех шага требует успешного результата action, включая выполнение траектории контроллером. +- [ ] Подключить отмену графа к goal handle MoveIt и штатной остановке. Проверить окно между отправкой цели и получением goal handle, потерю контроллера и повторный submit после отмены. `POST /stop` должен остановить граф, отправленный через ROS, HTTP или MCP. Текущий `_active_run_id` в `src/iiwa_web/iiwa_web/planner.py` учитывает только HTTP-запросы. Источник активного запуска и его отмены должен находиться в executor. Штатный `/cobot/stop` должен инвалидировать активную операцию и ожидающие шаги графа. В базовом режиме графы и обычные запросы MoveIt проходят через одну сериализацию. Проверить, что прямой HTTP-ввод траекторий не перебивает активный граф. +- [ ] Добавить тесты базового графа с подменой ROS action-сервера. Покрыть успешные именованную и TCP-цели, отсутствие `scene_json` на FRI, отказ при неизвестном имени и некорректных координатах, `not_ready` при устаревших суставах, ошибку контроллера, отмену до принятия цели и в процессе исполнения, дубликат и параллельный запуск. Прогнать существующие тесты Grant и `test_motion_priority.py`. + +## 3. Выдавать честный `scene_json` в базовом Webots + +- [ ] В `src/iiwa_scene_context/iiwa_scene_context/{node,snapshot}.py` добавить режим `base`. Источник актуальности базового снимка — `/joint_states` и TF из `base_link` к настроенному TCP, если трансформ доступен. Публиковать тип базового профиля, метку времени, наблюдаемое состояние, доступные базовые навыки и имена поз из активного SRDF. В пустом мире набор наблюдаемых объектов задачи пуст. У базового снимка нет номера опыта. Отсутствующие данные о камерах, объектах и установленном инструменте обозначать явно. Сцена без объектов задачи не подтверждает отсутствие препятствий для MoveIt. +- [ ] Сохранить `/iiwa_scene_context/get_scene`, QoS `RELIABLE`/`TRANSIENT_LOCAL` для `/iiwa_scene_context/scene_json` и действующий формат Grant. Базовый контекст не подписывается на `/iiwa_experement/object_state` и не ждёт `/iiwa_experement/scene_ready`. Возвращать понятный отказ до первого валидного состояния и при его устаревании. Для FRI сервис сцены не запускается, а HTTP/MCP сообщает, что `scene_json` доступен только в Webots. Маршруты отправки, статуса и отмены работают при включённом `web.enabled` в обоих режимах. +- [ ] Тесты проверяют свежесть и содержимое пустой сцены без Grant, достоверность источников полей и неизменный контракт сцены опыта. Проверить, что пропажа базового `scene_json` сама по себе не блокирует допустимую команду при наличии актуальной телеметрии и готового MoveIt. + +## 4. Собрать и проверить запуск из установленных пакетов + +- [ ] В обоих `docker/jazzy/{ros-iiwa7,ros-iiwa7-webots}/Dockerfile` добавить исходники `iiwa_experement`, `iiwa_scene_context` и `iiwa_llm_plan_executor` в контекст перед `rosdep` и `colcon`. Проверить зависимости из `package.xml` и доступность launch, конфигов и ROS-интерфейсов в release-образах после удаления `src`. Docker для контроллера не требует приложения Webots. Через `cobot docker-setup` собрать оба образа. Проверить `cobot run docker` на новом Webots-образе и стенде FRI с имитацией аппаратуры. +- [ ] С ROS 2 Jazzy запустить `cobot rebuild --symlink-install`, подключить `install/setup.bash` и выполнить `colcon test --base-paths src` и `colcon test-result --verbose`. Для общего запуска `pytest` по нескольким пакетам использовать `--import-mode=importlib` из-за совпадающих имён `test_launch.py`. Сверить результаты с исходным состоянием рабочей копии, включая незакоммиченный `src/iiwa_msgs/srv/SubmitPlan.srv`. +- [ ] В обычном Webots запустить `cobot run local` → «Симулятор Webots». Проверить один набор штатных узлов и базовый снимок через `ros2 service call /iiwa_scene_context/get_scene std_srvs/srv/Trigger '{}'`. Сначала отправить отклоняемый граф и убедиться, что контроллер не получил цель. Затем отправить разрешённую именованную цель и допустимую TCP-цель, выбранную для установленной модели и сцены. Для обеих сверить `/plan`, `/status`, фактическую позу и `completed`. Отдельно проверить отмену, общий stop и чистое завершение по Ctrl-C. Повторить на Docker. Проверить, что ручной запуск Grant с `experiment_id:=20` отдаёт прежнюю сцену и принимает прежний граф. +- [ ] Для FRI сначала проверить ветку запуска с имитацией action-сервера и контроллера либо с тестовым FRI-стендом из `src/iiwa_controller/test/`. Убедиться, что нет Webots и сервиса сцены, выключенная настройка исключает executor, а при включении граф до готовности возвращает `not_ready`. Физическую приёмку выполнять с оператором после проверки сети, активного режима FRI, лимитов, модели инструмента, Load Data и рабочего пространства. Начать с одной заранее проверенной именованной позиции на согласованной скорости. В отчёте указать отдельно результаты Webots, FRI-имитации и физической проверки. +- [ ] Описать операторский запуск и примеры графов в `src/iiwa_llm_plan_executor/doc/README.md`, контракт базовой сцены в `src/iiwa_scene_context/docs/interfaces.md` и настройки в `doc/lwc-doc/docs/getting-started/configuration{,.en}.md`. + +Пример тела `POST /planner/runs` для Webots. Перед запуском проверьте, что `work` есть в активном SRDF и применим к текущей сцене. + +```json +{ + "run_id": "base-webots-1", + "response": { + "status": "ready", + "entry": "go", + "plan": [{ + "id": "go", + "skill": "move_to_named", + "target": "work", + "transitions": [ + {"event": "success", "next": "completed"}, + {"event": "motion_failed", "next": "operator_required"} + ] + }] + } +} +``` + +## Ссылки для реализации + +- Проектные контракты по запуску, API и остановке. [Deployment](../../../openspec/specs/deployment/spec.md), [External API](../../../openspec/specs/external-api/spec.md), [Motion arbitration](../../../openspec/specs/motion-arbitration/spec.md). +- Документация ROS 2 Jazzy по вложенным launch-файлам и зависимостям. [Launch](https://github.com/ros2/ros2_documentation/blob/jazzy/source/Tutorials/Intermediate/Launch/Using-ROS2-Launch-For-Large-Projects.rst), [rosdep](https://github.com/ros2/ros2_documentation/blob/jazzy/source/Tutorials/Intermediate/Rosdep.rst). diff --git a/src/iiwa_experement/iiwa_experement/planning_scene.py b/src/iiwa_experement/iiwa_experement/planning_scene.py index c2a7306..4aa81e3 100644 --- a/src/iiwa_experement/iiwa_experement/planning_scene.py +++ b/src/iiwa_experement/iiwa_experement/planning_scene.py @@ -466,8 +466,10 @@ class PlanningSceneBridge: use_sim_time: bool = True, srdf_path: str | None = None, spin_fn: Callable[[float], None] | None = None, + on_timing: Callable[[dict], None] | None = None, ) -> None: self._owns_node = node is None + self._on_timing = on_timing if node is None: node = Node('planning_scene_bridge') node.set_parameters([Parameter( @@ -571,7 +573,10 @@ class PlanningSceneBridge: self._seen_world.clear() self._seen_attached.clear() self._publisher.publish(scene) - self._spin() + # A short non-blocking turn lets rclpy deliver the message to local + # subscribers. The authoritative synchronization check happens in + # assert_synchronized() through /get_planning_scene. + self._spin_once(0.0) def _fetch_acm(self) -> None: if getattr(self, '_get_scene', None) is None or self.node is None: @@ -632,31 +637,20 @@ class PlanningSceneBridge: return scene def _refresh_joint_state(self) -> None: - """Wait for the next /joint_states sample before a full scene update. + """Make sure a measured joint sample exists before a full scene update. - A full PlanningScene carries a robot state. Using a sample captured - before a preceding motion would make the MoveItPy planner start from - an old pose on the next trajectory. + The full scene is published with ``robot_state.is_diff=True`` so it + never overwrites MoveIt's current state. Waiting for the *next* + sample on every scene update added a fixed delay between motions + without improving that guarantee; the executor already checks joint + state freshness immediately before every plan. """ - if self.node is None: + if self.node is None or self._latest_joint_state is not None: return - previous = None - if self._latest_joint_state is not None: - previous = ( - self._latest_joint_state.header.stamp.sec, - self._latest_joint_state.header.stamp.nanosec, - ) - end = time.monotonic() + 0.5 + end = time.monotonic() + 0.1 while time.monotonic() < end: - self._spin_once(0.02) - current = self._latest_joint_state - if current is None: - continue - stamp = ( - current.header.stamp.sec, - current.header.stamp.nanosec, - ) - if previous is None or stamp != previous: + self._spin_once(0.01) + if self._latest_joint_state is not None: return def _relay_full_scene(self) -> None: @@ -667,11 +661,30 @@ class PlanningSceneBridge: move_group and republishing it once keeps both planners identical without a feedback loop. """ + started = time.monotonic() + relayed = False + try: + relayed = self._relay_full_scene_impl() + finally: + on_timing = getattr(self, '_on_timing', None) + if on_timing is not None: + try: + on_timing({ + 'stage': 'planning_scene_relay', + 'started_at': started, + 'finished_at': time.monotonic(), + 'duration_sec': time.monotonic() - started, + 'relayed': relayed, + }) + except Exception: + pass + + def _relay_full_scene_impl(self) -> bool: if getattr(self, '_get_scene', None) is None or self.node is None: - return + return False if not self._get_scene.service_is_ready(): if not self._get_scene.wait_for_service(timeout_sec=1.0): - return + return False expected_world = set(self._world_objects) expected_attached = set(self._attached) deadline = time.monotonic() + 3.0 @@ -715,10 +728,10 @@ class PlanningSceneBridge: scene.robot_state.is_diff = True self._publisher.publish(scene) self._spin() - return + return True # Never relay a stale scene: that would wipe the objects from the # MoveItPy monitor and make the next motion unverified. - return + return False def _publish_full(self) -> None: self._refresh_joint_state() @@ -913,8 +926,9 @@ class PlanningSceneBridge: last_world, last_attached = snapshot if world_expected <= last_world and attached_expected <= last_attached: if not served: - # The service proves move_group; still let the next - # monitored message confirm the second monitor. + # move_group already has the scene, but MoveItPy's + # monitor also needs to have seen the same ids before + # the next plan is allowed. continue return elif served: diff --git a/src/iiwa_llm_plan_executor/doc/README.md b/src/iiwa_llm_plan_executor/doc/README.md index 22500c8..117ea42 100644 --- a/src/iiwa_llm_plan_executor/doc/README.md +++ b/src/iiwa_llm_plan_executor/doc/README.md @@ -1,5 +1,23 @@ # Executing external Qwen3 graphs in Webots +## Краткая инструкция (русский) + +```bash +source /opt/ros/jazzy/setup.bash +colcon build --base-paths src --symlink-install +source install/setup.bash +ros2 launch iiwa_llm_plan_executor planner.launch.py experiment_id:=20 +``` + +Дождитесь готовности сцены, прочитайте `/planner/scene` или +`/iiwa_scene_context/get_scene`, передайте `scene_json` и инструкцию Qwen3 +снаружи, затем отправьте готовый граф в +`/iiwa_llm_plan_executor/submit_plan` (`iiwa_msgs/SubmitPlan`) или через +`POST /planner/runs` с телом `{"run_id": "...", "response": {...}}`. +`scene_revision` не передаётся и не возвращается. Отклонённый запрос не +регистрируется, поэтому тот же `run_id` можно использовать после исправления +графа. Подробности: `docs/interfaces.md`. + 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. @@ -35,17 +53,22 @@ 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`: +the answer and a client-chosen `run_id`; the executor obtains and validates its +own fresh scene snapshot, so no scene revision travels through the API: ```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 service call /iiwa_llm_plan_executor/submit_plan iiwa_msgs/srv/SubmitPlan \ + "{run_id: 'demo-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 legacy `/iiwa_llm_plan_executor/submit` (`webots_ros2_msgs/SetString`) still +routes into the same admission path. Rejections return `code` and `reason` +from `SubmitPlan`; a rejected `run_id` is not registered and can be reused with +a corrected graph. + 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 @@ -64,7 +87,7 @@ HTTP (existing `iiwa_web` server; default port 8007): 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":""}' + -d '{"run_id":"demo-1","response":""}' curl http://localhost:8007/planner/runs/demo-1 curl -X POST http://localhost:8007/planner/runs/demo-1/cancel ``` @@ -75,12 +98,14 @@ The same four operations are MCP tools built from the same router ## 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`). +Only one graph is active per robot. Repeating an identical accepted graph is +idempotent (`duplicate`); another graph with the same accepted `run_id` is a +`conflict`; a different `run_id` while a run is active is rejected (`busy`). +Accepted run ids stay bound after every terminal state. 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: +next node, the mounted tool, the held object and, at the end, the terminal. +The internal scene revision is never exposed. Terminals: * `completed` — the submitted graph and its checked postconditions finished. * `replan_required` — the graph stopped and a fresh snapshot was emitted; the diff --git a/src/iiwa_llm_plan_executor/doc/webots-qa.md b/src/iiwa_llm_plan_executor/doc/webots-qa.md index af6082a..1eb5b2a 100644 --- a/src/iiwa_llm_plan_executor/doc/webots-qa.md +++ b/src/iiwa_llm_plan_executor/doc/webots-qa.md @@ -17,11 +17,12 @@ colcon test --base-paths src --packages-select \ colcon test-result --verbose ``` -Result: build of 10 packages succeeded; `365 tests, 0 errors, 0 failures, -0 skipped` for the four runtime packages. The new camera/tool geometry tests -in `iiwa_utils` pass separately (`7 passed`); four pre-existing -`test_body_hatch_worlds.py` cases still fail because their `.wbt`/`.proto` -files are absent from this checkout. +Result: the five affected packages build successfully; `colcon test` reports +`402 tests, 0 errors, 0 failures, 0 skipped` for `iiwa_msgs`, +`iiwa_experement`, `iiwa_scene_context`, `iiwa_llm_plan_executor` and +`iiwa_web`. Four pre-existing `test_body_hatch_worlds.py` cases in other +packages still fail because their `.wbt`/`.proto` files are absent from this +checkout; they are unrelated to this change. `git diff --check` is clean. @@ -33,8 +34,9 @@ ros2 launch iiwa_llm_plan_executor planner.launch.py experiment_id:=20 * 19 scene objects spawned (fixture, body, cap, tray, 10 screws, 4 tools). * `/iiwa_experement/scene_ready` true; `/iiwa_scene_context/get_scene` served - `contract_version=1`, `catalog_version=1`, `scene_revision=1`, - `state_version=0` with all present ids and no coordinates. + `schema_version=2`, `contract_version=2`, `catalog_version=2`, + `state_version=0`, live poses and action rules. `scene_revision` is not part + of the public envelope. * The camera contract was corrected to the topics actually published by the existing Webots camera plugins: `/d455_top/image_raw/image_color` and `/camera/color/image_color`. @@ -54,12 +56,77 @@ client-supplied `run_id`; the accepted graph appeared in | `transfer(vacuum,vacuum_socket) → dock_tool(vacuum,vacuum_socket) → completed` | `completed`; status ended with `mounted_tool: null`. This was a separate graph after the previous one. | | `grab(cap) → transfer(cap,hatch_seat) → align → release(cap,hatch_seat)` (cup already mounted) | Cap grasp, transfer, align and release succeeded; the supervisor reported the hatch installed. The final `verify` first failed because of a web-wrapper attribute shadowing bug (`latest`), which was fixed afterwards; a clean repeat of the whole cap graph was not obtained in this session. | | cap graph without explicit `grab(vacuum)`, with a `grasp_failed` edge | Admitted; execution returned `grasp_failed` with `reason: "required tool vacuum is not mounted"`, `next: "operator_required"`, `mounted_tool: null`, `held_object: null`. The supervisor state stayed at version 0 with the cap `free` and `vacuum` `docked`; the log contains no planner call for that run. `/plan` held the original graph, `/status`, `GET /planner/runs/...` and the JSONL log held the actual event/reason/ownership/next edge. | -| stale `scene_revision=999` | Rejected: `stale_scene: submitted revision 999 does not match current revision 1`. | +| invalid graph with a stray node key | `POST /planner/runs` returned 422 with `code=invalid` and the exact schema reason; `GET /planner/runs/` returned 404 and no JSONL run log was created. The corrected graph with the **same** `run_id` was accepted, then `completed`. | +| different graph with an already accepted `run_id` | 409 `code=conflict`; the original run and its single `/plan` publication were unchanged. | +| identical accepted graph repeated with the same `run_id` | 200 `code=duplicate`; no second motion. | -The screw handoff graph (`grab(magnetic_tool) → grab(screw_1) → -transfer(screw_1,hole_1) → handoff_screw`) is covered by package tests but was -not completed in this Webots session because the preceding cap/cartesian -planner runs were not repeatable within the remaining time. +The screw handoff graph in experiment 11 was attempted in Webots. The tool +grab succeeded, but the subsequent screw approach was rejected by the strict +motion policy: `screw_1` failed at `tcp tilt ... 10.26 deg (limit 10.00)`, and +`screw_6` failed with `NO_IK_SOLUTION` at the far tray point. This is the same +planning limitation recorded below; it is not a physical-robot test. The +handoff logic itself remains covered by package tests. + +## 2026-11-11 contract and pause review + +### Live `scene_json` + +* Experiment 20: 14 components (fixture, body, cap, tray, 10 screws), four + tools, 14 destinations (work, hatch_seat, four holes, four sockets, four + contour sides). The live supervisor pose overrides the manifest pose; held + objects are republished with their current pose and `source=webots_supervisor`. +* Experiment 8: only fixture/body, `sealant_tool`, four contour sides and that + tool's socket; no cap, tray, screw or unrelated tool. `dispense` is present, + `handoff_screw` is absent. +* Experiment 11: cap, tray, 10 screws, `magnetic_tool`, four holes and its + socket; `handoff_screw` is present. +* `docs/scene_json_experiment_20.json` is generated by the same `build_scene` + function; a unit test fails if the example drifts. + +### Dispense and tool return (experiment 8) + +Graph: `grab(sealant_tool) → dispense(side_1) → transfer(sealant_tool, +sealant_tool_socket) → dock_tool(sealant_tool, sealant_tool_socket)`. +Result: `completed`. `side_1` was an existing contour destination, the +dispense used a continuous LIN segment, and the tool was returned to its own +socket. + +### Tool grab/return (experiment 20) + +Graph: `grab(vacuum) → transfer(vacuum, work) → transfer(vacuum, vacuum_socket) +→ dock_tool(vacuum, vacuum_socket)`. Result: `completed`; `mounted_tool` +ended as `null`. + +### Timing before/after + +The same approach/retreat graph was run in experiment 20 before and after the +planning-scene pause change. The monotonic JSONL stages now show: + +| stage | before (s) | after (s) | +| --- | --- | --- | +| inter-motion gap (`since_previous_trajectory_sec`) | 0.0015 | 0.0013 | +| `_wait_stationary` (normal) | 0.20–0.35 | 0.20–0.35 | +| PlanningScene relay | 0.10–0.21 | 0.10–0.12 | +| PlanningScene check | 0.10 | 0.10 | +| MoveIt plan | 3.6–7.5 | 3.6–7.5 | +| trajectory check | 0.002–0.004 | 0.002–0.004 | +| controller execution | 4.0–12.2 | 4.0–12.2 | +| supervisor response | 0.02–0.04 | 0.02–0.04 | + +The fixed `0.5 s` wait for the next `/joint_states` sample in every full +planning-scene publish was removed; freshness is still checked immediately +before every plan. The monitored-scene rendezvous remains because it is the +proof that MoveItPy also received the scene. The only measured ~3 s interval +is a genuine `_wait_stationary` of 3.3–3.7 s after a dock transfer while the +Webots controller is still decelerating; it is deliberately kept because it is +the required stationary-start check, not an added sleep. `start_delay_sec` +remains a single 3 s gate after scene load and is never applied per motion. + +### Tilt limitation + +The existing strict 10° tcp tilt limit rejected the far-tray screw approach in +experiment 11 (`screw_1`: 10.26°; `screw_6`: `NO_IK_SOLUTION`). No speed or +tilt limit was raised to hide it. ## J7 and tilt evidence @@ -116,7 +183,7 @@ source /opt/ros/jazzy/setup.bash source install/setup.bash ros2 launch iiwa_llm_plan_executor planner.launch.py experiment_id:=20 ros2 service call /iiwa_scene_context/get_scene std_srvs/srv/Trigger {} -# submit a graph through /iiwa_llm_plan_executor/submit; see doc/README.md +# submit a graph through /iiwa_llm_plan_executor/submit_plan; see doc/README.md ros2 topic echo /iiwa_llm_plan_executor/plan --qos-durability transient_local ros2 topic echo /iiwa_llm_plan_executor/status ``` diff --git a/src/iiwa_llm_plan_executor/docs/interfaces.md b/src/iiwa_llm_plan_executor/docs/interfaces.md index 6844bfe..7792ab3 100644 --- a/src/iiwa_llm_plan_executor/docs/interfaces.md +++ b/src/iiwa_llm_plan_executor/docs/interfaces.md @@ -1,11 +1,10 @@ -# `iiwa_llm_plan_executor` interfaces +# Интерфейсы `iiwa_llm_plan_executor` -Graph contract version: **1** (it matches `iiwa_scene_context` contract 1). -The executor never calls Qwen3 and never rewrites the model graph. +Версия контракта графа: **2** (совпадает с `iiwa_scene_context` 2). +Исполнитель не вызывает Qwen3 и не переписывает граф модели. Система +принимает готовый JSON-граф. -## Graph response - -The accepted response is exactly: +## Ответ графа ```json { @@ -28,206 +27,117 @@ The accepted response is exactly: } ``` -* `status` is `ready` or `blocked`. `blocked` is never executed. -* `plan` is a list of at most 256 nodes. Unknown keys, duplicate ids, - reserved terminal ids, missing `entry`, missing required events, unknown - events, unknown `next`, unreachable nodes and nodes that cannot reach a - terminal are rejected. -* Transitions may also be sent once at top level as - `{"from": "...", "event": "...", "to": "..."}`; they are merged into the - node by `parse_response`. Mixing both forms for the same event is rejected. -* Reserved terminals: `completed`, `replan_required`, `operator_required`. - `completed` is the verified end of the submitted graph; it is not a score of - the model against a Russian instruction. -* `params` is optional and currently only `dispense` accepts - `start_fraction`/`end_fraction` in `0..1`. +* `status` — `ready` или `blocked`; `blocked` никогда не исполняется. +* `plan` — не более 256 узлов. Отклоняются неизвестные ключи, дубли `id`, + зарезервированные терминалы, отсутствующий `entry`, пропущенные события, + неизвестные `next`, недостижимые узлы и узлы без пути к терминалу. +* Терминалы: `completed`, `replan_required`, `operator_required`. +* `params` пока поддерживает только `dispense.start_fraction` / + `dispense.end_fraction` в диапазоне `0..1`. -## Skills and aliases +## Навыки -Native: `detect`, `approach`, `lower`, `grab`, `lift`, `transfer`, `align`, -`release`, `retreat`, `capture`, `verify`, `stop`, `wait_scene_clear`, -`inspect_state`, `notify_operator`, `dispense`, `dock_tool`, -`handoff_screw`. +Реализованные навыки: `detect`, `approach`, `lower`, `grab`, `lift`, +`transfer`, `align`, `release`, `retreat`, `capture`, `verify`, `stop`, +`wait_scene_clear`, `inspect_state`, `notify_operator`, `dispense`, +`dock_tool`, `handoff_screw`. -Compatibility aliases: `pick` (= `grab`), `move_to_storage` -(= `transfer` + axial `lower`, keeping the grasp), `inspect` (= `verify`). -`tighten` is not in this profile and is rejected before any motion. +Алиасы: `pick` (= `grab`), `move_to_storage` (= `transfer` + осевой +`lower`), `inspect` (= `verify`). `tighten` не входит в профиль и +отклоняется до любого движения. -The contract, outcomes and available ids come from -`iiwa_scene_context/catalog.py` and the live `scene_json`; a model cannot add -a skill or a contract. +Набор навыков и правила приходят из `iiwa_scene_context/catalog.py` и живого +`scene_json`; модель не может добавить навык или контракт. В сцене без +герметика `dispense` не рекламируется, без винтов — `handoff_screw`, без +инструментов — `dock_tool`. -## Admission rules +## Приём графа -`parse_response(raw) -> PlanGraph` performs the strict JSON shape check. -`validate_graph(graph, scene_envelope, catalog, expected_scene_revision=...)` -returns `ValidationResult(ok, code, reason, details)` with codes: +`parse_response(raw) -> PlanGraph` выполняет строгую проверку формы. +`validate_graph(graph, scene_envelope, catalog)` возвращает +`ValidationResult(ok, code, reason, details)`. -| code | meaning | -| --- | --- | -| `ok` | admitted | -| `blocked` | the model returned `status=blocked`; no motion | -| `replan_required` | a fresh scene/plan is required before motion (missing explicit transfer, unconfirmed ownership, or a graph branch that contradicts the live ownership state) | -| `rejected` | structural or safety violation | -| `stale_scene` | the graph was built for another `scene_revision` | +Порядок приёма: -Important pre-movement rules: +1. формат JSON и схема графа; +2. готовность сцены/MoveIt/контроллера и истёкшая задержка старта; +3. собственный свежий снимок сцены исполнителем; +4. проверка свежести снимка; +5. валидация структуры, ветвей и владения относительно этого снимка; +6. публикация принятого графа. -* A missing required tool is **not** an admission error. `pick(cap)` / - `grab(cap)` is admitted even when `vacuum` is docked. At execution time the - skill re-reads the live supervisor state. If `vacuum` is not mounted it - returns `grasp_failed` with the reason - `required tool vacuum is not mounted`, performs no contact motion, sends no - capture command, changes no object state, and the engine follows exactly the - graph's `grasp_failed` edge. The `success` edge is not assumed. -* The same rule applies to screws without `magnetic_tool` - (`required tool magnetic_tool is not mounted`) and to other tool-dependent - skills, which return their contract's failure event. -* After a `grasp_failed` caused specifically by the missing tool, no contact - and no object change occurred. For the **same** `scene_revision` the - following `inspect_state` for that part may therefore only produce `empty`; - `holding`/`placed` are not simulated. Unknown ownership (a generic grasp - failure) keeps the conservative check of all `inspect_state` outcomes. - A known empty hatch that is already installed makes `placed` possible after - a release, while a free hatch makes `empty` possible. -* The validator still simulates the reachable ownership branches. It skips a - nominal success edge only when that success is impossible from the current - tool/held state; a recovery branch that then assumes the part is held is - rejected with its own ownership reason (for example `hatch is not held`), - not with a missing-tool reason. -* `release(vacuum)` without a target, or anywhere except its named socket and - after an explicit `transfer(vacuum, vacuum_socket)`, is `replan_required`. - The cup stays mounted. `release(vacuum, vacuum_socket)` and - `dock_tool(vacuum, vacuum_socket)` are accepted only at the reached socket. -* A graph ending in `work` with `vacuum` mounted is accepted; the tool stays in - `patron`. -* `grasp_failed` must go to `inspect_state` or `stop`; it can never become - `completed`. `collision` must reach `stop`/`notify_operator` or - `replan_required`/`operator_required`. -* Screws are handed to the virtual operator at a hole; the hole stays empty - and no insertion/tightening occurs. -* No task is inferred from `experiment_id`; only the initial scene is selected. +`RunRecord` создаётся **только после** успешного прохождения всех шагов и +публикации. Поэтому отклонённый `run_id` свободен: исправленный граф можно +отправить снова. Отклонённый запрос не появляется в +`GET /planner/runs/{run_id}`, в `/iiwa_llm_plan_executor/plan` и в JSONL-журнале +запусков. API возвращает конкретные `code`/`reason`. -## ROS execution interfaces +Коды отклонения: `invalid`, `rejected`, `blocked`, `replan_required`, +`not_ready`, `stale_scene`, `busy`, `conflict`. -| Direction | Name | Type | Payload | +Правила `run_id`: + +* пока запуск активен, другой `run_id` получает `busy`; +* повтор идентичного принятого графа с тем же `run_id` — `duplicate`, второго + движения нет; +* другой граф с уже принятым `run_id` — `conflict`; +* после `completed`, `operator_required`, `replan_required`, `failed` или + `canceled` `run_id` остаётся занятым; +* одновременные отправки защищены блокировкой, активный граф на робота один. + +Перед первым движением исполнитель повторно проверяет свежесть сцены и +`state_version`. Если сцена изменилась или стала недоступной, движение не +начинается. Во время выполнения внутренний счётчик (не публикуется) ловит +внешнее изменение сцены; изменения владения, подтверждённые собственными +`grab`/`release`/`dock_tool`/`handoff_screw`, разрешены. + +## ROS-интерфейсы + +| Направление | Имя | Тип | Payload | | --- | --- | --- | --- | -| service | `/iiwa_llm_plan_executor/submit` | `webots_ros2_msgs/SetString` | `{"run_id", "scene_revision", "response"}`. `success` means admitted; the rejection reason is in `/status` and `get_status`. | -| topic | `/iiwa_llm_plan_executor/plan` | `std_msgs/String` | `{"run_id","scene_revision","response"}` exactly for the accepted graph; transient-local, depth 1. Published once per `run_id` before the first node. Rejected graphs never appear. | -| topic | `/iiwa_llm_plan_executor/status` | `std_msgs/String` | Each node event and terminal. Fields: `run_id`, `state`, `reason`, `node_id`, `skill`, `event`, `next`, `scene_revision`, `terminal`, `mounted_tool`, `held_object`, `timestamp`, plus event details. | -| service | `/iiwa_llm_plan_executor/get_status` | `iiwa_msgs/GetRunStatus` | request `string run_id`; response `bool found`, `string status_json`. | -| service | `/iiwa_llm_plan_executor/cancel` | `webots_ros2_msgs/SetString` | `{"run_id": "..."}` or a plain run id; stops execution and preserves the grasp. | +| сервис | `/iiwa_llm_plan_executor/submit_plan` | `iiwa_msgs/SubmitPlan` | request `run_id`, `response` (JSON-строка графа). Ответ: `accepted`, `code`, `reason`, `status_json`. | +| сервис | `/iiwa_llm_plan_executor/submit` | `webots_ros2_msgs/SetString` | Устаревший совместимый вход `{"run_id","response"}`; направляется в тот же механизм приёма. Поле `scene_revision` игнорируется. | +| топик | `/iiwa_llm_plan_executor/plan` | `std_msgs/String` | `{"run_id","response"}` для принятого графа; transient-local, depth 1. Отклонённые графы сюда не попадают. | +| топик | `/iiwa_llm_plan_executor/status` | `std_msgs/String` | События и терминал: `run_id`, `state`, `reason`, `node_id`, `skill`, `event`, `next`, `terminal`, `mounted_tool`, `held_object`, `timestamp`, `event_detail`/`timing`. Поля `scene_revision` нет. | +| сервис | `/iiwa_llm_plan_executor/get_status` | `iiwa_msgs/GetRunStatus` | request `string run_id`; response `bool found`, `string status_json`. | +| сервис | `/iiwa_llm_plan_executor/cancel` | `webots_ros2_msgs/SetString` | `{"run_id": "..."}` или строка; останавливает выполнение, сохраняя захват. | -Every event is logged with the actual skill result, the failure reason when -present, `mounted_tool` and `held_object` read from the supervisor, and the -`next` node selected from the published graph. The same event is written to -the JSONL log and returned by `GET /planner/runs/{run_id}`. +## Тайминги этапов -Admission conditions: fresh `scene_json`, matching `scene_revision`, -`scene_ready`, a ready MoveIt/controller, fresh `joint_states`, and at least -3.0 s of simulation time since the first ready scene. Only one run may be -active. Repeating `submit` with the same `run_id` is idempotent and never -starts a second motion; a different `run_id` while active is rejected as -`busy`. +Каждый запуск пишет JSONL в `~/.ros/iiwa_llm_plan_executor/runs/.jsonl`. +События `motion_timing` и `timing` содержат монотонные `started_at`, +`finished_at`, `duration_sec` для этапов: -Terminals are `completed`, `replan_required`, `operator_required`, plus -`failed`/`canceled` for safety stops. `replan_required` emits the newest -`scene_json`; the external client decides whether to call Qwen3 again. -Execution runs in a worker thread, so `get_status` and `cancel` stay -responsive. A JSONL log with every event is written under `trace_dir` -(default `~/.ros/iiwa_llm_plan_executor/runs`). +* завершение предыдущей траектории — `since_previous_trajectory_sec`; +* `_wait_stationary` — `wait_stationary`; +* синхронизация PlanningScene — `planning_scene_check` / + `planning_scene_sync`; +* построение плана MoveIt — `moveit_plan`; +* проверка траектории — `trajectory_check`; +* ожидание контроллера — `controller_execution`; +* ответ supervisor — `supervisor_command` (`service_wait_sec`, + `service_call_sec`, `result_wait_sec`); +* захват кадра — `frame_capture`. +`start_delay_sec` — задержка после загрузки сцены, применяется один раз перед +стартом графа. Это не пауза между движениями. -## Skill phases +## Терминалы и журнал -A skill may contain the mandatory geometric phases of one action. `approach` -moves above the target and saves that height; `lower` moves along the tool -axis to contact; `grab` performs only the missing phases, confirms the grasp -through the supervisor, then lifts back to the saved approach pose. Explicit -`approach`/`lower`/`lift` nodes do not repeat motion already performed. +`completed` — граф и его проверенные постусловия завершены. +`replan_required` — нужен новый снимок и, возможно, новый вызов Qwen3. +`operator_required` — нужно решение человека; робот остановлен. +`failed` / `canceled` — безопасная остановка (устаревшее состояние, лимиты, +столкновение, отмена, лимит шагов); состояние захвата сохраняется. -`release` always lifts axially before any lateral motion or wrist rotation. -`transfer` stops at the safe height over the destination. `release(cap, -hatch_seat)` lowers, places, detaches and withdraws. `handoff_screw` lowers at -the hole, hands the screw to the virtual operator and withdraws; it never -inserts or tightens and the hole remains empty. +## Ограничения движения -`detect` first withdraws axially from a contact, moves to `work`, waits for a -fresh top frame and reads the exact pose from the supervisor. When the object -is configured for close inspection, a second fresh frame from the hand camera -is required; RGB/D is used for liveness only. - - -## Constrained motion adapter - -`MotionPolicy` (`config/motion_limits.yaml`) checks every sampled point before -the controller sees a plan: - -* Every joint stays inside the real per-joint position limits - (`iiwa_config/config/moveit/joint_limits.yaml`, soft limits included). - Joint 7 keeps the neutral `work` value as a documented preference, but the - former +/-90 deg hard corridor is no longer a rejection boundary; safe paths - may use the full joint range. -* The tcp local +Z (working direction) must point down within - `max_tilt_deg` (10 deg). -* Consecutive points may not jump more than `max_joint_jump_rad` (0.6 rad) or - `max_cartesian_jump_m` (0.15 m); the first point must match the measured - joints used as IK seed. -* The planned trajectory is resampled in joint space at - `trajectory.max_sample_step_rad` (0.01 rad). Every sample is checked for - joint limits, self-collision, scene collision, camera clearance and tcp - orientation before execution. -* `joint_states` and the planning scene freshness are checked before planning. - -Motion selection: - -| Purpose | Motion | -| --- | --- | -| between `work`/`home`, to an object, tool or placement point, part transfer, safe alignment | Pilz PTP | -| axial contact, axial lift/retreat, dispense contour | Pilz LIN | - -If a lateral PTP would start in a contact pose, the skill first performs the -axial LIN retreat to the saved approach height. The `dispense` contour itself -stays a continuous LIN contour; only its approach and retreat use PTP/axial -rules. - -One explicit exception is recorded: if the measured start posture is not yet the -downward working posture (the calibrated home pose points the tip up) and the -gripper is empty, the first reorientation to `work` may pass through larger -tilt angles. Joint limits, joint jumps, Cartesian jumps and the final -downward orientation are still checked, and the event records -`reorientation: true`. A reorientation with a held object is always rejected. - -`MoveItPyPlanner` uses the installed MoveItPy 2.12 API: - -```python -arm.set_start_state(robot_state=clamped_robot_state) # measured joints as IK seed -arm.set_path_constraints(empty_constraints) # no artificial J7 corridor -arm.set_goal_state(pose_stamped_msg=..., pose_link='tcp') -result = arm.plan(single_plan_parameters=params) -result.trajectory.get_robot_trajectory_msg() -# resample + joint/self/world/camera checks, then: -moveit.execute(result.trajectory, controllers=[]) -``` - -The planner never falls back to an unconstrained `MoveToPose` action. If a -plan is missing, the status names the phase, pipeline, planner id, target tcp -pose, start joints and MoveIt error code (for example `NO_IK_SOLUTION`). If -our validation rejects a plan, the status names the real cause: joint limit, -jump, orientation or the colliding link pair with the measured minimum -distance and required clearance. - -The measured start state is clamped to the same soft joint limits used by -MoveIt (`soft_min_position`/`soft_max_position`) before it becomes the IK -seed. Without that clamp a Webots-controller overshoot of ~0.00003 rad makes -`CheckStartStateBounds` abort every following plan. - -Camera collision model: the generated SRDF disables only constructive -contact pairs (`link7-camera_holder`, `camera_holder-patron`, -`camera_holder-camera_corner`, `camera_corner-camera_hand`, -`camera_holder-camera_hand`). Every camera-to-robot pair, including -`camera_hand-link6` and `camera_corner-link6`, remains checked. The D455 -collision box matches its visible bounds; the measured AABB clearance from the -camera corner/hand to link6/link7 is at least 35 mm at `home` and `work`, and -the planner requires a non-zero minimum sampled clearance of 3 mm. +Свободные перемещения между рабочими точками — Pilz PTP. Осевое опускание, +подъём и нанесение герметика — Pilz LIN. Каждый план плотно пересэмплируется +и проверяется по реальным лимитам суставов, self/scene collision, зазору до +камер, наклону TCP (≤10°) и скачкам суставов. Боковое движение из контактной +позы предваряется осевым LIN-отводом. Исполнитель не откатывается к +неограниченному `MoveToPose`. +Это проверка в Webots, а не валидация физического робота и не оценка +качества Qwen3. diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/engine.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/engine.py index cf95473..17d8270 100644 --- a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/engine.py +++ b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/engine.py @@ -44,8 +44,9 @@ class RunRecord: run_id: str state: str = 'new' reason: str = '' - scene_revision: int = 0 + state_version: int = 0 response: str = '' + canonical_response: str = '' node_id: str | None = None skill: str | None = None event: str | None = None @@ -71,14 +72,13 @@ class RunStatus: skill: str | None event: str | None next: str | None - scene_revision: int terminal: str | None mounted_tool: str | None held_object: str | None events: tuple[dict[str, Any], ...] = () def as_dict(self) -> dict[str, Any]: - payload = { + return { 'found': self.found, 'run_id': self.run_id, 'state': self.state, @@ -87,13 +87,11 @@ class RunStatus: 'skill': self.skill, 'event': self.event, 'next': self.next, - 'scene_revision': self.scene_revision, 'terminal': self.terminal, 'mounted_tool': self.mounted_tool, 'held_object': self.held_object, 'events': list(self.events), } - return payload def to_json(self) -> str: return json.dumps(self.as_dict(), ensure_ascii=False) @@ -106,6 +104,13 @@ class CancelResult: run_id: str +def _canonical(graph: PlanGraph) -> str: + return json.dumps( + graph.as_dict(), sort_keys=True, ensure_ascii=False, + separators=(',', ':'), + ) + + class GraphEngine: def __init__( self, @@ -182,117 +187,89 @@ class GraphEngine: self, run_id: str, response_json: str | Mapping[str, Any], - scene_revision: int, ) -> AdmissionResult: + """Admit one graph against a fresh scene snapshot. + + Nothing is registered until the format, readiness, freshness and + ownership checks have all passed and the accepted graph has been + published. A rejected request therefore never occupies its run_id. + """ run_id = str(run_id).strip() if not run_id: - return AdmissionResult(False, 'rejected', 'run_id must not be empty', '') + return AdmissionResult(False, 'invalid', 'run_id must not be empty', '') raw_response = ( response_json if isinstance(response_json, str) else json.dumps(response_json, ensure_ascii=False) ) + try: + graph = parse_response(raw_response) + except PlanSchemaError as error: + return AdmissionResult(False, 'invalid', str(error), run_id) + canonical = _canonical(graph) + with self._lock: existing = self._runs.get(run_id) if existing is not None: - if existing.state in ACTIVE_STATES: + if existing.canonical_response == canonical: return AdmissionResult( True, 'duplicate', - f'run {run_id!r} is already active; no second motion', + f'run {run_id!r} was already admitted; no second motion', run_id, ) - if existing.state == 'rejected': - return AdmissionResult( - False, existing.reason.split(':', 1)[0] or 'rejected', - existing.reason, run_id, - ) return AdmissionResult( - True, 'duplicate', - f'run {run_id!r} was already admitted; no second motion', + False, 'conflict', + f'run {run_id!r} is already bound to a different graph', run_id, ) active = [ record for record in self._runs.values() if record.state in ACTIVE_STATES ] - now = self._clock() - record = RunRecord( - run_id=run_id, - response=raw_response, - scene_revision=int(scene_revision), - ) - self._runs[run_id] = record if active: - record.state = 'rejected' - record.reason = 'busy: another run is active' return AdmissionResult( False, 'busy', 'another graph is already active', run_id, ) + now = self._clock() reason = self._not_ready(now) if reason is not None: - record.state = 'rejected' - record.reason = f'not_ready: {reason}' return AdmissionResult(False, 'not_ready', reason, run_id) envelope = self._envelope() if envelope is None: - record.state = 'rejected' - record.reason = 'not_ready: no fresh scene snapshot' return AdmissionResult( False, 'not_ready', 'no fresh scene snapshot', run_id, ) - envelope_revision = int(envelope.get('scene_revision', -1)) - if envelope_revision != int(scene_revision): - record.state = 'rejected' - record.reason = ( - f'stale_scene: submitted revision {scene_revision} does not ' - f'match current revision {envelope_revision}' - ) - return AdmissionResult( - False, 'stale_scene', - f'submitted scene_revision {scene_revision} does not match ' - f'the current revision {envelope_revision}', - run_id, - ) stale = self._scene_age(envelope, now) if stale is not None: - record.state = 'rejected' - record.reason = f'stale_scene: {stale}' return AdmissionResult(False, 'stale_scene', stale, run_id) - try: - graph = parse_response(raw_response) - except PlanSchemaError as error: - record.state = 'rejected' - record.reason = f'rejected: {error}' - return AdmissionResult(False, 'rejected', str(error), run_id) validation: ValidationResult = validate_graph( graph, envelope, self.catalog, - expected_scene_revision=scene_revision, ) if not validation.ok: - record.state = 'rejected' - record.reason = f'{validation.code}: {validation.reason}' - record.details = validation.details return AdmissionResult( False, validation.code, validation.reason, run_id, ) - record.state = 'admitted' - record.scene_revision = envelope_revision try: if self.on_plan is not None: self.on_plan({ 'run_id': run_id, - 'scene_revision': envelope_revision, 'response': raw_response, }) except Exception as error: - record.state = 'rejected' - record.reason = f'rejected: plan publication failed: {error}' return AdmissionResult( False, 'rejected', f'plan publication failed: {error}', run_id, ) + record = RunRecord( + run_id=run_id, + state='admitted', + response=canonical, + canonical_response=canonical, + state_version=int(envelope.get('state_version', 0)), + ) + self._runs[run_id] = record if self.threaded: thread = threading.Thread( target=self._execute, @@ -301,7 +278,16 @@ class GraphEngine: daemon=True, ) record.thread = thread - thread.start() + try: + thread.start() + except Exception as error: + # Startup failed before any motion: do not keep the run id + # bound to a request that was never executed. + self._runs.pop(run_id, None) + return AdmissionResult( + False, 'rejected', + f'could not start the worker: {error}', run_id, + ) return AdmissionResult(True, 'accepted', 'graph admitted', run_id) # ------------------------------------------------------------------ helpers @@ -314,7 +300,7 @@ class GraphEngine: return None if not isinstance(envelope.get('scene_json'), Mapping): return None - if 'scene_revision' not in envelope: + if 'state_version' not in envelope: return None return envelope @@ -338,7 +324,8 @@ class GraphEngine: if hasattr(context, 'held_object'): context.held_object = initial.get('held_object') or None if hasattr(context, 'scene_revision'): - context.scene_revision = int(envelope.get('scene_revision', 0)) + # Internal only: the executor never exposes this counter. + context.scene_revision = int(envelope.get('state_version', 0)) run.context = context return context @@ -357,7 +344,6 @@ class GraphEngine: 'skill': run.skill, 'event': run.event, 'next': run.next, - 'scene_revision': run.scene_revision, 'terminal': run.terminal, 'mounted_tool': run.mounted_tool, 'held_object': run.held_object, @@ -381,6 +367,8 @@ class GraphEngine: event: str, next_id: str, outcome: Any, + *, + timing: dict[str, Any] | None = None, ) -> None: context = run.context if context is not None: @@ -396,12 +384,13 @@ class GraphEngine: 'event': event, 'reason': details.get('reason', ''), 'next': next_id, - 'scene_revision': run.scene_revision, 'mounted_tool': run.mounted_tool, 'held_object': run.held_object, 'details': details, 'timestamp': self._clock(), } + if timing: + entry['timing'] = timing run.events.append(entry) self._emit_status(run, event_detail=entry) @@ -433,6 +422,17 @@ class GraphEngine: if envelope is None: self._terminal(run, 'failed', 'scene snapshot is unavailable') return + age = self._scene_age(envelope, self._clock()) + if age is not None: + self._terminal(run, 'failed', age) + return + if int(envelope.get('state_version', -1)) != run.state_version: + self._terminal( + run, 'failed', + 'scene changed before the first motion ' + f'({run.state_version} -> {envelope.get("state_version")})', + ) + return self._execute(run.run_id, graph, envelope) def _execute(self, run_id: str, graph: PlanGraph, first_envelope: Mapping[str, Any]) -> None: @@ -440,11 +440,31 @@ class GraphEngine: run = self._runs[run_id] run.state = 'running' try: - context = self._context(run, first_envelope) - envelope = first_envelope + # Recheck immediately before any motion: the graph was admitted + # against this state version, and the worker must not start on a + # scene that changed or became stale in between. + now = self._clock() + current = self._envelope() + if current is None: + self._terminal(run, 'failed', 'fresh scene snapshot is unavailable') + return + age = self._scene_age(current, now) + if age is not None: + self._terminal(run, 'failed', age) + return + if int(current.get('state_version', -1)) != run.state_version: + self._terminal( + run, 'failed', + 'scene changed before the first motion ' + f'({run.state_version} -> {current.get("state_version")})', + ) + return + context = self._context(run, current) + envelope = current node_id = graph.entry steps = 0 while True: + step_started = time.monotonic() if run.cancel_requested: self._terminal(run, 'canceled', 'cancel requested') return @@ -454,7 +474,7 @@ class GraphEngine: f'executed step limit of {self.max_steps} reached', ) return - now = self._clock() + robot_started = time.monotonic() if self.robot_fresh is not None: try: fresh, message = self.robot_fresh() @@ -463,6 +483,9 @@ class GraphEngine: if not fresh: self._terminal(run, 'failed', f'robot state: {message}') return + robot_check_sec = time.monotonic() - robot_started + now = self._clock() + scene_started = time.monotonic() current = self._envelope() if current is None: self._terminal(run, 'failed', 'fresh scene snapshot is unavailable') @@ -471,28 +494,38 @@ class GraphEngine: if age is not None: self._terminal(run, 'failed', age) return - revision = int(current.get('scene_revision', -1)) - if revision != run.scene_revision: + version = int(current.get('state_version', -1)) + if version != run.state_version: previous_node = graph.node(node_id) if node_id in graph.ids else None acknowledged = ( run.skill in OWNERSHIP_SKILLS and run.event == 'success' and previous_node is not None - and revision > run.scene_revision + and version > run.state_version ) if not acknowledged: self._terminal( run, 'failed', - 'scene_revision changed externally ' - f'({run.scene_revision} -> {revision}); motion stopped', + 'scene state changed externally ' + f'({run.state_version} -> {version}); motion stopped', ) return - run.scene_revision = revision + run.state_version = version if hasattr(context, 'scene_revision'): - context.scene_revision = revision + context.scene_revision = version envelope = current + scene_check_sec = time.monotonic() - scene_started node = graph.node(node_id) + skill_started = time.monotonic() outcome = self.skills.execute(node, context) + skill_sec = time.monotonic() - skill_started + timing = { + 'step': steps, + 'robot_check_sec': robot_check_sec, + 'scene_check_sec': scene_check_sec, + 'skill_sec': skill_sec, + 'step_sec': time.monotonic() - step_started, + } event = str(outcome.event) transitions = { transition.event: transition.next for transition in node.transitions @@ -502,7 +535,9 @@ class GraphEngine: run.event = event if event not in transitions: run.next = None - self._record_event(run, node, event, '', outcome) + self._record_event( + run, node, event, '', outcome, timing=timing, + ) self._terminal( run, 'failed', f'unknown skill outcome {event!r} for node {node.id!r}', @@ -510,7 +545,9 @@ class GraphEngine: return next_id = transitions[event] run.next = next_id - self._record_event(run, node, event, next_id, outcome) + self._record_event( + run, node, event, next_id, outcome, timing=timing, + ) steps += 1 if run.cancel_requested: self._terminal(run, 'canceled', 'cancel requested') @@ -525,11 +562,6 @@ class GraphEngine: ) except Exception: pass - if next_id == 'completed' and hasattr(context, 'held_object'): - if context.held_object is not None: - # The graph deliberately leaves an object in the - # gripper only if it never asked to release it. - pass self._terminal(run, next_id, f'graph reached {next_id}') return node_id = next_id @@ -544,7 +576,7 @@ class GraphEngine: if run is None: return RunStatus( False, run_id, 'unknown', 'run_id was not submitted', - None, None, None, None, 0, None, None, None, (), + None, None, None, None, None, None, None, (), ) return RunStatus( True, @@ -555,7 +587,6 @@ class GraphEngine: run.skill, run.event, run.next, - run.scene_revision, run.terminal, run.mounted_tool, run.held_object, diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/moveit_backend.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/moveit_backend.py index a9ad9eb..fda37ca 100644 --- a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/moveit_backend.py +++ b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/moveit_backend.py @@ -9,6 +9,8 @@ to run it. from __future__ import annotations import math +import time +from contextlib import contextmanager from dataclasses import dataclass, field from typing import Any, Callable, Mapping @@ -50,6 +52,23 @@ class MotionOutcome: } +@contextmanager +def _stage(timings: list[dict[str, Any]], name: str, **details): + started = time.monotonic() + try: + yield + finally: + finished = time.monotonic() + entry = { + 'stage': name, + 'started_at': started, + 'finished_at': finished, + 'duration_sec': finished - started, + } + entry.update(details) + timings.append(entry) + + _POSE_TO_MATRIX = None @@ -524,6 +543,7 @@ class MoveItBackend: self._on_event = on_event self.stopped = False self.stop_reason: str | None = None + self._last_motion_end: float | None = None def _emit(self, **payload) -> None: if self._on_event is not None: @@ -560,12 +580,49 @@ class MoveItBackend: phase: str = 'transfer', held_object: str | None = None, ) -> MotionOutcome: - blocked = self._guard() + started = time.monotonic() + since_previous = ( + None if self._last_motion_end is None + else max(0.0, started - self._last_motion_end) + ) + timings: list[dict[str, Any]] = [] + outcome: MotionOutcome | None = None + try: + outcome = self._plan_and_execute_impl( + goal, phase, held_object, timings, + ) + return outcome + finally: + finished = time.monotonic() + self._emit( + event='motion_timing', + phase=phase, + held_object=held_object, + success=bool(outcome is not None and outcome.success), + started_at=started, + finished_at=finished, + total_duration_sec=finished - started, + since_previous_trajectory_sec=since_previous, + stages=timings, + ) + if outcome is not None and outcome.success: + self._last_motion_end = finished + + def _plan_and_execute_impl( + self, + goal: Transform, + phase: str, + held_object: str | None, + timings: list[dict[str, Any]], + ) -> MotionOutcome: + with _stage(timings, 'state_guard'): + blocked = self._guard() if blocked is not None: return blocked if self._before_plan is not None: try: - self._before_plan() + with _stage(timings, 'wait_stationary'): + self._before_plan() except Exception as error: return self._fail(f'settle wait failed: {error}', 'stale_state') planned = None @@ -573,12 +630,14 @@ class MoveItBackend: last_error = '' for attempt in range(3): try: - candidate = self.planner.plan(goal, phase, held_object) + with _stage(timings, 'moveit_plan', attempt=attempt): + candidate = self.planner.plan(goal, phase, held_object) except Exception as error: last_error = str(error) if attempt + 1 < 3: try: - self._before_plan and self._before_plan() + with _stage(timings, 'wait_stationary', attempt=attempt): + self._before_plan and self._before_plan() except Exception: pass continue @@ -591,11 +650,12 @@ class MoveItBackend: current = None if current is None: current = candidate.current_joints - candidate_check = self.policy.validate_trajectory( - candidate.points, candidate.tcp_poses, current, - start_tcp_pose=candidate.start_tcp_pose, - held_object=held_object, - ) + with _stage(timings, 'trajectory_check', attempt=attempt): + candidate_check = self.policy.validate_trajectory( + candidate.points, candidate.tcp_poses, current, + start_tcp_pose=candidate.start_tcp_pose, + held_object=held_object, + ) if candidate_check.ok: planned = candidate check = candidate_check @@ -603,7 +663,8 @@ class MoveItBackend: last_error = candidate_check.reason if attempt + 1 < 3: try: - self._before_plan and self._before_plan() + with _stage(timings, 'wait_stationary', attempt=attempt): + self._before_plan and self._before_plan() except Exception: pass if planned is None or check is None: @@ -617,7 +678,8 @@ class MoveItBackend: ) if self._collision_checker is not None: try: - collision_reason = self._collision_checker(planned) + with _stage(timings, 'collision_check'): + collision_reason = self._collision_checker(planned) except Exception as error: collision_reason = f'collision check failed: {error}' if collision_reason: @@ -649,7 +711,8 @@ class MoveItBackend: planner_details=dict(planned.details), **check.details, ) try: - executed = self.planner.execute(planned) + with _stage(timings, 'controller_execution'): + executed = self.planner.execute(planned) except Exception as error: return self._fail(str(error), 'execution_failed') if not executed: diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/node.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/node.py index cabb4a1..56f2c92 100644 --- a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/node.py +++ b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/node.py @@ -7,7 +7,7 @@ import threading import time import uuid from pathlib import Path -from typing import Any, Mapping +from typing import Any, Callable, Mapping import rclpy from ament_index_python.packages import get_package_share_directory @@ -38,7 +38,7 @@ from iiwa_experement.planning_scene import ( screw_object_id, ) from iiwa_experement.scenario import ScenarioSpec, load_scenario -from iiwa_msgs.srv import GetRunStatus +from iiwa_msgs.srv import GetRunStatus, SubmitPlan from iiwa_scene_context.catalog import DEFAULT_CATALOG from iiwa_scene_context.node import SCENE_TOPIC @@ -53,6 +53,7 @@ COMMAND_SERVICE = '/iiwa_experement/object_command' STATE_TOPIC = '/iiwa_experement/object_state' SCENE_SERVICE = '/iiwa_scene_context/get_scene' SUBMIT_SERVICE = '/iiwa_llm_plan_executor/submit' +SUBMIT_PLAN_SERVICE = '/iiwa_llm_plan_executor/submit_plan' CANCEL_SERVICE = '/iiwa_llm_plan_executor/cancel' STATUS_SERVICE = '/iiwa_llm_plan_executor/get_status' PLAN_TOPIC = '/iiwa_llm_plan_executor/plan' @@ -94,8 +95,15 @@ class RunLog: class SupervisorFacade: """Thread-safe supervisor client that never spins its own node.""" - def __init__(self, node: Node, callback_group) -> None: + def __init__( + self, + node: Node, + callback_group, + *, + on_timing: Callable[[dict[str, Any]], None] | None = None, + ) -> None: self.node = node + self.on_timing = on_timing self._lock = threading.Condition() self._latest: dict[str, Any] | None = None self._client = node.create_client( @@ -138,9 +146,12 @@ class SupervisorFacade: target=None, target_name: str = '', ) -> dict[str, Any]: + request_started = time.monotonic() + service_wait_started = time.monotonic() if not self._client.service_is_ready(): if not self._client.wait_for_service(timeout_sec=5.0): raise RuntimeError(f'{COMMAND_SERVICE} is unavailable') + service_wait_sec = time.monotonic() - service_wait_started request_id = uuid.uuid4().hex payload: dict[str, Any] = { 'request_id': request_id, @@ -157,17 +168,21 @@ class SupervisorFacade: } request = webots_ros2_msgs.srv.SetString.Request() request.value = json.dumps(payload) + call_started = time.monotonic() future = self._client.call_async(request) done = threading.Event() future.add_done_callback(lambda _: done.set()) if not done.wait(timeout=15.0): raise RuntimeError(f'supervisor command {op} timed out') + service_call_sec = time.monotonic() - call_started try: response = future.result() except Exception as error: raise RuntimeError(f'supervisor command {op} failed: {error}') from error accepted = bool(response.success) + result_started = time.monotonic() state = self._wait_for_result(request_id, timeout=3.0) + result_wait_sec = time.monotonic() - result_started last = (state or {}).get('last_command_result') or {} if last.get('request_id') != request_id: raise RuntimeError( @@ -177,6 +192,21 @@ class SupervisorFacade: raise RuntimeError( last.get('error') or f'supervisor rejected {op}' ) + if self.on_timing is not None: + try: + self.on_timing({ + 'stage': 'supervisor_command', + 'op': op, + 'object': object_id, + 'started_at': request_started, + 'finished_at': time.monotonic(), + 'duration_sec': time.monotonic() - request_started, + 'service_wait_sec': service_wait_sec, + 'service_call_sec': service_call_sec, + 'result_wait_sec': result_wait_sec, + }) + except Exception: + pass return state def _wait_for_result(self, request_id: str, timeout: float) -> dict[str, Any]: @@ -201,13 +231,27 @@ class SceneAdapter: bridge: PlanningSceneBridge, spec: ScenarioSpec, geometry: GeometrySpec, + *, + on_timing: Callable[[dict[str, Any]], None] | None = None, ) -> None: self.bridge = bridge self.spec = spec self.geometry = geometry + self.on_timing = on_timing def _sync(self) -> None: + started = time.monotonic() self.bridge.assert_synchronized(*self.bridge.expected_sets()) + if self.on_timing is not None: + try: + self.on_timing({ + 'stage': 'planning_scene_sync', + 'started_at': started, + 'finished_at': time.monotonic(), + 'duration_sec': time.monotonic() - started, + }) + except Exception: + pass def attach_tool(self, tool: str) -> None: self.bridge.attach(attached_tool(tool, self.geometry)) @@ -367,6 +411,12 @@ class PlannerExecutorNode(Node): self._handle_submit, callback_group=self.callback_group, ) + self.create_service( + SubmitPlan, + SUBMIT_PLAN_SERVICE, + self._handle_submit_plan, + callback_group=self.callback_group, + ) self.create_service( webots_ros2_msgs.srv.SetString, CANCEL_SERVICE, @@ -402,7 +452,9 @@ class PlannerExecutorNode(Node): callback_group=self.callback_group, ) - self.supervisor = SupervisorFacade(self, self.callback_group) + self.supervisor = SupervisorFacade( + self, self.callback_group, on_timing=self._on_timing, + ) self._scene_client = self.create_client( Trigger, SCENE_SERVICE, callback_group=self.callback_group, ) @@ -415,8 +467,11 @@ class PlannerExecutorNode(Node): node=self, use_sim_time=True, spin_fn=lambda timeout: time.sleep(max(0.0, float(timeout))), + on_timing=self._on_timing, + ) + self.adapter = SceneAdapter( + self.bridge, self.spec, self.geometry, on_timing=self._on_timing, ) - self.adapter = SceneAdapter(self.bridge, self.spec, self.geometry) self.motion = MoveItBackend( self.geometry, self.policy, @@ -592,12 +647,20 @@ class PlannerExecutorNode(Node): def _scene_guard(self) -> tuple[bool, str]: if not self._bridge_ready: return False, 'planning scene bridge is not initialized' + started = time.monotonic() try: self.bridge.assert_synchronized( *self.bridge.expected_sets(), timeout_sec=2.0, ) except Exception as error: return False, f'planning scene is not synchronized: {error}' + finally: + self._on_timing({ + 'stage': 'planning_scene_check', + 'started_at': started, + 'finished_at': time.monotonic(), + 'duration_sec': time.monotonic() - started, + }) return True, 'ok' def _ensure_motion_ready(self) -> None: @@ -653,14 +716,32 @@ class PlannerExecutorNode(Node): condition = self._frame_conditions.get(topic) if condition is None: return None - deadline = time.monotonic() + self.frame_timeout + started = time.monotonic() + deadline = started + self.frame_timeout with condition: baseline = self._frame_counts[topic] while self._frame_counts[topic] <= baseline: remaining = deadline - time.monotonic() if remaining <= 0.0: + self._on_timing({ + 'stage': 'frame_capture', + 'camera': name, + 'started_at': started, + 'finished_at': time.monotonic(), + 'duration_sec': time.monotonic() - started, + 'captured': False, + }) return None condition.wait(timeout=remaining) + self._on_timing({ + 'stage': 'frame_capture', + 'camera': name, + 'started_at': started, + 'finished_at': time.monotonic(), + 'duration_sec': time.monotonic() - started, + 'captured': True, + 'stamp': self._frame_stamps.get(topic), + }) return self._frame_stamps.get(topic) # ------------------------------------------------------------------ engine @@ -669,7 +750,7 @@ class PlannerExecutorNode(Node): context = SkillContext( spec=self.spec, geometry=self.geometry, - scene_revision=int(envelope.get('scene_revision', 0)), + scene_revision=int(envelope.get('state_version', 0)), ) context.frame_provider = self._frame_provider context.scene_clear_provider = lambda: True @@ -680,18 +761,24 @@ class PlannerExecutorNode(Node): def _publish_plan(self, payload: dict[str, Any]) -> None: self._last_plan = dict(payload) + # Bind the timing/log context to the new run immediately; otherwise + # the first motion events of a run can be labelled with the previous + # run id until the first status message arrives. + self._last_status = { + 'run_id': payload.get('run_id'), + 'state': 'accepted', + 'reason': '', + } message = String() message.data = json.dumps(payload, ensure_ascii=False) self._plan_publisher.publish(message) self.log.write({ 'run_id': payload.get('run_id'), 'event': 'plan_accepted', - 'scene_revision': payload.get('scene_revision'), 'response': payload.get('response'), }) self.get_logger().info( - f"accepted graph for run {payload.get('run_id')} " - f"(scene_revision={payload.get('scene_revision')})" + f"accepted graph for run {payload.get('run_id')}" ) def _publish_status(self, payload: dict[str, Any]) -> None: @@ -706,14 +793,12 @@ class PlannerExecutorNode(Node): self.log.write({ 'run_id': run_id, 'event': 'replan_required', - 'scene_revision': None if envelope is None else envelope.get('scene_revision'), }) if envelope is not None: self._publish_status({ 'run_id': run_id, 'state': 'replan_scene', 'reason': 'fresh scene snapshot for the external planner', - 'scene_revision': envelope.get('scene_revision'), 'scene': envelope, 'timestamp': self._now(), }) @@ -721,8 +806,27 @@ class PlannerExecutorNode(Node): def _on_stop(self, reason: str) -> None: self.motion.stop(reason) + def _current_run_id(self) -> str | None: + return ( + (self._last_status or {}).get('run_id') + or (self._last_plan or {}).get('run_id') + ) + + def _on_timing(self, payload: dict[str, Any]) -> None: + """Write monotonic stage timings to the run JSONL log.""" + self.log.write({ + **payload, + 'event': 'timing', + 'run_id': self._current_run_id(), + 'timestamp': self._now(), + }) + def _on_motion_event(self, payload: dict[str, Any]) -> None: - self.log.write({**payload, 'event': payload.get('event', 'motion')}) + self.log.write({ + **payload, + 'event': payload.get('event', 'motion'), + 'run_id': self._current_run_id(), + }) def _notify_operator(self, reason: str, details: dict[str, Any]) -> bool: self.get_logger().warning(f'operator required: {reason}') @@ -736,22 +840,9 @@ class PlannerExecutorNode(Node): return True # ------------------------------------------------------------------ services - def _handle_submit(self, request, response): - try: - payload = json.loads(request.value) - if not isinstance(payload, dict): - raise ValueError('submit payload must be a JSON object') - run_id = str(payload.get('run_id', '')).strip() - scene_revision = int(payload.get('scene_revision')) - graph = payload.get('response') - if not isinstance(graph, (str, dict)): - raise ValueError('response must be a JSON string or object') - except Exception as error: - response.success = False - self.get_logger().error(f'invalid submit request: {error}') - return response - result = self.engine.submit(run_id, graph, scene_revision) - response.success = bool(result.accepted) + def _submit_payload(self, run_id: str, graph): + """Shared admission path for both submit services.""" + result = self.engine.submit(run_id, graph) if result.accepted: self.get_logger().info( f'run {run_id}: {result.code} ({result.message})' @@ -760,16 +851,53 @@ class PlannerExecutorNode(Node): self.get_logger().warning( f'run {run_id} rejected: {result.code}: {result.message}' ) - self._publish_status({ - 'run_id': run_id, - 'state': 'rejected', - 'reason': f'{result.code}: {result.message}', - 'terminal': result.code, - 'mounted_tool': None, - 'held_object': None, - 'scene_revision': scene_revision, - 'timestamp': self._now(), - }) + return result + + def _handle_submit(self, request, response): + """Legacy webots_ros2_msgs/SetString submission. + + It is kept for compatibility and routes into the same admission path. + A legacy ``scene_revision`` field is ignored: admission always uses the + executor's own fresh snapshot. + """ + try: + payload = json.loads(request.value) + if not isinstance(payload, dict): + raise ValueError('submit payload must be a JSON object') + run_id = str(payload.get('run_id', '')).strip() + graph = payload.get('response') + if not isinstance(graph, (str, dict)): + raise ValueError('response must be a JSON string or object') + except Exception as error: + response.success = False + self.get_logger().error(f'invalid submit request: {error}') + return response + result = self._submit_payload(run_id, graph) + response.success = bool(result.accepted) + return response + + def _handle_submit_plan(self, request, response): + try: + run_id = str(request.run_id or '').strip() + graph = request.response + if not isinstance(graph, str) or not graph.strip(): + raise ValueError('response must be a non-empty JSON string') + except Exception as error: + response.accepted = False + response.code = 'invalid' + response.reason = str(error) + response.status_json = '' + self.get_logger().error(f'invalid submit_plan request: {error}') + return response + result = self._submit_payload(run_id, graph) + response.accepted = bool(result.accepted) + response.code = result.code + response.reason = result.message + response.status_json = '' + if result.accepted: + status = self.engine.get_status(run_id) + if status.found: + response.status_json = status.to_json() return response def _handle_cancel(self, request, response): diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/validator.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/validator.py index 4c76677..4ae7cd0 100644 --- a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/validator.py +++ b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/validator.py @@ -689,8 +689,6 @@ def validate_graph( graph: PlanGraph, scene: Mapping[str, Any], catalog: SkillCatalog, - *, - expected_scene_revision: int | None = None, ) -> ValidationResult: """Return an admission decision; never mutates the graph or the scene.""" if graph.status == 'blocked': @@ -703,14 +701,6 @@ def validate_graph( if len(graph.nodes) > 256: return _reject('graph exceeds the 256-node limit') facts = _facts(scene) - if expected_scene_revision is not None: - revision = facts.envelope.get('scene_revision') - if revision is not None and int(revision) != int(expected_scene_revision): - return _reject( - f'graph was built for scene_revision {expected_scene_revision}, ' - f'current revision is {revision}', - code='stale_scene', - ) structural = _structural_checks(graph, facts) if structural is not None: return structural @@ -724,7 +714,7 @@ def validate_graph( { 'nodes': len(graph.nodes), 'entry': graph.entry, - 'scene_revision': facts.envelope.get('scene_revision'), + 'state_version': facts.envelope.get('state_version'), 'available_skills': len(facts.available_skills), }, ) diff --git a/src/iiwa_llm_plan_executor/test/test_engine.py b/src/iiwa_llm_plan_executor/test/test_engine.py index d230105..4d0a93a 100644 --- a/src/iiwa_llm_plan_executor/test/test_engine.py +++ b/src/iiwa_llm_plan_executor/test/test_engine.py @@ -5,8 +5,7 @@ import threading from pathlib import Path from types import SimpleNamespace -import pytest - +from iiwa_experement.geometry import load_geometry from iiwa_experement.scenario import load_scenario from iiwa_llm_plan_executor.engine import GraphEngine from iiwa_llm_plan_executor.schema import parse_response @@ -16,6 +15,7 @@ from iiwa_scene_context.snapshot import build_envelope, build_scene PACKAGE = Path(__file__).resolve().parents[1] SCENES = PACKAGE.parent / 'iiwa_experement' / 'config' / 'scenes.yaml' +GEOMETRY = load_geometry(PACKAGE.parent / 'iiwa_experement' / 'config' / 'geometry.yaml') SAFE = { 'collision': 'operator_required', 'grasp_failed': 'operator_required', @@ -77,10 +77,11 @@ def envelope(revision=1, stamp=5.0, experiment_id=1): 'holes': {}, }, DEFAULT_CATALOG, + GEOMETRY, + stamp=stamp, ) return build_envelope( scene, - scene_revision=revision, state_version=revision, stamp=stamp, experiment_id=experiment_id, @@ -129,6 +130,11 @@ GRAB_GRAPH = lambda: graph( next_on={'success': 'completed'}), ) +BAD_GRAPH = lambda: graph( + 'g', + node('g', 'grab', 'vacuum', next_on={'success': 'ghost'}), +) + def make_engine(skills=None, *, clock=5.0, provider=None, threaded=False, max_steps=512, **kwargs): @@ -137,6 +143,7 @@ def make_engine(skills=None, *, clock=5.0, provider=None, threaded=False, state = {'envelope': envelope(1, clock_box['now'])} if provider is not None: state['provider'] = provider + def default_provider(): return state['envelope'] events = { @@ -164,38 +171,51 @@ def make_engine(skills=None, *, clock=5.0, provider=None, threaded=False, def test_submission_waits_for_scene_and_the_three_second_delay(): - engine, _, _, _, clock = make_engine(clock=2.0) - rejected = engine.submit('r1', STOP_GRAPH().as_dict(), 1) + engine, _, _, _, _ = make_engine(clock=2.0) + rejected = engine.submit('r1', STOP_GRAPH().as_dict()) assert rejected.accepted is False assert rejected.code == 'not_ready' assert 'start delay' in rejected.message + assert engine.get_status('r1').found is False - engine, _, _, _, clock = make_engine(clock=3.01) - accepted = engine.submit('r1', STOP_GRAPH().as_dict(), 1) + engine, _, _, _, _ = make_engine(clock=3.01) + accepted = engine.submit('r1', STOP_GRAPH().as_dict()) assert accepted.accepted is True engine.run_accepted('r1') engine.update_readiness(scene_ready=False, ready_at=0.0, motion_ready=False) - rejected = engine.submit('r2', STOP_GRAPH().as_dict(), 1) + rejected = engine.submit('r2', STOP_GRAPH().as_dict()) assert rejected.code == 'not_ready' -def test_rejected_graphs_are_never_published_or_executed(): +def test_rejected_graphs_are_not_registered_published_or_executed(): engine, skills, events, _, _ = make_engine() - blocked = engine.submit('r1', {'status': 'blocked', 'entry': '', 'plan': []}, 1) + blocked = engine.submit('r1', {'status': 'blocked', 'entry': '', 'plan': []}) assert blocked.code == 'blocked' - bad_next = graph( - 'g', - node('g', 'grab', 'vacuum', next_on={'success': 'ghost'}), - ) - rejected = engine.submit('r2', bad_next.as_dict(), 1) + rejected = engine.submit('r2', BAD_GRAPH().as_dict()) assert rejected.accepted is False assert rejected.code == 'rejected' assert events['plans'] == [] assert skills.calls == [] - stale = engine.submit('r3', STOP_GRAPH().as_dict(), 2) - assert stale.code == 'stale_scene' - assert engine.get_status('r2').state == 'rejected' + assert engine.get_status('r1').found is False + assert engine.get_status('r2').found is False + assert not any( + event.get('state') == 'rejected' for event in events['statuses'] + ) + + +def test_rejected_run_id_can_be_reused_with_a_corrected_graph(): + engine, skills, events, _, _ = make_engine() + rejected = engine.submit('same-id', BAD_GRAPH().as_dict()) + assert rejected.accepted is False + assert engine.get_status('same-id').found is False + + accepted = engine.submit('same-id', STOP_GRAPH().as_dict()) + assert accepted.accepted is True + assert accepted.code == 'accepted' + assert len(events['plans']) == 1 + engine.run_accepted('same-id') + assert engine.get_status('same-id').state == 'completed' def test_cap_grab_without_vacuum_is_admitted_and_follows_grasp_failed(): @@ -207,7 +227,7 @@ def test_cap_grab_without_vacuum_is_admitted_and_follows_grasp_failed(): 'grasp_failed': 'operator_required', }), ) - accepted = engine.submit('r1', value.as_dict(), 1) + accepted = engine.submit('r1', value.as_dict()) assert accepted.accepted is True assert len(events['plans']) == 1 skills.events = {'g': 'grasp_failed'} @@ -232,7 +252,7 @@ def test_accepted_graph_is_published_once_before_the_first_motion(): engine.on_plan = on_plan skills.on_execute = lambda node, context: order.append(('move', node.id)) - accepted = engine.submit('r1', STOP_GRAPH().as_dict(), 1) + accepted = engine.submit('r1', STOP_GRAPH().as_dict()) assert accepted.accepted engine.run_accepted('r1') assert order[0] == ('plan', 'r1') @@ -241,25 +261,62 @@ def test_accepted_graph_is_published_once_before_the_first_motion(): status = engine.get_status('r1') assert status.state == 'completed' assert status.found is True + assert 'scene_revision' not in status.as_dict() -def test_same_run_id_is_idempotent_and_a_second_run_is_busy(): +def test_same_run_id_is_idempotent_and_a_different_graph_conflicts(): engine, _, events, _, _ = make_engine() - first = engine.submit('r1', STOP_GRAPH().as_dict(), 1) + first = engine.submit('r1', STOP_GRAPH().as_dict()) assert first.accepted - duplicate = engine.submit('r1', STOP_GRAPH().as_dict(), 1) + duplicate = engine.submit('r1', STOP_GRAPH().as_dict()) assert duplicate.accepted is True assert duplicate.code == 'duplicate' assert len(events['plans']) == 1 - busy = engine.submit('r2', STOP_GRAPH().as_dict(), 1) + + conflict = engine.submit('r1', GRAB_GRAPH().as_dict()) + assert conflict.accepted is False + assert conflict.code == 'conflict' + assert len(events['plans']) == 1 + + busy = engine.submit('r2', STOP_GRAPH().as_dict()) assert busy.accepted is False assert busy.code == 'busy' assert len(events['plans']) == 1 +def test_accepted_run_id_stays_bound_after_a_terminal_state(): + engine, _, events, _, _ = make_engine() + assert engine.submit('r1', STOP_GRAPH().as_dict()).accepted + engine.run_accepted('r1') + assert engine.get_status('r1').state == 'completed' + + duplicate = engine.submit('r1', STOP_GRAPH().as_dict()) + assert duplicate.accepted is True + assert duplicate.code == 'duplicate' + conflict = engine.submit('r1', GRAB_GRAPH().as_dict()) + assert conflict.accepted is False + assert conflict.code == 'conflict' + assert len(events['plans']) == 1 + + +def test_plan_publication_failure_does_not_register_the_run(): + engine, skills, _, _, _ = make_engine() + + def broken(_payload): + raise RuntimeError('publisher unavailable') + + engine.on_plan = broken + rejected = engine.submit('r1', STOP_GRAPH().as_dict()) + assert rejected.accepted is False + assert rejected.code == 'rejected' + assert 'publisher unavailable' in rejected.message + assert engine.get_status('r1').found is False + assert skills.calls == [] + + def test_each_event_selects_exactly_one_edge_and_is_visible_in_status(): engine, skills, _, _, _ = make_engine() - engine.submit('r1', STOP_GRAPH().as_dict(), 1) + engine.submit('r1', STOP_GRAPH().as_dict()) skills.events = {'a': 'success', 'b': 'success'} engine.run_accepted('r1') status = engine.get_status('r1') @@ -267,6 +324,7 @@ def test_each_event_selects_exactly_one_edge_and_is_visible_in_status(): assert [event['node_id'] for event in status.events] == ['a', 'b'] assert [event['next'] for event in status.events] == ['b', 'completed'] assert all(event['run_id'] == 'r1' for event in status.events) + assert all('scene_revision' not in event for event in status.events) def test_replan_returns_a_fresh_scene_and_operator_required_stops(): @@ -276,12 +334,12 @@ def test_replan_returns_a_fresh_scene_and_operator_required_stops(): node('d', 'detect', 'cap', next_on={'found': 'replan_required'}), ) - engine.submit('r1', replan_graph.as_dict(), 1) + engine.submit('r1', replan_graph.as_dict()) skills.events = {'d': 'found'} engine.run_accepted('r1') assert engine.get_status('r1').state == 'replan_required' assert len(events['replans']) == 1 - assert events['replans'][0]['scene_revision'] == 1 + assert events['replans'][0]['state_version'] == 1 engine, skills, events, _, _ = make_engine() operator_graph = graph( @@ -289,7 +347,7 @@ def test_replan_returns_a_fresh_scene_and_operator_required_stops(): node('d', 'detect', 'cap', next_on={'found': 'operator_required'}), ) - engine.submit('r2', operator_graph.as_dict(), 1) + engine.submit('r2', operator_graph.as_dict()) skills.events = {'d': 'found'} engine.run_accepted('r2') assert engine.get_status('r2').state == 'operator_required' @@ -297,7 +355,7 @@ def test_replan_returns_a_fresh_scene_and_operator_required_stops(): def test_unknown_skill_outcome_stops_execution(): engine, skills, events, _, _ = make_engine() - engine.submit('r1', STOP_GRAPH().as_dict(), 1) + engine.submit('r1', STOP_GRAPH().as_dict()) skills.events = {'a': 'bogus'} engine.run_accepted('r1') status = engine.get_status('r1') @@ -309,30 +367,30 @@ def test_unknown_skill_outcome_stops_execution(): def test_lost_freshness_and_missing_scene_stop_motion(): engine, _, events, _, _ = make_engine() engine.robot_fresh = lambda: (False, 'joint_states is stale') - engine.submit('r1', STOP_GRAPH().as_dict(), 1) + engine.submit('r1', STOP_GRAPH().as_dict()) engine.run_accepted('r1') assert engine.get_status('r1').state == 'failed' assert 'joint_states' in engine.get_status('r1').reason assert events['stops'] engine, skills, events, state, _ = make_engine() - engine.submit('r2', STOP_GRAPH().as_dict(), 1) + engine.submit('r2', STOP_GRAPH().as_dict()) skills.on_execute = lambda node, context: state.update(envelope=None) engine.run_accepted('r2') assert engine.get_status('r2').state == 'failed' assert 'scene snapshot' in engine.get_status('r2').reason -def test_scene_revision_change_stops_non_ownership_nodes_but_not_own_graph_changes(): +def test_scene_state_change_stops_non_ownership_nodes_but_not_own_graph_changes(): engine, skills, events, state, _ = make_engine() - engine.submit('r1', STOP_GRAPH().as_dict(), 1) + engine.submit('r1', STOP_GRAPH().as_dict()) skills.on_execute = lambda node, context: state.update(envelope=envelope(2)) engine.run_accepted('r1') assert engine.get_status('r1').state == 'failed' assert 'changed externally' in engine.get_status('r1').reason engine, skills, events, state, _ = make_engine() - engine.submit('r2', GRAB_GRAPH().as_dict(), 1) + engine.submit('r2', GRAB_GRAPH().as_dict()) def own_change(node, context): if node.id == 'g': @@ -343,10 +401,22 @@ def test_scene_revision_change_stops_non_ownership_nodes_but_not_own_graph_chang engine.run_accepted('r2') status = engine.get_status('r2') assert status.state == 'completed' - assert status.scene_revision == 2 assert status.mounted_tool == 'vacuum' +def test_changed_scene_before_the_first_motion_stops_the_run(): + engine, skills, events, state, _ = make_engine() + accepted = engine.submit('r1', STOP_GRAPH().as_dict()) + assert accepted.accepted + state['envelope'] = envelope(2) + engine.run_accepted('r1') + status = engine.get_status('r1') + assert status.state == 'failed' + assert 'before the first motion' in status.reason + assert skills.calls == [] + assert events['stops'] + + def test_step_limit_stops_a_recovering_cycle(): engine, skills, _, _, _ = make_engine(max_steps=3) cycle = graph( @@ -356,7 +426,7 @@ def test_step_limit_stops_a_recovering_cycle(): node('b', 'wait_scene_clear', 'workspace', next_on={'clear': 'a'}), ) - engine.submit('r1', cycle.as_dict(), 1) + engine.submit('r1', cycle.as_dict()) skills.events = {'a': 'clear', 'b': 'clear'} engine.run_accepted('r1') status = engine.get_status('r1') @@ -371,9 +441,8 @@ def test_cancellation_stops_the_active_graph_and_preserves_ownership(): lambda node, context: setattr(context, 'mounted_tool', node.object) if node.skill == 'grab' else None ) - engine.submit('r1', GRAB_GRAPH().as_dict(), 1) + engine.submit('r1', GRAB_GRAPH().as_dict()) assert skills.started.wait(timeout=2.0) - # A tool grab confirms ownership before the next node. result = engine.cancel('r1') assert result.accepted assert events['stops'] @@ -387,7 +456,7 @@ def test_cancellation_stops_the_active_graph_and_preserves_ownership(): def test_cancel_rejects_unknown_or_finished_runs(): engine, _, _, _, _ = make_engine() assert engine.cancel('missing').accepted is False - engine.submit('r1', STOP_GRAPH().as_dict(), 1) + engine.submit('r1', STOP_GRAPH().as_dict()) engine.run_accepted('r1') assert engine.cancel('r1').accepted is False @@ -399,15 +468,17 @@ def test_status_query_marks_unknown_ids(): assert status.state == 'unknown' -def test_not_ready_or_unavailable_scene_is_rejected(): +def test_not_ready_or_unavailable_scene_is_rejected_without_registration(): engine, _, _, state, _ = make_engine() state['envelope'] = None - rejected = engine.submit('r1', STOP_GRAPH().as_dict(), 1) + rejected = engine.submit('r1', STOP_GRAPH().as_dict()) assert rejected.code == 'not_ready' + assert engine.get_status('r1').found is False def test_stale_scene_timestamp_is_rejected_before_execution(): - engine, _, _, state, clock = make_engine(clock=10.0) + engine, _, _, state, _ = make_engine(clock=10.0) state['envelope'] = envelope(1, stamp=1.0) - rejected = engine.submit('r1', STOP_GRAPH().as_dict(), 1) + rejected = engine.submit('r1', STOP_GRAPH().as_dict()) assert rejected.code == 'stale_scene' + assert engine.get_status('r1').found is False diff --git a/src/iiwa_llm_plan_executor/test/test_full_qwen_graph.py b/src/iiwa_llm_plan_executor/test/test_full_qwen_graph.py index 5621f1d..cc7b1b2 100644 --- a/src/iiwa_llm_plan_executor/test/test_full_qwen_graph.py +++ b/src/iiwa_llm_plan_executor/test/test_full_qwen_graph.py @@ -87,10 +87,11 @@ def envelope(*, mounted: str | None = None): 'holes': {}, }, DEFAULT_CATALOG, + GEOMETRY, + stamp=5.0, ) return build_envelope( scene, - scene_revision=1, state_version=1, stamp=5.0, experiment_id=20, @@ -108,7 +109,7 @@ def make_engine(*, mounted: str | None = None): context = SkillContext( spec=SPEC, geometry=GEOMETRY, - scene_revision=int(env['scene_revision']), + scene_revision=int(env['state_version']), ) context.frame_provider = lambda name: 5.0 context.notifier = lambda reason, details: True @@ -128,15 +129,10 @@ def make_engine(*, mounted: str | None = None): def test_full_qwen_graph_empty_patron_follows_actual_failure_path(): engine, motion, supervisor, scene_envelope = make_engine(mounted=None) - result = validate_graph( - FULL_GRAPH, - scene_envelope, - DEFAULT_CATALOG, - expected_scene_revision=1, - ) + result = validate_graph(FULL_GRAPH, scene_envelope, DEFAULT_CATALOG) assert result.ok is True, result.reason - accepted = engine.submit('full-empty', FULL_GRAPH.as_dict(), 1) + accepted = engine.submit('full-empty', FULL_GRAPH.as_dict()) assert accepted.accepted is True engine.run_accepted('full-empty') status = engine.get_status('full-empty') @@ -205,7 +201,6 @@ def test_full_qwen_graph_with_vacuum_has_a_success_cycle_without_completed(): FULL_GRAPH, envelope(mounted='vacuum'), DEFAULT_CATALOG, - expected_scene_revision=1, ) assert result.ok is False assert 'a_retry_pick' in result.reason diff --git a/src/iiwa_llm_plan_executor/test/test_node.py b/src/iiwa_llm_plan_executor/test/test_node.py index aea8017..8225b5a 100644 --- a/src/iiwa_llm_plan_executor/test/test_node.py +++ b/src/iiwa_llm_plan_executor/test/test_node.py @@ -16,7 +16,7 @@ from iiwa_llm_plan_executor.node import ( _safe_name, plan_qos, ) -from iiwa_msgs.srv import GetRunStatus +from iiwa_msgs.srv import GetRunStatus, SubmitPlan PACKAGE = Path(__file__).resolve().parents[1] @@ -39,14 +39,14 @@ class FakeEngine: self.submitted = [] self.cancelled = [] - def submit(self, run_id, graph, revision): - self.submitted.append((run_id, graph, revision)) + def submit(self, run_id, graph): + self.submitted.append((run_id, graph)) return self.admission def get_status(self, run_id): return self.status or RunStatus( True, run_id, 'completed', 'done', 'a', 'stop', 'success', - 'completed', 1, 'completed', 'vacuum', None, (), + 'completed', 'completed', 'vacuum', None, (), ) def cancel(self, run_id): @@ -102,18 +102,19 @@ def test_safe_name_and_run_log_are_machine_readable(tmp_path): def test_plan_is_published_once_and_kept_for_late_subscribers(): node = fake_node() - payload = {'run_id': 'r1', 'scene_revision': 2, 'response': '{}'} + payload = {'run_id': 'r1', 'response': '{}'} PlannerExecutorNode._publish_plan(node, payload) PlannerExecutorNode._publish_plan(node, payload) assert len(node._plan_publisher.messages) == 2 assert node._last_plan == payload assert json.loads(node._plan_publisher.messages[-1])['run_id'] == 'r1' + assert 'scene_revision' not in json.loads( + node._plan_publisher.messages[-1] + ) def test_submit_parses_json_and_forwards_the_untouched_response(): node = fake_node() - published = [] - node._publish_status = lambda payload: published.append(payload) request = SimpleNamespace(value=json.dumps({ 'run_id': 'r1', 'scene_revision': 3, @@ -126,34 +127,65 @@ def test_submit_parses_json_and_forwards_the_untouched_response(): assert node.engine.submitted[0][1] == { 'status': 'ready', 'entry': 'a', 'plan': [], } - assert node.engine.submitted[0][2] == 3 + assert len(node.engine.submitted[0]) == 2 -def test_rejected_submit_returns_false_and_publishes_the_reason(): +def test_submit_plan_service_returns_code_and_reason_without_registration(): + node = fake_node(engine=FakeEngine( + admission=AdmissionResult(False, 'not_ready', 'waiting for scene', 'r1'), + )) + response = SubmitPlan.Response() + PlannerExecutorNode._handle_submit_plan( + node, + SimpleNamespace(run_id='r1', response='{"status":"blocked","entry":"","plan":[]}'), + response, + ) + assert response.accepted is False + assert response.code == 'not_ready' + assert response.reason == 'waiting for scene' + assert response.status_json == '' + assert len(node.engine.submitted) == 1 + + +def test_submit_plan_service_returns_the_status_of_an_accepted_run(): + node = fake_node() + response = SubmitPlan.Response() + PlannerExecutorNode._handle_submit_plan( + node, + SimpleNamespace(run_id='r1', response='{"status":"ready","entry":"a","plan":[]}'), + response, + ) + assert response.accepted is True + assert response.code == 'accepted' + status = json.loads(response.status_json) + assert status['run_id'] == 'r1' + assert 'scene_revision' not in status + + +def test_rejected_submit_returns_false_without_registering_or_publishing(): node = fake_node(engine=FakeEngine( admission=AdmissionResult(False, 'replan_required', 'need vacuum', 'r1'), )) published = [] node._publish_status = lambda payload: published.append(payload) request = SimpleNamespace(value=json.dumps({ - 'run_id': 'r1', 'scene_revision': 1, + 'run_id': 'r1', 'response': {'status': 'blocked', 'entry': '', 'plan': []}, })) response = SimpleNamespace(success=True) PlannerExecutorNode._handle_submit(node, request, response) assert response.success is False - assert published[-1]['state'] == 'rejected' - assert 'replan_required' in published[-1]['reason'] + assert published == [] def test_replan_scene_status_keeps_the_run_id(): node = fake_node() node._last_status = {'run_id': 'run-42', 'state': 'running'} - envelope = {'scene_revision': 7, 'scene_json': {'components': []}} + envelope = {'state_version': 7, 'scene_json': {'components': []}} PlannerExecutorNode._on_replan(node, envelope) payload = json.loads(node._status_publisher.messages[-1]) assert payload['run_id'] == 'run-42' - assert payload['scene_revision'] == 7 + assert 'scene_revision' not in payload assert payload['scene'] == envelope @@ -197,7 +229,7 @@ def test_scene_provider_enforces_freshness(): node._now = lambda: 10.0 node.scene_max_age = 2.0 node._latest_scene = { - 'timestamp': 9.0, 'scene_revision': 1, 'scene_json': {}, + 'timestamp': 9.0, 'state_version': 1, 'scene_json': {}, } assert PlannerExecutorNode._scene_provider(node) is not None node._latest_scene['timestamp'] = 7.0 @@ -213,7 +245,7 @@ def test_context_factory_sets_sources_and_inspection_configuration(): node._frame_provider = lambda name: 1.0 node._notify_operator = lambda reason, details: True context = PlannerExecutorNode._context_factory( - node, {'scene_revision': 4}, {}, + node, {'state_version': 4}, {}, ) assert context.scene_revision == 4 assert context.scene_adapter == 'adapter' @@ -229,10 +261,10 @@ def test_future_scene_timestamps_cannot_delay_the_start_gate(): node._scene_ready_at = None node._latest_scene = None message = String() - message.data = json.dumps({'timestamp': 500.0, 'scene_revision': 1}) + message.data = json.dumps({'timestamp': 500.0, 'state_version': 1}) PlannerExecutorNode._on_scene(node, message) assert node._scene_ready_at == 10.0 - message.data = json.dumps({'timestamp': 5.0, 'scene_revision': 1}) + message.data = json.dumps({'timestamp': 5.0, 'state_version': 1}) PlannerExecutorNode._on_scene(node, message) assert node._scene_ready_at == 5.0 diff --git a/src/iiwa_llm_plan_executor/test/test_validator.py b/src/iiwa_llm_plan_executor/test/test_validator.py index e1cebb9..4b0963a 100644 --- a/src/iiwa_llm_plan_executor/test/test_validator.py +++ b/src/iiwa_llm_plan_executor/test/test_validator.py @@ -1,10 +1,11 @@ -"""Graph admission: structure, safe branches, ownership and scene revision.""" +"""Graph admission: structure, safe branches and ownership.""" from __future__ import annotations from pathlib import Path import pytest +from iiwa_experement.geometry import load_geometry from iiwa_experement.scenario import load_scenario from iiwa_llm_plan_executor.schema import PlanSchemaError, parse_response from iiwa_llm_plan_executor.validator import validate_graph @@ -14,6 +15,7 @@ from iiwa_scene_context.snapshot import build_envelope, build_scene PACKAGE = Path(__file__).resolve().parents[1] SCENES = PACKAGE.parent / 'iiwa_experement' / 'config' / 'scenes.yaml' +GEOMETRY = load_geometry(PACKAGE.parent / 'iiwa_experement' / 'config' / 'geometry.yaml') SAFE_DEFAULTS = { 'collision': 'operator_required', 'grasp_failed': 'operator_required', @@ -33,6 +35,7 @@ def spec_for(experiment_id: int = 1): def scene_for(experiment_id: int = 1, *, mounted=None, held=None, cap_state=None, revision=1): + del revision spec = spec_for(experiment_id) tools = {tool.name: {'state': 'docked'} for tool in spec.tools} if mounted: @@ -50,10 +53,11 @@ def scene_for(experiment_id: int = 1, *, mounted=None, held=None, 'holes': {}, }, DEFAULT_CATALOG, + GEOMETRY, + stamp=10.0, ) return build_envelope( scene, - scene_revision=revision, state_version=1, stamp=10.0, experiment_id=experiment_id, @@ -93,13 +97,8 @@ def graph(entry_id, *nodes): }) -def expect(graph_value, scene, *, expected_revision=None): - return validate_graph( - graph_value, - scene, - DEFAULT_CATALOG, - expected_scene_revision=expected_revision, - ) +def expect(graph_value, scene): + return validate_graph(graph_value, scene, DEFAULT_CATALOG) def test_blocked_status_never_reaches_execution(): @@ -358,13 +357,6 @@ def test_cycles_are_bounded_and_must_keep_a_recovery_branch(): assert expect(recovering_cycle, scene_for(1)).ok is True -def test_wrong_scene_revision_is_rejected(): - value = graph('a', node('a', 'detect', 'cap')) - result = expect(value, scene_for(1, revision=3), expected_revision=2) - assert result.ok is False - assert result.code == 'stale_scene' - - def test_aliases_for_pick_move_to_storage_and_inspect_are_supported(): value = graph( 'grab_vacuum', diff --git a/src/iiwa_msgs/CMakeLists.txt b/src/iiwa_msgs/CMakeLists.txt index d59bb0d..6912342 100644 --- a/src/iiwa_msgs/CMakeLists.txt +++ b/src/iiwa_msgs/CMakeLists.txt @@ -13,6 +13,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/MoveToPose.srv" "srv/MoveToNamedPose.srv" "srv/GetRunStatus.srv" + "srv/SubmitPlan.srv" "action/MoveToPose.action" "action/MoveToJoints.action" DEPENDENCIES geometry_msgs action_msgs diff --git a/src/iiwa_msgs/srv/SubmitPlan.srv b/src/iiwa_msgs/srv/SubmitPlan.srv new file mode 100644 index 0000000..2477c9b --- /dev/null +++ b/src/iiwa_msgs/srv/SubmitPlan.srv @@ -0,0 +1,12 @@ +# Submit a ready-made plan graph without a user-supplied scene revision. +string run_id +# The untouched graph as a JSON string (object or string form). +string response +--- +bool accepted +# accepted | duplicate | invalid | rejected | not_ready | busy | conflict +string code +# Human-readable reason for the admission decision. +string reason +# JSON status of an accepted or duplicate run; empty when nothing was registered. +string status_json diff --git a/src/iiwa_scene_context/docs/interfaces.md b/src/iiwa_scene_context/docs/interfaces.md index 04112bf..c8d3858 100644 --- a/src/iiwa_scene_context/docs/interfaces.md +++ b/src/iiwa_scene_context/docs/interfaces.md @@ -1,75 +1,115 @@ -# `iiwa_scene_context` interfaces +# Интерфейсы `iiwa_scene_context` -Contract version: **1**. Catalog version: **1**. Scene schema version: **1**. -All JSON is UTF-8 and uses stable Webots object ids. +Версия схемы сцены: **2**. Версия контракта: **2**. Версия каталога: **2**. +Весь JSON — UTF-8, идентификаторы объектов стабильны и совпадают с Webots DEF. -## ROS interfaces +## ROS-интерфейсы -| Direction | Name | Type | Notes | +| Направление | Имя | Тип | Примечание | | --- | --- | --- | --- | -| service | `/iiwa_scene_context/get_scene` | `std_srvs/Trigger` | `success=true` and `message` = snapshot envelope JSON, or `success=false` and a reason (not ready / stale / no state). | -| topic | `/iiwa_scene_context/scene_json` | `std_msgs/String` | The same envelope JSON. QoS: reliable, transient-local, depth 1. A late subscriber receives the latest snapshot. | -| input | `/iiwa_experement/object_state` | `std_msgs/String` | Live supervisor JSON. Only logical ownership and object state are used. | -| input | `/iiwa_experement/scene_ready` | `std_srvs/Trigger` | Polled by the node. Until it returns `success=true`, no snapshot is published. | +| сервис | `/iiwa_scene_context/get_scene` | `std_srvs/Trigger` | `success=true`, `message` = JSON конверта; иначе причина (сцена не готова / устарела / нет состояния). | +| топик | `/iiwa_scene_context/scene_json` | `std_msgs/String` | Тот же конверт. QoS: reliable, transient-local, depth 1; поздний подписчик получает последний снимок. | +| вход | `/iiwa_experement/object_state` | `std_msgs/String` | Живое состояние supervisor: владение, состояния и позы объектов. | +| вход | `/iiwa_experement/scene_ready` | `std_srvs/Trigger` | Опрашивается узлом. До `success=true` снимок не публикуется. | -The node refuses with `success=false` while the scene is not ready, before the -first supervisor state, or when the last state is older than -`freshness_sec` (default 2.0 s). A stale snapshot must never be treated as -`found`/`success` by a consumer. +Узел отвечает `success=false`, пока сцена не готова, пока не пришло первое +состояние supervisor или пока последнее состояние старше `freshness_sec` +(по умолчанию 2.0 с). Устаревший снимок нельзя трактовать как актуальный. -## Snapshot envelope +## Конверт снимка ```json { - "schema_version": 1, - "contract_version": 1, - "catalog_version": 1, + "schema_version": 2, + "contract_version": 2, + "catalog_version": 2, "experiment_id": 20, "state_version": 7, - "scene_revision": 3, - "timestamp": 12.5, - "scene_json": { "...": "see below" } + "timestamp": 42.0, + "scene_json": { "...": "см. ниже" } } ``` -* `state_version` is the supervisor's discrete `version`. -* `scene_revision` starts at 1 and grows **only** when `state_version` - changes. Pose updates of a held object published between ownership changes - do not advance it. A graph built for another revision is rejected. -* `timestamp` is the local receive time of the snapshot. Consumers apply - their own freshness threshold. +* `state_version` — дискретная версия supervisor. Она растёт при подтверждённом + изменении владения; обновление позы удерживаемого предмета её не меняет. +* `scene_revision` **не входит** в публичный контракт: клиент не передаёт и не + получает его. Исполнитель сам хранит внутренний счётчик для защиты от + внешнего изменения сцены во время выполнения. +* `timestamp` — время получения снимка. Потребитель сам проверяет свежесть. ## `scene_json` -Fields: `schema_version`, `contract_version`, `catalog_version`, `profile`, -`experiment_id`, `domain`, `components`, `destinations`, `tools`, -`initial_state`, `cameras`, `available_skills`, `skill_contracts`, -`service_objects`, `recovery_attempts_per_component`. +Поля: `schema_version`, `contract_version`, `catalog_version`, `profile`, +`experiment_id`, `domain`, `pose_conventions`, `components`, `destinations`, +`tools`, `initial_state`, `cameras`, `available_skills`, `skill_contracts`, +`action_rules`, `service_objects`, `recovery_attempts_per_component`. -`components` contains only objects present in the selected experiment -(`fixture`, `body`, and conditionally `cap`, `tray`, `screw_1`…`screw_10`). -`destinations` contains `work`, `hatch_seat`, `hole_1`…`hole_4`, one -`_socket` for every present tool, and the configured sealant sides. -`tools` contains only present tools; every tool carries its own socket id. +`components` содержит только реально созданные объекты выбранного +эксперимента: всегда `fixture`, `body`; условно `cap`, `tray`, `screw_1`… +`screw_10`. `tools` содержит только присутствующие инструменты. +`destinations` содержит `work`, `hatch_seat`, `hole_1`…`hole_4`, гнездо +`_socket` для каждого инструмента и только те участки герметика +`side_i`, которые заданы в сцене. Для каждого участка публикуются начальная и +конечная позы. -`initial_state` is logical only: +### Позы + +Единые правила: `frame_id="world"`, метры, кватернион `xyzw`. ```json { - "tool_mounted": "vacuum", - "held_object": "cap", - "flags": ["tool_mounted:vacuum", "holding:cap", "hatch_held"], - "components": {"cap": "held", "screw_1": "tray"}, - "tools": {"vacuum": "docked", "magnetic_tool": "docked"}, - "holes": {"hole_1": {"state": "empty", "screw": null}}, - "scene_ready": true + "frame_id": "world", + "position": {"x": 0.3, "y": 0.12, "z": 0.823}, + "orientation": {"x": 0.0, "y": 0.0, "z": 0.3826834, "w": 0.9238795}, + "source": "webots_supervisor", + "stamp": 42.0 } ``` -No coordinates, rotations, TF frames, supervisor poses or other supervisor -internals are part of `scene_json`. Exact object poses are read from the -supervisor by skill execution and never by the model. +* `source="webots_supervisor"` — живая поза из supervisor. Пока предмет + удерживается, публикуется его текущая поза, а не начальная. +* `source="scenario_manifest"` — запасная начальная поза, если supervisor + ещё не прислал объект (например, в unit-тестах). +* `source="executor_geometry"` — производная цель. Она вычисляется теми же + функциями `world_target`, `sealant_point` и `tcp_goal`, что использует + исполнитель, поэтому поза в `scene_json` и цель движения не расходятся. -`available_skills` and `skill_contracts` come from the server catalog in -`iiwa_scene_context/catalog.py`; a graph cannot extend them. `tighten` is not -available in this stage. +Позы есть у `components` и `tools` в поле `pose`. У мест назначения: +`pose` (поза объекта/рабочей точки) и `tcp_pose` (цель TCP). Участок +герметика содержит `start`, `end`, `start_tcp`, `end_tcp`. Числа проверяются +на конечность, кадр — на `world`, кватернион — на нормировку. + +### Правила действий + +`action_rules` — компактные машиночитаемые правила для каждого доступного +навыка: + +```json +"grab": { + "object_types": ["hatch", "screw", "tool"], + "target_types": [], + "objects": ["cap", "screw_1", "vacuum", "..."], + "targets": [], + "required_tools": [], + "required_tool_by_kind": {"hatch": "vacuum", "screw": "magnetic_tool"}, + "preconditions": ["gripper is empty"], + "result": "tool mounted or part attached to the mounted tool" +} +``` + +Правила строятся из выбранной сцены, поэтому отсутствующие ID и навыки не +рекламируются. В частности: + +* `grab(cap)` требует `vacuum`, `grab(screw_i)` — `magnetic_tool`; +* `dispense` доступен только при наличии `sealant_tool` и участка `side_i`; +* `dock_tool` требует предшествующий `transfer(, _socket)` в + собственное гнездо; +* `handoff_screw` только передаёт винт виртуальному оператору у отверстия; + вставки и затяжки нет, отверстие остаётся пустым; +* `tighten` не рекламируется, пока навык не реализован; +* `move_to_storage` не применяется к инструменту и не ведёт `sealant_tool` в + `hatch_seat`. + +Сгенерированный пример для эксперимента 20: +`docs/scene_json_experiment_20.json`. Он создаётся тем же `build_scene`, а не +заменяет динамическую генерацию; тест сверяет пример с генератором. diff --git a/src/iiwa_scene_context/docs/scene_json_experiment_20.json b/src/iiwa_scene_context/docs/scene_json_experiment_20.json new file mode 100644 index 0000000..958d85c --- /dev/null +++ b/src/iiwa_scene_context/docs/scene_json_experiment_20.json @@ -0,0 +1,2391 @@ +{ + "schema_version": 2, + "contract_version": 2, + "catalog_version": 2, + "profile": "grant", + "experiment_id": 20, + "domain": "grant_assembly", + "pose_conventions": { + "frame_id": "world", + "length_unit": "m", + "orientation": "quaternion_xyzw" + }, + "components": [ + { + "id": "fixture", + "name": "оснастка", + "attributes": { + "kind": "fixture", + "movable": false + }, + "dependencies": [], + "destination": "", + "pose": { + "frame_id": "world", + "position": { + "x": 0.3, + "y": 0.12, + "z": 0.803 + }, + "orientation": { + "x": 0.6532814824381882, + "y": 0.27059805007309845, + "z": 0.2705980500730985, + "w": 0.6532814824381884 + }, + "source": "scenario_manifest", + "stamp": 42.0 + } + }, + { + "id": "body", + "name": "корпус", + "attributes": { + "kind": "body", + "movable": false + }, + "dependencies": [], + "destination": "", + "pose": { + "frame_id": "world", + "position": { + "x": 0.3, + "y": 0.12, + "z": 0.823 + }, + "orientation": { + "x": 0.6532814824381882, + "y": 0.27059805007309845, + "z": 0.2705980500730985, + "w": 0.6532814824381884 + }, + "source": "scenario_manifest", + "stamp": 42.0 + } + }, + { + "id": "cap", + "name": "люк", + "attributes": { + "kind": "hatch", + "movable": true + }, + "dependencies": [], + "destination": "hatch_seat", + "state": "free", + "pose": { + "frame_id": "world", + "position": { + "x": 0.35, + "y": -0.22, + "z": 0.790667 + }, + "orientation": { + "x": 0.6532814824381882, + "y": 0.27059805007309845, + "z": 0.2705980500730985, + "w": 0.6532814824381884 + }, + "source": "scenario_manifest", + "stamp": 42.0 + } + }, + { + "id": "tray", + "name": "лоток с винтами", + "attributes": { + "kind": "tray", + "movable": true + }, + "dependencies": [], + "destination": "", + "pose": { + "frame_id": "world", + "position": { + "x": 0.2, + "y": 0.46, + "z": 0.793 + }, + "orientation": { + "x": 0.7071067811865475, + "y": 0.0, + "z": 0.0, + "w": 0.7071067811865477 + }, + "source": "scenario_manifest", + "stamp": 42.0 + } + }, + { + "id": "screw_1", + "name": "винт 1", + "attributes": { + "kind": "screw", + "movable": true, + "allowed_destinations": [ + "hole_1", + "hole_2", + "hole_3", + "hole_4" + ] + }, + "dependencies": [], + "destination": "", + "state": "tray", + "hole": null, + "pose": { + "frame_id": "world", + "position": { + "x": 0.16, + "y": 0.445, + "z": 0.7954 + }, + "orientation": { + "x": 0.7071067811865475, + "y": 0.0, + "z": 0.0, + "w": 0.7071067811865477 + }, + "source": "scenario_manifest", + "stamp": 42.0 + } + }, + { + "id": "screw_2", + "name": "винт 2", + "attributes": { + "kind": "screw", + "movable": true, + "allowed_destinations": [ + "hole_1", + "hole_2", + "hole_3", + "hole_4" + ] + }, + "dependencies": [], + "destination": "", + "state": "tray", + "hole": null, + "pose": { + "frame_id": "world", + "position": { + "x": 0.24000000000000002, + "y": 0.445, + "z": 0.7954 + }, + "orientation": { + "x": 0.7071067811865475, + "y": 0.0, + "z": 0.0, + "w": 0.7071067811865477 + }, + "source": "scenario_manifest", + "stamp": 42.0 + } + }, + { + "id": "screw_3", + "name": "винт 3", + "attributes": { + "kind": "screw", + "movable": true, + "allowed_destinations": [ + "hole_1", + "hole_2", + "hole_3", + "hole_4" + ] + }, + "dependencies": [], + "destination": "", + "state": "tray", + "hole": null, + "pose": { + "frame_id": "world", + "position": { + "x": 0.16, + "y": 0.47500000000000003, + "z": 0.7954 + }, + "orientation": { + "x": 0.7071067811865475, + "y": 0.0, + "z": 0.0, + "w": 0.7071067811865477 + }, + "source": "scenario_manifest", + "stamp": 42.0 + } + }, + { + "id": "screw_4", + "name": "винт 4", + "attributes": { + "kind": "screw", + "movable": true, + "allowed_destinations": [ + "hole_1", + "hole_2", + "hole_3", + "hole_4" + ] + }, + "dependencies": [], + "destination": "", + "state": "tray", + "hole": null, + "pose": { + "frame_id": "world", + "position": { + "x": 0.24000000000000002, + "y": 0.47500000000000003, + "z": 0.7954 + }, + "orientation": { + "x": 0.7071067811865475, + "y": 0.0, + "z": 0.0, + "w": 0.7071067811865477 + }, + "source": "scenario_manifest", + "stamp": 42.0 + } + }, + { + "id": "screw_5", + "name": "винт 5", + "attributes": { + "kind": "screw", + "movable": true, + "allowed_destinations": [ + "hole_1", + "hole_2", + "hole_3", + "hole_4" + ] + }, + "dependencies": [], + "destination": "", + "state": "tray", + "hole": null, + "pose": { + "frame_id": "world", + "position": { + "x": 0.18000000000000002, + "y": 0.445, + "z": 0.7954 + }, + "orientation": { + "x": 0.7071067811865475, + "y": 0.0, + "z": 0.0, + "w": 0.7071067811865477 + }, + "source": "scenario_manifest", + "stamp": 42.0 + } + }, + { + "id": "screw_6", + "name": "винт 6", + "attributes": { + "kind": "screw", + "movable": true, + "allowed_destinations": [ + "hole_1", + "hole_2", + "hole_3", + "hole_4" + ] + }, + "dependencies": [], + "destination": "", + "state": "tray", + "hole": null, + "pose": { + "frame_id": "world", + "position": { + "x": 0.2, + "y": 0.445, + "z": 0.7954 + }, + "orientation": { + "x": 0.7071067811865475, + "y": 0.0, + "z": 0.0, + "w": 0.7071067811865477 + }, + "source": "scenario_manifest", + "stamp": 42.0 + } + }, + { + "id": "screw_7", + "name": "винт 7", + "attributes": { + "kind": "screw", + "movable": true, + "allowed_destinations": [ + "hole_1", + "hole_2", + "hole_3", + "hole_4" + ] + }, + "dependencies": [], + "destination": "", + "state": "tray", + "hole": null, + "pose": { + "frame_id": "world", + "position": { + "x": 0.22, + "y": 0.445, + "z": 0.7954 + }, + "orientation": { + "x": 0.7071067811865475, + "y": 0.0, + "z": 0.0, + "w": 0.7071067811865477 + }, + "source": "scenario_manifest", + "stamp": 42.0 + } + }, + { + "id": "screw_8", + "name": "винт 8", + "attributes": { + "kind": "screw", + "movable": true, + "allowed_destinations": [ + "hole_1", + "hole_2", + "hole_3", + "hole_4" + ] + }, + "dependencies": [], + "destination": "", + "state": "tray", + "hole": null, + "pose": { + "frame_id": "world", + "position": { + "x": 0.18000000000000002, + "y": 0.47500000000000003, + "z": 0.7954 + }, + "orientation": { + "x": 0.7071067811865475, + "y": 0.0, + "z": 0.0, + "w": 0.7071067811865477 + }, + "source": "scenario_manifest", + "stamp": 42.0 + } + }, + { + "id": "screw_9", + "name": "винт 9", + "attributes": { + "kind": "screw", + "movable": true, + "allowed_destinations": [ + "hole_1", + "hole_2", + "hole_3", + "hole_4" + ] + }, + "dependencies": [], + "destination": "", + "state": "tray", + "hole": null, + "pose": { + "frame_id": "world", + "position": { + "x": 0.2, + "y": 0.47500000000000003, + "z": 0.7954 + }, + "orientation": { + "x": 0.7071067811865475, + "y": 0.0, + "z": 0.0, + "w": 0.7071067811865477 + }, + "source": "scenario_manifest", + "stamp": 42.0 + } + }, + { + "id": "screw_10", + "name": "винт 10", + "attributes": { + "kind": "screw", + "movable": true, + "allowed_destinations": [ + "hole_1", + "hole_2", + "hole_3", + "hole_4" + ] + }, + "dependencies": [], + "destination": "", + "state": "tray", + "hole": null, + "pose": { + "frame_id": "world", + "position": { + "x": 0.22, + "y": 0.47500000000000003, + "z": 0.7954 + }, + "orientation": { + "x": 0.7071067811865475, + "y": 0.0, + "z": 0.0, + "w": 0.7071067811865477 + }, + "source": "scenario_manifest", + "stamp": 42.0 + } + } + ], + "destinations": [ + { + "id": "work", + "name": "рабочая позиция", + "kind": "workspace", + "capacity": 1, + "state": "available", + "pose": { + "frame_id": "world", + "position": { + "x": 0.1496, + "y": 0.0, + "z": 1.3139 + }, + "orientation": { + "x": 6.123233995736766e-17, + "y": 1.0, + "z": 6.123233995736766e-17, + "w": 3.749399456654644e-33 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "tcp_pose": { + "frame_id": "world", + "position": { + "x": 0.1496, + "y": 0.0, + "z": 1.3139 + }, + "orientation": { + "x": 6.123233995736766e-17, + "y": 1.0, + "z": 6.123233995736766e-17, + "w": 3.749399456654644e-33 + }, + "source": "executor_geometry", + "stamp": 42.0 + } + }, + { + "id": "hatch_seat", + "name": "посадочное место люка на корпусе", + "kind": "seat", + "capacity": 1, + "state": "empty", + "pose": { + "frame_id": "world", + "position": { + "x": 0.3, + "y": 0.12, + "z": 0.82594 + }, + "orientation": { + "x": 0.6532814824381882, + "y": 0.27059805007309845, + "z": 0.2705980500730985, + "w": 0.6532814824381884 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "tcp_pose": { + "frame_id": "world", + "position": { + "x": 0.29999999999999993, + "y": 0.12000000000000004, + "z": 0.97194 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650897, + "z": 7.252881433641315e-17, + "w": 1.9269848858034314e-16 + }, + "source": "executor_geometry", + "stamp": 42.0 + } + }, + { + "id": "hole_1", + "name": "отверстие 1", + "kind": "hole", + "capacity": 1, + "state": "empty", + "screw": null, + "pose": { + "frame_id": "world", + "position": { + "x": 0.2243, + "y": 0.10519999999999999, + "z": 0.8339399999999999 + }, + "orientation": { + "x": 0.6532814824381882, + "y": 0.27059805007309845, + "z": 0.2705980500730985, + "w": 0.6532814824381884 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "tcp_pose": { + "frame_id": "world", + "position": { + "x": 0.22429999999999997, + "y": 0.10520000000000003, + "z": 0.9829399999999999 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650897, + "z": 7.252881433641315e-17, + "w": 1.9269848858034314e-16 + }, + "source": "executor_geometry", + "stamp": 42.0 + } + }, + { + "id": "hole_2", + "name": "отверстие 2", + "kind": "hole", + "capacity": 1, + "state": "empty", + "screw": null, + "pose": { + "frame_id": "world", + "position": { + "x": 0.2852, + "y": 0.04429999999999999, + "z": 0.8339399999999999 + }, + "orientation": { + "x": 0.6532814824381882, + "y": 0.27059805007309845, + "z": 0.2705980500730985, + "w": 0.6532814824381884 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "tcp_pose": { + "frame_id": "world", + "position": { + "x": 0.28519999999999995, + "y": 0.044300000000000034, + "z": 0.9829399999999999 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650897, + "z": 7.252881433641315e-17, + "w": 1.9269848858034314e-16 + }, + "source": "executor_geometry", + "stamp": 42.0 + } + }, + { + "id": "hole_3", + "name": "отверстие 3", + "kind": "hole", + "capacity": 1, + "state": "empty", + "screw": null, + "pose": { + "frame_id": "world", + "position": { + "x": 0.3757, + "y": 0.1348, + "z": 0.8339399999999999 + }, + "orientation": { + "x": 0.6532814824381882, + "y": 0.27059805007309845, + "z": 0.2705980500730985, + "w": 0.6532814824381884 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "tcp_pose": { + "frame_id": "world", + "position": { + "x": 0.3756999999999999, + "y": 0.13480000000000003, + "z": 0.9829399999999999 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650897, + "z": 7.252881433641315e-17, + "w": 1.9269848858034314e-16 + }, + "source": "executor_geometry", + "stamp": 42.0 + } + }, + { + "id": "hole_4", + "name": "отверстие 4", + "kind": "hole", + "capacity": 1, + "state": "empty", + "screw": null, + "pose": { + "frame_id": "world", + "position": { + "x": 0.31479999999999997, + "y": 0.19569999999999999, + "z": 0.8339399999999999 + }, + "orientation": { + "x": 0.6532814824381882, + "y": 0.27059805007309845, + "z": 0.2705980500730985, + "w": 0.6532814824381884 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "tcp_pose": { + "frame_id": "world", + "position": { + "x": 0.3147999999999999, + "y": 0.1957, + "z": 0.9829399999999999 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650897, + "z": 7.252881433641315e-17, + "w": 1.9269848858034314e-16 + }, + "source": "executor_geometry", + "stamp": 42.0 + } + }, + { + "id": "vacuum_socket", + "name": "гнездо vacuum", + "kind": "tool_socket", + "capacity": 1, + "state": "occupied", + "tool": "vacuum", + "pose": { + "frame_id": "world", + "position": { + "x": 0.255, + "y": -0.42, + "z": 0.803 + }, + "orientation": { + "x": 4.3297802811774664e-17, + "y": 0.7071067811865475, + "z": 0.7071067811865477, + "w": 4.329780281177467e-17 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "tcp_pose": { + "frame_id": "world", + "position": { + "x": 0.255, + "y": -0.42000000000000004, + "z": 0.9430000000000001 + }, + "orientation": { + "x": 6.123233995736766e-17, + "y": 1.0, + "z": 2.220446049250313e-16, + "w": 1.2961131189273611e-32 + }, + "source": "executor_geometry", + "stamp": 42.0 + } + }, + { + "id": "magnetic_tool_socket", + "name": "гнездо magnetic_tool", + "kind": "tool_socket", + "capacity": 1, + "state": "occupied", + "tool": "magnetic_tool", + "pose": { + "frame_id": "world", + "position": { + "x": 0.165, + "y": -0.42, + "z": 0.803 + }, + "orientation": { + "x": 4.3297802811774664e-17, + "y": 0.7071067811865475, + "z": 0.7071067811865477, + "w": 4.329780281177467e-17 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "tcp_pose": { + "frame_id": "world", + "position": { + "x": 0.165, + "y": -0.42000000000000004, + "z": 0.9430000000000001 + }, + "orientation": { + "x": 6.123233995736766e-17, + "y": 1.0, + "z": 2.220446049250313e-16, + "w": 1.2961131189273611e-32 + }, + "source": "executor_geometry", + "stamp": 42.0 + } + }, + { + "id": "sealant_tool_socket", + "name": "гнездо sealant_tool", + "kind": "tool_socket", + "capacity": 1, + "state": "occupied", + "tool": "sealant_tool", + "pose": { + "frame_id": "world", + "position": { + "x": 0.075, + "y": -0.42, + "z": 0.803 + }, + "orientation": { + "x": 4.3297802811774664e-17, + "y": 0.7071067811865475, + "z": 0.7071067811865477, + "w": 4.329780281177467e-17 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "tcp_pose": { + "frame_id": "world", + "position": { + "x": 0.075, + "y": -0.42000000000000004, + "z": 0.9430000000000001 + }, + "orientation": { + "x": 6.123233995736766e-17, + "y": 1.0, + "z": 2.220446049250313e-16, + "w": 1.2961131189273611e-32 + }, + "source": "executor_geometry", + "stamp": 42.0 + } + }, + { + "id": "torque_tool_socket", + "name": "гнездо torque_tool", + "kind": "tool_socket", + "capacity": 1, + "state": "occupied", + "tool": "torque_tool", + "pose": { + "frame_id": "world", + "position": { + "x": -0.015, + "y": -0.42, + "z": 0.803 + }, + "orientation": { + "x": 4.3297802811774664e-17, + "y": 0.7071067811865475, + "z": 0.7071067811865477, + "w": 4.329780281177467e-17 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "tcp_pose": { + "frame_id": "world", + "position": { + "x": -0.015, + "y": -0.42000000000000004, + "z": 0.9430000000000001 + }, + "orientation": { + "x": 6.123233995736766e-17, + "y": 1.0, + "z": 2.220446049250313e-16, + "w": 1.2961131189273611e-32 + }, + "source": "executor_geometry", + "stamp": 42.0 + } + }, + { + "id": "side_1", + "name": "участок герметика side_1", + "kind": "contour", + "capacity": 1, + "state": "available", + "start": { + "frame_id": "world", + "position": { + "x": 0.23281999999999997, + "y": 0.10515, + "z": 0.8297699999999999 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650898, + "z": 2.343260202663149e-17, + "w": 5.6571305614385e-17 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "end": { + "frame_id": "world", + "position": { + "x": 0.28515, + "y": 0.05281999999999999, + "z": 0.8297699999999999 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650898, + "z": 2.343260202663149e-17, + "w": 5.6571305614385e-17 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "start_tcp": { + "frame_id": "world", + "position": { + "x": 0.23281999999999997, + "y": 0.10515000000000001, + "z": 0.9697699999999999 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650898, + "z": 2.343260202663149e-17, + "w": 5.6571305614385e-17 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "end_tcp": { + "frame_id": "world", + "position": { + "x": 0.28515, + "y": 0.052820000000000006, + "z": 0.9697699999999999 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650898, + "z": 2.343260202663149e-17, + "w": 5.6571305614385e-17 + }, + "source": "executor_geometry", + "stamp": 42.0 + } + }, + { + "id": "side_2", + "name": "участок герметика side_2", + "kind": "contour", + "capacity": 1, + "state": "available", + "start": { + "frame_id": "world", + "position": { + "x": 0.28515, + "y": 0.05281999999999999, + "z": 0.8297699999999999 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650898, + "z": 2.343260202663149e-17, + "w": 5.6571305614385e-17 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "end": { + "frame_id": "world", + "position": { + "x": 0.36718, + "y": 0.13485, + "z": 0.8297699999999999 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650898, + "z": 2.343260202663149e-17, + "w": 5.6571305614385e-17 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "start_tcp": { + "frame_id": "world", + "position": { + "x": 0.28515, + "y": 0.052820000000000006, + "z": 0.9697699999999999 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650898, + "z": 2.343260202663149e-17, + "w": 5.6571305614385e-17 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "end_tcp": { + "frame_id": "world", + "position": { + "x": 0.36718, + "y": 0.13485, + "z": 0.9697699999999999 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650898, + "z": 2.343260202663149e-17, + "w": 5.6571305614385e-17 + }, + "source": "executor_geometry", + "stamp": 42.0 + } + }, + { + "id": "side_3", + "name": "участок герметика side_3", + "kind": "contour", + "capacity": 1, + "state": "available", + "start": { + "frame_id": "world", + "position": { + "x": 0.36718, + "y": 0.13485, + "z": 0.8297699999999999 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650898, + "z": 2.343260202663149e-17, + "w": 5.6571305614385e-17 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "end": { + "frame_id": "world", + "position": { + "x": 0.31484999999999996, + "y": 0.18718, + "z": 0.8297699999999999 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650898, + "z": 2.343260202663149e-17, + "w": 5.6571305614385e-17 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "start_tcp": { + "frame_id": "world", + "position": { + "x": 0.36718, + "y": 0.13485, + "z": 0.9697699999999999 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650898, + "z": 2.343260202663149e-17, + "w": 5.6571305614385e-17 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "end_tcp": { + "frame_id": "world", + "position": { + "x": 0.31484999999999996, + "y": 0.18718, + "z": 0.9697699999999999 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650898, + "z": 2.343260202663149e-17, + "w": 5.6571305614385e-17 + }, + "source": "executor_geometry", + "stamp": 42.0 + } + }, + { + "id": "side_4", + "name": "участок герметика side_4", + "kind": "contour", + "capacity": 1, + "state": "available", + "start": { + "frame_id": "world", + "position": { + "x": 0.31484999999999996, + "y": 0.18718, + "z": 0.8297699999999999 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650898, + "z": 2.343260202663149e-17, + "w": 5.6571305614385e-17 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "end": { + "frame_id": "world", + "position": { + "x": 0.23281999999999997, + "y": 0.10515, + "z": 0.8297699999999999 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650898, + "z": 2.343260202663149e-17, + "w": 5.6571305614385e-17 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "start_tcp": { + "frame_id": "world", + "position": { + "x": 0.31484999999999996, + "y": 0.18718, + "z": 0.9697699999999999 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650898, + "z": 2.343260202663149e-17, + "w": 5.6571305614385e-17 + }, + "source": "executor_geometry", + "stamp": 42.0 + }, + "end_tcp": { + "frame_id": "world", + "position": { + "x": 0.23281999999999997, + "y": 0.10515000000000001, + "z": 0.9697699999999999 + }, + "orientation": { + "x": 0.9238795325112867, + "y": 0.3826834323650898, + "z": 2.343260202663149e-17, + "w": 5.6571305614385e-17 + }, + "source": "executor_geometry", + "stamp": 42.0 + } + } + ], + "tools": [ + { + "id": "vacuum", + "name": "вакуумная присоска", + "kind": "suction_cup", + "socket": "vacuum_socket", + "state": "docked", + "pose": { + "frame_id": "world", + "position": { + "x": 0.255, + "y": -0.42, + "z": 0.803 + }, + "orientation": { + "x": 4.3297802811774664e-17, + "y": 0.7071067811865475, + "z": 0.7071067811865477, + "w": 4.329780281177467e-17 + }, + "source": "scenario_manifest", + "stamp": 42.0 + } + }, + { + "id": "magnetic_tool", + "name": "магнитный инструмент", + "kind": "magnet", + "socket": "magnetic_tool_socket", + "state": "docked", + "pose": { + "frame_id": "world", + "position": { + "x": 0.165, + "y": -0.42, + "z": 0.803 + }, + "orientation": { + "x": 4.3297802811774664e-17, + "y": 0.7071067811865475, + "z": 0.7071067811865477, + "w": 4.329780281177467e-17 + }, + "source": "scenario_manifest", + "stamp": 42.0 + } + }, + { + "id": "sealant_tool", + "name": "инструмент нанесения герметика", + "kind": "dispenser", + "socket": "sealant_tool_socket", + "state": "docked", + "pose": { + "frame_id": "world", + "position": { + "x": 0.075, + "y": -0.42, + "z": 0.803 + }, + "orientation": { + "x": 4.3297802811774664e-17, + "y": 0.7071067811865475, + "z": 0.7071067811865477, + "w": 4.329780281177467e-17 + }, + "source": "scenario_manifest", + "stamp": 42.0 + } + }, + { + "id": "torque_tool", + "name": "моментный инструмент", + "kind": "torque_wrench", + "socket": "torque_tool_socket", + "state": "docked", + "pose": { + "frame_id": "world", + "position": { + "x": -0.015, + "y": -0.42, + "z": 0.803 + }, + "orientation": { + "x": 4.3297802811774664e-17, + "y": 0.7071067811865475, + "z": 0.7071067811865477, + "w": 4.329780281177467e-17 + }, + "source": "scenario_manifest", + "stamp": 42.0 + } + } + ], + "initial_state": { + "tool_mounted": null, + "held_object": null, + "flags": [ + "tool_docked", + "gripper_empty", + "hatch_free" + ], + "components": { + "fixture": "present", + "body": "present", + "cap": "free", + "tray": "present", + "screw_1": "tray", + "screw_2": "tray", + "screw_3": "tray", + "screw_4": "tray", + "screw_5": "tray", + "screw_6": "tray", + "screw_7": "tray", + "screw_8": "tray", + "screw_9": "tray", + "screw_10": "tray" + }, + "tools": { + "vacuum": "docked", + "magnetic_tool": "docked", + "sealant_tool": "docked", + "torque_tool": "docked" + }, + "holes": { + "hole_1": { + "state": "empty", + "screw": null + }, + "hole_2": { + "state": "empty", + "screw": null + }, + "hole_3": { + "state": "empty", + "screw": null + }, + "hole_4": { + "state": "empty", + "screw": null + } + }, + "scene_ready": true, + "stamp": 42.0 + }, + "cameras": { + "top": { + "topic": "/d455_top/image_raw/image_color", + "role": "overview", + "required_for": [ + "detect" + ] + }, + "hand": { + "topic": "/camera/color/image_color", + "role": "close_inspection", + "required_for": [ + "detect", + "capture" + ] + } + }, + "available_skills": [ + "detect", + "approach", + "lower", + "grab", + "lift", + "transfer", + "align", + "release", + "retreat", + "capture", + "verify", + "stop", + "wait_scene_clear", + "inspect_state", + "notify_operator", + "dispense", + "dock_tool", + "handoff_screw", + "pick", + "move_to_storage", + "inspect" + ], + "skill_contracts": { + "detect": { + "outcomes": { + "found": "Объект найден, актуальная локализация получена.", + "not_found": "Объект не найден, движение не выполнялось.", + "failed": "Наблюдение недоступно." + } + }, + "approach": { + "outcomes": { + "success": "Движение в безопасную точку над целью выполнено.", + "human_in_scene": "Шаг прерван из-за человека в зоне движения.", + "collision": "Исполнитель сообщил о столкновении.", + "failed": "Иная ошибка движения." + } + }, + "lower": { + "outcomes": { + "success": "Осевое опускание к контакту выполнено.", + "human_in_scene": "Шаг прерван из-за человека в зоне движения.", + "collision": "Исполнитель сообщил о столкновении.", + "failed": "Иная ошибка движения." + } + }, + "grab": { + "outcomes": { + "success": "Захват или установка инструмента подтверждены.", + "grasp_failed": "Захват не подтверждён; состояние нужно проверить.", + "human_in_scene": "Шаг прерван из-за человека в зоне движения.", + "collision": "Исполнитель сообщил о столкновении.", + "failed": "Иная ошибка захвата." + } + }, + "lift": { + "outcomes": { + "success": "Осевой подъём выполнен.", + "human_in_scene": "Шаг прерван из-за человека в зоне движения.", + "collision": "Исполнитель сообщил о столкновении.", + "failed": "Иная ошибка движения." + } + }, + "transfer": { + "outcomes": { + "success": "Перенос к цели на безопасной высоте выполнен.", + "human_in_scene": "Шаг прерван из-за человека в зоне движения.", + "collision": "Исполнитель сообщил о столкновении.", + "failed": "Иная ошибка движения." + } + }, + "align": { + "outcomes": { + "success": "Ориентация совмещена на безопасной высоте.", + "failed": "Совмещение не подтверждено." + } + }, + "release": { + "outcomes": { + "success": "Объект отпущен в разрешённом месте.", + "human_in_scene": "Шаг прерван из-за человека в зоне движения.", + "collision": "Исполнитель сообщил о столкновении.", + "failed": "Отпускание не подтверждено." + } + }, + "retreat": { + "outcomes": { + "success": "Осевой отвод выполнен.", + "human_in_scene": "Шаг прерван из-за человека в зоне движения.", + "collision": "Исполнитель сообщил о столкновении.", + "failed": "Иная ошибка движения." + } + }, + "capture": { + "outcomes": { + "success": "Свежий кадр получен.", + "failed": "Кадр не получен." + } + }, + "verify": { + "outcomes": { + "ok": "Результат в допуске.", + "not_ok": "Результат вне допуска.", + "failed": "Невозможно измерить результат." + } + }, + "stop": { + "outcomes": { + "success": "Остановка подтверждена, состояние захвата сохранено.", + "failed": "Остановку подтвердить не удалось." + } + }, + "wait_scene_clear": { + "outcomes": { + "clear": "Зона свободна, продолжение разрешено.", + "timeout": "Условие не наступило за отведённое время.", + "failed": "Невозможно подтвердить условие." + } + }, + "inspect_state": { + "outcomes": { + "empty": "Захват пуст, деталь доступна для новой попытки.", + "holding": "Нужная деталь удерживается.", + "placed": "Деталь на месте, захват пуст.", + "unknown": "Состояние владения не подтверждено." + } + }, + "notify_operator": { + "outcomes": { + "success": "Уведомление оператору отправлено.", + "failed": "Доставка уведомления не подтверждена." + } + }, + "dispense": { + "outcomes": { + "success": "Участок герметика пройден.", + "human_in_scene": "Шаг прерван из-за человека в зоне движения.", + "collision": "Исполнитель сообщил о столкновении.", + "failed": "Иная ошибка нанесения." + } + }, + "dock_tool": { + "outcomes": { + "success": "Инструмент возвращён в своё гнездо.", + "collision": "Исполнитель сообщил о столкновении.", + "failed": "Стыковка не подтверждена." + } + }, + "handoff_screw": { + "outcomes": { + "success": "Винт передан виртуальному оператору у отверстия.", + "grasp_failed": "Передача не подтверждена; состояние нужно проверить.", + "collision": "Исполнитель сообщил о столкновении.", + "failed": "Иная ошибка передачи." + } + }, + "pick": { + "outcomes": { + "success": "Захват подтверждён.", + "grasp_failed": "Захват не подтверждён; состояние нужно проверить.", + "human_in_scene": "Шаг прерван из-за человека в зоне движения.", + "collision": "Исполнитель сообщил о столкновении.", + "failed": "Иная ошибка захвата." + } + }, + "move_to_storage": { + "outcomes": { + "success": "Деталь установлена на опору, захват сохранён.", + "human_in_scene": "Шаг прерван из-за человека в зоне движения.", + "collision": "Исполнитель сообщил о столкновении.", + "failed": "Иная ошибка переноса." + } + }, + "inspect": { + "outcomes": { + "ok": "Результат в допуске.", + "not_ok": "Результат вне допуска.", + "failed": "Невозможно измерить результат." + } + } + }, + "action_rules": { + "detect": { + "object_types": [ + "body", + "fixture", + "hatch", + "screw", + "tray", + "tool" + ], + "target_types": [ + "contour", + "hole", + "seat", + "tool_socket", + "workspace" + ], + "objects": [ + "fixture", + "body", + "cap", + "tray", + "screw_1", + "screw_2", + "screw_3", + "screw_4", + "screw_5", + "screw_6", + "screw_7", + "screw_8", + "screw_9", + "screw_10", + "vacuum", + "magnetic_tool", + "sealant_tool", + "torque_tool" + ], + "targets": [ + "work", + "hatch_seat", + "hole_1", + "hole_2", + "hole_3", + "hole_4", + "vacuum_socket", + "magnetic_tool_socket", + "sealant_tool_socket", + "torque_tool_socket", + "side_1", + "side_2", + "side_3", + "side_4" + ], + "required_tools": [], + "preconditions": [], + "result": "fresh pose of a present object or destination" + }, + "approach": { + "object_types": [ + "body", + "fixture", + "hatch", + "screw", + "tray", + "tool" + ], + "target_types": [ + "contour", + "hole", + "seat", + "tool_socket", + "workspace", + "component", + "tool" + ], + "objects": [ + "fixture", + "body", + "cap", + "tray", + "screw_1", + "screw_2", + "screw_3", + "screw_4", + "screw_5", + "screw_6", + "screw_7", + "screw_8", + "screw_9", + "screw_10", + "vacuum", + "magnetic_tool", + "sealant_tool", + "torque_tool" + ], + "targets": [ + "work", + "hatch_seat", + "hole_1", + "hole_2", + "hole_3", + "hole_4", + "vacuum_socket", + "magnetic_tool_socket", + "sealant_tool_socket", + "torque_tool_socket", + "side_1", + "side_2", + "side_3", + "side_4", + "fixture", + "body", + "cap", + "tray", + "screw_1", + "screw_2", + "screw_3", + "screw_4", + "screw_5", + "screw_6", + "screw_7", + "screw_8", + "screw_9", + "screw_10", + "vacuum", + "magnetic_tool", + "sealant_tool", + "torque_tool" + ], + "required_tools": [], + "preconditions": [], + "result": "tcp above the target at approach height" + }, + "lower": { + "object_types": [ + "body", + "fixture", + "hatch", + "screw", + "tray", + "tool" + ], + "target_types": [ + "contour", + "hole", + "seat", + "tool_socket", + "workspace", + "component", + "tool" + ], + "objects": [ + "fixture", + "body", + "cap", + "tray", + "screw_1", + "screw_2", + "screw_3", + "screw_4", + "screw_5", + "screw_6", + "screw_7", + "screw_8", + "screw_9", + "screw_10", + "vacuum", + "magnetic_tool", + "sealant_tool", + "torque_tool" + ], + "targets": [ + "work", + "hatch_seat", + "hole_1", + "hole_2", + "hole_3", + "hole_4", + "vacuum_socket", + "magnetic_tool_socket", + "sealant_tool_socket", + "torque_tool_socket", + "side_1", + "side_2", + "side_3", + "side_4", + "fixture", + "body", + "cap", + "tray", + "screw_1", + "screw_2", + "screw_3", + "screw_4", + "screw_5", + "screw_6", + "screw_7", + "screw_8", + "screw_9", + "screw_10", + "vacuum", + "magnetic_tool", + "sealant_tool", + "torque_tool" + ], + "required_tools": [], + "preconditions": [], + "result": "tcp at the axial contact pose" + }, + "grab": { + "object_types": [ + "hatch", + "screw", + "tool" + ], + "target_types": [], + "objects": [ + "cap", + "screw_1", + "screw_2", + "screw_3", + "screw_4", + "screw_5", + "screw_6", + "screw_7", + "screw_8", + "screw_9", + "screw_10", + "vacuum", + "magnetic_tool", + "sealant_tool", + "torque_tool" + ], + "targets": [], + "required_tools": [], + "preconditions": [ + "tool socket is free for a tool", + "gripper is empty" + ], + "result": "tool mounted or part attached to the mounted tool", + "required_tool_by_kind": { + "hatch": "vacuum", + "screw": "magnetic_tool" + } + }, + "lift": { + "object_types": [ + "body", + "fixture", + "hatch", + "screw", + "tray", + "tool" + ], + "target_types": [ + "contour", + "hole", + "seat", + "tool_socket", + "workspace", + "component", + "tool" + ], + "objects": [ + "fixture", + "body", + "cap", + "tray", + "screw_1", + "screw_2", + "screw_3", + "screw_4", + "screw_5", + "screw_6", + "screw_7", + "screw_8", + "screw_9", + "screw_10", + "vacuum", + "magnetic_tool", + "sealant_tool", + "torque_tool" + ], + "targets": [ + "work", + "hatch_seat", + "hole_1", + "hole_2", + "hole_3", + "hole_4", + "vacuum_socket", + "magnetic_tool_socket", + "sealant_tool_socket", + "torque_tool_socket", + "side_1", + "side_2", + "side_3", + "side_4", + "fixture", + "body", + "cap", + "tray", + "screw_1", + "screw_2", + "screw_3", + "screw_4", + "screw_5", + "screw_6", + "screw_7", + "screw_8", + "screw_9", + "screw_10", + "vacuum", + "magnetic_tool", + "sealant_tool", + "torque_tool" + ], + "required_tools": [], + "preconditions": [], + "result": "tcp at the saved approach height" + }, + "transfer": { + "object_types": [ + "body", + "fixture", + "hatch", + "screw", + "tray", + "tool" + ], + "target_types": [ + "contour", + "hole", + "seat", + "tool_socket", + "workspace" + ], + "objects": [ + "fixture", + "body", + "cap", + "tray", + "screw_1", + "screw_2", + "screw_3", + "screw_4", + "screw_5", + "screw_6", + "screw_7", + "screw_8", + "screw_9", + "screw_10", + "vacuum", + "magnetic_tool", + "sealant_tool", + "torque_tool" + ], + "targets": [ + "work", + "hatch_seat", + "hole_1", + "hole_2", + "hole_3", + "hole_4", + "vacuum_socket", + "magnetic_tool_socket", + "sealant_tool_socket", + "torque_tool_socket", + "side_1", + "side_2", + "side_3", + "side_4" + ], + "required_tools": [], + "preconditions": [ + "object is held or tool is mounted" + ], + "result": "tcp above the destination at transfer height", + "targets_by_object_kind": { + "tool": [ + "work", + "vacuum_socket", + "magnetic_tool_socket", + "sealant_tool_socket", + "torque_tool_socket" + ], + "hatch": [ + "work", + "hatch_seat" + ], + "screw": [ + "work", + "hole_1", + "hole_2", + "hole_3", + "hole_4" + ] + } + }, + "align": { + "object_types": [ + "body", + "fixture", + "hatch", + "screw", + "tray", + "tool" + ], + "target_types": [ + "contour", + "hole", + "seat", + "tool_socket", + "workspace" + ], + "objects": [ + "fixture", + "body", + "cap", + "tray", + "screw_1", + "screw_2", + "screw_3", + "screw_4", + "screw_5", + "screw_6", + "screw_7", + "screw_8", + "screw_9", + "screw_10", + "vacuum", + "magnetic_tool", + "sealant_tool", + "torque_tool" + ], + "targets": [ + "work", + "hatch_seat", + "hole_1", + "hole_2", + "hole_3", + "hole_4", + "vacuum_socket", + "magnetic_tool_socket", + "sealant_tool_socket", + "torque_tool_socket", + "side_1", + "side_2", + "side_3", + "side_4" + ], + "required_tools": [], + "preconditions": [ + "a part is held or a tool is mounted" + ], + "result": "orientation aligned above the target" + }, + "release": { + "object_types": [ + "hatch", + "screw", + "tool" + ], + "target_types": [ + "seat", + "hole", + "tool_socket" + ], + "objects": [ + "cap", + "screw_1", + "screw_2", + "screw_3", + "screw_4", + "screw_5", + "screw_6", + "screw_7", + "screw_8", + "screw_9", + "screw_10", + "vacuum", + "magnetic_tool", + "sealant_tool", + "torque_tool" + ], + "targets": [ + "hatch_seat", + "hole_1", + "hole_2", + "hole_3", + "hole_4", + "vacuum_socket", + "magnetic_tool_socket", + "sealant_tool_socket", + "torque_tool_socket" + ], + "required_tools": [], + "preconditions": [ + "release(tool) requires the matching _socket", + "an explicit transfer to the socket precedes dock_tool" + ], + "result": "screw handed to the virtual operator at a hole; no insertion or tightening" + }, + "retreat": { + "object_types": [ + "body", + "fixture", + "hatch", + "screw", + "tray", + "tool" + ], + "target_types": [ + "contour", + "hole", + "seat", + "tool_socket", + "workspace", + "component", + "tool" + ], + "objects": [ + "fixture", + "body", + "cap", + "tray", + "screw_1", + "screw_2", + "screw_3", + "screw_4", + "screw_5", + "screw_6", + "screw_7", + "screw_8", + "screw_9", + "screw_10", + "vacuum", + "magnetic_tool", + "sealant_tool", + "torque_tool" + ], + "targets": [ + "work", + "hatch_seat", + "hole_1", + "hole_2", + "hole_3", + "hole_4", + "vacuum_socket", + "magnetic_tool_socket", + "sealant_tool_socket", + "torque_tool_socket", + "side_1", + "side_2", + "side_3", + "side_4", + "fixture", + "body", + "cap", + "tray", + "screw_1", + "screw_2", + "screw_3", + "screw_4", + "screw_5", + "screw_6", + "screw_7", + "screw_8", + "screw_9", + "screw_10", + "vacuum", + "magnetic_tool", + "sealant_tool", + "torque_tool" + ], + "required_tools": [], + "preconditions": [], + "result": "tcp at work after an axial withdrawal" + }, + "capture": { + "object_types": [ + "camera" + ], + "target_types": [], + "objects": [ + "top", + "hand" + ], + "targets": [], + "required_tools": [], + "preconditions": [], + "result": "a fresh frame is available" + }, + "verify": { + "object_types": [ + "hatch", + "screw" + ], + "target_types": [ + "seat", + "hole" + ], + "objects": [ + "cap", + "screw_1", + "screw_2", + "screw_3", + "screw_4", + "screw_5", + "screw_6", + "screw_7", + "screw_8", + "screw_9", + "screw_10" + ], + "targets": [ + "hatch_seat", + "hole_1", + "hole_2", + "hole_3", + "hole_4" + ], + "required_tools": [], + "preconditions": [], + "result": "measured object state matches the expected target" + }, + "stop": { + "object_types": [ + "service_object" + ], + "target_types": [], + "objects": [ + "robot" + ], + "targets": [], + "required_tools": [], + "preconditions": [], + "result": "motion stopped, grasp preserved" + }, + "wait_scene_clear": { + "object_types": [ + "service_object" + ], + "target_types": [], + "objects": [ + "workspace" + ], + "targets": [], + "required_tools": [], + "preconditions": [], + "result": "continue only after a bounded clear condition" + }, + "inspect_state": { + "object_types": [ + "body", + "fixture", + "hatch", + "screw", + "tray", + "tool" + ], + "target_types": [], + "objects": [ + "fixture", + "body", + "cap", + "tray", + "screw_1", + "screw_2", + "screw_3", + "screw_4", + "screw_5", + "screw_6", + "screw_7", + "screw_8", + "screw_9", + "screw_10", + "vacuum", + "magnetic_tool", + "sealant_tool", + "torque_tool" + ], + "targets": [], + "required_tools": [], + "preconditions": [], + "result": "empty, holding, placed or unknown ownership" + }, + "notify_operator": { + "object_types": [ + "service_object" + ], + "target_types": [], + "objects": [ + "operator" + ], + "targets": [], + "required_tools": [], + "preconditions": [], + "result": "operator notified; no motion" + }, + "dispense": { + "object_types": [ + "tool" + ], + "target_types": [ + "contour" + ], + "objects": [ + "sealant_tool" + ], + "targets": [ + "side_1", + "side_2", + "side_3", + "side_4" + ], + "required_tools": [ + "sealant_tool" + ], + "preconditions": [ + "sealant_tool is mounted", + "target is an existing contour side_i" + ], + "result": "continuous LIN segment along the named contour side" + }, + "dock_tool": { + "object_types": [ + "tool" + ], + "target_types": [ + "tool_socket" + ], + "objects": [ + "vacuum", + "magnetic_tool", + "sealant_tool", + "torque_tool" + ], + "targets": [ + "vacuum_socket", + "magnetic_tool_socket", + "sealant_tool_socket", + "torque_tool_socket" + ], + "required_tools": [], + "preconditions": [ + "the named tool is mounted", + "transfer(, _socket) precedes dock_tool", + "target is that tool's own socket" + ], + "result": "tool returned to its own socket" + }, + "handoff_screw": { + "object_types": [ + "screw" + ], + "target_types": [ + "hole" + ], + "objects": [ + "screw_1", + "screw_2", + "screw_3", + "screw_4", + "screw_5", + "screw_6", + "screw_7", + "screw_8", + "screw_9", + "screw_10" + ], + "targets": [ + "hole_1", + "hole_2", + "hole_3", + "hole_4" + ], + "required_tools": [ + "magnetic_tool" + ], + "preconditions": [ + "the screw is held by the magnetic tool" + ], + "result": "screw handed to the virtual operator at the hole; the hole stays empty, no insertion or tightening" + }, + "pick": { + "object_types": [ + "hatch", + "screw", + "tool" + ], + "target_types": [], + "objects": [ + "cap", + "screw_1", + "screw_2", + "screw_3", + "screw_4", + "screw_5", + "screw_6", + "screw_7", + "screw_8", + "screw_9", + "screw_10", + "vacuum", + "magnetic_tool", + "sealant_tool", + "torque_tool" + ], + "targets": [], + "required_tools": [], + "preconditions": [ + "tool socket is free for a tool", + "gripper is empty" + ], + "result": "alias of grab", + "required_tool_by_kind": { + "hatch": "vacuum", + "screw": "magnetic_tool" + } + }, + "move_to_storage": { + "object_types": [ + "hatch", + "screw" + ], + "target_types": [ + "workspace", + "seat", + "hole" + ], + "objects": [ + "cap", + "screw_1", + "screw_2", + "screw_3", + "screw_4", + "screw_5", + "screw_6", + "screw_7", + "screw_8", + "screw_9", + "screw_10" + ], + "targets": [ + "work", + "hatch_seat", + "hole_1", + "hole_2", + "hole_3", + "hole_4" + ], + "required_tools": [], + "preconditions": [ + "a hatch or screw is held", + "this alias is not valid for a tool" + ], + "result": "held part lowered at work, hatch_seat or a hole", + "forbidden_targets": [ + "hatch_seat" + ] + }, + "inspect": { + "object_types": [ + "hatch", + "screw" + ], + "target_types": [ + "seat", + "hole" + ], + "objects": [ + "cap", + "screw_1", + "screw_2", + "screw_3", + "screw_4", + "screw_5", + "screw_6", + "screw_7", + "screw_8", + "screw_9", + "screw_10" + ], + "targets": [ + "hatch_seat", + "hole_1", + "hole_2", + "hole_3", + "hole_4" + ], + "required_tools": [], + "preconditions": [], + "result": "alias of verify" + } + }, + "service_objects": { + "robot": "робот", + "workspace": "рабочая зона", + "camera": "камера", + "operator": "виртуальный оператор" + }, + "recovery_attempts_per_component": 1 +} diff --git a/src/iiwa_scene_context/iiwa_scene_context/catalog.py b/src/iiwa_scene_context/iiwa_scene_context/catalog.py index b6f32b6..4a0f446 100644 --- a/src/iiwa_scene_context/iiwa_scene_context/catalog.py +++ b/src/iiwa_scene_context/iiwa_scene_context/catalog.py @@ -13,9 +13,9 @@ from typing import Any from iiwa_experement.scenario import Tool, ScenarioSpec -SCENE_SCHEMA_VERSION = 1 -CONTRACT_VERSION = 1 -CATALOG_VERSION = 1 +SCENE_SCHEMA_VERSION = 2 +CONTRACT_VERSION = 2 +CATALOG_VERSION = 2 NATIVE_SKILLS = ( 'detect', 'approach', 'lower', 'grab', 'lift', 'transfer', 'align', @@ -41,6 +41,7 @@ TOOL_KINDS = { _TOOL_SOCKET_SUFFIX = '_socket' _ALLOWED_HOLE_IDS = ('hole_1', 'hole_2', 'hole_3', 'hole_4') _ALLOWED_SEALANT_SIDES = ('side_1', 'side_2', 'side_3', 'side_4') +CAMERA_IDS = ('top', 'hand') class CatalogError(ValueError): @@ -206,10 +207,9 @@ class SkillContract: outcomes: dict[str, str] def as_dict(self) -> dict[str, Any]: - return { - 'description': self.description, - 'outcomes': dict(self.outcomes), - } + # Descriptions live once in action_rules; the contract only needs the + # event names so the graph and the validator agree on the edges. + return {'outcomes': dict(self.outcomes)} @dataclass(frozen=True) @@ -220,15 +220,29 @@ class SkillCatalog: catalog_version: int = CATALOG_VERSION profile: str = 'grant' - def skill_names(self) -> tuple[str, ...]: - return AVAILABLE_SKILLS + def skill_names(self, spec: ScenarioSpec | None = None) -> tuple[str, ...]: + """Skills that the real executor can run in this selected scene.""" + names = list(AVAILABLE_SKILLS) + if spec is not None: + if not spec.sealant_sides or 'sealant_tool' not in spec.tool_names: + names.remove('dispense') + if not spec.tools: + names.remove('dock_tool') + if not spec.screws: + names.remove('handoff_screw') + if spec.cap_state == 'absent' and not spec.screws: + names.remove('move_to_storage') + return tuple(names) - def skill_contracts(self) -> dict[str, dict[str, Any]]: + def skill_contracts( + self, + spec: ScenarioSpec | None = None, + ) -> dict[str, dict[str, Any]]: return { name: SkillContract( name, SKILL_DESCRIPTIONS[name], SKILL_OUTCOMES[name], ).as_dict() - for name in AVAILABLE_SKILLS + for name in self.skill_names(spec) } def outcomes(self, skill: str) -> dict[str, str]: @@ -322,6 +336,261 @@ class SkillCatalog: if side not in targets: raise CatalogError(f'sealant side {side!r} is not a destination') + # ------------------------------------------------------------------ rules + def _socket_ids(self, spec: ScenarioSpec) -> list[str]: + return [self.tool_socket_id(tool.name) for tool in spec.tools] + + def _screw_ids(self, spec: ScenarioSpec) -> list[str]: + return [screw.name for screw in spec.screws] + + def _component_kinds(self, spec: ScenarioSpec) -> list[str]: + return sorted({ + self.component_kind(object_id, spec) + for object_id in self.component_ids(spec) + }) + + def _destination_kinds(self, spec: ScenarioSpec) -> list[str]: + return sorted({ + self.destination_kind(destination_id, spec) + for destination_id in self.destination_ids(spec) + }) + + def action_rules(self, spec: ScenarioSpec) -> dict[str, dict[str, Any]]: + """Compact machine-readable action rules for the selected scene. + + The lists contain only ids that are present in this experiment, so the + external planner cannot advertise an action on a missing object. + """ + components = list(self.component_ids(spec)) + tools = list(self.tool_ids(spec)) + destinations = list(self.destination_ids(spec)) + screws = self._screw_ids(spec) + sockets = self._socket_ids(spec) + holes = [name for name in _ALLOWED_HOLE_IDS if name in destinations] + sides = list(spec.sealant_sides) + cap_present = spec.cap_state != 'absent' + object_components = components + tools + + def rule( + object_types: list[str], + target_types: list[str], + objects: list[str], + targets: list[str], + *, + required_tools: list[str] | None = None, + required_tool_by_kind: dict[str, str] | None = None, + preconditions: list[str] | None = None, + result: str = '', + targets_by_object_kind: dict[str, list[str]] | None = None, + forbidden_targets: list[str] | None = None, + ) -> dict[str, Any]: + entry: dict[str, Any] = { + 'object_types': object_types, + 'target_types': target_types, + 'objects': objects, + 'targets': targets, + 'required_tools': required_tools or [], + 'preconditions': preconditions or [], + 'result': result, + } + if required_tool_by_kind: + entry['required_tool_by_kind'] = required_tool_by_kind + if targets_by_object_kind: + entry['targets_by_object_kind'] = targets_by_object_kind + if forbidden_targets: + entry['forbidden_targets'] = forbidden_targets + return entry + + rules: dict[str, dict[str, Any]] = { + 'detect': rule( + self._component_kinds(spec) + ['tool'], + self._destination_kinds(spec), + object_components, + destinations, + result='fresh pose of a present object or destination', + ), + 'approach': rule( + self._component_kinds(spec) + ['tool'], + self._destination_kinds(spec) + ['component', 'tool'], + object_components, + destinations + components + tools, + result='tcp above the target at approach height', + ), + 'lower': rule( + self._component_kinds(spec) + ['tool'], + self._destination_kinds(spec) + ['component', 'tool'], + object_components, + destinations + components + tools, + result='tcp at the axial contact pose', + ), + 'lift': rule( + self._component_kinds(spec) + ['tool'], + self._destination_kinds(spec) + ['component', 'tool'], + object_components, + destinations + components + tools, + result='tcp at the saved approach height', + ), + 'retreat': rule( + self._component_kinds(spec) + ['tool'], + self._destination_kinds(spec) + ['component', 'tool'], + object_components, + destinations + components + tools, + result='tcp at work after an axial withdrawal', + ), + 'grab': rule( + ['hatch', 'screw', 'tool'], [], + (['cap'] if cap_present else []) + screws + tools, + [], + required_tool_by_kind={ + 'hatch': 'vacuum', + 'screw': 'magnetic_tool', + }, + preconditions=[ + 'tool socket is free for a tool', + 'gripper is empty', + ], + result='tool mounted or part attached to the mounted tool', + ), + 'pick': rule( + ['hatch', 'screw', 'tool'], [], + (['cap'] if cap_present else []) + screws + tools, + [], + required_tool_by_kind={ + 'hatch': 'vacuum', + 'screw': 'magnetic_tool', + }, + preconditions=[ + 'tool socket is free for a tool', + 'gripper is empty', + ], + result='alias of grab', + ), + 'transfer': rule( + self._component_kinds(spec) + ['tool'], + self._destination_kinds(spec), + object_components, + destinations, + preconditions=['object is held or tool is mounted'], + result='tcp above the destination at transfer height', + targets_by_object_kind={ + 'tool': ['work'] + sockets, + 'hatch': ['work', 'hatch_seat'], + 'screw': ['work'] + holes, + }, + ), + 'move_to_storage': rule( + ['hatch', 'screw'], + ['workspace', 'seat', 'hole'], + (['cap'] if cap_present else []) + screws, + ['work'] + (['hatch_seat'] if cap_present else []) + holes, + preconditions=[ + 'a hatch or screw is held', + 'this alias is not valid for a tool', + ], + result='held part lowered at work, hatch_seat or a hole', + forbidden_targets=['hatch_seat'] if 'sealant_tool' in tools else [], + ), + 'align': rule( + self._component_kinds(spec) + ['tool'], + self._destination_kinds(spec), + object_components, + destinations, + preconditions=['a part is held or a tool is mounted'], + result='orientation aligned above the target', + ), + 'release': rule( + ['hatch', 'screw', 'tool'], + ['seat', 'hole', 'tool_socket'], + (['cap'] if cap_present else []) + screws + tools, + (['hatch_seat'] if cap_present else []) + holes + sockets, + preconditions=[ + 'release(tool) requires the matching _socket', + 'an explicit transfer to the socket precedes dock_tool', + ], + result=( + 'screw handed to the virtual operator at a hole; ' + 'no insertion or tightening' + ), + ), + 'capture': rule( + ['camera'], [], list(CAMERA_IDS), [], + result='a fresh frame is available', + ), + 'verify': rule( + ['hatch', 'screw'], + ['seat', 'hole'], + (['cap'] if cap_present else []) + screws, + (['hatch_seat'] if cap_present else []) + holes, + result='measured object state matches the expected target', + ), + 'inspect': rule( + ['hatch', 'screw'], + ['seat', 'hole'], + (['cap'] if cap_present else []) + screws, + (['hatch_seat'] if cap_present else []) + holes, + result='alias of verify', + ), + 'stop': rule(['service_object'], [], ['robot'], [], + result='motion stopped, grasp preserved'), + 'wait_scene_clear': rule( + ['service_object'], [], ['workspace'], [], + result='continue only after a bounded clear condition', + ), + 'inspect_state': rule( + self._component_kinds(spec) + ['tool'], [], + object_components, [], + result='empty, holding, placed or unknown ownership', + ), + 'notify_operator': rule( + ['service_object'], [], ['operator'], [], + result='operator notified; no motion', + ), + } + if sides and 'sealant_tool' in tools: + rules['dispense'] = rule( + ['tool'], + ['contour'], + ['sealant_tool'], + sides, + required_tools=['sealant_tool'], + preconditions=[ + 'sealant_tool is mounted', + 'target is an existing contour side_i', + ], + result='continuous LIN segment along the named contour side', + ) + if tools: + rules['dock_tool'] = rule( + ['tool'], + ['tool_socket'], + tools, + sockets, + preconditions=[ + 'the named tool is mounted', + 'transfer(, _socket) precedes dock_tool', + "target is that tool's own socket", + ], + result='tool returned to its own socket', + ) + if screws: + rules['handoff_screw'] = rule( + ['screw'], + ['hole'], + screws, + holes, + required_tools=['magnetic_tool'], + preconditions=['the screw is held by the magnetic tool'], + result=( + 'screw handed to the virtual operator at the hole; ' + 'the hole stays empty, no insertion or tightening' + ), + ) + return { + name: rules[name] + for name in self.skill_names(spec) + if name in rules + } + DEFAULT_CATALOG = SkillCatalog() diff --git a/src/iiwa_scene_context/iiwa_scene_context/node.py b/src/iiwa_scene_context/iiwa_scene_context/node.py index 45a7b49..c8384a0 100644 --- a/src/iiwa_scene_context/iiwa_scene_context/node.py +++ b/src/iiwa_scene_context/iiwa_scene_context/node.py @@ -13,6 +13,7 @@ from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy from std_msgs.msg import String from std_srvs.srv import Trigger +from iiwa_experement.geometry import GeometrySpec, load_geometry from iiwa_experement.scenario import ScenarioSpec, load_scenario from .catalog import SkillCatalog, DEFAULT_CATALOG @@ -48,9 +49,11 @@ class SceneStore: catalog: SkillCatalog = DEFAULT_CATALOG, *, freshness_sec: float = DEFAULT_FRESHNESS_SEC, + geometry: GeometrySpec | None = None, ) -> None: self.spec = spec self.catalog = catalog + self.geometry = geometry self.freshness_sec = float(freshness_sec) self.scene_ready = False self.ready_message = 'scene not ready' @@ -103,8 +106,13 @@ class SceneStore: assert self.state is not None and self.received_at is not None assert self._last_version is not None envelope = build_envelope( - build_scene(self.spec, self.state, self.catalog), - scene_revision=self.scene_revision, + build_scene( + self.spec, + self.state, + self.catalog, + self.geometry, + stamp=float(self.received_at), + ), state_version=self._last_version, stamp=float(self.received_at), experiment_id=self.spec.experiment_id, @@ -125,19 +133,24 @@ class SceneContextNode(Node): self.declare_parameter( 'scene_config', str(package_share / 'config' / 'scenes.yaml'), ) + self.declare_parameter( + 'geometry_path', str(package_share / 'config' / 'geometry.yaml'), + ) self.declare_parameter('freshness_sec', DEFAULT_FRESHNESS_SEC) self.declare_parameter('publish_period_sec', 0.5) self.declare_parameter('ready_poll_period_sec', 1.0) experiment_id = int(self.get_parameter('experiment_id').value) scene_config = Path(str(self.get_parameter('scene_config').value)) + geometry_path = Path(str(self.get_parameter('geometry_path').value)) self.spec = load_scenario(experiment_id, scene_config) self.store = SceneStore( self.spec, DEFAULT_CATALOG, freshness_sec=float(self.get_parameter('freshness_sec').value), + geometry=load_geometry(geometry_path), ) - self._last_published: tuple[int, int] | None = None + self._last_published: tuple[int, ...] | None = None self._last_publish_time = -1.0 self._ready_client = self.create_client(Trigger, READY_SERVICE) self._ready_request_pending = False @@ -199,7 +212,7 @@ class SceneContextNode(Node): envelope, _ = self.store.snapshot(self._now()) if envelope is None: return - key = (int(envelope['scene_revision']), int(envelope['state_version'])) + key = (int(envelope['state_version']),) now = self._now() period = max( 0.05, float(self.get_parameter('publish_period_sec').value), diff --git a/src/iiwa_scene_context/iiwa_scene_context/snapshot.py b/src/iiwa_scene_context/iiwa_scene_context/snapshot.py index b140650..099c9a9 100644 --- a/src/iiwa_scene_context/iiwa_scene_context/snapshot.py +++ b/src/iiwa_scene_context/iiwa_scene_context/snapshot.py @@ -1,13 +1,27 @@ """Pure snapshot builders for the external planner contract. -Coordinates never leave this module's inputs: the model receives logical ids, -logical ownership and camera names only. Exact poses are resolved at skill -execution time from the live supervisor. +The snapshot is a live view of the selected experiment: only objects that the +Webots supervisor really created are listed, every pose carries its frame, +source and validity stamp, and derived goals use the same geometry module as +the graph executor. """ from __future__ import annotations +from pathlib import Path from typing import Any, Mapping +import numpy as np +from scipy.spatial.transform import Rotation + +from iiwa_experement.geometry import ( + GeometrySpec, + Transform, + load_geometry, + object_pose_transform, + sealant_point, + tcp_goal, + world_target, +) from iiwa_experement.scenario import ScenarioSpec from .catalog import ( @@ -41,6 +55,41 @@ SERVICE_OBJECTS = { 'operator': 'виртуальный оператор', } +POSE_CONVENTIONS = { + 'frame_id': 'world', + 'length_unit': 'm', + 'orientation': 'quaternion_xyzw', +} + +_OBJECT_DEFS = { + 'fixture': 'grant_fixture', + 'body': 'grant_body', + 'tray': 'grant_tray', + 'cap': 'grant_cap', +} + + +def _default_geometry() -> GeometrySpec: + """Locate geometry.yaml in an installed or source checkout.""" + candidates: list[Path] = [] + try: + from ament_index_python.packages import get_package_share_directory + + candidates.append( + Path(get_package_share_directory('iiwa_experement')) + / 'config' / 'geometry.yaml' + ) + except Exception: + pass + candidates.append( + Path(__file__).resolve().parents[2] + / 'iiwa_experement' / 'config' / 'geometry.yaml' + ) + for path in candidates: + if path.is_file(): + return load_geometry(path) + raise FileNotFoundError('geometry.yaml is unavailable for the scene snapshot') + def _state_mapping(supervisor_state: Mapping[str, Any] | None) -> dict[str, Any]: if supervisor_state is None: @@ -50,6 +99,69 @@ def _state_mapping(supervisor_state: Mapping[str, Any] | None) -> dict[str, Any] return dict(supervisor_state) +def _transform(entry: Any, child: str) -> Transform | None: + if not isinstance(entry, Mapping): + return None + xyz = entry.get('xyz') + rotation = entry.get('rotation') + if not isinstance(xyz, (list, tuple)) or len(xyz) != 3: + return None + if not isinstance(rotation, (list, tuple)) or len(rotation) != 3: + return None + if any(not isinstance(row, (list, tuple)) or len(row) != 3 for row in rotation): + return None + try: + values = np.asarray(rotation, dtype=float) + translation = np.asarray(xyz, dtype=float) + except (TypeError, ValueError): + return None + if not np.all(np.isfinite(values)) or not np.all(np.isfinite(translation)): + return None + return Transform( + xyz=tuple(float(value) for value in translation), + rotation=tuple(tuple(float(value) for value in row) for row in values), + parent='world', + child=child, + ) + + +def _pose_json( + transform: Transform | None, + *, + source: str, + stamp: float, +) -> dict[str, Any] | None: + if transform is None: + return None + position = np.asarray(transform.xyz, dtype=float) + matrix = np.asarray(transform.rotation, dtype=float) + if position.shape != (3,) or matrix.shape != (3, 3): + raise ValueError('pose must contain a 3-vector and a 3x3 rotation') + if not np.all(np.isfinite(position)) or not np.all(np.isfinite(matrix)): + raise ValueError('pose contains a non-finite number') + quaternion = Rotation.from_matrix(matrix).as_quat() + norm = float(np.linalg.norm(quaternion)) + if not np.isfinite(norm) or norm < 1e-9: + raise ValueError('pose quaternion is not normalizable') + quaternion = quaternion / norm + return { + 'frame_id': 'world', + 'position': { + 'x': float(position[0]), + 'y': float(position[1]), + 'z': float(position[2]), + }, + 'orientation': { + 'x': float(quaternion[0]), + 'y': float(quaternion[1]), + 'z': float(quaternion[2]), + 'w': float(quaternion[3]), + }, + 'source': source, + 'stamp': float(stamp), + } + + def _flags(state: Mapping[str, Any]) -> list[str]: flags: list[str] = [] mounted = state.get('mounted_tool') @@ -116,29 +228,183 @@ def _hole_state( } +def _object_pose( + state: Mapping[str, Any], + definition: str, + fallback: Transform, + *, + stamp: float, +) -> tuple[Transform, str]: + live = _transform((state.get('objects') or {}).get(definition), definition) + if live is not None: + return live, 'webots_supervisor' + return fallback, 'scenario_manifest' + + +def _component_pose( + spec: ScenarioSpec, + geometry: GeometrySpec, + state: Mapping[str, Any], + object_id: str, + kind: str, + stamp: float, +) -> tuple[Transform | None, str]: + if kind == 'fixture': + fallback = object_pose_transform( + spec.fixture.x, spec.fixture.y, spec.fixture.z, + spec.fixture.yaw_deg, child='fixture', + ) + return _object_pose(state, 'grant_fixture', fallback, stamp=stamp) + if kind == 'body': + fallback = object_pose_transform( + spec.body.x, spec.body.y, spec.body.z, + spec.body.yaw_deg, child='body', + ) + return _object_pose(state, 'grant_body', fallback, stamp=stamp) + if kind == 'tray': + assert spec.tray_pose is not None + fallback = object_pose_transform( + spec.tray_pose.x, spec.tray_pose.y, spec.tray_pose.z, + spec.tray_pose.yaw_deg, child='tray', + ) + return _object_pose(state, 'grant_tray', fallback, stamp=stamp) + if kind == 'hatch': + live = _transform((state.get('cap') or {}).get('pose'), 'cap') + if live is not None: + return live, 'webots_supervisor' + return world_target(spec, geometry, 'cap'), 'scenario_manifest' + if kind == 'screw': + live = _transform( + (state.get('screws') or {}).get(object_id, {}).get('pose'), + object_id, + ) + if live is not None: + return live, 'webots_supervisor' + return world_target(spec, geometry, f'screw:{object_id}'), 'scenario_manifest' + return None, 'scenario_manifest' + + +def _tool_pose( + spec: ScenarioSpec, + geometry: GeometrySpec, + state: Mapping[str, Any], + tool_name: str, + stamp: float, +) -> tuple[Transform | None, str]: + live = _transform( + (state.get('tools') or {}).get(tool_name, {}).get('pose'), + tool_name, + ) + if live is not None: + return live, 'webots_supervisor' + fallback = world_target(spec, geometry, f'tool:{tool_name}') + return _object_pose(state, f'grant_{tool_name}', fallback, stamp=stamp) + + +def _destination_poses( + spec: ScenarioSpec, + geometry: GeometrySpec, + destination_id: str, + kind: str, + stamp: float, +) -> dict[str, Any]: + source = 'executor_geometry' + if kind == 'workspace': + work = geometry.safe_poses['work'] + pose = _pose_json(work, source=source, stamp=stamp) + return {'pose': pose, 'tcp_pose': pose} + if kind == 'seat': + target = world_target(spec, geometry, 'cap_seat') + return { + 'pose': _pose_json(target, source=source, stamp=stamp), + 'tcp_pose': _pose_json( + tcp_goal(target, geometry.cap_tcp_to_node), + source=source, stamp=stamp, + ), + } + if kind == 'hole': + target = world_target(spec, geometry, f'hole:{destination_id}') + return { + 'pose': _pose_json(target, source=source, stamp=stamp), + 'tcp_pose': _pose_json( + tcp_goal(target, geometry.screw_tcp_to_node), + source=source, stamp=stamp, + ), + } + if kind == 'tool_socket': + tool = destination_id[: -len('_socket')] + target = world_target(spec, geometry, f'tool:{tool}') + return { + 'pose': _pose_json(target, source=source, stamp=stamp), + 'tcp_pose': _pose_json( + tcp_goal(target, geometry.tool_tcp_to_nodes[tool]), + source=source, stamp=stamp, + ), + } + if kind == 'contour': + start = sealant_point(geometry, spec, destination_id, 0.0) + end = sealant_point(geometry, spec, destination_id, 1.0) + tip = geometry.tool_work_tips['sealant_tool'] + return { + 'start': _pose_json(start, source=source, stamp=stamp), + 'end': _pose_json(end, source=source, stamp=stamp), + 'start_tcp': _pose_json( + tcp_goal(start, tip), source=source, stamp=stamp, + ), + 'end_tcp': _pose_json( + tcp_goal(end, tip), source=source, stamp=stamp, + ), + } + return {} + + def build_scene( spec: ScenarioSpec, supervisor_state: Mapping[str, Any], catalog: SkillCatalog, + geometry: GeometrySpec | None = None, + *, + stamp: float | None = None, ) -> dict[str, Any]: - """Build the coordinate-free ``scene_json`` object for one snapshot.""" + """Build the live ``scene_json`` object for one snapshot.""" state = _state_mapping(supervisor_state) catalog.validate_ids(spec) + if geometry is None: + geometry = _default_geometry() + if stamp is None: + try: + stamp = float(state.get('simulation_time', 0.0)) + except (TypeError, ValueError): + stamp = 0.0 + stamp = float(stamp) mounted = state.get('mounted_tool') held = state.get('held_object') - components = [ - build_component_entry(catalog, spec, object_id, state) - for object_id in catalog.component_ids(spec) - ] - destinations = [ - destination_entry(catalog, spec, destination_id, state) - for destination_id in catalog.destination_ids(spec) - ] - tools = [ - tool_entry(catalog, spec, tool, state) - for tool in spec.tools - ] + components = [] + for object_id in catalog.component_ids(spec): + kind = catalog.component_kind(object_id, spec) + entry = build_component_entry(catalog, spec, object_id, state) + pose, source = _component_pose( + spec, geometry, state, object_id, kind, stamp, + ) + entry['pose'] = _pose_json(pose, source=source, stamp=stamp) + components.append(entry) + + destinations = [] + for destination_id in catalog.destination_ids(spec): + kind = catalog.destination_kind(destination_id, spec) + entry = destination_entry(catalog, spec, destination_id, state) + entry.update(_destination_poses( + spec, geometry, destination_id, kind, stamp, + )) + destinations.append(entry) + + tools = [] + for tool in spec.tools: + entry = tool_entry(catalog, spec, tool, state) + pose, source = _tool_pose(spec, geometry, state, tool.name, stamp) + entry['pose'] = _pose_json(pose, source=source, stamp=stamp) + tools.append(entry) initial_state = { 'tool_mounted': mounted if isinstance(mounted, str) else None, @@ -148,6 +414,7 @@ def build_scene( 'tools': _tool_state(catalog, spec, state), 'holes': _hole_state(spec, state), 'scene_ready': bool(state.get('ready', True)), + 'stamp': stamp, } return { @@ -157,13 +424,15 @@ def build_scene( 'profile': catalog.profile, 'experiment_id': int(spec.experiment_id), 'domain': 'grant_assembly', + 'pose_conventions': dict(POSE_CONVENTIONS), 'components': components, 'destinations': destinations, 'tools': tools, 'initial_state': initial_state, 'cameras': DEFAULT_CAMERAS, - 'available_skills': list(catalog.skill_names()), - 'skill_contracts': catalog.skill_contracts(), + 'available_skills': list(catalog.skill_names(spec)), + 'skill_contracts': catalog.skill_contracts(spec), + 'action_rules': catalog.action_rules(spec), 'service_objects': dict(SERVICE_OBJECTS), 'recovery_attempts_per_component': 1, } @@ -172,14 +441,11 @@ def build_scene( def build_envelope( scene: Mapping[str, Any], *, - scene_revision: int, state_version: int, stamp: float, experiment_id: int | None = None, ) -> dict[str, Any]: """Wrap ``scene_json`` in the transport envelope used by every interface.""" - if scene_revision < 0: - raise ValueError('scene_revision must be non-negative') if state_version < 0: raise ValueError('state_version must be non-negative') envelope = { @@ -192,7 +458,6 @@ def build_envelope( else int(scene.get('experiment_id', 0)) ), 'state_version': int(state_version), - 'scene_revision': int(scene_revision), 'timestamp': float(stamp), 'scene_json': dict(scene), } diff --git a/src/iiwa_scene_context/package.xml b/src/iiwa_scene_context/package.xml index 889661a..61ae508 100644 --- a/src/iiwa_scene_context/package.xml +++ b/src/iiwa_scene_context/package.xml @@ -11,6 +11,8 @@ ament_index_python iiwa_experement + python3-numpy + python3-scipy python3-yaml rclpy std_msgs diff --git a/src/iiwa_scene_context/setup.py b/src/iiwa_scene_context/setup.py index c9524c9..2698bce 100644 --- a/src/iiwa_scene_context/setup.py +++ b/src/iiwa_scene_context/setup.py @@ -12,6 +12,7 @@ setup( ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), ('share/' + package_name + '/doc', glob('docs/*.md')), + ('share/' + package_name + '/doc', glob('docs/*.json')), ], install_requires=['setuptools', 'PyYAML'], tests_require=['pytest'], diff --git a/src/iiwa_scene_context/test/test_node.py b/src/iiwa_scene_context/test/test_node.py index 4cb01be..34db1e2 100644 --- a/src/iiwa_scene_context/test/test_node.py +++ b/src/iiwa_scene_context/test/test_node.py @@ -1,10 +1,11 @@ -"""Readiness, freshness and scene_revision rules of the snapshot node.""" +"""Readiness, freshness and internal scene version rules of the snapshot node.""" from __future__ import annotations from pathlib import Path from std_srvs.srv import Trigger +from iiwa_experement.geometry import load_geometry from iiwa_experement.scenario import load_scenario from iiwa_scene_context.node import SceneStore, SceneContextNode, scene_qos from iiwa_scene_context.snapshot import build_scene @@ -12,6 +13,7 @@ from iiwa_scene_context.snapshot import build_scene PACKAGE = Path(__file__).resolve().parents[1] SCENES = PACKAGE.parent / 'iiwa_experement' / 'config' / 'scenes.yaml' +GEOMETRY = load_geometry(PACKAGE.parent / 'iiwa_experement' / 'config' / 'geometry.yaml') def state(version: int, *, mounted=None, held=None): @@ -29,7 +31,7 @@ def state(version: int, *, mounted=None, held=None): def make_store(): spec = load_scenario(1, SCENES) - return SceneStore(spec, freshness_sec=2.0) + return SceneStore(spec, freshness_sec=2.0, geometry=GEOMETRY) def test_snapshot_requires_scene_ready_and_fresh_state(): @@ -49,22 +51,23 @@ def test_snapshot_requires_scene_ready_and_fresh_state(): assert 'stale' in reason -def test_revision_advances_on_state_version_but_not_on_pose_updates(): +def test_internal_version_advances_on_state_version_but_not_on_pose_updates(): store = make_store() store.set_scene_ready(True) store.update(state(1), received_at=10.0) - assert store.snapshot(10.1)[0]['scene_revision'] == 1 + assert store.scene_revision == 1 + assert 'scene_revision' not in store.snapshot(10.1)[0] # A pose-only publish of the same held object carries the same version. store.update(state(1, mounted='vacuum', held='cap'), received_at=10.5) - assert store.snapshot(10.6)[0]['scene_revision'] == 1 + assert store.scene_revision == 1 # A confirmed ownership change increments the supervisor version. store.update(state(2, mounted='vacuum', held='cap'), received_at=11.0) - assert store.snapshot(11.1)[0]['scene_revision'] == 2 + assert store.scene_revision == 2 store.update(state(3, mounted='vacuum', held=None), received_at=11.5) - assert store.snapshot(11.6)[0]['scene_revision'] == 3 + assert store.scene_revision == 3 def test_late_subscriber_qos_is_transient_local_and_depth_one(): @@ -91,7 +94,8 @@ def test_scene_service_returns_json_or_a_reason(): result = SceneContextNode._handle_get_scene(node, None, response) assert result.success is True assert '"scene_json"' in result.message - assert '"scene_revision": 1' in result.message + assert '"scene_revision"' not in result.message + assert '"pose_conventions"' in result.message def test_snapshot_scene_is_the_catalog_scene_not_the_state(): @@ -99,9 +103,12 @@ def test_snapshot_scene_is_the_catalog_scene_not_the_state(): store.set_scene_ready(True) store.update(state(1), received_at=10.0) envelope, _ = store.snapshot(10.1) - expected = build_scene(store.spec, store.state, store.catalog) + expected = build_scene( + store.spec, store.state, store.catalog, store.geometry, + stamp=store.received_at, + ) assert envelope['scene_json'] == expected - assert 'pose' not in envelope['scene_json'] + assert 'objects' not in envelope['scene_json'] def test_scene_topic_is_republished_to_keep_the_timestamp_fresh(): diff --git a/src/iiwa_scene_context/test/test_snapshot.py b/src/iiwa_scene_context/test/test_snapshot.py index dab466a..89342a2 100644 --- a/src/iiwa_scene_context/test/test_snapshot.py +++ b/src/iiwa_scene_context/test/test_snapshot.py @@ -1,10 +1,13 @@ -"""Scene snapshots expose only present ids and logical state.""" +"""Scene snapshots expose only present ids, live poses and action rules.""" from __future__ import annotations +import json +import math from pathlib import Path import pytest +from iiwa_experement.geometry import load_geometry from iiwa_experement.scenario import load_scenario from iiwa_scene_context.catalog import DEFAULT_CATALOG from iiwa_scene_context.snapshot import build_envelope, build_scene @@ -13,9 +16,10 @@ from iiwa_scene_context.snapshot import build_envelope, build_scene PACKAGE = Path(__file__).resolve().parents[1] ROOT = PACKAGE.parent SCENES = ROOT / 'iiwa_experement' / 'config' / 'scenes.yaml' +GEOMETRY = load_geometry(ROOT / 'iiwa_experement' / 'config' / 'geometry.yaml') FORBIDDEN_KEYS = { - 'pose', 'poses', 'xyz', 'rotation', 'tcp', 'objects', 'simulation_time', - 'last_command_result', 'last_error', 'attached', 'version', + 'simulation_time', 'last_command_result', 'last_error', + 'attached', 'scene_revision', } @@ -51,6 +55,7 @@ def state_for(spec, *, version=3, mounted=None, held=None, cap=None, 'tools': tools, 'screws': screw_entries, 'holes': hole_entries, + 'simulation_time': 12.5, } @@ -68,10 +73,16 @@ def walk_keys(value): yield from walk_keys(item) +def quaternion_norm(orientation): + return math.sqrt(sum( + float(orientation[axis]) ** 2 for axis in ('x', 'y', 'z', 'w') + )) + + @pytest.mark.parametrize('experiment_id', [1, 20]) def test_scene_lists_only_present_objects_and_goals(experiment_id): spec = spec_for(experiment_id) - scene = build_scene(spec, state_for(spec), DEFAULT_CATALOG) + scene = build_scene(spec, state_for(spec), DEFAULT_CATALOG, GEOMETRY) components = ids(scene['components']) destinations = ids(scene['destinations']) assert 'fixture' in components and 'body' in components @@ -97,11 +108,46 @@ def test_scene_lists_only_present_objects_and_goals(experiment_id): assert len([name for name in components if name.startswith('screw_')]) >= 10 +@pytest.mark.parametrize('experiment_id', range(1, 21)) +def test_every_experiment_scene_matches_the_created_scene(experiment_id): + spec = spec_for(experiment_id) + scene = build_scene(spec, state_for(spec), DEFAULT_CATALOG, GEOMETRY) + expected_components = {'fixture', 'body'} + if spec.cap_state != 'absent': + expected_components.add('cap') + if spec.tray_pose is not None: + expected_components.add('tray') + expected_components.update( + f'screw_{index}' for index in range(1, 11) + ) + assert set(ids(scene['components'])) == expected_components + assert set(ids(scene['tools'])) == set(spec.tool_names) + assert set(ids(scene['destinations'])) == { + 'work', 'hatch_seat', 'hole_1', 'hole_2', 'hole_3', 'hole_4', + *[f'{tool.name}_socket' for tool in spec.tools], + *spec.sealant_sides, + } + assert set(scene['available_skills']) == set(scene['action_rules']) + for rule in scene['action_rules'].values(): + assert set(rule['objects']) <= set(scene['service_objects']) | set( + ids(scene['components']) + ) | set(ids(scene['tools'])) | set(scene['cameras']) + assert set(rule['targets']) <= set(ids(scene['destinations'])) | set( + ids(scene['components']) + ) | set(ids(scene['tools'])) + if 'dispense' in scene['available_skills']: + assert scene['action_rules']['dispense']['targets'] == list( + spec.sealant_sides + ) + else: + assert not spec.sealant_sides + + def test_each_present_tool_has_a_named_socket_and_stable_ids(): for experiment_id in (1, 20): spec = spec_for(experiment_id) - first = build_scene(spec, state_for(spec), DEFAULT_CATALOG) - second = build_scene(spec, state_for(spec), DEFAULT_CATALOG) + first = build_scene(spec, state_for(spec), DEFAULT_CATALOG, GEOMETRY) + second = build_scene(spec, state_for(spec), DEFAULT_CATALOG, GEOMETRY) assert first['components'] == second['components'] assert first['destinations'] == second['destinations'] tool_ids = [tool['id'] for tool in first['tools']] @@ -111,19 +157,70 @@ def test_each_present_tool_has_a_named_socket_and_stable_ids(): assert tool['socket'] in ids(first['destinations']) -def test_scene_json_has_no_coordinates_or_supervisor_internals(): +def test_scene_json_has_live_poses_and_no_supervisor_internals(): spec = spec_for(20) state = state_for(spec, mounted='vacuum', held='cap', cap={'state': 'held'}) - state['tcp'] = {'xyz': [1, 2, 3]} - state['objects'] = {'grant_cap': {'xyz': [0, 0, 0]}} - state['simulation_time'] = 12.5 - scene = build_scene(spec, state, DEFAULT_CATALOG) + scene = build_scene(spec, state, DEFAULT_CATALOG, GEOMETRY) keys = set(walk_keys(scene)) assert not (keys & FORBIDDEN_KEYS) assert 'timestamp' not in keys assert scene['experiment_id'] == 20 - assert scene['contract_version'] == 1 - assert scene['catalog_version'] == 1 + assert scene['contract_version'] == 2 + assert scene['catalog_version'] == 2 + assert scene['schema_version'] == 2 + assert scene['pose_conventions'] == { + 'frame_id': 'world', + 'length_unit': 'm', + 'orientation': 'quaternion_xyzw', + } + + for entry in scene['components'] + scene['tools']: + pose = entry['pose'] + assert pose['frame_id'] == 'world' + assert pose['stamp'] == 12.5 + assert pose['source'] in ('webots_supervisor', 'scenario_manifest') + assert abs(quaternion_norm(pose['orientation']) - 1.0) < 1e-9 + assert all(math.isfinite(value) for value in pose['position'].values()) + + for entry in scene['destinations']: + if entry['kind'] == 'contour': + assert entry['start']['frame_id'] == 'world' + assert entry['end']['frame_id'] == 'world' + assert entry['start_tcp']['frame_id'] == 'world' + assert entry['end_tcp']['frame_id'] == 'world' + else: + assert entry['pose']['frame_id'] == 'world' + assert entry['tcp_pose']['frame_id'] == 'world' + + +def test_live_supervisor_pose_overrides_the_manifest_pose(): + spec = spec_for(20) + state = state_for(spec) + state['objects'] = { + 'grant_body': { + 'xyz': [0.31, 0.13, 0.82], + 'rotation': [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]], + }, + } + scene = build_scene(spec, state, DEFAULT_CATALOG, GEOMETRY) + body = next(entry for entry in scene['components'] if entry['id'] == 'body') + assert body['pose']['position'] == {'x': 0.31, 'y': 0.13, 'z': 0.82} + assert body['pose']['source'] == 'webots_supervisor' + + +def test_held_object_pose_is_updated_and_marked_live(): + spec = spec_for(20) + state = state_for(spec, mounted='vacuum', held='cap', cap={ + 'state': 'held', + 'pose': { + 'xyz': [0.4, -0.2, 0.9], + 'rotation': [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]], + }, + }) + scene = build_scene(spec, state, DEFAULT_CATALOG, GEOMETRY) + cap = next(entry for entry in scene['components'] if entry['id'] == 'cap') + assert cap['pose']['position'] == {'x': 0.4, 'y': -0.2, 'z': 0.9} + assert cap['pose']['source'] == 'webots_supervisor' def test_logical_state_reflects_the_live_supervisor_snapshot(): @@ -139,6 +236,7 @@ def test_logical_state_reflects_the_live_supervisor_snapshot(): holes={'hole_1': {'state': 'empty', 'screw': 'screw_1'}}, ), DEFAULT_CATALOG, + GEOMETRY, ) initial = scene['initial_state'] assert initial['tool_mounted'] == 'vacuum' @@ -151,25 +249,83 @@ def test_logical_state_reflects_the_live_supervisor_snapshot(): assert 'holding:cap' in initial['flags'] -def test_scene_json_carries_server_skill_contracts_not_model_contracts(): - spec = spec_for(1) - scene = build_scene(spec, state_for(spec), DEFAULT_CATALOG) +def test_scene_json_carries_server_skill_contracts_and_scene_rules(): + spec = spec_for(20) + scene = build_scene(spec, state_for(spec), DEFAULT_CATALOG, GEOMETRY) assert 'grab' in scene['available_skills'] assert 'tighten' not in scene['available_skills'] assert set(scene['skill_contracts']) == set(scene['available_skills']) + assert set(scene['action_rules']) == set(scene['available_skills']) assert scene['skill_contracts']['grab']['outcomes']['grasp_failed'] assert 'grasp_failed' in scene['skill_contracts']['pick']['outcomes'] + grab = scene['action_rules']['grab'] + assert grab['required_tool_by_kind']['hatch'] == 'vacuum' + assert grab['required_tool_by_kind']['screw'] == 'magnetic_tool' + assert set(grab['objects']) == { + 'cap', + *[f'screw_{index}' for index in range(1, 11)], + 'vacuum', 'magnetic_tool', 'sealant_tool', 'torque_tool', + } + + dispense = scene['action_rules']['dispense'] + assert dispense['required_tools'] == ['sealant_tool'] + assert dispense['targets'] == ['side_1', 'side_2', 'side_3', 'side_4'] + + dock = scene['action_rules']['dock_tool'] + assert 'transfer(, _socket) precedes dock_tool' in ( + dock['preconditions'] + ) + assert 'handoff_screw' in scene['action_rules'] + assert 'no insertion or tightening' in ( + scene['action_rules']['handoff_screw']['result'] + ) + assert 'sealant_tool' not in scene['action_rules']['move_to_storage']['objects'] + assert 'hatch_seat' in scene['action_rules']['move_to_storage']['forbidden_targets'] + + +def test_smaller_scene_does_not_advertise_unavailable_actions(): + spec = spec_for(1) + scene = build_scene(spec, state_for(spec), DEFAULT_CATALOG, GEOMETRY) + assert 'dispense' not in scene['available_skills'] + assert 'handoff_screw' not in scene['available_skills'] + assert 'dispense' not in scene['action_rules'] + assert 'side_1' not in ids(scene['destinations']) + assert not any( + entry['id'].startswith('screw_') for entry in scene['components'] + ) + + +def test_experiment_20_example_is_the_generated_live_snapshot(): + example_path = PACKAGE / 'docs' / 'scene_json_experiment_20.json' + example = json.loads(example_path.read_text(encoding='utf-8')) + spec = spec_for(20) + generated = build_scene( + spec, + state_for(spec), + DEFAULT_CATALOG, + GEOMETRY, + stamp=42.0, + ) + assert example == generated + assert example['experiment_id'] == 20 + assert len(example['components']) == 14 + assert len(example['tools']) == 4 + assert len(example['destinations']) == 14 + assert [entry['id'] for entry in example['destinations']].count( + 'side_1' + ) == 1 + def test_envelope_keeps_transport_separate_from_scene(): spec = spec_for(20) - scene = build_scene(spec, state_for(spec), DEFAULT_CATALOG) + scene = build_scene(spec, state_for(spec), DEFAULT_CATALOG, GEOMETRY) envelope = build_envelope( - scene, scene_revision=7, state_version=4, stamp=123.5, + scene, state_version=4, stamp=123.5, ) assert envelope['experiment_id'] == 20 assert envelope['state_version'] == 4 - assert envelope['scene_revision'] == 7 + assert 'scene_revision' not in envelope assert envelope['timestamp'] == 123.5 assert envelope['scene_json'] == scene assert envelope['scene_json'] is not envelope diff --git a/src/iiwa_web/iiwa_web/planner.py b/src/iiwa_web/iiwa_web/planner.py index 21d4706..614199c 100644 --- a/src/iiwa_web/iiwa_web/planner.py +++ b/src/iiwa_web/iiwa_web/planner.py @@ -13,13 +13,14 @@ from fastapi import APIRouter, HTTPException from pydantic import BaseModel, Field from std_srvs.srv import Trigger import webots_ros2_msgs.srv -from iiwa_msgs.srv import GetRunStatus +from iiwa_msgs.srv import GetRunStatus, SubmitPlan from .ros_node import get_bridge SCENE_SERVICE = '/iiwa_scene_context/get_scene' -SUBMIT_SERVICE = '/iiwa_llm_plan_executor/submit' +SUBMIT_SERVICE = '/iiwa_llm_plan_executor/submit_plan' +LEGACY_SUBMIT_SERVICE = '/iiwa_llm_plan_executor/submit' CANCEL_SERVICE = '/iiwa_llm_plan_executor/cancel' STATUS_SERVICE = '/iiwa_llm_plan_executor/get_status' @@ -30,7 +31,6 @@ _active_run_id: str | None = None class RunRequest(BaseModel): run_id: str = Field(..., min_length=1, max_length=128) - scene_revision: int = Field(..., ge=0) response: dict[str, Any] | str = Field( ..., description='The untouched Qwen3 graph response', ) @@ -54,15 +54,14 @@ def _status(run_id: str) -> dict[str, Any] | None: return payload if isinstance(payload, dict) else None -def _rejection(run_id: str) -> tuple[int, str, dict[str, Any] | None]: - status = _status(run_id) - reason = '' if status is None else str(status.get('reason', '')) - lowered = reason.lower() - if 'busy' in lowered: - return 409, reason or 'another graph is already active', status - if 'not_ready' in lowered: - return 503, reason or 'the executor is not ready yet', status - return 422, reason or 'graph was rejected', status +def _http_status(code: str) -> int: + if code in ('busy', 'conflict', 'stale_scene'): + return 409 + if code == 'not_ready': + return 503 + if code == 'invalid': + return 422 + return 422 @router.get('/scene', summary='Текущий scene_json для внешнего планировщика') @@ -87,30 +86,37 @@ def get_scene(): @router.post('/runs', summary='Передать готовый граф Qwen3 исполнителю') def submit_run(request: RunRequest): global _active_run_id - payload = { - 'run_id': request.run_id, - 'scene_revision': int(request.scene_revision), - 'response': request.response, - } + graph = ( + request.response + if isinstance(request.response, str) + else json.dumps(request.response, ensure_ascii=False) + ) try: response = get_bridge().call_service( - webots_ros2_msgs.srv.SetString, + SubmitPlan, SUBMIT_SERVICE, - webots_ros2_msgs.srv.SetString.Request( - value=json.dumps(payload, ensure_ascii=False), + SubmitPlan.Request( + run_id=request.run_id, + response=graph, ), ) except (RuntimeError, TimeoutError) as error: raise HTTPException(503, str(error)) from error - if not response.success: - code, reason, status = _rejection(request.run_id) - raise HTTPException(code, detail=reason) + if not response.accepted: + raise HTTPException( + _http_status(response.code), + detail={ + 'code': response.code, + 'reason': response.reason, + 'run_id': request.run_id, + }, + ) with _lock: _active_run_id = request.run_id return { - 'status': 'accepted', + 'status': 'duplicate' if response.code == 'duplicate' else 'accepted', + 'code': response.code, 'run_id': request.run_id, - 'scene_revision': request.scene_revision, } @@ -122,7 +128,7 @@ def get_run(run_id: str): raise HTTPException(404, f'run {run_id!r} was not submitted') if status.get('state') in ( 'completed', 'replan_required', 'operator_required', 'failed', - 'canceled', 'rejected', + 'canceled', ): with _lock: if _active_run_id == run_id: diff --git a/src/iiwa_web/test/test_planner.py b/src/iiwa_web/test/test_planner.py index 477edae..34fa7ec 100644 --- a/src/iiwa_web/test/test_planner.py +++ b/src/iiwa_web/test/test_planner.py @@ -14,7 +14,6 @@ from iiwa_web import main, planner def envelope(): return { 'experiment_id': 20, - 'scene_revision': 4, 'state_version': 2, 'timestamp': 10.0, 'scene_json': {'experiment_id': 20, 'components': []}, @@ -24,7 +23,9 @@ def envelope(): class FakeBridge: def __init__(self): self.calls = [] - self.submit_success = True + self.submit_accepted = True + self.submit_code = 'accepted' + self.submit_reason = 'graph admitted' self.cancel_success = True self.scene_success = True self.status_payload = { @@ -45,7 +46,12 @@ class FakeBridge: message=json.dumps(envelope()) if self.scene_success else 'scene not ready', ) if srv_name == planner.SUBMIT_SERVICE: - return SimpleNamespace(success=self.submit_success) + return SimpleNamespace( + accepted=self.submit_accepted, + code=self.submit_code, + reason=self.submit_reason, + status_json='{}', + ) if srv_name == planner.CANCEL_SERVICE: return SimpleNamespace(success=self.cancel_success) if srv_name == planner.STATUS_SERVICE: @@ -68,11 +74,12 @@ def bridge(monkeypatch): return fake, TestClient(app) -def test_scene_returns_the_envelope(bridge): +def test_scene_returns_the_envelope_without_scene_revision(bridge): fake, client = bridge response = client.get('/planner/scene') assert response.status_code == 200 - assert response.json()['scene_revision'] == 4 + assert response.json()['state_version'] == 2 + assert 'scene_revision' not in response.json() assert fake.calls[0][0] == planner.SCENE_SERVICE @@ -84,11 +91,10 @@ def test_scene_not_ready_and_service_failure(bridge): assert client.get('/planner/scene').status_code == 503 -def test_submit_accepts_without_waiting_for_completion(bridge): +def test_submit_accepts_without_scene_revision_or_waiting_for_completion(bridge): fake, client = bridge response = client.post('/planner/runs', json={ 'run_id': 'r1', - 'scene_revision': 4, 'response': { 'status': 'ready', 'entry': 'a', @@ -97,39 +103,55 @@ def test_submit_accepts_without_waiting_for_completion(bridge): }) assert response.status_code == 200 assert response.json()['status'] == 'accepted' + assert 'scene_revision' not in response.json() service, request = fake.calls[0] assert service == planner.SUBMIT_SERVICE - payload = json.loads(request.value) - assert payload['run_id'] == 'r1' - assert payload['scene_revision'] == 4 - assert payload['response']['entry'] == 'a' + assert request.run_id == 'r1' + assert json.loads(request.response)['entry'] == 'a' assert planner._active_run_id == 'r1' -def test_submit_conflict_and_not_ready_mapping(bridge): +def test_duplicate_submit_does_not_start_a_second_motion(bridge): fake, client = bridge - fake.submit_success = False - fake.status_payload = { - 'found': True, 'run_id': 'r1', 'state': 'rejected', - 'reason': 'busy: another run is active', - } + fake.submit_code = 'duplicate' response = client.post('/planner/runs', json={ - 'run_id': 'r1', 'scene_revision': 4, + 'run_id': 'r1', + 'response': {'status': 'ready', 'entry': 'a', 'plan': []}, + }) + assert response.status_code == 200 + assert response.json()['status'] == 'duplicate' + + +@pytest.mark.parametrize('code,http_status', [ + ('busy', 409), + ('conflict', 409), + ('stale_scene', 409), + ('not_ready', 503), + ('invalid', 422), + ('rejected', 422), + ('replan_required', 422), +]) +def test_submit_errors_include_the_exact_code_and_reason(bridge, code, http_status): + fake, client = bridge + fake.submit_accepted = False + fake.submit_code = code + fake.submit_reason = f'{code} because of a concrete reason' + response = client.post('/planner/runs', json={ + 'run_id': 'r1', 'response': {'status': 'blocked', 'entry': '', 'plan': []}, }) - assert response.status_code == 409 - assert 'busy' in response.json()['detail'] + assert response.status_code == http_status + detail = response.json()['detail'] + assert detail['code'] == code + assert detail['reason'] == f'{code} because of a concrete reason' + assert detail['run_id'] == 'r1' - fake.status_payload['reason'] = 'not_ready: scene is not ready' - response = client.post('/planner/runs', json={ - 'run_id': 'r2', 'scene_revision': 4, - 'response': {'status': 'blocked', 'entry': '', 'plan': []}, - }) - assert response.status_code == 503 +def test_submit_service_failure_is_503(bridge): + fake, client = bridge fake.raise_on.add(planner.SUBMIT_SERVICE) assert client.post('/planner/runs', json={ - 'run_id': 'r3', 'scene_revision': 4, + 'run_id': 'r3', 'response': {'status': 'blocked', 'entry': '', 'plan': []}, }).status_code == 503 @@ -139,6 +161,7 @@ def test_status_returns_404_for_unknown_run(bridge): response = client.get('/planner/runs/r1') assert response.status_code == 200 assert response.json()['state'] == 'running' + assert 'scene_revision' not in response.json() fake.status_payload = None assert client.get('/planner/runs/nope').status_code == 404