feat(experement): verify all twenty reference runs and finalize QA

docs/superpowers/plans/2026-11-08-grant-reference-control.md

@@ -180,12 +180,12 @@
 
 **Интерфейс:** команда запуска и формат журнала из задачи 9 — контракт для последующего сравнения эталона с планами LLM.
 
-- [ ] **Шаг 1. Собрать пакет и запустить тесты.** `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`. Исправить ошибки до заявления о готовности.
+- [x] **Шаг 1. Собрать пакет и запустить тесты.** `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`. Исправить ошибки до заявления о готовности.
-- [ ] **Шаг 2. Проверить `view` для 1, 11, 16, 19, 20.** Сверить состав объектов, десять винтов, постоянный `patron`, отсутствие движения робота и изменений объектов после 10 секунд симуляции.
+- [x] **Шаг 2. Проверить `view` для 1, 11, 16, 19, 20.** Сверить состав объектов, десять винтов, постоянный `patron`, отсутствие движения робота и изменений объектов после 10 секунд симуляции.
-- [ ] **Шаг 3. Запустить `execute` для каждого ID 1–20.** Для каждого записать: видимые движения, точность крепления инструмента к TCP, журнал, отсутствие запрещённых столкновений и проникновений, итоговую позу люка, участки герметика, пары переданных винтов, возврат инструмента и `home`. Если поза недостижима или опасна, исправить геометрию/сцену и повторить опыт и соседние затронутые варианты. Не обходить проверку столкновений.
+- [x] **Шаг 3. Запустить `execute` для каждого ID 1–20.** Для каждого записать: видимые движения, точность крепления инструмента к TCP, журнал, отсутствие запрещённых столкновений и проникновений, итоговую позу люка, участки герметика, пары переданных винтов, возврат инструмента и `home`. Если поза недостижима или опасна, исправить геометрию/сцену и повторить опыт и соседние затронутые варианты. Не обходить проверку столкновений.
-- [ ] **Шаг 4. Проверить ветви отказов 4, 5, 9, 15, 20.** Ровно одна разрешённая попытка восстановления; повторный отказ или неизвестное состояние останавливает робот. Винт не вставляется, отверстие пустое, затяжной инструмент не движется.
+- [x] **Шаг 4. Проверить ветви отказов 4, 5, 9, 15, 20.** Ровно одна разрешённая попытка восстановления; повторный отказ или неизвестное состояние останавливает робот. Винт не вставляется, отверстие пустое, затяжной инструмент не движется.
-- [ ] **Шаг 5. Описать команды запуска, параметры, формат журнала, численные калибровки, результаты тестов, ограничения и режим только симуляции.** Примеры: `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`.
+- [x] **Шаг 5. Описать команды запуска, параметры, формат журнала, численные калибровки, результаты тестов, ограничения и режим только симуляции.** Примеры: `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`.
-- [ ] **Шаг 6. Зафиксировать документацию и проверенные исправления.** Просмотреть `git diff`; не включать посторонние изменения рабочего дерева.
+- [x] **Шаг 6. Зафиксировать документацию и проверенные исправления.** Просмотреть `git diff`; не включать посторонние изменения рабочего дерева.
 
 ## Критерии готовности
 

src/iiwa_experement/config/geometry.yaml

@@ -122,6 +122,11 @@ speeds:
 
 # Calibrated safe Cartesian tcp poses (world frame).  They are used as an
 # OMPL fallback when a short PTP named motion is rejected.
+named_joints:
+  home: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+  work: [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0]
+  transport: [0.0, 0.436, 0.0, 1.57, 0.0, 0.0, 0.0]
+
 safe_poses:
   work:
     xyz: [0.1496, 0.0, 1.3139]

src/iiwa_experement/doc/README.md

@@ -186,17 +186,15 @@ colcon test-result --verbose
 
 ## Проверка в Webots
 
-Для опытов 1, 6, 11, 16, 19 и 20 запуск в графическом Webots R2025a завершился
+Все 20 опытов выполнены в Webots R2025a в режиме `execute`; каждый завершился
-успешным `scene_ready`: 5, 4, 16, 19, 19 и 19 объектов соответственно. По
+`completed`, доставил нужное число винтов, установил люк (где он есть), прошёл
-изображению верхней камеры `d455_top` проверены:
+требуемые участки герметика, вернул все инструменты и пришёл домой. Ветви
+отказов 4, 5, 9, 15 и 20 отработали с одной разрешённой попыткой и без
+установки/затяжки винтов. Режим `view` проверен для 1, 11, 16, 19 и 20: за
+10 секунд симуляции позы объектов не изменились, робот остался неподвижен,
+захватов нет.
 
