Эталонное управление экспериментами Grant

Назначение

Эталонный исполнитель физически перемещает робот iiwa 7 в Webots для всех 20 экспериментов. Его журнал действий и результатов нужен для сравнения с планами LLM. Один запуск выбирает один эксперимент и один режим: view показывает готовую начальную сцену без движения, execute после готовности сцены и настраиваемой задержки автоматически выполняет эталонный сценарий. Оба режима используют существующую команду ros2 launch iiwa_experement experiment.launch.py experiment_id:=N mode:=....

Принятый подход

Пользователь выбрал детерминированный захват: отдельный supervisor пакета iiwa_experement перемещает взятый объект вслед за фактической позой конца patron; состояние объекта и прикреплённая геометрия синхронизируются с MoveIt. Физические соединители Webots не применяются. Постоянный patron никогда не снимается. Установка люка, движение с ним и движения вдоль участков герметика и между отверстиями должны быть видимыми траекториями робота; перемещать деталь прямо к цели без движения робота нельзя. Исключение — удаление переданного винта из сцены после подтверждённого отпускания у отверстия, что моделирует получение винта человеком вне сцены.

Рассмотренная альтернатива — Webots Connector/VacuumGripper. Она даёт более физический контакт, но требует переделки модели робота и инструментов и добавляет нестабильность контактной физики в эталонные прогоны. Для повторяемого сравнения планов выбран supervisor и явный автомат состояний.

Источники и ограничения

Ветка разработки: experement; вся новая логика, конфигурация, launch и документация лежат в src/iiwa_experement/. Существующие OBJ/MTL и постоянная модель patron остаются в src/iiwa_description/.
Мир: src/iiwa_experement/worlds/grant/iiwa-grant.wbt; начальные позы и состав объектов: src/iiwa_experement/config/scenes.yaml. Справочные файлы: src/iiwa_experement/doc/experements.md, src/iiwa_experement/doc/experiment-scenes.md, а также исходные docs/superpowers/specs/2026-11-08-grant-experiments.md и docs/superpowers/specs/2026-11-08-grant-scenes.md.
Базовый мир и геометрия: src/iiwa_description/worlds/iiwa.wbt, src/iiwa_description/resource/protos/CollaborativeRobotTable.proto, src/iiwa_description/objects/grant/ и output/reference_photo/grant/.
В patron.xacro фрейм tcp находится на 0,0552 м от начала звена patron; текущий сервер движения использует pose_link: tcp. Координаты Webots и base_link отличаются позой робота (-0.25, 0, 0.79); преобразование надо брать из активных настроек/TF, а не дублировать в коде.
Текущий /Ros2Supervisor имеет создание и удаление узлов, но не сервис произвольного перемещения. Для этого нужен отдельный supervisor в пакете эксперимента; существующий спавнер остаётся владельцем начальной сцены.
Исполнитель запускается только в симуляции. Пакет не должен посылать движения реальному контроллеру FRI.

Контракт режимов запуска

mode:=view — режим по умолчанию для обратной совместимости. Проверяет ID и конфигурацию, открывает мир, создаёт начальные объекты, оставляет робот в исходном положении и публикует scene_ready. Не запускает эталонный исполнитель, не захватывает и не меняет объекты. mode:=execute делает то же, затем ждёт scene_ready=true, готовности controller/MoveIt/supervisor и свежих joint_states, после чего ждёт start_delay_sec секунд симуляционного времени (по умолчанию 3,0 с) и начинает движение. Неверный режим или отрицательная/нечисловая задержка отклоняются до запуска Webots. Тайм-аут готовности даёт ошибку без движения. При паузе Webots симуляционная задержка не истекает.

Геометрия и захват

Для каждого инструмента задаются калиброванные преобразования T_tcp_mount и T_tcp_work_tip, корпус столкновений и рабочая ось. Модель инструмента не крепится по центру: локальная ось OBJ направлена по Y, после импорта по Z; длина вакуумного, магнитного и затяжного инструмента 0,14 м, герметик масштабируется в 10 раз до 0,14 м. Успешное grab/dock_tool разрешается только после достижения позы контакта и проверки расстояния/ориентации. В Webots объект становится управляемым supervisor и следует за фактической позой tcp; в MoveIt объект удаляется из мира и прикрепляется к роботу. Обратные операции сначала подтверждают посадку в гнезде/на корпусе, потом меняют владение. Допускается не более одного рабочего инструмента в patron и одной переносимой детали на соответствующем инструменте.

