fix(llm-plan-executor): report missing tools as runtime grasp failures

This commit is contained in:
Даниил Грабарь
2026-11-11 11:28:12 +10:00
parent 112673f1f4
commit 48c190d75b
10 changed files with 304 additions and 66 deletions
@@ -16,7 +16,7 @@
- Создать ровно два дополнительных ROS 2 пакета: `src/iiwa_scene_context` и `src/iiwa_llm_plan_executor`. Изменения `iiwa_experement` и `iiwa_web` ограничить интеграцией. Не делать систему оценки Qwen3, её обучение или вызов модели: внешний клиент передаёт **готовый** граф.
- Номер эксперимента выбирает исходную сцену, но не задаёт робототехническое задание. Исполнять только переданный граф. Не вставлять операции взятия инструмента, герметизации, установки люка или передачи винтов на основании номера эксперимента.
- Внутри *одного* навыка допустимы обязательные геометрические фазы: безопасный подход сверху, осевое опускание, действие, осевой подъём к сохранённой высоте. Явные узлы этих фаз не должны вызывать повторное действие. Неизвестный объект, навык, инструмент, цель, положение или неоднозначная зависимость дают отказ до движения либо `replan_required`.
- `release(vacuum)` не перемещает инструмент к стойке. Чтобы вернуть присоску, граф должен явно содержать `transfer(vacuum,vacuum_socket)` и `release(vacuum,vacuum_socket)` либо `dock_tool(vacuum,vacuum_socket)` в достигнутом гнезде. `release(vacuum)` из `work` без безопасной цели отклоняется; присоска остаётся в `patron`. Для `grab(cap)` присоска в `patron` обязательна; если из исходного состояния она находится в стойке, граф должен явно предписать её взятие. Винт подносится к отверстию и передаётся виртуальному человеку: без вставки и затяжки. Навык `tighten` в профиле Grant первого этапа не поддерживается.
- `release(vacuum)` не перемещает инструмент к стойке. Чтобы вернуть присоску, граф должен явно содержать `transfer(vacuum,vacuum_socket)` и `release(vacuum,vacuum_socket)` либо `dock_tool(vacuum,vacuum_socket)` в достигнутом гнезде. `release(vacuum)` из `work` без безопасной цели отклоняется; присоска остаётся в `patron`. `grab(cap)` без присоски не отклоняется на входе: при исполнении проверяется живой supervisor, контакт не выполняется, возвращается `grasp_failed` с причиной `required tool vacuum is not mounted` и выбирается переход графа для этого события; автоматического взятия инструмента нет. Винт подносится к отверстию и передаётся виртуальному человеку: без вставки и затяжки. Навык `tighten` в профиле Grant первого этапа не поддерживается.
- `detect`: перейти в `work`, дождаться свежего кадра верхней камеры, получить точную позу из supervisor. При настроенном близком осмотре получить также новый кадр камеры на роботе; RGB/D используется для контроля, а не для вычисления позы.
- Рабочий кончик направлен вниз; при рабочих движениях наклон не более 10° и J7 в жёстком коридоре ±90° относительно `work` (сейчас 0 рад). Применять ограничения к *траектории*, не только к конечной позе. Если траектория недоступна, остановиться; не переходить на ветвь с разворотом J7 около −180°. Поворот кисти и переход в `work` только после осевого отвода от контакта.
- Один активный граф на робота. Внешний клиент передаёт ответ Qwen3 через `/iiwa_llm_plan_executor/submit`; принятый граф можно прочитать из ROS 2 топика `/iiwa_llm_plan_executor/plan`, а события и итог — из отдельного `/iiwa_llm_plan_executor/status`. `status=blocked` не запускает движение. `replan_required` выдаёт новый снимок сцены, но не вызывает Qwen3. `completed` означает выполненный граф и проверенные постусловия, а не оценку соответствия произвольному текстовому запросу.
@@ -99,7 +99,7 @@
- [x] Написать тесты: показанный пользователем формат `status/entry/plan/transitions` разбирается; `status=blocked` не допускается к исполнению; дубликаты ID, неизвестные навыки/объекты/цели/события, пропущенный `entry`, несуществующие `next`, недостижимые узлы, пустые обязательные переходы и превышение лимитов отвергаются.
- [x] Написать тесты ветвей отказа: `collision` не ведёт к продолжению операции; `grasp_failed` не превращается в `success`; цикл с допустимым восстановлением имеет ограничение 256 узлов и отдельный предел выполненных шагов; неверный `scene_revision` отклоняется.
- [x] Написать тест на исходную присоску в стойке: граф `grab(cap)` без явного `grab(vacuum)` получает `replan_required` **до первого движения**. Граф `grab(vacuum) → transfer(vacuum,work) → completed` принимается и оставляет инструмент в `patron`; граф `grab(vacuum) → transfer(vacuum,work) → release(vacuum)` отклоняется. Граф с явным `transfer(vacuum,vacuum_socket) → dock_tool(vacuum,vacuum_socket)` допускает возврат. Не выводить задачу из `experiment_id`.
- [x] Написать тест на исходную присоску в стойке: граф `grab(cap)` без явного `grab(vacuum)` принимается, при исполнении даёт `grasp_failed` до контакта и идёт по ветке `grasp_failed`; граф `grab(vacuum) → transfer(vacuum,work) → completed` принимается и оставляет инструмент в `patron`; граф `grab(vacuum) → transfer(vacuum,work) → release(vacuum)` отклоняется. Граф с явным `transfer(vacuum,vacuum_socket) → dock_tool(vacuum,vacuum_socket)` допускает возврат. Не выводить задачу из `experiment_id`.
- [x] Прочитать несовпадение имён в приложенных наборах. Ввести версию контракта и только явно проверенные алиасы `pick`, `move_to_storage`, `inspect`; `align` и Grant-расширения — самостоятельные записи каталога. Неизвестный `tighten` отклоняется до движения. Проверить тестами и зафиксировать.
- [x] Запустить `pytest -q src/iiwa_llm_plan_executor/test/test_schema.py src/iiwa_llm_plan_executor/test/test_validator.py` сначала до, затем после реализации; сохранить контракт и отдельный коммит.
@@ -121,7 +121,7 @@
**Интерфейсы:** `SkillContext` хранит текущий `scene_revision`, удерживаемый объект/инструмент, сохранённую безопасную позу подхода, фазу контакта и последнюю проверенную позу. `SkillRegistry.execute(node: PlanNode, context: SkillContext) -> SkillOutcome(event, details)`. Команды supervisor отправлять с `request_id` и подтверждать по следующему `object_state`.
- [x] Написать тесты фаз на фиктивном backend: `approach` идёт над целью и сохраняет высоту; `grab` без предварительного подхода выполняет подход, осевое опускание, захват и подъём; явные `approach/lower/lift` не дублируют движение. После `release` сначала осевой подъём, только затем горизонтальный уход или поворот.
- [x] Написать тесты инструментов: `grab(vacuum)` стыкуется кончиком `patron` к нужной точке; `release(vacuum)` из `work` **не двигает робот и не отделяет инструмент**; только после явного `transfer(vacuum,vacuum_socket)` навыки `release(vacuum,vacuum_socket)` и `dock_tool(vacuum,vacuum_socket)` опускаются в гнездо, отделяют и поднимаются. Отсутствие/занятость гнезда даёт отказ. `grab(cap)` без присоски не начинается.
- [x] Написать тесты инструментов: `grab(vacuum)` стыкуется кончиком `patron` к нужной точке; `release(vacuum)` из `work` **не двигает робот и не отделяет инструмент**; только после явного `transfer(vacuum,vacuum_socket)` навыки `release(vacuum,vacuum_socket)` и `dock_tool(vacuum,vacuum_socket)` опускаются в гнездо, отделяют и поднимаются. Отсутствие/занятость гнезда даёт отказ. `grab(cap)` без присоски не начинает захват: проверяется supervisor, контакт не выполняется, возвращается `grasp_failed`.
- [x] Написать тесты объектов: `transfer/align/release(cap,hatch_seat)` реально переносит и сажает люк; `dispense` движется вдоль заданного участка; `handoff_screw(screw_1,hole_1)` подносит винт и отпускает виртуальному человеку, не занимает отверстие установленным винтом. Неподдерживаемый тип цели отвергается.
- [x] Написать тесты `detect`: сначала осевой отвод и `work`, затем свежий верхний кадр + поза из supervisor; близкий кадр по настройке; просроченный кадр возвращает `failed`. `inspect_state` различает `empty`, `holding`, `placed`, `unknown`.
- [x] Реализовать навыки реестра из спецификации, используя общие геометрические примитивы. Каждый результат отображать в одно событие контракта; неизвестный исход не подменять успехом. Прогнать `pytest -q src/iiwa_llm_plan_executor/test/test_skills.py` и зафиксировать коммит.
@@ -18,7 +18,7 @@
Выбран **граф + реестр навыков**. Движения, неизбежные внутри одного навыка, задаются его проверяемым контрактом: например, `grab` может сам подойти сверху, опуститься, захватить и подняться до сохранённой высоты, если явных узлов `approach`/`lower`/`lift` не было. Если они уже выполнялись, повторного движения нет. Это геометрические фазы **того же** действия, а не добавление нового технологического задания.
Исполнитель не придумывает следующую деталь, нужный инструмент или цель из номера эксперимента. Если `grab(cap)` требует вакуумный инструмент, а в `patron` его нет, граф должен явно содержать взятие инструмента; иначе предзапусковая проверка возвращает `replan_required` без движения. `release` инструмента **не возвращает его автоматически**. Для возврата граф должен явно содержать перемещение к гнезду `vacuum_socket` и затем `release(vacuum, vacuum_socket)` либо `dock_tool(vacuum, vacuum_socket)`; оба последних навыка только выполняют стыковку и отделение в уже достигнутом гнезде. `release(vacuum)` из `work` без подходящего места для инструмента отклоняется, присоска остаётся в `patron`. Если граф заканчивается в `work` с присоской в `patron`, она остаётся там до следующего явного задания. Новый процесс может подключить другой адаптер и свои контракты; неизвестный навык или объект не исполняется.
Исполнитель не придумывает следующую деталь, нужный инструмент или цель из номера эксперимента. Если `grab(cap)` требует вакуумный инструмент, а в `patron` его нет, граф не отклоняется на входе: при исполнении узла проверяется живое состояние supervisor, захват не начинается, контакт не выполняется, и узел возвращает `grasp_failed` с причиной `required tool vacuum is not mounted`; далее исполняется ровно тот переход `grasp_failed`, который указала модель. Если в графе нет явного узла `grab(vacuum)`, инструмент автоматически не берётся. `release` инструмента **не возвращает его автоматически**. Для возврата граф должен явно содержать перемещение к гнезду `vacuum_socket` и затем `release(vacuum, vacuum_socket)` либо `dock_tool(vacuum, vacuum_socket)`; оба последних навыка только выполняют стыковку и отделение в уже достигнутом гнезде. `release(vacuum)` из `work` без подходящего места для инструмента отклоняется, присоска остаётся в `patron`. Если граф заканчивается в `work` с присоской в `patron`, она остаётся там до следующего явного задания. Новый процесс может подключить другой адаптер и свои контракты; неизвестный навык или объект не исполняется.
Рассмотренный, но отвергнутый подход — переписывать граф модели и автоматически вставлять недостающие технологические действия. Это могло бы выполнить лишние операции при произвольных запросах. Второй отвергнутый подход — напрямую переводить ID навыка в фиксированную траекторию без проверки текущей позы и состояния; он не учитывает смещения объектов и ветви событий.
+2 -2
View File
@@ -17,7 +17,7 @@ colcon test --base-paths src --packages-select \
colcon test-result --verbose
```
Result: build of 10 packages succeeded; `348 tests, 0 errors, 0 failures,
Result: build of 10 packages succeeded; `359 tests, 0 errors, 0 failures,
0 skipped`.
`git diff --check` is clean.
@@ -50,7 +50,7 @@ client-supplied `run_id`; the accepted graph appeared in
| `grab(vacuum) → transfer(vacuum,work) → completed` | `completed`; status ended with `mounted_tool: "vacuum"`, `held_object: null`. The cup stayed in `patron`. |
| `transfer(vacuum,vacuum_socket) → dock_tool(vacuum,vacuum_socket) → completed` | `completed`; status ended with `mounted_tool: null`. This was a separate graph after the previous one. |
| `grab(cap) → transfer(cap,hatch_seat) → align → release(cap,hatch_seat)` (cup already mounted) | Cap grasp, transfer, align and release succeeded; the supervisor reported the hatch installed. The final `verify` first failed because of a web-wrapper attribute shadowing bug (`latest`), which was fixed afterwards; a clean repeat of the whole cap graph was not obtained in this session. |
| cap graph without explicit `grab(vacuum)` | Rejected before motion: `replan_required: 'gc' requires the vacuum tool in patron; mounted tool is None`. |
| cap graph without explicit `grab(vacuum)`, with a `grasp_failed` edge | Admitted; execution returned `grasp_failed` with `reason: "required tool vacuum is not mounted"`, `next: "operator_required"`, `mounted_tool: null`, `held_object: null`. The supervisor state stayed at version 0 with the cap `free` and `vacuum` `docked`; the log contains no planner call for that run. `/plan` held the original graph, `/status`, `GET /planner/runs/...` and the JSONL log held the actual event/reason/ownership/next edge. |
| stale `scene_revision=999` | Rejected: `stale_scene: submitted revision 999 does not match current revision 1`. |
The screw handoff graph (`grab(magnetic_tool) → grab(screw_1) →
+21 -4
View File
@@ -67,15 +67,27 @@ returns `ValidationResult(ok, code, reason, details)` with codes:
| --- | --- |
| `ok` | admitted |
| `blocked` | the model returned `status=blocked`; no motion |
| `replan_required` | a fresh scene/plan is required before motion (missing tool, missing explicit transfer, unconfirmed ownership) |
| `replan_required` | a fresh scene/plan is required before motion (missing explicit transfer, unconfirmed ownership, or a graph branch that contradicts the live ownership state) |
| `rejected` | structural or safety violation |
| `stale_scene` | the graph was built for another `scene_revision` |
Important pre-movement rules:
* `grab(cap)` requires `vacuum` in `patron`. If the scene starts with the cup
docked, the graph must explicitly `grab(vacuum)`; otherwise admission is
`replan_required` and nothing moves.
* A missing required tool is **not** an admission error. `pick(cap)` /
`grab(cap)` is admitted even when `vacuum` is docked. At execution time the
skill re-reads the live supervisor state. If `vacuum` is not mounted it
returns `grasp_failed` with the reason
`required tool vacuum is not mounted`, performs no contact motion, sends no
capture command, changes no object state, and the engine follows exactly the
graph's `grasp_failed` edge. The `success` edge is not assumed.
* The same rule applies to screws without `magnetic_tool`
(`required tool magnetic_tool is not mounted`) and to other tool-dependent
skills, which return their contract's failure event.
* The validator still simulates the reachable ownership branches. It skips a
nominal success edge only when that success is impossible from the current
tool/held state; a recovery branch that then assumes the part is held is
rejected with its own ownership reason (for example `hatch is not held`),
not with a missing-tool reason.
* `release(vacuum)` without a target, or anywhere except its named socket and
after an explicit `transfer(vacuum, vacuum_socket)`, is `replan_required`.
The cup stays mounted. `release(vacuum, vacuum_socket)` and
@@ -99,6 +111,11 @@ Important pre-movement rules:
| service | `/iiwa_llm_plan_executor/get_status` | `iiwa_msgs/GetRunStatus` | request `string run_id`; response `bool found`, `string status_json`. |
| service | `/iiwa_llm_plan_executor/cancel` | `webots_ros2_msgs/SetString` | `{"run_id": "..."}` or a plain run id; stops execution and preserves the grasp. |
Every event is logged with the actual skill result, the failure reason when
present, `mounted_tool` and `held_object` read from the supervisor, and the
`next` node selected from the published graph. The same event is written to
the JSONL log and returned by `GET /planner/runs/{run_id}`.
Admission conditions: fresh `scene_json`, matching `scene_revision`,
`scene_ready`, a ready MoveIt/controller, fresh `joint_states`, and at least
3.0 s of simulation time since the first ready scene. Only one run may be
@@ -382,16 +382,24 @@ class GraphEngine:
next_id: str,
outcome: Any,
) -> None:
context = run.context
if context is not None:
if hasattr(context, 'mounted_tool'):
run.mounted_tool = context.mounted_tool
if hasattr(context, 'held_object'):
run.held_object = context.held_object
details = getattr(outcome, 'details', {}) or {}
entry = {
'run_id': run.run_id,
'node_id': node.id,
'skill': node.skill,
'event': event,
'reason': details.get('reason', ''),
'next': next_id,
'scene_revision': run.scene_revision,
'mounted_tool': run.mounted_tool,
'held_object': run.held_object,
'details': getattr(outcome, 'details', {}) or {},
'details': details,
'timestamp': self._clock(),
}
run.events.append(entry)
@@ -143,6 +143,23 @@ class SkillRegistry:
return {}
return state if isinstance(state, dict) else {}
def _refresh_live_ownership(self, context: SkillContext) -> str | None:
"""Read the tool and held object from the live supervisor.
Context memory is not authoritative between commands or runs: the
supervisor may have been changed by another accepted graph or by a
previous failed attempt.
"""
state = self._state()
if not state:
return 'supervisor state is unavailable'
context.supervisor_state = state
mounted = state.get('mounted_tool')
held = state.get('held_object')
context.mounted_tool = mounted if isinstance(mounted, str) else None
context.held_object = held if isinstance(held, str) else None
return None
def _move(
self,
context: SkillContext,
@@ -430,18 +447,35 @@ class SkillRegistry:
# ------------------------------------------------------------------ grab
def _grab(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
obj = node.object
unavailable = self._refresh_live_ownership(context)
if unavailable is not None:
return SkillOutcome('grasp_failed', {'reason': unavailable})
if obj in context.spec.tool_names:
return self._grab_tool(node, context)
if obj == 'cap':
if context.mounted_tool != 'vacuum':
return SkillOutcome('failed', {
'reason': 'grab(cap) requires the vacuum tool in patron',
return SkillOutcome('grasp_failed', {
'reason': 'required tool vacuum is not mounted',
'required_tool': 'vacuum',
'mounted_tool': context.mounted_tool,
})
if context.held_object is not None:
return SkillOutcome('grasp_failed', {
'reason': f'hand is not free: {context.held_object!r} is held',
'held_object': context.held_object,
})
return self._grab_part(node, context, 'grab_cap', 'vacuum')
if obj.startswith('screw_'):
if context.mounted_tool != 'magnetic_tool':
return SkillOutcome('failed', {
'reason': 'grab(screw) requires magnetic_tool in patron',
return SkillOutcome('grasp_failed', {
'reason': 'required tool magnetic_tool is not mounted',
'required_tool': 'magnetic_tool',
'mounted_tool': context.mounted_tool,
})
if context.held_object is not None:
return SkillOutcome('grasp_failed', {
'reason': f'hand is not free: {context.held_object!r} is held',
'held_object': context.held_object,
})
return self._grab_part(
node, context, 'grab_screw', 'magnetic_tool',
@@ -548,6 +582,9 @@ class SkillRegistry:
def _transfer(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
obj = node.object
target = node.target
unavailable = self._refresh_live_ownership(context)
if unavailable is not None:
return SkillOutcome('failed', {'reason': unavailable})
if obj in context.spec.tool_names:
if context.mounted_tool != obj:
return SkillOutcome('failed', {
@@ -601,6 +638,9 @@ class SkillRegistry:
def _release(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
obj = node.object
target = node.target
unavailable = self._refresh_live_ownership(context)
if unavailable is not None:
return SkillOutcome('failed', {'reason': unavailable})
if obj in context.spec.tool_names:
return self._release_tool(node, context)
if obj == 'cap':
@@ -614,7 +654,9 @@ class SkillRegistry:
socket = f'{tool}_socket'
if context.mounted_tool != tool:
return SkillOutcome('failed', {
'reason': f'{tool!r} is not mounted',
'reason': f'required tool {tool} is not mounted',
'required_tool': tool,
'mounted_tool': context.mounted_tool,
'moved': False,
})
if node.target != socket:
@@ -664,7 +706,10 @@ class SkillRegistry:
def _release_cap(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
if context.held_object != 'cap':
return SkillOutcome('failed', {'reason': 'hatch is not held'})
return SkillOutcome('failed', {
'reason': 'hatch is not held',
'held_object': context.held_object,
})
target = node.target or 'hatch_seat'
self._ensure_approach(context, node)
adapter = self._adapter(context)
@@ -692,6 +737,9 @@ class SkillRegistry:
return SkillOutcome('success', {'object': 'cap', 'target': target})
def _dock_tool(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
unavailable = self._refresh_live_ownership(context)
if unavailable is not None:
return SkillOutcome('failed', {'reason': unavailable})
if node.object not in context.spec.tool_names:
return SkillOutcome('failed', {'reason': f'{node.object!r} is not a tool'})
socket = f'{node.object}_socket'
@@ -704,12 +752,20 @@ class SkillRegistry:
def _handoff_screw(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
screw = node.object
hole = node.target
unavailable = self._refresh_live_ownership(context)
if unavailable is not None:
return SkillOutcome('grasp_failed', {'reason': unavailable})
if context.mounted_tool != 'magnetic_tool':
return SkillOutcome('grasp_failed', {
'reason': 'required tool magnetic_tool is not mounted',
'required_tool': 'magnetic_tool',
'mounted_tool': context.mounted_tool,
})
if context.held_object != screw:
return SkillOutcome('grasp_failed', {
'reason': f'{screw!r} is not held',
'held_object': context.held_object,
})
if context.mounted_tool != 'magnetic_tool':
return SkillOutcome('failed', {'reason': 'magnetic_tool is not mounted'})
self._ensure_approach(context, node)
adapter = self._adapter(context)
prepare = getattr(adapter, 'prepare_handoff_screw', None)
@@ -752,8 +808,15 @@ class SkillRegistry:
return SkillOutcome('success', {'tcp': list(work.xyz)})
def _dispense(self, node: PlanNode, context: SkillContext) -> SkillOutcome:
unavailable = self._refresh_live_ownership(context)
if unavailable is not None:
return SkillOutcome('failed', {'reason': unavailable})
if context.mounted_tool != 'sealant_tool':
return SkillOutcome('failed', {'reason': 'sealant_tool is not mounted'})
return SkillOutcome('failed', {
'reason': 'required tool sealant_tool is not mounted',
'required_tool': 'sealant_tool',
'mounted_tool': context.mounted_tool,
})
side = node.object or node.target
start_fraction = float(node.params.get('start_fraction', 0.0))
end_fraction = float(node.params.get('end_fraction', 1.0))
@@ -323,11 +323,8 @@ def _precondition(
if obj == 'cap':
if 'cap' not in facts.components:
return _reject(f'{node.id!r}: hatch is absent from this scene')
if state.mounted != 'vacuum':
return _replan(
f'{node.id!r} requires the vacuum tool in patron; '
f'mounted tool is {state.mounted!r}'
)
# A missing vacuum tool is a runtime grasp_failed condition, not
# a reason to reject the graph before execution.
if state.held is not None:
return _replan(
f'{node.id!r} cannot grab the hatch while holding '
@@ -335,11 +332,7 @@ def _precondition(
)
return None
if obj in facts.components and facts.components[obj] == 'screw':
if state.mounted != 'magnetic_tool':
return _replan(
f'{node.id!r} requires magnetic_tool in patron; '
f'mounted tool is {state.mounted!r}'
)
# A missing magnetic tool is a runtime grasp_failed condition.
if state.held is not None:
return _replan(
f'{node.id!r} cannot grab a screw while holding '
@@ -351,11 +344,6 @@ def _precondition(
if skill == 'release':
if obj in facts.tools:
socket = facts.tools[obj]
if state.mounted != obj:
return _replan(
f'{node.id!r}: {obj!r} is not mounted '
f'(mounted={state.mounted!r})'
)
if target != socket:
return _replan(
f'{node.id!r}: release({obj}) must name its socket '
@@ -388,8 +376,6 @@ def _precondition(
if skill in ('transfer', 'move_to_storage'):
if obj in facts.tools:
if state.mounted != obj:
return _replan(f'{node.id!r}: {obj!r} is not mounted')
if target not in ('work', facts.tools[obj]):
return _replan(
f'{node.id!r}: tool transfer must target work or its socket'
@@ -419,10 +405,6 @@ def _precondition(
return None
if skill == 'dispense':
if state.mounted != 'sealant_tool':
return _replan(
f'{node.id!r}: dispense requires sealant_tool in patron'
)
side = obj or target
if facts.destinations.get(side) != 'contour':
return _reject(
@@ -434,11 +416,6 @@ def _precondition(
socket = facts.tools.get(obj)
if socket is None:
return _reject(f'{node.id!r}: {obj!r} is not a present tool')
if state.mounted != obj:
return _replan(
f'{node.id!r}: {obj!r} is not mounted '
f'(mounted={state.mounted!r})'
)
if target != socket:
return _replan(
f'{node.id!r}: dock_tool must target socket {socket!r}'
@@ -455,10 +432,6 @@ def _precondition(
return _reject(f'{node.id!r}: {obj!r} is not a present screw')
if state.held != obj:
return _replan(f'{node.id!r}: {obj!r} is not held')
if state.mounted != 'magnetic_tool':
return _replan(
f'{node.id!r}: handoff_screw requires magnetic_tool'
)
if facts.destinations.get(target) != 'hole':
return _replan(
f'{node.id!r}: handoff_screw target must be a hole'
@@ -471,6 +444,58 @@ def _precondition(
return None
def _success_feasible(
node: PlanNode,
state: _State,
facts: _Facts,
) -> bool:
"""Whether this node's nominal success can happen from ``state``.
A missing required tool must not reject the graph, but its success branch
must not be treated as already achieved either. The validator therefore
skips only the success edge for that state and continues through the
failure events the graph actually declared.
"""
skill = node.skill
obj = node.object
if skill in ('grab', 'pick'):
if obj in facts.tools:
return (
state.mounted in (None, obj)
and state.held is None
)
if obj == 'cap':
return state.mounted == 'vacuum' and state.held is None
if obj and facts.components.get(obj) == 'screw':
return state.mounted == 'magnetic_tool' and state.held is None
return False
if skill in ('transfer', 'move_to_storage'):
if obj in facts.tools:
return state.mounted == obj
if obj == 'cap':
return state.held == 'cap'
if obj and facts.components.get(obj) == 'screw':
return state.held == obj
return False
if skill == 'release':
if obj in facts.tools:
return state.mounted == obj
if obj == 'cap':
return state.held == 'cap'
if obj and facts.components.get(obj) == 'screw':
return state.held == obj and state.mounted == 'magnetic_tool'
return False
if skill == 'align':
return state.held is not None or state.mounted is not None
if skill == 'dispense':
return state.mounted == 'sealant_tool'
if skill == 'dock_tool':
return state.mounted == obj
if skill == 'handoff_screw':
return state.held == obj and state.mounted == 'magnetic_tool'
return True
def _apply_event(
node: PlanNode,
event: str,
@@ -540,6 +565,14 @@ def _ownership_checks(graph: PlanGraph, facts: _Facts) -> ValidationResult | Non
for transition in node.transitions:
if transition.next not in graph.ids:
continue
if (
transition.event in PROGRESS_EVENTS
and not _success_feasible(node, state, facts)
):
# Do not simulate a success that cannot happen (for example
# pick(cap) without the vacuum tool). The grasp_failed edge
# from the same state is still traversed and checked.
continue
next_state = _apply_event(node, transition.event, state, facts)
if (transition.next, next_state) not in seen:
queue.append((transition.next, next_state))
+34 -5
View File
@@ -42,6 +42,7 @@ class FakeSkills:
def __init__(self):
self.calls = []
self.events = {}
self.details = {}
self.on_execute = None
self.block = False
self.started = threading.Event()
@@ -54,7 +55,10 @@ class FakeSkills:
self.started.set()
self.release.wait(timeout=5.0)
self.calls.append(node.id)
return SimpleNamespace(event=self.events.get(node.id, 'success'), details={})
return SimpleNamespace(
event=self.events.get(node.id, 'success'),
details=self.details.get(node.id, {}),
)
def envelope(revision=1, stamp=5.0, experiment_id=1):
@@ -180,12 +184,13 @@ def test_rejected_graphs_are_never_published_or_executed():
engine, skills, events, _, _ = make_engine()
blocked = engine.submit('r1', {'status': 'blocked', 'entry': '', 'plan': []}, 1)
assert blocked.code == 'blocked'
cap_without_vacuum = graph(
'g', node('g', 'grab', 'cap', next_on={'success': 'completed'}),
bad_next = graph(
'g',
node('g', 'grab', 'vacuum', next_on={'success': 'ghost'}),
)
rejected = engine.submit('r2', cap_without_vacuum.as_dict(), 1)
rejected = engine.submit('r2', bad_next.as_dict(), 1)
assert rejected.accepted is False
assert rejected.code == 'replan_required'
assert rejected.code == 'rejected'
assert events['plans'] == []
assert skills.calls == []
stale = engine.submit('r3', STOP_GRAPH().as_dict(), 2)
@@ -193,6 +198,30 @@ def test_rejected_graphs_are_never_published_or_executed():
assert engine.get_status('r2').state == 'rejected'
def test_cap_grab_without_vacuum_is_admitted_and_follows_grasp_failed():
engine, skills, events, _, _ = make_engine()
value = graph(
'g',
node('g', 'grab', 'cap', next_on={
'success': 'completed',
'grasp_failed': 'operator_required',
}),
)
accepted = engine.submit('r1', value.as_dict(), 1)
assert accepted.accepted is True
assert len(events['plans']) == 1
skills.events = {'g': 'grasp_failed'}
skills.details = {'g': {'reason': 'required tool vacuum is not mounted'}}
engine.run_accepted('r1')
status = engine.get_status('r1')
assert status.state == 'operator_required'
assert status.events[0]['event'] == 'grasp_failed'
assert status.events[0]['next'] == 'operator_required'
assert status.events[0]['reason']
assert status.mounted_tool is None
assert status.held_object is None
def test_accepted_graph_is_published_once_before_the_first_motion():
engine, skills, events, _, _ = make_engine()
order = []
+45 -6
View File
@@ -225,10 +225,11 @@ def test_grab_keeps_target_in_collision_scene_during_approach():
self.events.append(('prepare_grab_cap', object_id))
adapter = RecordingScene()
registry, context, motion, _ = make_registry(
registry, context, motion, supervisor = make_registry(
motion=FakeMotion(success=False), adapter=adapter,
)
context.mounted_tool = 'vacuum'
supervisor.state['mounted_tool'] = 'vacuum'
result = registry.execute(node('g', 'grab', 'cap'), context)
assert result.event == 'failed'
assert phases(motion) == ['approach']
@@ -253,10 +254,11 @@ def test_grab_restores_target_when_lowering_fails():
)
adapter = RecordingScene()
registry, context, motion, _ = make_registry(
registry, context, motion, supervisor = make_registry(
motion=FailOnLower(), adapter=adapter,
)
context.mounted_tool = 'vacuum'
supervisor.state['mounted_tool'] = 'vacuum'
result = registry.execute(node('g', 'grab', 'cap'), context)
assert result.event == 'failed'
assert phases(motion) == ['approach', 'lower']
@@ -292,11 +294,46 @@ def test_explicit_phases_are_not_repeated_by_grab():
assert phases(motion) == ['approach', 'lower', 'retreat']
def test_grab_cap_without_vacuum_does_not_move():
registry, context, motion, _ = make_registry()
def test_grab_cap_without_vacuum_reports_grasp_failed_without_motion_or_command():
registry, context, motion, supervisor = make_registry()
before = copy.deepcopy(supervisor.state)
result = registry.execute(node('g', 'grab', 'cap'), context)
assert result.event == 'failed'
assert result.event == 'grasp_failed'
assert result.details['reason'] == 'required tool vacuum is not mounted'
assert result.details['required_tool'] == 'vacuum'
assert motion.calls == []
assert supervisor.commands == []
assert registry.scene_adapter.events == []
assert supervisor.state['cap']['state'] == before['cap']['state']
assert supervisor.state['held_object'] is None
assert context.held_object is None
assert context.mounted_tool is None
def test_supervisor_state_overrides_stale_context_for_tool_checks():
registry, context, motion, supervisor = make_registry()
context.mounted_tool = 'vacuum' # stale memory; supervisor says no tool
result = registry.execute(node('g', 'grab', 'cap'), context)
assert result.event == 'grasp_failed'
assert result.details['reason'] == 'required tool vacuum is not mounted'
assert motion.calls == []
assert supervisor.commands == []
assert context.mounted_tool is None
def test_grab_screw_without_magnetic_tool_reports_grasp_failed_without_motion():
registry, context, motion, supervisor = make_registry()
before = copy.deepcopy(supervisor.state)
result = registry.execute(node('g', 'grab', 'screw_1'), context)
assert result.event == 'grasp_failed'
assert result.details['reason'] == 'required tool magnetic_tool is not mounted'
assert result.details['required_tool'] == 'magnetic_tool'
assert motion.calls == []
assert supervisor.commands == []
assert registry.scene_adapter.events == []
assert supervisor.state['screws']['screw_1']['state'] == before['screws']['screw_1']['state']
assert supervisor.state['held_object'] is None
assert context.held_object is None
def test_tool_grab_docks_at_the_socket_and_lifts_out():
@@ -400,8 +437,10 @@ def test_release_lifts_axially_before_any_lateral_motion():
def test_dispense_moves_along_the_segment_without_changing_ownership():
registry, context, motion, _ = make_registry()
registry, context, motion, supervisor = make_registry()
context.mounted_tool = 'sealant_tool'
supervisor.state['mounted_tool'] = 'sealant_tool'
supervisor.state['tools']['sealant_tool']['state'] = 'mounted'
result = registry.execute(
node('d', 'dispense', 'side_1', params={
'start_fraction': 0.0, 'end_fraction': 1.0,
@@ -111,15 +111,64 @@ def test_blocked_status_never_reaches_execution():
assert result.code == 'blocked'
def test_cap_grab_without_explicit_vacuum_is_replan_required_before_motion():
def test_cap_grab_without_vacuum_is_admitted_for_runtime_grasp_failed():
value = graph(
'grab_cap',
node('grab_cap', 'grab', 'cap', next_on={'success': 'completed'}),
node('grab_cap', 'grab', 'cap', next_on={
'success': 'completed',
'grasp_failed': 'operator_required',
}),
)
result = expect(value, scene_for(1))
assert result.ok is True
# The success branch is kept in the graph but is not simulated as if the
# hatch were already held without a vacuum tool.
with_success_branch = graph(
'grab_cap',
node('grab_cap', 'grab', 'cap', next_on={
'success': 'transfer',
'grasp_failed': 'operator_required',
}),
node('transfer', 'transfer', 'cap', 'hatch_seat',
next_on={'success': 'completed'}),
)
assert expect(with_success_branch, scene_for(1)).ok is True
def test_screw_grab_without_magnetic_tool_is_admitted():
value = graph(
'pick_screw',
node('pick_screw', 'pick', 'screw_1', next_on={
'success': 'completed',
'grasp_failed': 'operator_required',
}),
)
result = expect(value, scene_for(20))
assert result.ok is True
def test_recovery_branch_that_assumes_a_held_hatch_has_its_own_reason():
value = graph(
'grab_cap',
node('grab_cap', 'grab', 'cap', next_on={
'success': 'completed',
'grasp_failed': 'inspect',
}),
node('inspect', 'inspect_state', 'cap', next_on={
'empty': 'transfer',
'holding': 'transfer',
'placed': 'completed',
'unknown': 'operator_required',
}),
node('transfer', 'transfer', 'cap', 'hatch_seat',
next_on={'success': 'completed'}),
)
result = expect(value, scene_for(1))
assert result.ok is False
assert result.code == 'replan_required'
assert 'vacuum' in result.reason
assert 'hatch is not held' in result.reason
assert 'vacuum' not in result.reason
def test_take_vacuum_and_leave_it_in_patron_is_accepted():
@@ -354,12 +403,12 @@ def test_screw_handoff_is_to_the_operator_and_leaves_the_hole_empty():
assert result.ok is True
def test_dispense_requires_the_sealant_tool():
def test_dispense_requires_the_sealant_tool_at_runtime():
without = graph(
'dispense', node('dispense', 'dispense', 'side_1',
next_on={'success': 'completed'}),
)
assert expect(without, scene_for(20)).code == 'replan_required'
assert expect(without, scene_for(20)).ok is True
with_tool = graph(
'grab_sealant',
node('grab_sealant', 'grab', 'sealant_tool',