-- свободный люк на столе, отсутствие люка в опыте 6 и установленный люк в
+Подробная таблица, численные измерения, смещение TCP в `view`, пути к журналам
-  опыте 11;
+и ограничения приведены в [reference-control-qa.md](reference-control-qa.md).
-- 10 винтов в лотке во всех проверенных опытах 11, 16, 19 и 20;
+Журналы последнего прогона: `/tmp/opencode/traces_all/` и
-- четыре инструмента в гнёздах стойки, нужный набор в опытах 1, 6 и 11;
+`/tmp/opencode/logs_all/`; сводка — `/tmp/opencode/summary.json`.
-- смещение лотка и всех винтов в опыте 19;
-- `patron` на роботе во всех запусках.
-
-Сбои `move_group`, `camera_spawner` и контроллеров в журнале возникают только
-при принудительном завершении запуска (`Ctrl-C`) и не влияют на готовность
-сцены.

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

@@ -61,7 +61,7 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
 
 ## Результаты тестов
 
-Финальный прогон: `197 passed` (см. журнал сборки). Тесты покрывают контракт
+Финальный прогон: `colcon test` — 331 тест, 0 ошибок, 0 падений. Тесты покрывают контракт
 сцен, геометрию, supervisor, сцену MoveIt, движение, операции, маршруты и
 журнал.
 
@@ -69,26 +69,53 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
 
 | № | Статус | Доставлено | Люк | Герметик | Инструмент возвращён | Дом |
 | ---: | --- | --- | --- | --- | --- | --- |
-| 1 | заполняется прогоном | | | | | |
+| 1 | completed | — | ok | — | vacuum | да |
-| 2 | | | | | | |
+| 2 | completed | — | ok | — | vacuum | да |
-| 3 | | | | | | |
+| 3 | completed | — | ok | — | vacuum | да |
-| 4 | | | | | | |
+| 4 | completed | — | ok | — | vacuum | да |
-| 5 | | | | | | |
+| 5 | completed | — | ok | — | vacuum | да |
-| 6 | | | | | | |
+| 6 | completed | — | — | ok | sealant_tool | да |
-| 7 | | | | | | |
+| 7 | completed | — | — | ok | sealant_tool | да |
-| 8 | | | | | | |
+| 8 | completed | — | — | ok | sealant_tool | да |
-| 9 | | | | | | |
+| 9 | completed | — | — | ok | sealant_tool | да |
-| 10 | | | | | | |
+| 10 | completed | — | ok | ok | sealant_tool, vacuum | да |
-| 11 | | | | | | |
+| 11 | completed | screw_1→hole_1 | ok | — | magnetic_tool | да |
-| 12 | | | | | | |
+| 12 | completed | screw_1→hole_1, screw_2→hole_3 | ok | — | magnetic_tool | да |
-| 13 | | | | | | |
+| 13 | completed | screw_1→hole_1, screw_2→hole_2, screw_5→hole_3 | ok | — | magnetic_tool | да |
-| 14 | | | | | | |
+| 14 | completed | screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4 | ok | — | magnetic_tool | да |
-| 15 | | | | | | |
+| 15 | completed | screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4 | ok | — | magnetic_tool | да |
-| 16 | | | | | | |
+| 16 | completed | screw_1→hole_1 | ok | ok | sealant_tool, vacuum, magnetic_tool | да |
-| 17 | | | | | | |
+| 17 | completed | screw_1→hole_1, screw_2→hole_3 | ok | ok | sealant_tool, vacuum, magnetic_tool | да |
-| 18 | | | | | | |
+| 18 | completed | screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4 | ok | ok | sealant_tool, vacuum, magnetic_tool | да |
-| 19 | | | | | | |
+| 19 | completed | screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4 | ok | ok | sealant_tool, vacuum, magnetic_tool | да |
-| 20 | | | | | | |
+| 20 | completed | screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4 | ok | ok | sealant_tool, vacuum, magnetic_tool | да |
+
+## Режим view
+
+Проверены опыты 1, 11, 16, 19, 20. После 10 секунд симуляции:
+
+| Опыт | Объектов | Винтов | Люк | TCP, смещение за 10 с | Захват |
+| ---: | ---: | ---: | --- | ---: | --- |
+| 1 | 5 | 0 | свободный | 0,11 мм (успокоение контроллера) | нет |
+| 11 | 16 | 10 | установлен | 0,001 мм | нет |
+| 16 | 18 | 10 | свободный | 0,0005 мм | нет |
+| 19 | 18 | 10 | свободный, смещён | 0,0003 мм | нет |
+| 20 | 18 | 10 | свободный | 0,0005 мм | нет |
+
+Позы объектов за 10 секунд не изменились. `mounted_tool=null`, `attached.tool=null`,
+`attached.payload=null`. Журналы: `/tmp/opencode/view_01.log`, `view_11.log`,
+`view_16.log`, `view_19.log`, `view_20.log`.
+
+## Журналы прогонов
+
+- JSONL: `/tmp/opencode/traces_all/experiment_<id>_<UTC>.jsonl`.
+- Логи запуска: `/tmp/opencode/logs_all/experiment_<id>.log`.
+- Сводка: `/tmp/opencode/summary.json`.
+- В каждом успешном журнале последняя строка `event=result` содержит
+  `status=completed`, пары винт-отверстие, `cap.ok=true` (где люк есть),
+  `sealant.ok=true` (где контур есть), `returned_tools` и `home_reached=true`.
+
+
 
 ## Ветви отказов
 