Люк захватывается присоской, переносится роботом, ориентируется относительно hatch_seat, опускается до посадочной позы и после отпускания фиксируется в сцене. Для опыта 5 первая установка намеренно смещена по X на 0,003 м, после чего выполняется одна корректировка. Герметик — состояние четырёх именованных сторон, но сопло действительно проходит их геометрические траектории в Webots. Прерывание на стороне 3 восстанавливается с последней подтверждённой точки, а не повторяет весь контур.

Винты не вставляются и не затягиваются. Магнит берёт один из десяти ещё доступных винтов; робот переносит его вдоль безопасных переходов, подводит к оси конкретного hole_i и к позе передачи над отверстием. Только после достигнутой позы выполняется release/handoff_screw: винт исчезает из сцены, состояние становится delivered(screw_i, hole_i), отверстие остаётся пустым. Оператор в сцене не моделируется. seat_screw, tighten, проверки момента и затяжной инструмент исключаются из эталонных сценариев и документации.

Движение и предотвращение столкновений

MoveIt получает геометрию стола, стапеля, корпуса, стойки, лотка, оставшихся винтов, люка и инструментов. Переносимый инструмент/деталь представлен как прикреплённый объект с проверенным преобразованием и габаритами. Состояние сцены подтверждается в MoveIt до следующего планирования. Свободное перемещение выполняется через проверенную безопасную область над препятствиями, контактный подход и отход — короткими осевыми движениями; непрерывные стороны герметика проходят подряд на рабочей высоте. Высоты, позы и допуски хранятся в конфигурации и подтверждаются измерением геометрии, IK и прогонами Webots. Контакт разрешается только между рабочим кончиком и текущей целью в соответствующей фазе, без общего отключения проверок столкновений.

После каждого движения проверяются результат action, фактическая поза TCP, состояние крепления и отсутствие запрещённого контакта. При коллизии, недостижимой позе, устаревшей позе, пропаже объекта или неизвестном состоянии — остановка, запись причины и запрет следующего действия. Никакая фиксированная высота или зазор не считаются гарантией безопасности без проверки всех 20 сцен.

Эталонные сценарии и результат

Сохраняются 4 категории по 5 задач с нарастанием сложности: 1–5 установка люка; 6–10 герметик и люк; 11–15 передача одного, двух, трёх, четырёх, затем четырёх винтов с восстановлением; 16–20 композиции герметика, люка и соответственно одного, двух, четырёх, четырёх при смещённой геометрии, четырёх с отказами. Опыты 14–20 не используют затяжку. Сбой захвата в 15 и 20 относится к выбранному винту; события прерывания герметика и неверной первой посадки люка остаются.

Для каждого прогона нужен машиночитаемый журнал в ~/.ros/iiwa_experement/runs/ по умолчанию (путь переопределяется аргументом trace_dir) с experiment_id, временем симуляции, именем операции/объекта/цели, исходом, фактическими позами и переходом состояния. Финальный отчёт различает completed, stopped, failed и содержит обработанные стороны, позу люка, пары доставленных винтов и оставшиеся винты. Это эталон поведения для последующего сравнения с планом LLM; автоматический оценщик LLM в данную работу не входит.

Проверка результата

Сборка и тесты пакета ROS 2 Jazzy, затем Webots-прогоны всех 20 опытов в execute. Для каждого подтверждаются движение робота и объекта, отсутствие запрещённых пересечений, корректное положение инструментов на кончике patron, возврат инструмента и домашняя поза в успешном маршруте. Отдельно проверяются отказы 4, 5, 9, 15, 20 и view: в нём робот и все объекты остаются неподвижными. Настоящее оборудование не применяется.

13 KiB Raw Blame History Unescape Escape