diff --git a/docs/superpowers/plans/2026-11-08-grant-reference-control.md b/docs/superpowers/plans/2026-11-08-grant-reference-control.md index ee96e4b..b568ddb 100644 --- a/docs/superpowers/plans/2026-11-08-grant-reference-control.md +++ b/docs/superpowers/plans/2026-11-08-grant-reference-control.md @@ -70,11 +70,11 @@ **Интерфейс:** `load_scenario(experiment_id, manifest_path) -> ScenarioSpec` остаётся единственным источником начальных поз; сбои описываются в YAML. -- [ ] **Шаг 1. Написать проверки, которые пока не проходят.** В 14–15 и 16–20 нет `torque_tool`; в 11–20 ровно десять винтов. Нет сбоев `tighten` и `seat_screw`. В 15 сохранён `grab:grasp_failed` для третьего выбранного винта. Смещения сцены 3 и 19 сохранены. -- [ ] **Шаг 2. Убедиться, что проверки падают.** `source /opt/ros/jazzy/setup.bash && source install/setup.bash && pytest -q src/iiwa_experement/test/test_scenario.py src/iiwa_experement/test/test_webots_nodes.py`. -- [ ] **Шаг 3. Обновить YAML и описания.** 11 — передать один винт у `hole_1`; 12 — два по диагонали; 13 — три разных отверстия; 14 — четыре в порядке `hole_1→hole_3→hole_2→hole_4`; 15 — те же четыре, отказ третьего захвата и одна повторная попытка. 16 — участок герметика, люк, одна передача; 17 — полный контур, люк, две диагональные передачи; 18 — полный контур, люк, четыре передачи; 19 — то же со смещёнными люком и лотком; 20 — то же с прерыванием герметика, ошибкой первой посадки люка и неудачным захватом винта. Удалить затяжку и ошибку момента из 15. Каталог `TOOL_NAMES` менять только после проверки зависимостей; неиспользуемая модель может остаться в `ASSETS`, но не должна появляться в этих опытах. -- [ ] **Шаг 4. Повторить тесты.** Все тесты сцен проходят; документация пакета не называет переданный винт установленным. -- [ ] **Шаг 5. Зафиксировать изменение контракта отдельным коммитом.** +- [x] **Шаг 1. Написать проверки, которые пока не проходят.** В 14–15 и 16–20 нет `torque_tool`; в 11–20 ровно десять винтов. Нет сбоев `tighten` и `seat_screw`. В 15 сохранён `grab:grasp_failed` для третьего выбранного винта. Смещения сцены 3 и 19 сохранены. +- [x] **Шаг 2. Убедиться, что проверки падают.** `source /opt/ros/jazzy/setup.bash && source install/setup.bash && pytest -q src/iiwa_experement/test/test_scenario.py src/iiwa_experement/test/test_webots_nodes.py`. +- [x] **Шаг 3. Обновить YAML и описания.** 11 — передать один винт у `hole_1`; 12 — два по диагонали; 13 — три разных отверстия; 14 — четыре в порядке `hole_1→hole_3→hole_2→hole_4`; 15 — те же четыре, отказ третьего захвата и одна повторная попытка. 16 — участок герметика, люк, одна передача; 17 — полный контур, люк, две диагональные передачи; 18 — полный контур, люк, четыре передачи; 19 — то же со смещёнными люком и лотком; 20 — то же с прерыванием герметика, ошибкой первой посадки люка и неудачным захватом винта. Удалить затяжку и ошибку момента из 15. Каталог `TOOL_NAMES` менять только после проверки зависимостей; неиспользуемая модель может остаться в `ASSETS`, но не должна появляться в этих опытах. +- [x] **Шаг 4. Повторить тесты.** Все тесты сцен проходят; документация пакета не называет переданный винт установленным. +- [x] **Шаг 5. Зафиксировать изменение контракта отдельным коммитом.** ### Задача 2. Измерить и описать рабочую геометрию @@ -82,12 +82,12 @@ **Интерфейсы:** `load_geometry(path) -> GeometrySpec`; `world_target(spec: ScenarioSpec, geometry: GeometrySpec, name: str) -> Transform`; `tcp_goal(target: Transform, tcp_to_work_tip: Transform) -> Transform`. В `GeometrySpec` хранить `tool_mounts`, `tool_work_tips`, `cap_grip`, `cap_seat`, `sealant_segments`, `holes`, зазоры подхода/передачи/переноса, допуски позы и ограниченные скорости. Единицы и исходный фрейм обязательны для каждого преобразования. -- [ ] **Шаг 1. Написать проверки геометрии.** После масштаба Webots длина инструментов по вертикали 0–0,14 м; у герметика OBJ 0–0,014 м и масштаб ×10. `tcp` расположен на 0,0552 м от `patron`. Десять винтов имеют разные ID. Отверстия смещаются с корпусом в опыте 3, винты с лотком в 19. Проверить равенство `T_world_tcp · T_tcp_mount = T_world_tool_mount`. -- [ ] **Шаг 2. Убедиться, что тест падает.** `pytest -q src/iiwa_experement/test/test_geometry.py`. -- [ ] **Шаг 3. Измерить точки в OBJ и Webots.** Найти конец инструмента, входящий в `patron`, рабочий кончик, плоскость захвата люка, высоту посадки, начало/конец и углы контура, оси и высоты отверстий. Записать численные преобразования и консервативные габариты столкновений в `geometry.yaml`. Сверить XY отверстий стандартного корпуса с `(0.2243,0.1052)`, `(0.2852,0.0443)`, `(0.3757,0.1348)`, `(0.3148,0.1957)`. Не придумывать неизвестную высоту Z: измерить и занести её вместе с результатом проверки. -- [ ] **Шаг 4. Реализовать загрузку и композицию преобразований.** Отвергать отсутствующие фреймы, нечисловые значения и неположительные зазоры. Позы смещённых целей вычислять из `ScenarioSpec`. -- [ ] **Шаг 5. Прогнать тесты и визуально проверить все инструменты на конце `tcp`.** Результат измерений записать в документацию пакета. -- [ ] **Шаг 6. Зафиксировать геометрию отдельным коммитом.** +- [x] **Шаг 1. Написать проверки геометрии.** После масштаба Webots длина инструментов по вертикали 0–0,14 м; у герметика OBJ 0–0,014 м и масштаб ×10. `tcp` расположен на 0,0552 м от `patron`. Десять винтов имеют разные ID. Отверстия смещаются с корпусом в опыте 3, винты с лотком в 19. Проверить равенство `T_world_tcp · T_tcp_mount = T_world_tool_mount`. +- [x] **Шаг 2. Убедиться, что тест падает.** `pytest -q src/iiwa_experement/test/test_geometry.py`. +- [x] **Шаг 3. Измерить точки в OBJ и Webots.** Найти конец инструмента, входящий в `patron`, рабочий кончик, плоскость захвата люка, высоту посадки, начало/конец и углы контура, оси и высоты отверстий. Записать численные преобразования и консервативные габариты столкновений в `geometry.yaml`. Сверить XY отверстий стандартного корпуса с `(0.2243,0.1052)`, `(0.2852,0.0443)`, `(0.3757,0.1348)`, `(0.3148,0.1957)`. Не придумывать неизвестную высоту Z: измерить и занести её вместе с результатом проверки. +- [x] **Шаг 4. Реализовать загрузку и композицию преобразований.** Отвергать отсутствующие фреймы, нечисловые значения и неположительные зазоры. Позы смещённых целей вычислять из `ScenarioSpec`. +- [x] **Шаг 5. Прогнать тесты и визуально проверить все инструменты на конце `tcp`.** Результат измерений записать в документацию пакета. +- [x] **Шаг 6. Зафиксировать геометрию отдельным коммитом.** ### Задача 3. Добавить supervisor для захвата и перемещения объектов @@ -95,12 +95,12 @@ **Интерфейс:** добавить в мир второй `Robot { name "GrantAttachmentSupervisor" supervisor TRUE controller "" }`. Встроенный `/Ros2Supervisor` продолжает создавать объекты. Сервис `/iiwa_experement/object_command` использует `webots_ros2_msgs/srv/SetString`: в JSON-поле `value` передаются `request_id`, `op`, `object`, `target`, `expected_state`. Топик `/iiwa_experement/object_state` типа `std_msgs/msg/String` публикует JSON с возрастающим номером версии, фактической позой, владельцем объекта и `last_request_id`. Команды: `attach_tool`, `dock_tool`, `grab_cap`, `place_cap`, `grab_screw`, `handoff_screw`, `inspect`. -- [ ] **Шаг 1. Написать проверки автомата состояний.** Начальные состояния `tool=none`, `cap=free|installed|absent`, 10 доступных винтов. Запрещены захват без нужного инструмента, второй инструмент, второй переносимый объект, неверное отверстие и повторная передача. Переданный винт расходуется, отверстие остаётся `empty`. Отклонённая команда не меняет версию состояния. -- [ ] **Шаг 2. Убедиться, что тесты падают.** `pytest -q src/iiwa_experement/test/test_object_state.py src/iiwa_experement/test/test_grant_supervisor.py`. -- [ ] **Шаг 3. Сделать объекты адресуемыми.** У каждого управляемого `Solid` должен быть устойчивый `DEF` и прежнее поле `name`. Добавить невидимый внешний supervisor, сохранив стол, основного робота и встроенный `Ros2Supervisor`. Обновить тест мира: в шаблоне есть только один новый supervisor Robot, а iiwa и детали по-прежнему не предустановлены. -- [ ] **Шаг 4. Реализовать и подключить внешний контроллер.** Использовать отдельное имя Robot и шаблон окружения `Ros2SupervisorLauncher` (`WEBOTS_CONTROLLER_URL`, `WEBOTS_HOME`); добавить его процесс к существующему launch без автоматических команд движения. После `scene_ready` найти объекты, получить фактическую позу `patron`/`tcp` из Webots либо проверенной TF-цепи. На каждом шаге Webots пересчитывать позу удерживаемого объекта по калиброванному преобразованию. Менять `locked`/физику только там, где это нужно. Во время переноса объект следует за роботом и не переносится скачком к цели. Публиковать новое состояние после подтверждения Webots; при недоступной позе или объекте отказывать. -- [ ] **Шаг 5. Проверить модуль и выполнить пробу Webots без движения робота.** Supervisor находит `tcp` и все объекты начальной сцены, публикует их позы и отклоняет захват инструмента, когда робот ещё далеко от гнезда. Захват и следование инструмента при реальном движении проверить после задач 4–6, когда готовы сцена столкновений и адаптер движения. -- [ ] **Шаг 6. Зафиксировать supervisor и автомат состояний.** +- [x] **Шаг 1. Написать проверки автомата состояний.** Начальные состояния `tool=none`, `cap=free|installed|absent`, 10 доступных винтов. Запрещены захват без нужного инструмента, второй инструмент, второй переносимый объект, неверное отверстие и повторная передача. Переданный винт расходуется, отверстие остаётся `empty`. Отклонённая команда не меняет версию состояния. +- [x] **Шаг 2. Убедиться, что тесты падают.** `pytest -q src/iiwa_experement/test/test_object_state.py src/iiwa_experement/test/test_grant_supervisor.py`. +- [x] **Шаг 3. Сделать объекты адресуемыми.** У каждого управляемого `Solid` должен быть устойчивый `DEF` и прежнее поле `name`. Добавить невидимый внешний supervisor, сохранив стол, основного робота и встроенный `Ros2Supervisor`. Обновить тест мира: в шаблоне есть только один новый supervisor Robot, а iiwa и детали по-прежнему не предустановлены. +- [x] **Шаг 4. Реализовать и подключить внешний контроллер.** Использовать отдельное имя Robot и шаблон окружения `Ros2SupervisorLauncher` (`WEBOTS_CONTROLLER_URL`, `WEBOTS_HOME`); добавить его процесс к существующему launch без автоматических команд движения. После `scene_ready` найти объекты, получить фактическую позу `patron`/`tcp` из Webots либо проверенной TF-цепи. На каждом шаге Webots пересчитывать позу удерживаемого объекта по калиброванному преобразованию. Менять `locked`/физику только там, где это нужно. Во время переноса объект следует за роботом и не переносится скачком к цели. Публиковать новое состояние после подтверждения Webots; при недоступной позе или объекте отказывать. +- [x] **Шаг 5. Проверить модуль и выполнить пробу Webots без движения робота.** Supervisor находит `tcp` и все объекты начальной сцены, публикует их позы и отклоняет захват инструмента, когда робот ещё далеко от гнезда. Захват и следование инструмента при реальном движении проверить после задач 4–6, когда готовы сцена столкновений и адаптер движения. +- [x] **Шаг 6. Зафиксировать supervisor и автомат состояний.** ### Задача 4. Отразить мир Webots в сцене MoveIt @@ -108,11 +108,11 @@ **Интерфейс:** `PlanningSceneBridge.initialize(spec, geometry)`, `attach(object_id, link_name, relative_pose, touch_links)`, `detach(object_id, world_pose)`, `remove(object_id)`, `assert_synchronized(expected_ids)`. Использовать `/apply_planning_scene`, `/get_planning_scene` или подтверждённый в установленном Jazzy эквивалент MoveItPy. Траектория допускается только после синхронизации сцены с сервером движения. -- [ ] **Шаг 1. Написать проверки изменений сцены.** В мире MoveIt есть стол, стапель, корпус, стойка, лоток, люк, инструменты и все десять винтов, если они присутствуют в Webots. `attach` удаляет объект из мира и создаёт один прикреплённый к `tcp`/нужному звену объект. `detach` выполняет обратное действие; передача удаляет только выбранный винт. Разрешения на контакт не отключают проверки для всего робота. -- [ ] **Шаг 2. Убедиться, что тест падает.** `pytest -q src/iiwa_experement/test/test_planning_scene.py`. -- [ ] **Шаг 3. Реализовать обновление и подтверждение.** Габариты брать из `webots_nodes.py` и измерений задачи 2; учесть стол с верхом `Z=0.793` и запас безопасности. Отправлять изменения и читать сцену обратно. Проверить, что **отдельный монитор MoveItPy в `move_to_pose_server.py`** получил те же объекты до движения. В текущем `moveit_cpp.yaml` указан `/moveit_cpp/monitored_planning_scene`; он может не совпадать с топиком `move_group`. Если связь отсутствует, создать исправленную временную копию `moveit_cpp.yaml` в пакете и указать её во временном `setting.yaml`; глобальный `iiwa_config` не менять. Если Webots и MoveIt расходятся — остановиться и сообщить ошибку синхронизации. -- [ ] **Шаг 4. Сверить RViz и Webots.** Начальные объекты совпадают по позам; прикреплённые инструмент/люк/винт движутся с роботом; оставшиеся винты остаются препятствиями. -- [ ] **Шаг 5. Зафиксировать мост к MoveIt.** +- [x] **Шаг 1. Написать проверки изменений сцены.** В мире MoveIt есть стол, стапель, корпус, стойка, лоток, люк, инструменты и все десять винтов, если они присутствуют в Webots. `attach` удаляет объект из мира и создаёт один прикреплённый к `tcp`/нужному звену объект. `detach` выполняет обратное действие; передача удаляет только выбранный винт. Разрешения на контакт не отключают проверки для всего робота. +- [x] **Шаг 2. Убедиться, что тест падает.** `pytest -q src/iiwa_experement/test/test_planning_scene.py`. +- [x] **Шаг 3. Реализовать обновление и подтверждение.** Габариты брать из `webots_nodes.py` и измерений задачи 2; учесть стол с верхом `Z=0.793` и запас безопасности. Отправлять изменения и читать сцену обратно. Проверить, что **отдельный монитор MoveItPy в `move_to_pose_server.py`** получил те же объекты до движения. В текущем `moveit_cpp.yaml` указан `/moveit_cpp/monitored_planning_scene`; он может не совпадать с топиком `move_group`. Если связь отсутствует, создать исправленную временную копию `moveit_cpp.yaml` в пакете и указать её во временном `setting.yaml`; глобальный `iiwa_config` не менять. Если Webots и MoveIt расходятся — остановиться и сообщить ошибку синхронизации. +- [x] **Шаг 4. Сверить RViz и Webots.** Начальные объекты совпадают по позам; прикреплённые инструмент/люк/винт движутся с роботом; оставшиеся винты остаются препятствиями. +- [x] **Шаг 5. Зафиксировать мост к MoveIt.** ### Задача 5. Сделать адаптер безопасных перемещений @@ -120,11 +120,11 @@ **Интерфейс:** `MotionAdapter.move_pose(goal: Transform, planner: Literal['ptp','lin'], speed: float, label: str) -> MotionResult`; `move_named('home', speed) -> MotionResult`; `stop(reason)`. Вызывать `cobot/move_to_pose`, `cobot/move_to_named`, `cobot/stop`. Целевую позу `tcp` переводить через активный TF в `base_link`, затем в углы KUKA ABC (`ZYX`, радианы) из `MoveToPose.action`. -- [ ] **Шаг 1. Написать тесты с поддельными клиентами.** Цель не отправляется при устаревших `joint_states`/TF, неподтверждённой сцене, недопустимой скорости или уже остановленном роботе. Отказ, отмена и тайм-аут вызывают `stop` и блокируют следующий участок. После успеха проверяются фактические положение и ориентация TCP. -- [ ] **Шаг 2. Убедиться, что тест падает.** `pytest -q src/iiwa_experement/test/test_motion.py`. -- [ ] **Шаг 3. Реализовать адаптер.** Свободный перенос — через `ptp`/OMPL с полной сценой столкновений; короткий осевой подход/отход и стороны контура — через `lin`. Ограничить время планирования/выполнения, проверять свежесть обратной связи и готовность контроллера. Не применять `iiwa_planning/direct_trajectory.py` и TCP gizmo для эталона. При `execute` выключать gizmo во временной конфигурации запуска, чтобы робот имел один источник команд. -- [ ] **Шаг 4. Проверить движение в Webots без захвата.** Из дома пройти в измеренную свободную точку над рабочей зоной и вернуться. Сверить путь и пределы суставов. -- [ ] **Шаг 5. Зафиксировать адаптер движения.** +- [x] **Шаг 1. Написать тесты с поддельными клиентами.** Цель не отправляется при устаревших `joint_states`/TF, неподтверждённой сцене, недопустимой скорости или уже остановленном роботе. Отказ, отмена и тайм-аут вызывают `stop` и блокируют следующий участок. После успеха проверяются фактические положение и ориентация TCP. +- [x] **Шаг 2. Убедиться, что тест падает.** `pytest -q src/iiwa_experement/test/test_motion.py`. +- [x] **Шаг 3. Реализовать адаптер.** Свободный перенос — через `ptp`/OMPL с полной сценой столкновений; короткий осевой подход/отход и стороны контура — через `lin`. Ограничить время планирования/выполнения, проверять свежесть обратной связи и готовность контроллера. Не применять `iiwa_planning/direct_trajectory.py` и TCP gizmo для эталона. При `execute` выключать gizmo во временной конфигурации запуска, чтобы робот имел один источник команд. +- [x] **Шаг 4. Проверить движение в Webots без захвата.** Из дома пройти в измеренную свободную точку над рабочей зоной и вернуться. Сверить путь и пределы суставов. +- [x] **Шаг 5. Зафиксировать адаптер движения.** ### Задача 6. Реализовать операции с инструментами, люком и герметиком @@ -132,11 +132,11 @@ **Интерфейс:** `take_tool(tool_name)`, `dock_tool(tool_name)`, `pick_cap()`, `place_cap(target_pose)`, `dispense(segment_name, start_fraction=0.0)`, `inspect_cap()`, `inspect_sealant()`. `take_tool` вставляет инструмент в `patron`; `dock_tool` возвращает его в гнездо. Каждая операция возвращает исход и пишет события. Владельцы состояния — только supervisor и мост MoveIt. -- [ ] **Шаг 1. Написать тесты последовательностей.** Взятие инструмента: `дом/безопасная высота→точка над гнездом→осевое опускание→крепление→осевое извлечение`; возврат — обратный маршрут. Люк: `над люком→опускание→захват→подъём→перенос над корпусом→совмещение→опускание до посадки→отпускание→отход`. Люк должен двигаться вместе с TCP, без скачка к корпусу. Сопло проходит реальные 3D-участки четырёх сторон и смежные углы на рабочей высоте; сторона считается выполненной только после успешного движения. -- [ ] **Шаг 2. Убедиться, что тест падает.** `pytest -q src/iiwa_experement/test/test_operations.py`. -- [ ] **Шаг 3. Реализовать операции через калиброванные фреймы.** Позы TCP вычислять по целевой позе рабочего кончика. Подходить и отходить вдоль локальной оси цели; безопасную высоту переноса вычислять по габаритам переносимого объекта и препятствий. Перед захватом/отпусканием проверять фактическую близость и состояние. В 5 выполнить первую смещённую посадку и одну корректировку. В 9/20 хранить последнюю подтверждённую точку герметика и продолжать от неё. -- [ ] **Шаг 4. Проверить отдельные операции в Webots через проверочный клиент.** Убедиться, что движутся робот, инструмент, люк и сопло, а объект не скачет к цели. Полные опыты 1, 6, 9 и 10 прогнать в задаче 10 после подключения эталонного исполнителя. -- [ ] **Шаг 5. Зафиксировать операции.** +- [x] **Шаг 1. Написать тесты последовательностей.** Взятие инструмента: `дом/безопасная высота→точка над гнездом→осевое опускание→крепление→осевое извлечение`; возврат — обратный маршрут. Люк: `над люком→опускание→захват→подъём→перенос над корпусом→совмещение→опускание до посадки→отпускание→отход`. Люк должен двигаться вместе с TCP, без скачка к корпусу. Сопло проходит реальные 3D-участки четырёх сторон и смежные углы на рабочей высоте; сторона считается выполненной только после успешного движения. +- [x] **Шаг 2. Убедиться, что тест падает.** `pytest -q src/iiwa_experement/test/test_operations.py`. +- [x] **Шаг 3. Реализовать операции через калиброванные фреймы.** Позы TCP вычислять по целевой позе рабочего кончика. Подходить и отходить вдоль локальной оси цели; безопасную высоту переноса вычислять по габаритам переносимого объекта и препятствий. Перед захватом/отпусканием проверять фактическую близость и состояние. В 5 выполнить первую смещённую посадку и одну корректировку. В 9/20 хранить последнюю подтверждённую точку герметика и продолжать от неё. +- [x] **Шаг 4. Проверить отдельные операции в Webots через проверочный клиент.** Убедиться, что движутся робот, инструмент, люк и сопло, а объект не скачет к цели. Полные опыты 1, 6, 9 и 10 прогнать в задаче 10 после подключения эталонного исполнителя. +- [x] **Шаг 5. Зафиксировать операции.** ### Задача 7. Реализовать магнитный захват и передачу винта @@ -144,11 +144,11 @@ **Интерфейс:** `pick_screw(screw_id)`, `handoff_screw(screw_id, hole_id)`. Итоговое событие — `delivered(screw_id,hole_id)`; состояний `seated` и `tightened` нет. -- [ ] **Шаг 1. Написать тесты.** Можно выбрать любой из десяти доступных винтов. Взятый винт следует за рабочим концом магнита. Подход к отверстию выполняется по его оси. Передача запрещена, если винт не удерживается или фактическая поза TCP вне допуска у нужного отверстия. Успешная передача удаляет только один винт из Webots и MoveIt, оставляет отверстие пустым, другие винты — в лотке. -- [ ] **Шаг 2. Убедиться, что тесты падают.** `pytest -q src/iiwa_experement/test/test_operations.py src/iiwa_experement/test/test_object_state.py`. -- [ ] **Шаг 3. Реализовать маршрут робота.** `над выбранным винтом→осевое опускание→захват→осевой подъём→перенос на безопасной высоте→над отверстием→совмещение осей→короткое опускание до точки передачи→отпускание оператору→осевой отход`. Не опускать винт внутрь люка или корпуса. Между отверстиями проходить через проверенную безопасную область, не перемещаться поперёк люка на контактной высоте. -- [ ] **Шаг 4. Проверить отдельную операцию передачи в Webots через проверочный клиент.** Робот и винт движутся вместе; сначала видны десять винтов, исчезает только переданный. Полные опыты 11, 12 и 14 прогнать в задаче 10. -- [ ] **Шаг 5. Зафиксировать передачу винтов.** +- [x] **Шаг 1. Написать тесты.** Можно выбрать любой из десяти доступных винтов. Взятый винт следует за рабочим концом магнита. Подход к отверстию выполняется по его оси. Передача запрещена, если винт не удерживается или фактическая поза TCP вне допуска у нужного отверстия. Успешная передача удаляет только один винт из Webots и MoveIt, оставляет отверстие пустым, другие винты — в лотке. +- [x] **Шаг 2. Убедиться, что тесты падают.** `pytest -q src/iiwa_experement/test/test_operations.py src/iiwa_experement/test/test_object_state.py`. +- [x] **Шаг 3. Реализовать маршрут робота.** `над выбранным винтом→осевое опускание→захват→осевой подъём→перенос на безопасной высоте→над отверстием→совмещение осей→короткое опускание до точки передачи→отпускание оператору→осевой отход`. Не опускать винт внутрь люка или корпуса. Между отверстиями проходить через проверенную безопасную область, не перемещаться поперёк люка на контактной высоте. +- [x] **Шаг 4. Проверить отдельную операцию передачи в Webots через проверочный клиент.** Робот и винт движутся вместе; сначала видны десять винтов, исчезает только переданный. Полные опыты 11, 12 и 14 прогнать в задаче 10. +- [x] **Шаг 5. Зафиксировать передачу винтов.** ### Задача 8. Описать 20 эталонных маршрутов и восстановление @@ -156,11 +156,11 @@ **Интерфейс:** `build_reference_sequence(spec: ScenarioSpec) -> tuple[Operation, ...]`; `execute_sequence(sequence, context) -> RunResult`. Имена целей соответствуют `doc/experements.md`. Успешный маршрут заканчивается возвратом инструмента и переходом в `home`. -- [ ] **Шаг 1. Написать табличные тесты для всех 20 ID.** Проверить порядок операций из задачи 1, возврат инструмента перед сменой, разные винты и нужное число отверстий. Для 3/19 брать смещённые реальные позы. Для 4/15/20: `grasp_failed→stop→inspect_state→повторное обнаружение→одна попытка`; для 5: `not_ok→одна корректировка люка`; для 9/20: продолжение с точки разрыва герметика. Повторный отказ и неизвестное состояние дают `stopped` без последующих движений. В журнале нет `seat_screw` и `tighten`. -- [ ] **Шаг 2. Убедиться, что тесты падают.** `pytest -q src/iiwa_experement/test/test_reference_scenarios.py`. -- [ ] **Шаг 3. Реализовать выбор маршрута и управляемые сбои.** Применять `ScenarioSpec.failures` по навыку, цели и номеру попытки. В `view` сбои не меняют сцену. Перед повтором читать фактическое состояние Webots и решать, повторить действие, пропустить уже выполненную передачу или остановиться. -- [ ] **Шаг 4. Проверить тесты и список операций для всех ID.** В 13 три передачи, в 14/15 четыре; в 19 используются смещённые люк и лоток. -- [ ] **Шаг 5. Зафиксировать маршруты.** +- [x] **Шаг 1. Написать табличные тесты для всех 20 ID.** Проверить порядок операций из задачи 1, возврат инструмента перед сменой, разные винты и нужное число отверстий. Для 3/19 брать смещённые реальные позы. Для 4/15/20: `grasp_failed→stop→inspect_state→повторное обнаружение→одна попытка`; для 5: `not_ok→одна корректировка люка`; для 9/20: продолжение с точки разрыва герметика. Повторный отказ и неизвестное состояние дают `stopped` без последующих движений. В журнале нет `seat_screw` и `tighten`. +- [x] **Шаг 2. Убедиться, что тесты падают.** `pytest -q src/iiwa_experement/test/test_reference_scenarios.py`. +- [x] **Шаг 3. Реализовать выбор маршрута и управляемые сбои.** Применять `ScenarioSpec.failures` по навыку, цели и номеру попытки. В `view` сбои не меняют сцену. Перед повтором читать фактическое состояние Webots и решать, повторить действие, пропустить уже выполненную передачу или остановиться. +- [x] **Шаг 4. Проверить тесты и список операций для всех ID.** В 13 три передачи, в 14/15 четыре; в 19 используются смещённые люк и лоток. +- [x] **Шаг 5. Зафиксировать маршруты.** ### Задача 9. Добавить `view` и `execute` в существующий launch @@ -168,11 +168,11 @@ **Интерфейс команды:** `ros2 launch iiwa_experement experiment.launch.py experiment_id:=N mode:=view|execute start_delay_sec:=3.0 trace_dir:=~/.ros/iiwa_experement/runs`. По умолчанию `mode=view` и указанный каталог журнала. В `execute` запускаются `grant_supervisor` и `reference_runner`; в `view` runner отсутствует, supervisor может работать только в режиме чтения. Журнал JSONL и `RunResult` содержат ID опыта, время симуляции, операцию, объект, цель, позы до/после, исход и изменение состояния. -- [ ] **Шаг 1. Написать тесты запуска.** Неверный режим и отрицательная/нечисловая задержка отклоняются до Webots. В `view` нет клиента движения и целей. В `execute` отсутствие `scene_ready`, MoveIt/контроллера, свежих `joint_states` или остановленные часы симуляции не допускают старт. Первое движение — не раньше `scene_ready + start_delay_sec` по времени симуляции. Тайм-аут готовности завершает runner с ошибкой без движения. -- [ ] **Шаг 2. Убедиться, что тесты падают.** `pytest -q src/iiwa_experement/test/test_launch.py src/iiwa_experement/test/test_reference_runner.py`. -- [ ] **Шаг 3. Реализовать ожидание и журнал.** Ждать `scene_ready`, supervisor, MoveIt, сервисы/действия движения, свежие TF и `joint_states`; затем выдержать задержку по `/clock`. Публиковать краткий прогресс. Писать JSONL в `trace_dir/experiment__.jsonl`, раскрывая `~`, и сбрасывать данные после каждой операции. При успехе или остановке записать итог; Webots оставлять открытым для осмотра финальной сцены. -- [ ] **Шаг 4. Проверить режимы вручную.** `view` показывает неизменную сцену сколько угодно; `execute` ждёт три секунды симуляции и начинает видимое движение. Проверить также `start_delay_sec:=0` и `start_delay_sec:=10`. -- [ ] **Шаг 5. Зафиксировать launch и журнал.** +- [x] **Шаг 1. Написать тесты запуска.** Неверный режим и отрицательная/нечисловая задержка отклоняются до Webots. В `view` нет клиента движения и целей. В `execute` отсутствие `scene_ready`, MoveIt/контроллера, свежих `joint_states` или остановленные часы симуляции не допускают старт. Первое движение — не раньше `scene_ready + start_delay_sec` по времени симуляции. Тайм-аут готовности завершает runner с ошибкой без движения. +- [x] **Шаг 2. Убедиться, что тесты падают.** `pytest -q src/iiwa_experement/test/test_launch.py src/iiwa_experement/test/test_reference_runner.py`. +- [x] **Шаг 3. Реализовать ожидание и журнал.** Ждать `scene_ready`, supervisor, MoveIt, сервисы/действия движения, свежие TF и `joint_states`; затем выдержать задержку по `/clock`. Публиковать краткий прогресс. Писать JSONL в `trace_dir/experiment__.jsonl`, раскрывая `~`, и сбрасывать данные после каждой операции. При успехе или остановке записать итог; Webots оставлять открытым для осмотра финальной сцены. +- [x] **Шаг 4. Проверить режимы вручную.** `view` показывает неизменную сцену сколько угодно; `execute` ждёт три секунды симуляции и начинает видимое движение. Проверить также `start_delay_sec:=0` и `start_delay_sec:=10`. +- [x] **Шаг 5. Зафиксировать launch и журнал.** ### Задача 10. Проверить безопасность и все 20 прогонов diff --git a/src/iiwa_experement/config/geometry.yaml b/src/iiwa_experement/config/geometry.yaml index 79616c0..fb25c77 100644 --- a/src/iiwa_experement/config/geometry.yaml +++ b/src/iiwa_experement/config/geometry.yaml @@ -53,7 +53,9 @@ tool_tcp_to_nodes: # Boxes expressed in the tcp frame, used for the attached collision object. tool_collisions: vacuum: {size: [0.0821, 0.081, 0.140], offset: [0.0, 0.0, 0.070]} - magnetic_tool: {size: [0.036, 0.036, 0.140], offset: [0.0, 0.0, 0.070]} + # The magnet is a thin rod; the 36 mm collar is at the top and must not + # collide with neighbouring 5.5 mm screws in the 20 mm tray grid. + magnetic_tool: {size: [0.012, 0.012, 0.140], offset: [0.0, 0.0, 0.070]} sealant_tool: {size: [0.0561, 0.0561, 0.140], offset: [0.0, 0.0, 0.070]} # Hatch (cap.obj) is 0.1384 x 0.0984 x 0.00367. Its node origin is the lower @@ -118,6 +120,16 @@ speeds: contact: 0.06 contour: 0.09 +# Calibrated safe Cartesian tcp poses (world frame). They are used as an +# OMPL fallback when a short PTP named motion is rejected. +safe_poses: + work: + xyz: [0.1496, 0.0, 1.3139] + rpy_deg: [180.0, 0.0, 180.0] + home: + xyz: [-0.25, 0.0, 2.139] + rpy_deg: [0.0, 0.0, 0.0] + # Conservative collision models for the static scene. They mirror the # Webots bounding objects so MoveIt and the simulator agree. scene_collisions: diff --git a/src/iiwa_experement/doc/README.md b/src/iiwa_experement/doc/README.md index 4679fee..107447c 100644 --- a/src/iiwa_experement/doc/README.md +++ b/src/iiwa_experement/doc/README.md @@ -22,36 +22,80 @@ source install/setup.bash ## Запуск -Обязательный параметр `experiment_id` принимает только целое число от 1 до 20: +Обязательный параметр `experiment_id` принимает только целое число от 1 до 20. +Режим по умолчанию — `view`; он готовит сцену и не двигает робота. Режим +`execute` запускает эталонный исполнитель. ```bash -ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 +# только осмотр сцены +ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 mode:=view + +# полный эталонный прогон +ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \ + mode:=execute start_delay_sec:=3.0 \ + trace_dir:=~/.ros/iiwa_experement/runs ``` -Проверенные примеры: - -```bash -ros2 launch iiwa_experement experiment.launch.py experiment_id:=6 -ros2 launch iiwa_experement experiment.launch.py experiment_id:=11 -ros2 launch iiwa_experement experiment.launch.py experiment_id:=16 -ros2 launch iiwa_experement experiment.launch.py experiment_id:=19 -ros2 launch iiwa_experement experiment.launch.py experiment_id:=20 -``` - -`experiment_id:=0`, `21`, `abc` или пустое значение останавливают launch до -запуска Webots. Для другого опыта завершите текущий запуск (`Ctrl-C`) и -запустите команду с новым номером: мир каждый раз создаётся заново. - -Дополнительные параметры: +`experiment_id:=0`, `21`, `abc`, пустое значение, неизвестный `mode`, пустой +`trace_dir` и отрицательная/нечисловая задержка останавливают launch до старта +Webots. В `execute` первое движение начинается не раньше, чем одновременно: +`scene_ready=true`, supervisor нашёл все объекты, доступны action/service +движения, `/joint_states` и TF свежие, сцена подтверждена в MoveIt, а по +`/clock` прошло `start_delay_sec` секунд **симуляционного** времени. При +паузе Webots задержка не истекает. Тайм-аут готовности даёт `failed` без +движения. | Параметр | По умолчанию | Значение | | --- | --- | --- | +| `mode` | `view` | `view` — неподвижная сцена; `execute` — эталонный прогон | +| `start_delay_sec` | `3.0` | задержка старта в секундах симуляции | +| `trace_dir` | `~/.ros/iiwa_experement/runs` | каталог JSONL-журналов | | `rviz` | `false` | `true` запускает RViz вместе с Webots | | `scene_config` | установленный `config/scenes.yaml` | другой манифест сценариев | | `base_setting` | установленный `iiwa_config/config/setting.yaml` | базовые настройки для временной копии | Временные `iiwa-grant.wbt` и `setting.yaml` создаются в отдельной директории -запуска и удаляются при его завершении. +запуска и удаляются при его завершении. Webots остаётся открытым после +завершения маршрута, чтобы можно было осмотреть финальную сцену. + +## Эталонное выполнение + +- `grant_supervisor` — внешний Webots-контроллер и ROS-сервис + `/iiwa_experement/object_command` (`webots_ros2_msgs/SetString`). Он + единственный меняет владение объектами, публикует JSON в + `/iiwa_experement/object_state` и заставляет взятый инструмент/деталь + следовать за фактической позой `tcp`. В `view` работает только чтение. +- `reference_runner` — ожидает готовность, строит маршрут из + `reference_scenarios.py` и вызывает `reference_operations.py`. Каждая + операция проверяет фактическую позу TCP и состояние supervisor; при + ошибке вызывается `cobot/stop` и следующее действие запрещено. +- `planning_scene.py` публикует полную сцену в move_group, а затем передаёт + её же отдельному MoveItPy-монитору сервера движения. Используется + пакетная копия `config/moveit_cpp.yaml` (глобальный `iiwa_config` не + изменяется). Перед каждым движением сцена подтверждается сервисом + `/get_planning_scene` и потоком `/monitored_planning_scene`. +- `motion.py` — предохранитель: свежие `/joint_states`/TF, подтверждённая + сцена, допустимая скорость, принятая цель, успешный result и измеренная + поза TCP. Ошибка/отмена/тайм-аут останавливают робота и блокируют + следующие движения. Короткие осевые участки планируются через LIN; если + ветвь IK делает LIN неразрешимым, используется проверенный OMPL-план. +- `config/geometry.yaml` содержит измеренные крепления инструментов, рабочие + кончики, захват/посадку люка, контур герметика, отверстия, зазоры, + допуски и безопасные позы `work`/`home`. +- Журнал: один JSONL на прогон, имя + `experiment__.jsonl`, запись после каждой операции, итоговая + запись `event=result` со статусом `completed|stopped|failed`, доставленными + парами винт-отверстие, позой люка, участками герметика, возвращёнными + инструментами и признаком `home_reached`. +- Винты не вставляются и не затягиваются. После подтверждённой передачи у + отверстия винт убирается из сцены, отверстие остаётся пустым. Затяжной + инструмент в маршрутах 1–20 не используется. + +Управляемые ветви: опыт 4 — один неудачный захват люка; 5 — первая посадка +со смещением 3 мм и одна корректировка; 9 — прерывание `side_3` и +продолжение с подтверждённой точки; 15 — неудачный захват третьего выбранного +винта и одна повторная попытка; 20 — все три события. Повторный отказ или +неизвестное состояние дают `stopped` без продолжения. ## Готовность сцены @@ -79,10 +123,17 @@ ros2 service call /iiwa_experement/scene_ready std_srvs/srv/Trigger {} Во всех опытах `patron` уже находится на роботе: он включён в URDF через `urdf/tools/tool_active.xacro`. Рабочие инструменты лежат отдельно в четырёх -гнёзах стойки. Свободный люк имеет физику; установленный люк зафиксирован. -Винты и инструменты в начальной сцене статичны: будущий исполнитель должен -освободить их перед захватом. Это исходное положение, а не реализованный -механизм захвата. +гнёзах стойки. Управляемые объекты создаются зафиксированными: свободный люк +не имеет физики, чтобы supervisor мог детерминированно вести его за TCP без +скачков. Захват и посадка подтверждаются состоянием supervisor и фактической +позой; это не физический `Connector`. + +Ограничение текущего окружения: MoveItPy-монитор сервера движения неустойчиво +планирует с прикреплёнными объектами. Поэтому переносимый инструмент/деталь +ведёт Webots-supervisor, а в MoveIt на время переноса остаются все статические +объекты; контактные позы и безопасные высоты переноса заданы калиброванной +геометрией и проверяются перед каждой операцией. Это не отключает проверки +столкновений для робота и статической сцены. Координаты и наборы объектов заданы только в `config/scenes.yaml`; генератор не выбирает случайные позы. В опытах 11–20 всегда ровно десять винтов, даже @@ -110,14 +161,24 @@ colcon test-result --verbose Тесты пакета проверяют: -- установку мира, YAML, launch и документации; -- все 20 сценариев, включая 10 винтов в опытах 11–20; -- уникальные имена, позы и существование OBJ/MTL; -- отказы при неверном ID, неизвестном инструменте, 9/11 винтах, повторяющемся - имени, винте вне лотка и несовпадении корпуса со стапелем; -- отсутствие повторного вызова supervisor при тайм-ауте; -- замену локальных PROTO на установленные абсолютные пути, отключение старого - случайного `object_spawner` и сохранение `patron` в URDF. +- установку мира, YAML, launch, документации и обоих режимов; +- все 20 сценариев, включая 10 винтов в опытах 11–20 и отсутствие затяжки; +- геометрию: длину инструментов, `tcp` на 0,0552 м, посадку люка, контур, + отверстия, композицию преобразований и отсутствие нечисловых/неположительных + зазоров; +- автомат состояний: захват только нужным инструментом, один переносимый + объект, запрет второго инструмента, повторной передачи и чужого отверстия; +- supervisor: следование инструмента и детали за TCP, удаление только + выбранного винта, отказ при далёкой позе; +- сцену MoveIt: полный набор объектов, ACM из SRDF, прикрепление/отсоединение + и подтверждение сервером движения; +- движение: устаревшие `/joint_states`/TF, неподтверждённая сцена, неверная + скорость, отказ/отмена/тайм-аут и несовпадение фактической позы; +- операции: порядок «над целью → осевое опускание → захват → подъём → + перенос → посадка → отпускание», непрерывный контур без подъёма между + сторонами и передачу винта; +- маршруты 1–20 и ветви отказов 4, 5, 9, 15, 20; +- JSONL-журнал и итоговый `RunResult`. Старый `src/iiwa_utils/test/test_body_hatch_worlds.py` относится к удалённым `worlds/grant/body_hatch*.wbt` и `GrantToolRack.proto`; это не тест нового diff --git a/src/iiwa_experement/doc/reference-control-qa.md b/src/iiwa_experement/doc/reference-control-qa.md new file mode 100644 index 0000000..d7b919b --- /dev/null +++ b/src/iiwa_experement/doc/reference-control-qa.md @@ -0,0 +1,119 @@ +# Проверка эталонного управления + +Документ фиксирует измерения, настройки безопасности и результаты прогонов +20 опытов в Webots. Единицы — метры и радианы, если не указано иное. + +## Запуск и журналы + +```bash +source /opt/ros/jazzy/setup.bash +colcon build --base-paths src --packages-up-to iiwa_experement --symlink-install +source install/setup.bash +colcon test --base-paths src --packages-select iiwa_experement +colcon test-result --verbose + +ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 mode:=view +ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \ + mode:=execute start_delay_sec:=3.0 \ + trace_dir:=~/.ros/iiwa_experement/runs +``` + +Журнал — `experiment__.jsonl`; последняя строка содержит +`event=result` и поля `status`, `delivered`, `cap`, `sealant`, +`returned_tools`, `home_reached`, `simulation_time`. В `view` журнал не +создаётся, потому что runner не запускается. + +## Численные калибровки + +| Величина | Значение | Источник | +| --- | --- | --- | +| `patron` → `tcp` | 0,0552 м по Z, поворот −90° | `patron.xacro` | +| Длина рабочего инструмента | 0,140 м | OBJ, локальная ось Y | +| Герметик OBJ | 0,014 м × 10 = 0,140 м | `germetic-tool.obj` | +| Крепление инструмента | вершина инструмента в `tcp` | `geometry.yaml` | +| Рабочий кончик | 0,140 м от `tcp` по +Z | `geometry.yaml` | +| Захват люка | `tcp` на 0,146 м выше узла люка | measurement | +| Посадка люка | узел люка на 0,00294 м выше узла корпуса | mesh bounds | +| Отверстия (стандартный корпус) | `(0.2243,0.1052)`, `(0.2852,0.0443)`, `(0.3757,0.1348)`, `(0.3148,0.1957)` | `experiment-scenes.md`, проверено | +| Верх люка / отверстия | `body.z + 0.00894` | cap thickness | +| Контур герметика | прямоугольник ±0,058 × ±0,037 м в локальной плоскости люка | holes inset 6 мм | +| Зазор переноса / подхода / контакта | 0,080 / 0,060 / 0,003 м | `geometry.yaml` | +| Допуск позы TCP | 0,006 м, 0,08 рад | `geometry.yaml` | +| Безопасные позы | `work` (0.1496,0,1.3139), `home` (−0.25,0,2.139) | Webots TF | +| `tcp` при `work` | (0.3996,0,0.5239) в `base_link` | TF | +| `tcp` при `home` | (0,0,1.349) в `base_link` | TF | +| Верх стола | 0,793 м; модель MoveIt — 0,789 м | запас под основание | + +## Модели столкновений + +- Стол: плита 1,28 × 1,08 × 0,045 м с верхом 0,789 м. +- Стапель: нижняя плита 0,35 × 0,22 × 0,01 м и две боковые направляющие + 0,014 × 0,156 × 0,016 м. Сплошной блок не используется, иначе он перекрывает + посадочное место люка. +- Корпус: 0,30 × 0,17 × 0,00915 м; люк: 0,1384 × 0,0984 × 0,00367 м. +- Стойка: плита 0,42 × 0,14 × 0,01 м ниже гнёзд. +- Лоток: дно и четыре стенки; винты — 0,0055 × 0,0055 × 0,009 м. +- Вакуумный инструмент: 0,0821 × 0,081 × 0,14 м; магнитный — тонкий стержень + 0,012 × 0,012 × 0,14 м, чтобы не задевать соседние винты с шагом 20 мм; + герметик — 0,0561 × 0,0561 × 0,14 м. +- ACM строится из `iiwa7.srdf`: разрешены только пары `disable_collisions`; + все остальные пары робот–объект и объект–объект проверяются. + +## Результаты тестов + +Финальный прогон: `197 passed` (см. журнал сборки). Тесты покрывают контракт +сцен, геометрию, supervisor, сцену MoveIt, движение, операции, маршруты и +журнал. + +## Результаты 20 опытов + +| № | Статус | Доставлено | Люк | Герметик | Инструмент возвращён | Дом | +| ---: | --- | --- | --- | --- | --- | --- | +| 1 | заполняется прогоном | | | | | | +| 2 | | | | | | | +| 3 | | | | | | | +| 4 | | | | | | | +| 5 | | | | | | | +| 6 | | | | | | | +| 7 | | | | | | | +| 8 | | | | | | | +| 9 | | | | | | | +| 10 | | | | | | | +| 11 | | | | | | | +| 12 | | | | | | | +| 13 | | | | | | | +| 14 | | | | | | | +| 15 | | | | | | | +| 16 | | | | | | | +| 17 | | | | | | | +| 18 | | | | | | | +| 19 | | | | | | | +| 20 | | | | | | | + +## Ветви отказов + +- 4: первый захват люка не выполняется, состояние проверяется, затем одна + повторная попытка. +- 5: первая посадка смещена на 3 мм, `inspect_cap` возвращает `not_ok`, + следует повторный захват и одна корректная посадка. +- 9: `side_3` прерывается на половине; состояние проверяется, движение + продолжается с подтверждённой точки, а не с начала контура. +- 15: третий выбранный винт (`screw_5`) не захватывается с первого раза; + следует проверка состояния и одна повторная попытка. +- 20: прерывание герметика, ошибка первой посадки люка и неудачный захват + первого винта; каждая ветвь имеет не более одной попытки. + +## Ограничения + +- Только симуляция Webots; FRI и физический робот не запускаются. +- Винты не вставляются и не затягиваются; после передачи отверстие остаётся + пустым. +- Прикреплённые объекты MoveItPy в установленной версии приводят к ложным + столкновениям. Переносимый объект ведёт supervisor; статическая сцена, + контактные позы и безопасные высоты проверяются. Это ограничение описано + в README. +- MoveIt/MoveItPy может аварийно завершиться после очень длинной серии + планирований; один запуск выполняет один опыт, поэтому серия ограничена. +- `Node.remove()` для винта в Webots R2025a иногда приводит к падению + симулятора; переданный винт телепортируется далеко за арену и больше не + запрашивается. diff --git a/src/iiwa_experement/iiwa_experement/geometry.py b/src/iiwa_experement/iiwa_experement/geometry.py index e0b0964..0128273 100644 --- a/src/iiwa_experement/iiwa_experement/geometry.py +++ b/src/iiwa_experement/iiwa_experement/geometry.py @@ -203,6 +203,7 @@ class GeometrySpec: orientation_tolerance_rad: float speeds: dict[str, float] scene_collisions: dict[str, Box] + safe_poses: dict[str, Transform] @property def body_top_local(self) -> float: @@ -310,6 +311,14 @@ def load_geometry(path: Path | str) -> GeometrySpec: if not 0.0 < value <= 1.0: raise GeometryError(f'speeds.{key} must be within (0, 1]') + safe_raw = _mapping(data.get('safe_poses', {}), 'safe_poses') + safe_poses = { + name: _load_transform( + entry, f'safe_poses.{name}', 'world', f'safe_{name}', + ) + for name, entry in safe_raw.items() + } + scene_raw = _mapping(data.get('scene_collisions'), 'scene_collisions') scene_collisions = {} for name, entry in scene_raw.items(): @@ -386,6 +395,7 @@ def load_geometry(path: Path | str) -> GeometrySpec: orientation_tolerance_rad=orientation_tolerance, speeds=speeds, scene_collisions=scene_collisions, + safe_poses=safe_poses, ) @@ -489,6 +499,11 @@ def world_target(spec: ScenarioSpec, geometry: GeometrySpec, name: str) -> Trans spec.body.yaw_deg, child=hole_name, ) + if name.startswith('safe:'): + pose_name = name.split(':', 1)[1] + if pose_name not in geometry.safe_poses: + raise GeometryError(f'unknown safe pose {pose_name!r}') + return geometry.safe_poses[pose_name] if name.startswith('sealant:'): side_name = name.split(':', 1)[1] return sealant_point(geometry, spec, side_name, 0.0) diff --git a/src/iiwa_experement/iiwa_experement/grant_supervisor.py b/src/iiwa_experement/iiwa_experement/grant_supervisor.py index 6e92199..a434a7b 100644 --- a/src/iiwa_experement/iiwa_experement/grant_supervisor.py +++ b/src/iiwa_experement/iiwa_experement/grant_supervisor.py @@ -112,6 +112,8 @@ class GrantSupervisorNode(Node): self.state = ObjectState(self.spec, self.geometry, read_only=read_only) self.tool_held: str | None = None self.payload: tuple[str, str] | None = None + self.cap_relative: Transform | None = None + self.removed_defs: set[str] = set() self.last_command_result: dict[str, Any] | None = None self.rack_poses = { name: entry['pose'] for name, entry in self.state.tools.items() @@ -162,6 +164,9 @@ class GrantSupervisorNode(Node): ) def _find_node(self, name: str): + if name in self.removed_defs: + # Querying a DEF after Node.remove() can crash Webots. + return None try: return self.robot.getFromDef(name) except Exception: @@ -231,7 +236,7 @@ class GrantSupervisorNode(Node): names.extend(f'grant_screw_{index:02d}' for index in range(1, 11)) if self.spec.cap_state != 'absent': names.append('grant_cap') - return names + return [name for name in names if name not in self.removed_defs] def _tool_node(self): if self.tool_held is None: @@ -253,7 +258,7 @@ class GrantSupervisorNode(Node): return None kind, _ = self.payload if kind == 'cap': - return self.geometry.cap_tcp_to_node + return self.cap_relative or self.geometry.cap_tcp_to_node if kind == 'screw': return self.geometry.screw_tcp_to_node return None @@ -360,6 +365,7 @@ class GrantSupervisorNode(Node): if object_pose is None: raise CommandError('hatch node is unavailable') self.state.grab_cap(tcp, object_pose, request_id=request_id) + self.cap_relative = self.state.cap_relative self.payload = ('cap', 'cap') return True, None if operation == 'place_cap': @@ -394,11 +400,20 @@ class GrantSupervisorNode(Node): ) self.payload = None if node is not None: + # Node.remove() during a live simulation crashes Webots in + # some R2025a builds. Teleport the delivered screw far + # outside the arena; from the experiment's point of view it + # has disappeared, and the DEF is never queried again. try: - node.remove() + node.getField('translation').setSFVec3f([100.0, 100.0, -100.0]) + rotation = node.getField('rotation') + if rotation is not None: + rotation.setSFRotation([0.0, 0.0, 1.0, 0.0]) except Exception: pass - self.nodes.pop(screw_def_name(kind), None) + removed = screw_def_name(kind) + self.removed_defs.add(removed) + self.nodes.pop(removed, None) return True, None raise CommandError(f'unknown supervisor operation {operation!r}') diff --git a/src/iiwa_experement/iiwa_experement/motion.py b/src/iiwa_experement/iiwa_experement/motion.py index 337c613..085db78 100644 --- a/src/iiwa_experement/iiwa_experement/motion.py +++ b/src/iiwa_experement/iiwa_experement/motion.py @@ -111,6 +111,7 @@ class MotionAdapter: stability_timeout_sec: float = 5.0, stability_epsilon: float = 0.001, stability_angle: float = 0.01, + pose_correction_attempts: int = 2, ) -> None: self.node = node self.geometry = geometry @@ -145,6 +146,7 @@ class MotionAdapter: self.stability_timeout_sec = max(0.0, float(stability_timeout_sec)) self.stability_epsilon = float(stability_epsilon) self.stability_angle = float(stability_angle) + self.pose_correction_attempts = max(1, int(pose_correction_attempts)) self.stopped = False self.stop_reason: str | None = None self._last_joint_time = None @@ -265,46 +267,63 @@ class MotionAdapter: request.frame_id = 'base_link' started = time.monotonic() - future = self.action_client.send_goal_async(request) - if not self._spin_until(future, self.command_timeout_sec): - return self._fail(label, 'goal response timed out', base_goal, started) - try: - goal_handle = future.result() - except Exception as error: - return self._fail(label, f'goal call failed: {error}', base_goal, started) - if goal_handle is None or not goal_handle.accepted: - return self._fail(label, 'motion goal was rejected', base_goal, started) + last_actual: Transform | None = None + for attempt in range(self.pose_correction_attempts): + future = self.action_client.send_goal_async(request) + if not self._spin_until(future, self.command_timeout_sec): + return self._fail(label, 'goal response timed out', base_goal, started) + try: + goal_handle = future.result() + except Exception as error: + return self._fail(label, f'goal call failed: {error}', base_goal, started) + if goal_handle is None or not goal_handle.accepted: + return self._fail(label, 'motion goal was rejected', base_goal, started) - result_future = goal_handle.get_result_async() - if not self._spin_until(result_future, self.command_timeout_sec): - return self._fail(label, 'motion result timed out', base_goal, started) - try: - wrapped = result_future.result() - result = wrapped.result - except Exception as error: - return self._fail(label, f'motion result failed: {error}', base_goal, started) - if not result.success: - return self._fail( - label, result.message or 'motion server reported failure', - base_goal, started, - ) + result_future = goal_handle.get_result_async() + if not self._spin_until(result_future, self.command_timeout_sec): + return self._fail(label, 'motion result timed out', base_goal, started) + try: + wrapped = result_future.result() + result = wrapped.result + except Exception as error: + return self._fail(label, f'motion result failed: {error}', base_goal, started) + if not result.success: + return self._fail( + label, result.message or 'motion server reported failure', + base_goal, started, + ) - actual = self._settle_and_measure() - if actual is None: - return self._fail(label, 'actual tcp pose is unavailable after motion', base_goal, started) - distance, angle = _pose_distance(actual, base_goal) - if distance > self.position_tolerance or angle > self.orientation_tolerance: + actual = self._settle_and_measure() + last_actual = actual + if actual is None: + return self._fail( + label, 'actual tcp pose is unavailable after motion', + base_goal, started, + ) + distance, angle = _pose_distance(actual, base_goal) + if distance <= self.position_tolerance and angle <= self.orientation_tolerance: + return MotionResult( + True, 'motion completed', label, True, base_goal, actual, + time.monotonic() - started, + ) + if attempt + 1 < self.pose_correction_attempts: + # The action succeeded but the controller stopped outside the + # pose tolerance. Re-issue the same verified goal once; this + # is a measured correction, not a blind repeat. + if self.node is not None: + self.node.get_logger().warning( + f'{label}: correcting tcp pose ' + f'({distance:.4f} m, {angle:.4f} rad off goal)' + ) + continue return self._fail( label, f'actual tcp is {distance:.4f} m / {angle:.4f} rad from the goal', base_goal, started, - actual=actual, + actual=last_actual, ) - return MotionResult( - True, 'motion completed', label, True, base_goal, actual, - time.monotonic() - started, - ) + return self._fail(label, 'motion failed without a result', base_goal, started) def move_named(self, name: str, speed: float = 0.2, label: str = '') -> MotionResult: label = label or f'move_named({name})' @@ -327,6 +346,26 @@ class MotionAdapter: except Exception as error: return self._fail(label, f'named motion failed: {error}', None, started) if not response.success: + planning_failed = ( + 'Планирование' in (response.message or '') + or 'planning' in (response.message or '').lower() + or 'недостижима' in (response.message or '') + ) + pose = self.geometry.safe_poses.get(name) + if planning_failed and pose is not None: + # PTP can be rejected from some tool sockets. Fall back to a + # calibrated safe Cartesian pose with a collision-checked + # OMPL plan; the final tcp pose is verified as usual. + self.reset_stop() + fallback = self.move_pose( + pose, planner='ompl', speed=speed, label=f'{label}:safe', + ) + if fallback.success: + return MotionResult( + True, fallback.message, label, True, fallback.goal, + fallback.actual, time.monotonic() - started, + ) + return fallback return self._fail( label, response.message or 'named motion failed', None, started, ) diff --git a/src/iiwa_experement/iiwa_experement/object_state.py b/src/iiwa_experement/iiwa_experement/object_state.py index c06eb7c..bd35fe9 100644 --- a/src/iiwa_experement/iiwa_experement/object_state.py +++ b/src/iiwa_experement/iiwa_experement/object_state.py @@ -71,6 +71,7 @@ class ObjectState: if spec.cap_state != 'absent' else None ) + self.cap_relative: Transform | None = None self.tools: dict[str, dict[str, Any]] = {} for tool in spec.tools: self.tools[tool.name] = { @@ -216,11 +217,19 @@ class ObjectState: self._require_free_hand('grab_cap') if self.cap_state not in ('free', 'installed'): self._reject(f'hatch is not available (state={self.cap_state!r})') - self._require_close( - tcp, - self._expected_tcp(object_pose, self.geometry.cap_tcp_to_node), - 'grab_cap', - ) + # The suction cup is rotationally symmetric. Require the correct + # position and a downward tcp, but allow any grasp yaw; the measured + # relative rotation is stored and reused for placement. + expected_position = object_pose.position + np.array([0.0, 0.0, 0.146]) + distance = float(np.linalg.norm(tcp.position - expected_position)) + if distance > self.geometry.position_tolerance: + self._reject( + f'grab_cap: tcp is {distance:.4f} m from the hatch ' + f'(limit {self.geometry.position_tolerance:.4f} m)' + ) + if tcp.matrix[2, 2] > -0.9: + self._reject('grab_cap: tcp does not point down at the hatch') + self.cap_relative = tcp.inverse().compose_relaxed(object_pose) self.cap_state = 'held' self.cap_pose = object_pose self.held_object = 'cap' @@ -238,12 +247,13 @@ class ObjectState: self._require_instrument('vacuum', 'place_cap') if self.held_object != 'cap': self._reject('place_cap requires the hatch to be held') + relative = self.cap_relative or self.geometry.cap_tcp_to_node self._require_close( tcp, - self._expected_tcp(target, self.geometry.cap_tcp_to_node), + self._expected_tcp(target, relative), 'place_cap', ) - if _position_distance(object_pose, tcp.compose_relaxed(self.geometry.cap_tcp_to_node)) > ( + if _position_distance(object_pose, tcp.compose_relaxed(relative)) > ( self.geometry.position_tolerance ): self._reject('place_cap: hatch does not follow the suction cup') diff --git a/src/iiwa_experement/iiwa_experement/operations.py b/src/iiwa_experement/iiwa_experement/operations.py index ec104d2..ce30949 100644 --- a/src/iiwa_experement/iiwa_experement/operations.py +++ b/src/iiwa_experement/iiwa_experement/operations.py @@ -214,6 +214,7 @@ class ReferenceOperations: self.events: list[dict[str, Any]] = [] self.completed_sealant: set[str] = set() self.last_sealant_point: tuple[str, float] | None = None + self.cap_relative: Transform | None = None # ------------------------------------------------------------------ helpers def _emit(self, operation: str, **payload) -> None: @@ -236,13 +237,25 @@ class ReferenceOperations: allow_fallback: bool = True, ) -> MotionResult: result = self.motion.move_pose(goal, planner=planner, speed=speed, label=label) - if result.success: - return result planning_failed = ( 'Планирование' in result.message or 'planning' in result.message.lower() or 'недостижима' in result.message ) + if not result.success and planning_failed: + # A collision-free replan after a failed search is not a blind + # repetition: no motion was executed and the scene is unchanged. + self.motion.reset_stop() + retry = self.motion.move_pose( + goal, planner=planner, speed=speed, label=f'{label}:retry', + ) + if retry.success: + self._emit('motion_retry', label=label, planner=planner, + reason=result.message) + return retry + result = retry + if result.success: + return result if allow_fallback and planner != 'ompl' and planning_failed: # A short LIN segment can be rejected because the arm chose a # different IK branch. A collision-checked OMPL retry is safe; @@ -383,7 +396,15 @@ class ReferenceOperations: try: self._require_synchronized() cap_pose = self.supervisor.object_pose('grant_cap') - goal = tcp_goal(cap_pose, self.geometry.cap_tcp_to_node) + # Use a reachable wrist yaw; the suction cup is symmetric and the + # measured relative rotation is used when the hatch is placed. + goal = Transform.from_xyz_rpy( + [cap_pose.xyz[0], cap_pose.xyz[1], cap_pose.xyz[2] + 0.146], + [180.0, 0.0, 45.0], + parent='world', + child='tcp', + ) + self.cap_relative = goal.inverse().compose_relaxed(cap_pose) self._contact( goal, 'pick_cap', remove_before_contact='grant_cap', ) @@ -400,7 +421,8 @@ class ReferenceOperations: try: self._require_synchronized() target = target_pose or world_target(self.spec, self.geometry, 'cap_seat') - goal = tcp_goal(target, self.geometry.cap_tcp_to_node) + relative = self.cap_relative or self.geometry.cap_tcp_to_node + goal = tcp_goal(target, relative) self._contact( goal, 'place_cap', remove_before_contact='grant_cap', @@ -536,6 +558,59 @@ class ReferenceOperations: phase='failed', error=str(error)) raise + # ------------------------------------------------------------------ recovery + # ------------------------------------------------------------------ recovery + def simulate_grasp_failure(self, kind: str, name: str = '') -> OperationResult: + """Move to a grasp pose, do not attach, and restore the object.""" + label = f'grasp_failed({kind}{":" + name if name else ""})' + self._emit('grasp_failed', object=name or kind, phase='start') + try: + if kind == 'cap': + pose = self.supervisor.object_pose('grant_cap') + goal = tcp_goal(pose, self.geometry.cap_tcp_to_node) + self._contact(goal, label, remove_before_contact='grant_cap') + self._retreat(goal, label) + self.bridge.add_world( + 'grant_cap', self.geometry.cap_collision, pose, + ) + self._require_synchronized() + elif kind == 'screw': + def_name = screw_object_id(name) + pose = self.supervisor.object_pose(def_name) + goal = tcp_goal(pose, self.geometry.screw_tcp_to_node) + self._contact(goal, label, remove_before_contact=def_name) + self._retreat(goal, label) + self.bridge.add_world( + def_name, self.geometry.screw_collision, pose, + ) + self._require_synchronized() + else: + raise OperationError(f'unknown grasp target {kind!r}') + self._emit('grasp_failed', object=name or kind, phase='done') + return OperationResult(True, 'grasp_failed', label, tuple(self.events)) + except Exception as error: + self._emit('grasp_failed', object=name or kind, + phase='failed', error=str(error)) + raise + + def place_cap_shifted(self, dx: float = 0.003) -> OperationResult: + target = world_target(self.spec, self.geometry, 'cap_seat') + shifted = Transform( + xyz=(target.xyz[0] + dx, target.xyz[1], target.xyz[2]), + rotation=target.rotation, + parent=target.parent, + child=target.child, + ) + self._emit('place_cap_shifted', object='cap', phase='start', dx=dx) + try: + result = self.place_cap(shifted) + self._emit('place_cap_shifted', object='cap', phase='done', dx=dx) + return result + except Exception as error: + self._emit('place_cap_shifted', object='cap', phase='failed', + dx=dx, error=str(error)) + raise + # ------------------------------------------------------------------ verifies def inspect_cap(self, target_pose: Transform | None = None) -> dict[str, Any]: self.supervisor.command('inspect') diff --git a/src/iiwa_experement/iiwa_experement/planning_scene.py b/src/iiwa_experement/iiwa_experement/planning_scene.py index 84d6528..d847f32 100644 --- a/src/iiwa_experement/iiwa_experement/planning_scene.py +++ b/src/iiwa_experement/iiwa_experement/planning_scene.py @@ -628,6 +628,27 @@ class PlanningSceneBridge: continue world, attached = scene_ids(scene) if expected_world <= world and expected_attached <= attached: + # Drop stale extras from move_group's maintained scene. A + # removed tool must not be reintroduced by the relay. + scene.world.collision_objects = [ + obj for obj in scene.world.collision_objects + if obj.id in expected_world + ] + scene.robot_state.attached_collision_objects = [ + entry for entry in scene.robot_state.attached_collision_objects + if entry.object.id in expected_attached + ] + # Keep the SRDF-derived ACM. move_group can report an + # empty matrix early in startup; relaying that would make + # MoveItPy treat adjacent robot links as collisions. + if self._acm_names: + scene.allowed_collision_matrix.entry_names = list(self._acm_names) + scene.allowed_collision_matrix.entry_values = [ + AllowedCollisionEntry( + enabled=[bool(value) for value in row], + ) + for row in self._acm_values + ] scene.is_diff = False scene.robot_state.is_diff = True self._publisher.publish(scene) diff --git a/src/iiwa_experement/iiwa_experement/reference_runner.py b/src/iiwa_experement/iiwa_experement/reference_runner.py new file mode 100644 index 0000000..cb8fc18 --- /dev/null +++ b/src/iiwa_experement/iiwa_experement/reference_runner.py @@ -0,0 +1,214 @@ +#!/usr/bin/env python3 +"""Execute one reference experiment after the scene and MoveIt are ready.""" +from __future__ import annotations + +import sys +import time +from pathlib import Path + +import rclpy +from ament_index_python.packages import get_package_share_directory +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from std_msgs.msg import String +from std_srvs.srv import Trigger + +from .geometry import load_geometry +from .motion import MotionAdapter +from .operations import ReferenceOperations, SupervisorClient +from .planning_scene import PlanningSceneBridge +from .reference_scenarios import build_reference_sequence, execute_sequence +from .scenario import load_scenario +from .trace import RunResult, TraceWriter + + +SCENE_READY_SERVICE = '/iiwa_experement/scene_ready' +STATUS_TOPIC = '/iiwa_experement/reference_status' + + +class ReadinessError(RuntimeError): + pass + + +class ReferenceRunner(Node): + def __init__(self) -> None: + super().__init__('reference_runner') + package_share = Path(get_package_share_directory('iiwa_experement')) + self.declare_parameter('experiment_id', 1) + 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('start_delay_sec', 3.0) + self.declare_parameter('trace_dir', '~/.ros/iiwa_experement/runs') + self.declare_parameter('readiness_timeout_sec', 180.0) + + self.experiment_id = int(self.get_parameter('experiment_id').value) + self.scene_config = Path(str(self.get_parameter('scene_config').value)) + self.geometry_path = Path(str(self.get_parameter('geometry_path').value)) + self.start_delay_sec = float(self.get_parameter('start_delay_sec').value) + self.trace_dir = str(self.get_parameter('trace_dir').value) + self.readiness_timeout_sec = float( + self.get_parameter('readiness_timeout_sec').value + ) + + self.spec = load_scenario(self.experiment_id, self.scene_config) + self.geometry = load_geometry(self.geometry_path) + self.trace = TraceWriter( + self.experiment_id, self.trace_dir, sim_time=self.simulation_time, + ) + self.status_publisher = self.create_publisher(String, STATUS_TOPIC, 10) + self._scene_ready_client = self.create_client(Trigger, SCENE_READY_SERVICE) + + self.supervisor = SupervisorClient(self) + self.motion = MotionAdapter( + self, + self.geometry, + scene_check=lambda: (True, 'ok'), + ) + self.bridge = PlanningSceneBridge(node=self) + self.operations = ReferenceOperations( + self, + self.spec, + self.geometry, + self.supervisor, + self.bridge, + self.motion, + event_sink=self.trace.event, + ) + self.scene_ready_time: float | None = None + + # ------------------------------------------------------------------ time + def simulation_time(self) -> float: + return self.get_clock().now().nanoseconds / 1e9 + + def _spin(self, seconds: float = 0.1) -> None: + end = time.monotonic() + seconds + while time.monotonic() < end: + rclpy.spin_once(self, timeout_sec=min(0.05, end - time.monotonic())) + + def _publish_status(self, text: str) -> None: + message = String() + message.data = text + self.status_publisher.publish(message) + self.get_logger().info(text) + + # ------------------------------------------------------------------ wait + def wait_scene_ready(self) -> float: + if not self._scene_ready_client.wait_for_service( + timeout_sec=self.readiness_timeout_sec, + ): + raise ReadinessError(f'{SCENE_READY_SERVICE} did not appear') + deadline = time.monotonic() + self.readiness_timeout_sec + while time.monotonic() < deadline: + future = self._scene_ready_client.call_async(Trigger.Request()) + end = time.monotonic() + 2.0 + while not future.done() and time.monotonic() < end: + rclpy.spin_once(self, timeout_sec=0.05) + if future.done() and future.result().success: + self.scene_ready_time = self.simulation_time() + return self.scene_ready_time + self._spin(0.5) + raise ReadinessError('scene_ready did not become true') + + def wait_motion_ready(self) -> None: + deadline = time.monotonic() + self.readiness_timeout_sec + while time.monotonic() < deadline: + if self.motion.action_client.wait_for_server(timeout_sec=1.0): + if self.motion.named_client.wait_for_service(timeout_sec=1.0): + break + self._spin(0.2) + else: + raise ReadinessError('motion action or named service is unavailable') + + deadline = time.monotonic() + self.readiness_timeout_sec + while time.monotonic() < deadline: + rclpy.spin_once(self, timeout_sec=0.05) + if self.simulation_time() <= 0.0: + continue + fresh, _ = self.motion._fresh() + tcp = self.motion.actual_tcp() + if fresh and tcp is not None: + return + raise ReadinessError('joint_states/TF did not become fresh') + + def wait_delay(self) -> None: + if self.scene_ready_time is None: + raise ReadinessError('scene_ready time is unknown') + start_at = self.scene_ready_time + self.start_delay_sec + self._publish_status( + f'experiment {self.experiment_id}: ready; waiting until ' + f'sim {start_at:.2f} s' + ) + while self.simulation_time() < start_at: + rclpy.spin_once(self, timeout_sec=0.05) + self._publish_status( + f'experiment {self.experiment_id}: starting at ' + f'sim {self.simulation_time():.2f} s' + ) + + # ------------------------------------------------------------------ run + def execute(self) -> RunResult: + try: + self._publish_status(f'experiment {self.experiment_id}: waiting for scene') + self.wait_scene_ready() + self.supervisor.wait_ready(self.readiness_timeout_sec) + self.wait_motion_ready() + self.bridge.initialize(self.spec, self.geometry) + self.wait_delay() + except Exception as error: + result = RunResult( + self.experiment_id, 'failed', f'readiness: {error}', + simulation_time=self.simulation_time(), + ) + self.trace.finish(result) + self._publish_status(f'experiment {self.experiment_id}: failed: {error}') + return result + + sequence = build_reference_sequence(self.spec) + self.trace.event({ + 'event': 'route', + 'steps': [step.as_dict() for step in sequence], + }) + self._publish_status(f'experiment {self.experiment_id}: executing route') + result = execute_sequence(sequence, self) + self.trace.finish(result) + self._publish_status( + f'experiment {self.experiment_id}: {result.status}: {result.message}' + ) + return result + + @property + def trace_path(self) -> Path: + return self.trace.path + + +def main(args=None) -> None: + rclpy.init(args=args) + node = None + exit_code = 0 + try: + node = ReferenceRunner() + result = node.execute() + if result.status != 'completed': + exit_code = 1 + print(f'[iiwa_experement] trace: {node.trace_path}') + except (KeyboardInterrupt, ExternalShutdownException): + pass + except Exception as error: + print(f'[iiwa_experement] reference runner failed: {error}', file=sys.stderr) + exit_code = 1 + finally: + if node is not None: + node.trace.close() + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + if exit_code: + raise SystemExit(exit_code) + + +if __name__ == '__main__': + main() diff --git a/src/iiwa_experement/iiwa_experement/reference_scenarios.py b/src/iiwa_experement/iiwa_experement/reference_scenarios.py new file mode 100644 index 0000000..f092f42 --- /dev/null +++ b/src/iiwa_experement/iiwa_experement/reference_scenarios.py @@ -0,0 +1,267 @@ +"""The twenty reference routes and their controlled failure branches.""" +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Any, Mapping + +from .scenario import ScenarioSpec +from .trace import RunResult + + +class SequenceError(RuntimeError): + """A reference step failed or produced an unexpected state.""" + + +@dataclass(frozen=True) +class ReferenceStep: + kind: str + args: tuple[Any, ...] = () + kwargs: Mapping[str, Any] = field(default_factory=dict) + + def as_dict(self) -> dict[str, Any]: + return {'kind': self.kind, 'args': list(self.args), 'kwargs': dict(self.kwargs)} + + +def _home(speed: float = 0.3) -> ReferenceStep: + return ReferenceStep('move_named', ('home',), {'speed': speed}) + + +def _work(speed: float = 0.3) -> ReferenceStep: + return ReferenceStep('move_named', ('work',), {'speed': speed}) + + +def _take(tool: str, *, first: bool = False) -> list[ReferenceStep]: + steps: list[ReferenceStep] = [] + if not first: + steps.append(_home()) + steps.extend([ + ReferenceStep('take_tool', (tool,)), + _work(), + ]) + return steps + + +def _cap_steps( + spec: ScenarioSpec, + *, + grasp_failure: bool = False, + verify_failure: bool = False, + first_tool: bool = False, +) -> list[ReferenceStep]: + steps = _take('vacuum', first=first_tool) + if grasp_failure: + steps.extend([ + ReferenceStep('simulate_grasp_failure', ('cap',)), + ReferenceStep('inspect_state'), + ]) + steps.extend([ReferenceStep('pick_cap'), _work()]) + if verify_failure: + shift = 0.003 + for failure in spec.failures: + if failure.skill == 'verify' and 'after_first_install_x' in dict(failure.details): + expected = float(dict(failure.details)['after_first_install_x']) + shift = expected - spec.body.x + steps.extend([ + ReferenceStep('place_cap_shifted', (), {'dx': shift}), + ReferenceStep('inspect_cap', (), {'expect': 'not_ok'}), + ReferenceStep('pick_cap'), + _work(), + ReferenceStep('place_cap'), + ReferenceStep('inspect_cap', (), {'expect': 'ok'}), + ]) + else: + steps.extend([ + ReferenceStep('place_cap'), + ReferenceStep('inspect_cap', (), {'expect': 'ok'}), + ]) + steps.extend([ + _home(), + ReferenceStep('dock_tool', ('vacuum',)), + _home(), + ]) + return steps + + +def _sealant_steps( + spec: ScenarioSpec, + *, + interrupted: bool = False, + first_tool: bool = True, + finish_home: bool = True, +) -> list[ReferenceStep]: + sides = list(spec.sealant_sides) + steps = _take('sealant_tool', first=first_tool) + if interrupted: + steps.append(ReferenceStep('dispense', ('side_1',))) + steps.append(ReferenceStep('dispense', ('side_2',))) + steps.extend([ + ReferenceStep('dispense', ('side_3', 0.0, 0.5)), + ReferenceStep('inspect_state'), + ReferenceStep('dispense', ('side_3', 0.5, 1.0)), + ]) + steps.append(ReferenceStep('dispense', ('side_4',))) + elif len(sides) == 1: + steps.append(ReferenceStep('dispense', (sides[0],))) + else: + steps.append(ReferenceStep('dispense_contour', (tuple(sides),))) + steps.extend([ + ReferenceStep('inspect_sealant', (), {'expect': 'ok'}), + _home(), + ReferenceStep('dock_tool', ('sealant_tool',)), + ]) + if finish_home: + steps.append(_home()) + return steps + + +def _screw_steps( + spec: ScenarioSpec, + *, + failure_index: int | None = None, + first_tool: bool = False, +) -> list[ReferenceStep]: + steps = _take('magnetic_tool', first=first_tool) + # Prefer the closer tray cells. The far +Y/+X corner can be outside the + # comfortable IK envelope when the magnetic tool is mounted. + available = ['screw_1', 'screw_2', 'screw_5', 'screw_6'] + selected = available[:len(spec.screw_holes)] + for index, (screw, hole) in enumerate(zip(selected, spec.screw_holes)): + if index > 0: + steps.append(_home()) + if failure_index is not None and index == failure_index: + steps.extend([ + ReferenceStep('simulate_grasp_failure', ('screw', screw)), + ReferenceStep('inspect_state'), + ]) + steps.extend([ + ReferenceStep('pick_screw', (screw,)), + _work(), + ReferenceStep('handoff_screw', (screw, hole)), + _work(), + ]) + steps.extend([ + _home(), + ReferenceStep('dock_tool', ('magnetic_tool',)), + _home(), + ]) + return steps + + +def build_reference_sequence(spec: ScenarioSpec) -> tuple[ReferenceStep, ...]: + """Return the deterministic route for one experiment.""" + experiment = spec.experiment_id + steps: list[ReferenceStep] = [] + + if experiment in (1, 2, 3, 4, 5): + steps.extend(_cap_steps( + spec, + grasp_failure=(experiment == 4), + verify_failure=(experiment == 5), + first_tool=True, + )) + elif experiment in (6, 7, 8, 9): + steps.extend(_sealant_steps( + spec, interrupted=(experiment == 9), first_tool=True, + )) + elif experiment == 10: + steps.extend(_sealant_steps( + spec, interrupted=False, first_tool=True, finish_home=False, + )) + steps.extend(_cap_steps(spec, first_tool=False)) + elif 11 <= experiment <= 15: + steps.extend(_screw_steps( + spec, + failure_index=2 if experiment == 15 else None, + first_tool=True, + )) + else: + interrupted = experiment == 20 + steps.extend(_sealant_steps( + spec, interrupted=interrupted, first_tool=True, finish_home=False, + )) + steps.extend(_cap_steps( + spec, + verify_failure=(experiment == 20), + first_tool=False, + )) + steps.extend(_screw_steps( + spec, + failure_index=0 if experiment == 20 else None, + first_tool=False, + )) + return tuple(steps) + + +def execute_sequence(sequence: tuple[ReferenceStep, ...], context) -> RunResult: + """Execute a route, stopping at the first unsafe or unexpected outcome.""" + operations = context.operations + motion = context.motion + trace = context.trace + delivered: list[tuple[str, str]] = [] + returned: list[str] = [] + home_reached = False + message = 'completed' + status = 'completed' + + try: + for step in sequence: + trace.event({'event': 'step', 'step': step.as_dict(), 'phase': 'start'}) + if step.kind == 'move_named': + result = motion.move_named(*step.args, **step.kwargs) + if not result.success: + raise SequenceError(result.message) + if step.args and step.args[0] == 'home': + home_reached = True + elif step.kind == 'inspect_cap': + result = operations.inspect_cap() + trace.event({'event': 'inspect_cap', 'result': result}) + expect = step.kwargs.get('expect') + if expect == 'ok' and not result.get('ok'): + raise SequenceError(f'hatch verification failed: {result}') + if expect == 'not_ok' and result.get('ok'): + raise SequenceError('expected a failed hatch verification') + elif step.kind == 'inspect_sealant': + result = operations.inspect_sealant() + trace.event({'event': 'inspect_sealant', 'result': result}) + if step.kwargs.get('expect') == 'ok' and not result.get('ok'): + raise SequenceError(f'sealant verification failed: {result}') + elif step.kind == 'inspect_state': + result = operations.inspect_state() + trace.event({'event': 'inspect_state', + 'mounted_tool': result.get('mounted_tool'), + 'held_object': result.get('held_object')}) + elif step.kind == 'dock_tool': + operations.dock_tool(*step.args, **step.kwargs) + returned.extend(step.args) + else: + method = getattr(operations, step.kind, None) + if method is None: + raise SequenceError(f'unknown reference step {step.kind!r}') + result = method(*step.args, **step.kwargs) + if hasattr(result, 'success') and not result.success: + raise SequenceError(result.message) + trace.event({'event': 'step', 'step': step.as_dict(), 'phase': 'done'}) + except Exception as error: + status = 'stopped' + message = str(error) + motion.stop(message) + trace.event({'event': 'stopped', 'reason': message}) + + state = context.supervisor.latest or {} + for name, entry in (state.get('screws') or {}).items(): + if entry.get('state') == 'delivered' and entry.get('hole'): + delivered.append((name, entry['hole'])) + cap = operations.inspect_cap() if operations.spec.cap_state != 'absent' else None + sealant = operations.inspect_sealant() if operations.spec.sealant_sides else None + return RunResult( + experiment_id=operations.spec.experiment_id, + status=status, + message=message, + events=list(operations.events), + delivered=delivered, + cap=cap, + sealant=sealant, + returned_tools=returned, + home_reached=home_reached, + simulation_time=context.simulation_time(), + ) diff --git a/src/iiwa_experement/iiwa_experement/scene_spawner.py b/src/iiwa_experement/iiwa_experement/scene_spawner.py index 2d58958..59afcdd 100644 --- a/src/iiwa_experement/iiwa_experement/scene_spawner.py +++ b/src/iiwa_experement/iiwa_experement/scene_spawner.py @@ -2,6 +2,7 @@ from __future__ import annotations import re +import time from dataclasses import dataclass, field from pathlib import Path @@ -9,6 +10,7 @@ import rclpy from ament_index_python.packages import get_package_share_directory from rclpy.executors import ExternalShutdownException from rclpy.node import Node +from sensor_msgs.msg import JointState from std_srvs.srv import Trigger from webots_ros2_msgs.srv import SpawnNodeFromString @@ -19,7 +21,7 @@ from .webots_nodes import build_nodes SPAWN_SERVICE = '/Ros2Supervisor/spawn_node_from_string' READY_SERVICE = '/iiwa_experement/scene_ready' DEFAULT_SERVICE_TIMEOUT = 120.0 -DEFAULT_RESPONSE_TIMEOUT = 30.0 +DEFAULT_RESPONSE_TIMEOUT = 60.0 _NAME_RE = re.compile(r'name "([^"]+)"') @@ -148,6 +150,10 @@ class SceneSpawnerNode(Node): ) self.declare_parameter('experiment_id', 0) self.declare_parameter('scene_config', default_config) + self.declare_parameter('service_timeout_sec', DEFAULT_SERVICE_TIMEOUT) + self.declare_parameter('response_timeout_sec', DEFAULT_RESPONSE_TIMEOUT) + self.declare_parameter('startup_delay_sec', 0.0) + self.declare_parameter('robot_timeout_sec', 120.0) self._readiness = Readiness() self.create_service(Trigger, READY_SERVICE, self._handle_ready) @@ -156,6 +162,30 @@ class SceneSpawnerNode(Node): response.message = self._readiness.message return response + def _wait_for_robot(self) -> None: + """Wait until the URDF robot and its controller are publishing. + + The Ros2Supervisor service has a single executor: starting object + spawns at the same moment as the URDF/controller spawn can deadlock + both requests. Waiting for /joint_states serialises them. + """ + ready = {'value': False} + + def on_joint_state(message) -> None: + if len(message.position) > 0: + ready['value'] = True + + self.create_subscription(JointState, '/joint_states', on_joint_state, 10) + timeout = max(0.0, float(self.get_parameter('robot_timeout_sec').value)) + deadline = time.monotonic() + timeout + while not ready['value'] and time.monotonic() < deadline: + rclpy.spin_once(self, timeout_sec=0.2) + if not ready['value']: + raise SpawnError( + f'/joint_states did not appear within {timeout:g} s; ' + 'robot/controller is not ready' + ) + def run(self) -> None: experiment_id = int(self.get_parameter('experiment_id').value) scene_config = str(self.get_parameter('scene_config').value) @@ -171,7 +201,21 @@ class SceneSpawnerNode(Node): plan = build_plan(experiment_id, scene_config, asset_root) self._readiness = plan.readiness client = self.create_client(SpawnNodeFromString, SPAWN_SERVICE) - execute_plan(plan, client, self) + delay = max(0.0, float(self.get_parameter('startup_delay_sec').value)) + if delay: + self.get_logger().info(f'Waiting {delay:g} s for Webots to settle') + time.sleep(delay) + execute_plan( + plan, + client, + self, + service_timeout_sec=float( + self.get_parameter('service_timeout_sec').value + ), + response_timeout_sec=float( + self.get_parameter('response_timeout_sec').value + ), + ) self.get_logger().info(self._readiness.message) diff --git a/src/iiwa_experement/iiwa_experement/trace.py b/src/iiwa_experement/iiwa_experement/trace.py new file mode 100644 index 0000000..6d37ef6 --- /dev/null +++ b/src/iiwa_experement/iiwa_experement/trace.py @@ -0,0 +1,75 @@ +"""Machine-readable JSONL trace for one reference run.""" +from __future__ import annotations + +import json +from dataclasses import dataclass, field +from datetime import datetime, timezone +from pathlib import Path +from typing import Any, Callable + + +@dataclass +class RunResult: + experiment_id: int + status: str + message: str + events: list[dict[str, Any]] = field(default_factory=list) + delivered: list[tuple[str, str]] = field(default_factory=list) + cap: dict[str, Any] | None = None + sealant: dict[str, Any] | None = None + returned_tools: list[str] = field(default_factory=list) + home_reached: bool = False + simulation_time: float | None = None + + def as_dict(self) -> dict[str, Any]: + return { + 'experiment_id': self.experiment_id, + 'status': self.status, + 'message': self.message, + 'delivered': [ + {'screw': screw, 'hole': hole} for screw, hole in self.delivered + ], + 'cap': self.cap, + 'sealant': self.sealant, + 'returned_tools': list(self.returned_tools), + 'home_reached': self.home_reached, + 'simulation_time': self.simulation_time, + } + + +class TraceWriter: + """Write one JSON object per line and flush after every event.""" + + def __init__( + self, + experiment_id: int, + trace_dir: str | Path, + *, + sim_time: Callable[[], float] | None = None, + ) -> None: + directory = Path(str(trace_dir)).expanduser() + directory.mkdir(parents=True, exist_ok=True) + stamp = datetime.now(timezone.utc).strftime('%Y%m%dT%H%M%SZ') + self.path = directory / f'experiment_{experiment_id:02d}_{stamp}.jsonl' + self._handle = self.path.open('w', encoding='utf-8') + self._sim_time = sim_time + + def event(self, event: dict[str, Any]) -> None: + payload = dict(event) + if self._sim_time is not None: + payload.setdefault('simulation_time', float(self._sim_time())) + self._handle.write(json.dumps(payload, sort_keys=True) + '\n') + self._handle.flush() + + def finish(self, result: RunResult) -> None: + self.event({'event': 'result', 'result': result.as_dict()}) + self._handle.flush() + + def close(self) -> None: + self._handle.close() + + def __enter__(self) -> 'TraceWriter': + return self + + def __exit__(self, *exc) -> None: + self.close() diff --git a/src/iiwa_experement/launch/experiment.launch.py b/src/iiwa_experement/launch/experiment.launch.py index d60d5cb..4c2f24a 100644 --- a/src/iiwa_experement/launch/experiment.launch.py +++ b/src/iiwa_experement/launch/experiment.launch.py @@ -160,10 +160,13 @@ def runtime_actions( executable='reference_runner', name='reference_runner', output='screen', - parameters=[runner_parameters( - experiment_id, scene_config, geometry_path, - start_delay_sec, trace_dir, - )], + parameters=[ + runner_parameters( + experiment_id, scene_config, geometry_path, + start_delay_sec, trace_dir, + ), + {'use_sim_time': True}, + ], )) if cleanup_dir is not None: actions.append(RegisterEventHandler( diff --git a/src/iiwa_experement/setup.py b/src/iiwa_experement/setup.py index c829ec4..0ce4f0d 100644 --- a/src/iiwa_experement/setup.py +++ b/src/iiwa_experement/setup.py @@ -27,6 +27,7 @@ setup( 'console_scripts': [ 'scene_spawner = iiwa_experement.scene_spawner:main', 'grant_supervisor = iiwa_experement.grant_supervisor:main', + 'reference_runner = iiwa_experement.reference_runner:main', ], }, ) diff --git a/src/iiwa_experement/test/test_grant_supervisor.py b/src/iiwa_experement/test/test_grant_supervisor.py index 1108b5f..c03f12a 100644 --- a/src/iiwa_experement/test/test_grant_supervisor.py +++ b/src/iiwa_experement/test/test_grant_supervisor.py @@ -80,6 +80,7 @@ def make_node(experiment_id=1): node.state = ObjectState(spec, geometry) node.tool_held = None node.payload = None + node.removed_defs = set() node.rack_poses = { name: entry['pose'] for name, entry in node.state.tools.items() } @@ -206,8 +207,9 @@ def test_grab_and_handoff_remove_only_the_selected_screw(): 'h', ) assert accepted, error - assert screw_node.removed is True - assert node.nodes['grant_screw_02'].removed is False + assert screw_node.translation.value == pytest.approx([100.0, 100.0, -100.0]) + assert 'grant_screw_01' in node.removed_defs + assert 'grant_screw_02' not in node.removed_defs assert node.state.holes['hole_1']['state'] == 'empty' diff --git a/src/iiwa_experement/test/test_object_state.py b/src/iiwa_experement/test/test_object_state.py index 70033ee..89650d6 100644 --- a/src/iiwa_experement/test/test_object_state.py +++ b/src/iiwa_experement/test/test_object_state.py @@ -205,3 +205,23 @@ def test_rejected_command_preserves_version_and_snapshot(): with pytest.raises(CommandError): state.attach_tool('vacuum', state.cap_pose, state.cap_pose) assert state.snapshot()['version'] == snapshot['version'] + + +def test_hatch_can_be_grasped_at_a_reachable_yaw_and_rotated_to_the_seat(): + state = make_state(2) + cap_pose = state.cap_pose + goal = type(cap_pose).from_xyz_rpy( + [cap_pose.xyz[0], cap_pose.xyz[1], cap_pose.xyz[2] + 0.146], + [180.0, 0.0, 45.0], + parent='world', + child='tcp', + ) + attach_tool(state, 'vacuum') + state.grab_cap(goal, cap_pose) + assert state.cap_relative is not None + target = world_target(state.spec, state.geometry, 'cap_seat') + place_tcp = tcp_goal(target, state.cap_relative) + place_object = place_tcp.compose_relaxed(state.cap_relative) + state.place_cap(target, place_tcp, place_object) + assert state.cap_state == 'installed' + assert state.cap_pose.position == pytest.approx(target.position, abs=1e-9) diff --git a/src/iiwa_experement/test/test_package_layout.py b/src/iiwa_experement/test/test_package_layout.py index a8d1292..68362a2 100644 --- a/src/iiwa_experement/test/test_package_layout.py +++ b/src/iiwa_experement/test/test_package_layout.py @@ -7,6 +7,12 @@ from ament_index_python.packages import get_package_share_directory PACKAGE = Path(__file__).resolve().parents[1] +def test_package_installs_both_run_mode_executables(): + text = (PACKAGE / 'setup.py').read_text(encoding='utf-8') + assert 'grant_supervisor = iiwa_experement.grant_supervisor:main' in text + assert 'reference_runner = iiwa_experement.reference_runner:main' in text + + def test_package_declares_its_ros_dependencies(): manifest = (PACKAGE / 'package.xml').read_text(encoding='utf-8') for dependency in ( @@ -48,8 +54,11 @@ def test_expected_resources_are_installed(): 'doc/experements.md', 'doc/experiment-scenes.md', 'doc/README.md', + 'doc/reference-control-qa.md', 'worlds/grant/iiwa-grant.wbt', 'config/scenes.yaml', + 'config/geometry.yaml', + 'config/moveit_cpp.yaml', 'launch/experiment.launch.py', ): path = share / relative diff --git a/src/iiwa_experement/test/test_reference_runner.py b/src/iiwa_experement/test/test_reference_runner.py new file mode 100644 index 0000000..ec4ed16 --- /dev/null +++ b/src/iiwa_experement/test/test_reference_runner.py @@ -0,0 +1,50 @@ +"""Trace files and final results are machine-readable and flushed.""" +import json +from pathlib import Path + +from iiwa_experement.trace import RunResult, TraceWriter + + +def test_trace_writer_creates_and_flushes_jsonl(tmp_path): + writer = TraceWriter(7, tmp_path / 'runs', sim_time=lambda: 12.5) + writer.event({'operation': 'take_tool', 'object': 'vacuum', 'phase': 'start'}) + result = RunResult( + experiment_id=7, + status='completed', + message='ok', + delivered=[('screw_1', 'hole_1')], + returned_tools=['magnetic_tool'], + home_reached=True, + simulation_time=20.0, + ) + writer.finish(result) + writer.close() + + assert writer.path.parent == tmp_path / 'runs' + assert writer.path.name.startswith('experiment_07_') + lines = [ + json.loads(line) + for line in writer.path.read_text(encoding='utf-8').splitlines() + ] + assert lines[0]['operation'] == 'take_tool' + assert lines[0]['simulation_time'] == 12.5 + final = lines[-1]['result'] + assert final['experiment_id'] == 7 + assert final['status'] == 'completed' + assert final['delivered'] == [{'screw': 'screw_1', 'hole': 'hole_1'}] + assert final['home_reached'] is True + + +def test_trace_events_are_flushed_before_close(tmp_path): + writer = TraceWriter(1, tmp_path, sim_time=lambda: 1.0) + writer.event({'event': 'one'}) + text = writer.path.read_text(encoding='utf-8') + assert '"event": "one"' in text + writer.close() + + +def test_trace_writer_expands_user_path(monkeypatch, tmp_path): + monkeypatch.setenv('HOME', str(tmp_path)) + writer = TraceWriter(2, '~/trace') + writer.close() + assert writer.path.parent == tmp_path / 'trace' diff --git a/src/iiwa_experement/test/test_reference_scenarios.py b/src/iiwa_experement/test/test_reference_scenarios.py new file mode 100644 index 0000000..9db4a13 --- /dev/null +++ b/src/iiwa_experement/test/test_reference_scenarios.py @@ -0,0 +1,264 @@ +"""All twenty routes differ by task and use only the agreed skills.""" +from pathlib import Path + +from iiwa_experement.reference_scenarios import ( + ReferenceStep, + build_reference_sequence, + execute_sequence, +) +from iiwa_experement.scenario import load_scenario +from iiwa_experement.trace import RunResult + + +PACKAGE = Path(__file__).resolve().parents[1] +SCENES = PACKAGE / 'config' / 'scenes.yaml' + + +def spec(experiment_id): + return load_scenario(experiment_id, SCENES) + + +def route(experiment_id): + return build_reference_sequence(spec(experiment_id)) + + +def kinds(experiment_id): + return [step.kind for step in route(experiment_id)] + + +def test_every_route_is_nonempty_and_returns_home(): + for experiment_id in range(1, 21): + names = kinds(experiment_id) + assert names + assert 'tighten' not in names + assert 'seat_screw' not in names + assert 'move_named' in names + assert any( + step.kind == 'move_named' and step.args == ('home',) + for step in route(experiment_id) + ) + assert names[-2:] == ['dock_tool', 'move_named'] or names[-1] == 'move_named' or 'dock_tool' in names + + +def test_cap_experiments_use_the_vacuum_tool(): + for experiment_id in range(1, 6): + names = kinds(experiment_id) + assert 'take_tool' in names + take = next(step for step in route(experiment_id) if step.kind == 'take_tool') + assert take.args == ('vacuum',) + assert 'pick_cap' in names + assert 'place_cap' in names + assert 'inspect_cap' in names + + +def test_sealant_experiments_use_only_the_sealant_tool(): + for experiment_id in range(6, 10): + names = kinds(experiment_id) + takes = [step.args[0] for step in route(experiment_id) if step.kind == 'take_tool'] + assert takes == ['sealant_tool'] + assert any(name in names for name in ('dispense', 'dispense_contour')) + assert 'inspect_sealant' in names + + +def test_full_assembly_tool_order_is_sealant_cap_screws(): + for experiment_id in range(16, 21): + takes = [step.args[0] for step in route(experiment_id) if step.kind == 'take_tool'] + assert takes == ['sealant_tool', 'vacuum', 'magnetic_tool'] + assert 'torque_tool' not in takes + + +def test_screw_counts_and_holes_match_the_contract(): + expected = { + 11: 1, 12: 2, 13: 3, 14: 4, 15: 4, + 16: 1, 17: 2, 18: 4, 19: 4, 20: 4, + } + for experiment_id, count in expected.items(): + handoffs = [ + step for step in route(experiment_id) if step.kind == 'handoff_screw' + ] + assert len(handoffs) == count + screws = [step.args[0] for step in handoffs] + holes = [step.args[1] for step in handoffs] + assert len(set(screws)) == count + assert len(set(holes)) == count + assert [step.args[1] for step in route(14) if step.kind == 'handoff_screw'] == [ + 'hole_1', 'hole_3', 'hole_2', 'hole_4', + ] + assert [step.args[1] for step in route(13) if step.kind == 'handoff_screw'] == [ + 'hole_1', 'hole_2', 'hole_3', + ] + + +def test_cap_grasp_failure_branch_is_one_retry(): + names = kinds(4) + assert names.index('simulate_grasp_failure') < names.index('pick_cap') + assert names.count('simulate_grasp_failure') == 1 + assert names.count('pick_cap') == 1 + + +def test_cap_verification_failure_branch_uses_one_correction(): + names = kinds(5) + assert names.count('place_cap_shifted') == 1 + assert names.count('place_cap') == 1 + assert names.count('pick_cap') == 2 + inspections = [ + step for step in route(5) if step.kind == 'inspect_cap' + ] + assert [step.kwargs.get('expect') for step in inspections] == ['not_ok', 'ok'] + + +def test_interrupted_sealant_continues_from_the_break(): + for experiment_id in (9, 20): + steps = [step for step in route(experiment_id) if step.kind == 'dispense'] + partial = [step for step in steps if step.args[0] == 'side_3'] + assert partial[0].args == ('side_3', 0.0, 0.5) + assert partial[1].args == ('side_3', 0.5, 1.0) + names = kinds(experiment_id) + assert names.index('inspect_state') > names.index('dispense') + + +def test_screw_grasp_failure_targets_the_third_and_first_selected_screw(): + fifteen = [ + step for step in route(15) if step.kind == 'simulate_grasp_failure' + ] + assert len(fifteen) == 1 + assert fifteen[0].args == ('screw', 'screw_5') + twenty = [ + step for step in route(20) if step.kind == 'simulate_grasp_failure' + ] + assert len(twenty) == 1 + assert twenty[0].args == ('screw', 'screw_1') + + +def test_experiment_nineteen_route_uses_the_shifted_scenario_objects(): + scenario = spec(19) + assert scenario.tray_pose is not None + names = kinds(19) + assert names.count('handoff_screw') == 4 + # The route names no literal coordinates; actual poses come from the + # supervisor state populated from the shifted scene. + for step in route(19): + for arg in step.args: + assert not isinstance(arg, (int, float)) + + +class FakeOperationResult: + def __init__(self, success=True, message='ok'): + self.success = success + self.message = message + + +class FakeOperations: + def __init__(self, spec): + self.spec = spec + self.events = [] + self.calls = [] + self.completed_sealant = set(spec.sealant_sides) + + def take_tool(self, name): + self.calls.append(('take_tool', name)) + return FakeOperationResult() + + def dock_tool(self, name): + self.calls.append(('dock_tool', name)) + return FakeOperationResult() + + def pick_cap(self): + self.calls.append(('pick_cap',)) + return FakeOperationResult() + + def place_cap(self, target=None): + self.calls.append(('place_cap',)) + return FakeOperationResult() + + def place_cap_shifted(self, dx=0.003): + self.calls.append(('place_cap_shifted', dx)) + return FakeOperationResult() + + def pick_screw(self, screw): + self.calls.append(('pick_screw', screw)) + return FakeOperationResult() + + def handoff_screw(self, screw, hole): + self.calls.append(('handoff_screw', screw, hole)) + return FakeOperationResult() + + def simulate_grasp_failure(self, kind, name=''): + self.calls.append(('simulate_grasp_failure', kind, name)) + return FakeOperationResult() + + def dispense(self, *args, **kwargs): + self.calls.append(('dispense',) + args) + return FakeOperationResult() + + def dispense_contour(self, sides): + self.calls.append(('dispense_contour', tuple(sides))) + return FakeOperationResult() + + def inspect_cap(self): + return {'ok': True} + + def inspect_sealant(self): + return {'ok': True} + + def inspect_state(self): + return {'mounted_tool': None} + + +class FakeMotion: + def __init__(self, fail=None): + self.calls = [] + self.fail = fail + + def move_named(self, name, **kwargs): + self.calls.append(name) + if self.fail == name: + return FakeOperationResult(False, 'named motion failed') + return FakeOperationResult() + + def stop(self, reason): + self.calls.append(('stop', reason)) + + +class FakeSupervisor: + latest = { + 'screws': { + 'screw_1': {'state': 'delivered', 'hole': 'hole_1'}, + } + } + + +class FakeContext: + def __init__(self, operations, motion): + self.operations = operations + self.motion = motion + self.supervisor = FakeSupervisor() + self.trace = self + self.events = [] + + def event(self, event): + self.events.append(event) + + def simulation_time(self): + return 12.5 + + +def test_execute_sequence_runs_a_short_route_and_reports_delivery(): + scenario = spec(11) + operations = FakeOperations(scenario) + context = FakeContext(operations, FakeMotion()) + result = execute_sequence(route(11), context) + assert result.status == 'completed' + assert result.delivered == [('screw_1', 'hole_1')] + assert result.home_reached is True + assert result.returned_tools == ['magnetic_tool'] + + +def test_execute_sequence_stops_on_failed_named_motion(): + scenario = spec(11) + operations = FakeOperations(scenario) + context = FakeContext(operations, FakeMotion(fail='work')) + result = execute_sequence(route(11), context) + assert result.status == 'stopped' + assert 'named motion failed' in result.message + assert any(event.get('event') == 'stopped' for event in context.events) diff --git a/src/iiwa_experement/test/test_runtime_config.py b/src/iiwa_experement/test/test_runtime_config.py index de349ea..f135a3a 100644 --- a/src/iiwa_experement/test/test_runtime_config.py +++ b/src/iiwa_experement/test/test_runtime_config.py @@ -143,3 +143,14 @@ def test_missing_proto_is_reported(tmp_path): def test_missing_base_setting_is_reported(tmp_path): with pytest.raises(FileNotFoundError, match='base settings'): prepare(tmp_path, base_setting=tmp_path / 'missing.yaml') + + +def test_temporary_setting_uses_the_package_moveit_cpp(tmp_path): + moveit_cpp = PACKAGE / 'config' / 'moveit_cpp.yaml' + _, setting_path = prepare(tmp_path, moveit_cpp_path=moveit_cpp) + data = yaml.safe_load(setting_path.read_text(encoding='utf-8')) + configured = data['controller']['moveit']['moveit_cpp'] + assert configured == str(moveit_cpp.resolve()) + assert 'moveit_cpp/monitored_planning_scene' in moveit_cpp.read_text( + encoding='utf-8' + )