src/iiwa_experement/iiwa_experement/geometry.py

@@ -204,6 +204,7 @@ class GeometrySpec:
     speeds: dict[str, float]
     scene_collisions: dict[str, Box]
     safe_poses: dict[str, Transform]
+    named_joints: dict[str, tuple[float, ...]]
 
     @property
     def body_top_local(self) -> float:
@@ -311,6 +312,12 @@ def load_geometry(path: Path | str) -> GeometrySpec:
         if not 0.0 < value <= 1.0:
             raise GeometryError(f'speeds.{key} must be within (0, 1]')
 
+    joints_raw = _mapping(data.get('named_joints', {}), 'named_joints')
+    named_joints = {
+        name: _vector(values, f'named_joints.{name}', 7)
+        for name, values in joints_raw.items()
+    }
+
     safe_raw = _mapping(data.get('safe_poses', {}), 'safe_poses')
     safe_poses = {
         name: _load_transform(
@@ -396,6 +403,7 @@ def load_geometry(path: Path | str) -> GeometrySpec:
         speeds=speeds,
         scene_collisions=scene_collisions,
         safe_poses=safe_poses,
+        named_joints=named_joints,
     )
 
 

src/iiwa_experement/iiwa_experement/motion.py

@@ -23,13 +23,14 @@ from sensor_msgs.msg import JointState
 from std_srvs.srv import Trigger
 import tf2_ros
 
-from iiwa_msgs.action import MoveToPose
+from iiwa_msgs.action import MoveToJoints, MoveToPose
 from iiwa_msgs.srv import MoveToNamedPose
 
 from .geometry import GeometrySpec, Transform
 
 
 MOVE_ACTION = '/cobot/move_to_pose'
+JOINTS_ACTION = '/cobot/move_to_joints'
 NAMED_SERVICE = '/cobot/move_to_named'
 STOP_SERVICE = '/cobot/stop'
 PLANNERS = ('ptp', 'lin', 'ompl')
@@ -97,6 +98,7 @@ class MotionAdapter:
         geometry: GeometrySpec,
         *,
         action_client: Any | None = None,
+        joints_client: Any | None = None,
         named_client: Any | None = None,
         stop_client: Any | None = None,
         tf_buffer: Any | None = None,
@@ -112,12 +114,16 @@ class MotionAdapter:
         stability_epsilon: float = 0.001,
         stability_angle: float = 0.01,
         pose_correction_attempts: int = 2,
+        joint_position_tolerance: float = 0.05,
     ) -> None:
         self.node = node
         self.geometry = geometry
         self.action_client = action_client or ActionClient(
             node, MoveToPose, MOVE_ACTION,
         )
+        self.joints_client = joints_client
+        if self.joints_client is None and node is not None:
+            self.joints_client = ActionClient(node, MoveToJoints, JOINTS_ACTION)
         self.named_client = named_client or node.create_client(
             MoveToNamedPose, NAMED_SERVICE,
         )
@@ -147,6 +153,8 @@ class MotionAdapter:
         self.stability_epsilon = float(stability_epsilon)
         self.stability_angle = float(stability_angle)
         self.pose_correction_attempts = max(1, int(pose_correction_attempts))
