lightweight-cobot/src/iiwa_experement/doc/reference-control-qa.md

Проверка эталонного управления

Документ фиксирует измерения, настройки безопасности и результаты прогонов 20 опытов в Webots. Единицы — метры и радианы, если не указано иное.

Запуск и журналы

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_<id>_<UTC>.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 м; затяжной — 0,049 × 0,049 × 0,14 м.
• ACM строится из iiwa7.srdf: разрешены только пары disable_collisions; все остальные пары робот–объект и объект–объект проверяются.

Политика движений

• После захвата инструмент поднимается на 8 см, затем смещается на 10 см по +Y от стойки, и только потом выполняется суставное перемещение. Полный вертикальный подъём на 14–16 см упирается в предел запястья; боковой выход даёт тот же зазор и не ломает достижимость work.
• Между операциями используется ближняя нейтральная поза work. home остаётся финальной позой маршрута, позой перед установкой инструмента в стойку и аварийной реконфигурацией.
• Если планировщик отклоняет подход из ветви work (дальние ячейки лотка, отдельные гнёзда стойки), операция один раз переходит в home и повторяет подход. Событие reconfigure с причиной пишется в журнал; в успешных прогонах оно не появляется.
• TCP при работе направлен вниз; затяжной инструмент вращается вокруг своей оси, не меняя точку контакта.
• Положение после суставного перемещения проверяется по установившимся показаниям /joint_states, а не по первому отсчёту после ответа контроллера. При кратковременном отклонении цель повторяется один раз.

Результаты тестов

Финальный прогон: colcon test — 336 тестов, 0 ошибок, 0 падений. Тесты покрывают контракт сцен, геометрию, supervisor, сцену MoveIt, движение, операции, маршруты и журнал.

Результаты 20 опытов

Каждый сценарий должен хотя бы один раз завершиться со статусом completed. При транзиентном отказе планировщика (Планирование не удалось) запуск повторялся с чистой сценой; таблица показывает подтверждённый успешный прогон. Внутри одного запуска маршрут по-прежнему останавливается на первом отказе.

№	Статус	Доставлено	Люк	Герметик	Инструмент возвращён	Дом
1	completed	—	ok	—	vacuum	да
2	completed	—	ok	—	vacuum	да
3	completed	—	ok	—	vacuum	да
4	completed	—	ok	—	vacuum	да
5	completed	—	ok	—	vacuum	да
6	completed	—	—	ok	sealant_tool	да
7	completed	—	—	ok	sealant_tool	да
8	completed	—	—	ok	sealant_tool	да
9	completed	—	—	ok	sealant_tool	да
10	completed	—	ok	ok	sealant_tool, vacuum	да
11	completed	screw_1→hole_1	ok	—	magnetic_tool	да
12	completed	screw_1→hole_1, screw_2→hole_3	ok	—	magnetic_tool	да
13	completed	screw_1→hole_1, screw_2→hole_2, screw_5→hole_3	ok	—	magnetic_tool	да
14	completed	screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4	ok	—	magnetic_tool, torque_tool	да
15	completed	screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4	ok	—	magnetic_tool, torque_tool	да
16	completed	screw_1→hole_1	ok	ok	sealant_tool, vacuum, magnetic_tool, torque_tool	да
17	completed	screw_1→hole_1, screw_2→hole_3	ok	ok	sealant_tool, vacuum, magnetic_tool, torque_tool	да
18	completed	screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4	ok	ok	sealant_tool, vacuum, magnetic_tool, torque_tool	да
19	completed	screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4	ok	ok	sealant_tool, vacuum, magnetic_tool, torque_tool	да
20	completed	screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4	ok	ok	sealant_tool, vacuum, magnetic_tool, torque_tool	да

Режим view

Проверены опыты 1, 11, 16, 19, 20. После 10 секунд симуляции:

Опыт	Объектов	Винтов	Люк	TCP, смещение за 10 с	Захват
1	5	0	свободный	0,11 мм (успокоение контроллера)	нет
11	16	10	установлен	0,001 мм	нет
16	19	10	свободный	0,0007 мм	нет
19	19	10	свободный, смещён	0,0006 мм	нет
20	19	10	свободный	0,0007 мм	нет

Позы объектов за 10 секунд не изменились. mounted_tool=null, attached.tool=null, attached.payload=null. Журналы: /tmp/opencode/view_01.log, view_11.log, view_16.log, view_19.log, view_20.log.

Журналы прогонов

• JSONL: /tmp/opencode/traces_all/experiment_<id>_<UTC>.jsonl.
• Логи запуска: /tmp/opencode/logs_all/experiment_<id>.log.
• Сводка: /tmp/opencode/summary.json.
• В каждом успешном журнале последняя строка event=result содержит status=completed, пары винт-отверстие, cap.ok=true (где люк есть), sealant.ok=true (где контур есть), returned_tools и home_reached=true.

Имитация затяжки

В опытах 14, 15 и 16–20 после передач винтов берётся torque_tool. Для каждого отверстия выполняется tighten_simulated: подъём над отверстием, осевое опускание и поворот последнего сустава на 45°. Вращается только фланец, поэтому инструмент поворачивается вокруг своей оси, не уходя в другую ветвь IK. Если положительное направление упирается в предел, используется отрицательное. После поворота робот уходит в work, как после передачи винта. Отверстие остаётся пустым; винт не вставляется. Инструмент возвращается в стойку, затем робот уходит домой.

Ветви отказов

• 4: первый захват люка не выполняется, состояние проверяется, затем одна повторная попытка.
• 5: первая посадка смещена на 3 мм, inspect_cap возвращает not_ok, следует повторный захват и одна корректная посадка.
• 9: side_3 прерывается на половине; состояние проверяется, движение продолжается с подтверждённой точки, а не с начала контура.
• 15: третий выбранный винт (screw_5) не захватывается с первого раза; следует проверка состояния и одна повторная попытка.
• 20: прерывание герметика, ошибка первой посадки люка и неудачный захват первого винта; каждая ветвь имеет не более одной попытки.

Ограничения

• Только симуляция Webots; FRI и физический робот не запускаются.
• Винты не вставляются; после передачи отверстие остаётся пустым.
• Затяжка имитационная: torque_tool подходит к отверстию, поворачивается на 45° вокруг оси инструмента и отходит. Винт к этому моменту уже передан оператору и удалён из сцены.
• Прикреплённые объекты MoveItPy в установленной версии приводят к ложным столкновениям. Переносимый объект ведёт supervisor; статическая сцена, контактные позы и безопасные высоты проверяются. Это ограничение описано в README.
• MoveIt/MoveItPy может аварийно завершиться после очень длинной серии планирований; один запуск выполняет один опыт, поэтому серия ограничена.
• Node.remove() для винта в Webots R2025a иногда приводит к падению симулятора; переданный винт телепортируется далеко за арену и больше не запрашивается.