feat(experement): complete reference routes, runner and QA documentation
This commit is contained in:
Даниил Грабарь
@@ -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:
|
||||
|
||||
@@ -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_<id>_<UTC>.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`; это не тест нового
|
||||
|
||||
@@ -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_<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 м.
|
||||
- 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 иногда приводит к падению
|
||||
симулятора; переданный винт телепортируется далеко за арену и больше не
|
||||
запрашивается.
|
||||
@@ -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)
|
||||
|
||||
@@ -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}')
|
||||
|
||||
|
||||
@@ -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,
|
||||
)
|
||||
|
||||
@@ -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')
|
||||
|
||||
@@ -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')
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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()
|
||||
@@ -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(),
|
||||
)
|
||||
@@ -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)
|
||||
|
||||
|
||||
|
||||
@@ -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()
|
||||
@@ -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(
|
||||
|
||||
@@ -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',
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
@@ -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'
|
||||
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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'
|
||||
@@ -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)
|
||||
@@ -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'
|
||||
)
|
||||
|
||||
Reference in New Issue
Block a user