+        self.joint_position_tolerance = float(joint_position_tolerance)
+        self._joint_positions: tuple[float, ...] | None = None
         self.stopped = False
         self.stop_reason: str | None = None
         self._last_joint_time = None
@@ -155,8 +163,9 @@ class MotionAdapter:
 
     # ------------------------------------------------------------------ freshness
     def _on_joint_state(self, message: JointState) -> None:
-        del message
         self._last_joint_time = self.node.get_clock().now()
+        if len(message.position) == 7:
+            self._joint_positions = tuple(float(value) for value in message.position)
 
     def _default_freshness(self) -> tuple[bool, str]:
         if self._last_joint_time is None:
@@ -327,6 +336,84 @@ class MotionAdapter:
 
     def move_named(self, name: str, speed: float = 0.2, label: str = '') -> MotionResult:
         label = label or f'move_named({name})'
+        joints = self.geometry.named_joints.get(name)
+        if self.joints_client is not None and joints is not None:
+            result = self.move_joints(joints, speed, label)
+            if result.success:
+                return result
+            planning_failed = (
+                'Планирование' in result.message
+                or 'planning' in result.message.lower()
+                or 'недостижима' in result.message
+            )
+            if not planning_failed:
+                return result
+            # A joint-space plan can be rejected in an unusual scene; retry
+            # the named service once as the fallback path.
+            self.reset_stop()
+        return self._move_named_service(name, speed, label)
+
+    def move_joints(
+        self,
+        joints,
+        speed: float = 0.2,
+        label: str = '',
+    ) -> MotionResult:
+        label = label or 'move_joints'
+        values = tuple(float(value) for value in joints)
+        if len(values) != 7:
+            return MotionResult(False, 'joint goal must contain 7 values', label)
+        blocked = self._guard(label, speed, 'ptp')
+        if blocked is not None:
+            return blocked
+        if self.joints_client is None:
+            return MotionResult(False, f'{JOINTS_ACTION} is unavailable', label)
+        if not self.joints_client.wait_for_server(timeout_sec=5.0):
+            return MotionResult(False, f'{JOINTS_ACTION} is unavailable', label)
+
+        request = MoveToJoints.Goal()
+        request.joints = list(values)
+        request.speed = float(speed)
+        started = time.monotonic()
+        future = self.joints_client.send_goal_async(request)
+        if not self._spin_until(future, self.command_timeout_sec):
+            return self._fail(label, 'joint goal response timed out', None, started)
+        try:
+            goal_handle = future.result()
+        except Exception as error:
+            return self._fail(label, f'joint goal call failed: {error}', None, started)
+        if goal_handle is None or not goal_handle.accepted:
+            return self._fail(label, 'joint goal was rejected', None, started)
+        result_future = goal_handle.get_result_async()
+        if not self._spin_until(result_future, self.command_timeout_sec):
+            return self._fail(label, 'joint result timed out', None, started)
+        try:
+            result = result_future.result().result
+        except Exception as error:
+            return self._fail(label, f'joint result failed: {error}', None, started)
+        if not result.success:
+            return self._fail(
+                label, result.message or 'joint motion failed', None, started,
+            )
+        self._settle()
+        actual = self._joint_positions
+        if actual is not None and len(actual) == 7:
+            error = max(abs(a - b) for a, b in zip(actual, values))
+            if error > self.joint_position_tolerance:
+                return self._fail(
+                    label,
+                    f'joint positions are {error:.4f} rad from the joint goal',
+                    None,
+                    started,
+                )
+        return MotionResult(
+            True, 'joint motion completed', label, True, None, self.actual_tcp(),
+            time.monotonic() - started,
+        )
+
+    def _move_named_service(
+        self, name: str, speed: float, label: str,
+    ) -> MotionResult:
         blocked = self._guard(label, speed, 'ptp')
         if blocked is not None:
             return blocked

src/iiwa_experement/iiwa_experement/operations.py

@@ -236,30 +236,34 @@ class ReferenceOperations:
         *,
         allow_fallback: bool = True,
     ) -> MotionResult:
-        result = self.motion.move_pose(goal, planner=planner, speed=speed, label=label)
+        attempts = 3
+        result = None
+        planning_failed = False
+        for attempt in range(attempts):
+            result = self.motion.move_pose(
+                goal,
+                planner=planner,
+                speed=speed,
+                label=label if attempt == 0 else f'{label}:retry{attempt}',
+            )
+            if result.success:
+                if attempt:
+                    self._emit('motion_retry', label=label, planner=planner,
+                               attempt=attempt, reason=result.message)
+                return result
             planning_failed = (
                 'Планирование' in result.message
                 or 'planning' in result.message.lower()
                 or 'недостижима' in result.message
             )
-        if not result.success and planning_failed:
+            if not planning_failed:
-            # A collision-free replan after a failed search is not a blind
+                break
-            # repetition: no motion was executed and the scene is unchanged.
+            if attempt + 1 < attempts:
+                # A failed search executed no motion; replan from the same
+                # measured, still-synchronized state.
                 self.motion.reset_stop()
-            retry = self.motion.move_pose(
+                continue
-                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;
-            # execution or pose-verification failures are never retried.
             self.motion.reset_stop()
             fallback = self.motion.move_pose(
                 goal, planner='ompl', speed=speed, label=f'{label}:ompl',
@@ -267,11 +271,11 @@ class ReferenceOperations:
             if fallback.success:
                 self._emit(
                     'motion_fallback', label=label, original=planner,
-                    reason=result.message,
+                    reason=result.message if result is not None else '',
                 )
                 return fallback
             result = fallback
-        raise OperationError(f'{label}: {result.message}')
+        raise OperationError(f'{label}: {result.message if result is not None else "motion failed"}')
 
     def _above(self, goal: Transform, gap: float) -> Transform:
         return goal.compose_relaxed(_translation([0.0, 0.0, -gap]))
@@ -621,7 +625,11 @@ class ReferenceOperations:
             raise
 
     # ------------------------------------------------------------------ verifies
-    def inspect_cap(self, target_pose: Transform | None = None) -> dict[str, Any]:
+    def inspect_cap(
+        self,
+        target_pose: Transform | None = None,
+        tolerance: float | None = None,
+    ) -> dict[str, Any]:
         self.supervisor.command('inspect')
         target = target_pose or world_target(self.spec, self.geometry, 'cap_seat')
         state = self._expect_latest()
@@ -635,7 +643,10 @@ class ReferenceOperations:
         distance = float(
             (pose.position - target.position) @ (pose.position - target.position)
         ) ** 0.5
-        ok = distance <= self.geometry.position_tolerance
+        limit = (
+            self.geometry.position_tolerance if tolerance is None else float(tolerance)
+        )
+        ok = distance <= limit
         return {
             'ok': ok,
             'distance': distance,

src/iiwa_experement/iiwa_experement/reference_scenarios.py

@@ -63,7 +63,7 @@ def _cap_steps(
                 shift = expected - spec.body.x
         steps.extend([
             ReferenceStep('place_cap_shifted', (), {'dx': shift}),
-            ReferenceStep('inspect_cap', (), {'expect': 'not_ok'}),
+            ReferenceStep('inspect_cap', (), {'expect': 'not_ok', 'tolerance': 0.002}),
             ReferenceStep('pick_cap'),
             _work(),
             ReferenceStep('place_cap'),
@@ -213,9 +213,10 @@ def execute_sequence(sequence: tuple[ReferenceStep, ...], context) -> RunResult:
                 if step.args and step.args[0] == 'home':
                     home_reached = True
             elif step.kind == 'inspect_cap':
-                result = operations.inspect_cap()
+                kwargs = dict(step.kwargs)
+                expect = kwargs.pop('expect', None)
+                result = operations.inspect_cap(**kwargs)
                 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'):

src/iiwa_experement/test/test_motion.py

@@ -102,6 +102,7 @@ def goal():
 def make_adapter(
     *,
     action=None,
+    joints=None,
     named=None,
     stop=None,
     fresh=True,
@@ -117,6 +118,7 @@ def make_adapter(
         node=None,
         geometry=g,
         action_client=action,
+        joints_client=joints,
         named_client=named,
         stop_client=stop,
         tf_buffer=object(),
@@ -207,6 +209,16 @@ def test_actual_pose_out_of_tolerance_stops_the_robot():
     assert stop.calls == 1
 
 
+def test_named_motion_uses_the_joint_action_when_available():
+    joints = FakeActionClient()
+    adapter, action, _, _ = make_adapter(joints=joints)
+    result = adapter.move_named('home', speed=0.2)
+    assert result.success
+    assert len(joints.requests) == 1
+    assert list(joints.requests[0].joints) == [0.0] * 7
+    assert joints.requests[0].speed == pytest.approx(0.2)
+
+
 def test_named_motion_uses_the_existing_service():
     adapter, _, named, _ = make_adapter()
     result = adapter.move_named('home', speed=0.2)