diff --git a/docs/superpowers/plans/2026-11-08-grant-reference-control.md b/docs/superpowers/plans/2026-11-08-grant-reference-control.md index b568ddb..6a5a24a 100644 --- a/docs/superpowers/plans/2026-11-08-grant-reference-control.md +++ b/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`. Исправить ошибки до заявления о готовности. -- [ ] **Шаг 2. Проверить `view` для 1, 11, 16, 19, 20.** Сверить состав объектов, десять винтов, постоянный `patron`, отсутствие движения робота и изменений объектов после 10 секунд симуляции. -- [ ] **Шаг 3. Запустить `execute` для каждого ID 1–20.** Для каждого записать: видимые движения, точность крепления инструмента к TCP, журнал, отсутствие запрещённых столкновений и проникновений, итоговую позу люка, участки герметика, пары переданных винтов, возврат инструмента и `home`. Если поза недостижима или опасна, исправить геометрию/сцену и повторить опыт и соседние затронутые варианты. Не обходить проверку столкновений. -- [ ] **Шаг 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`. -- [ ] **Шаг 6. Зафиксировать документацию и проверенные исправления.** Просмотреть `git diff`; не включать посторонние изменения рабочего дерева. +- [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`. Исправить ошибки до заявления о готовности. +- [x] **Шаг 2. Проверить `view` для 1, 11, 16, 19, 20.** Сверить состав объектов, десять винтов, постоянный `patron`, отсутствие движения робота и изменений объектов после 10 секунд симуляции. +- [x] **Шаг 3. Запустить `execute` для каждого ID 1–20.** Для каждого записать: видимые движения, точность крепления инструмента к TCP, журнал, отсутствие запрещённых столкновений и проникновений, итоговую позу люка, участки герметика, пары переданных винтов, возврат инструмента и `home`. Если поза недостижима или опасна, исправить геометрию/сцену и повторить опыт и соседние затронутые варианты. Не обходить проверку столкновений. +- [x] **Шаг 4. Проверить ветви отказов 4, 5, 9, 15, 20.** Ровно одна разрешённая попытка восстановления; повторный отказ или неизвестное состояние останавливает робот. Винт не вставляется, отверстие пустое, затяжной инструмент не движется. +- [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`. +- [x] **Шаг 6. Зафиксировать документацию и проверенные исправления.** Просмотреть `git diff`; не включать посторонние изменения рабочего дерева. ## Критерии готовности diff --git a/src/iiwa_experement/config/geometry.yaml b/src/iiwa_experement/config/geometry.yaml index fb25c77..e043c23 100644 --- a/src/iiwa_experement/config/geometry.yaml +++ b/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] diff --git a/src/iiwa_experement/doc/README.md b/src/iiwa_experement/doc/README.md index 107447c..b5ed30f 100644 --- a/src/iiwa_experement/doc/README.md +++ b/src/iiwa_experement/doc/README.md @@ -186,17 +186,15 @@ colcon test-result --verbose ## Проверка в Webots -Для опытов 1, 6, 11, 16, 19 и 20 запуск в графическом Webots R2025a завершился -успешным `scene_ready`: 5, 4, 16, 19, 19 и 19 объектов соответственно. По -изображению верхней камеры `d455_top` проверены: +Все 20 опытов выполнены в Webots R2025a в режиме `execute`; каждый завершился +`completed`, доставил нужное число винтов, установил люк (где он есть), прошёл +требуемые участки герметика, вернул все инструменты и пришёл домой. Ветви +отказов 4, 5, 9, 15 и 20 отработали с одной разрешённой попыткой и без +установки/затяжки винтов. Режим `view` проверен для 1, 11, 16, 19 и 20: за +10 секунд симуляции позы объектов не изменились, робот остался неподвижен, +захватов нет. -- свободный люк на столе, отсутствие люка в опыте 6 и установленный люк в - опыте 11; -- 10 винтов в лотке во всех проверенных опытах 11, 16, 19 и 20; -- четыре инструмента в гнёздах стойки, нужный набор в опытах 1, 6 и 11; -- смещение лотка и всех винтов в опыте 19; -- `patron` на роботе во всех запусках. - -Сбои `move_group`, `camera_spawner` и контроллеров в журнале возникают только -при принудительном завершении запуска (`Ctrl-C`) и не влияют на готовность -сцены. +Подробная таблица, численные измерения, смещение TCP в `view`, пути к журналам +и ограничения приведены в [reference-control-qa.md](reference-control-qa.md). +Журналы последнего прогона: `/tmp/opencode/traces_all/` и +`/tmp/opencode/logs_all/`; сводка — `/tmp/opencode/summary.json`. diff --git a/src/iiwa_experement/doc/reference-control-qa.md b/src/iiwa_experement/doc/reference-control-qa.md index d7b919b..22b3fcd 100644 --- a/src/iiwa_experement/doc/reference-control-qa.md +++ b/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 | заполняется прогоном | | | | | | -| 2 | | | | | | | -| 3 | | | | | | | -| 4 | | | | | | | -| 5 | | | | | | | -| 6 | | | | | | | -| 7 | | | | | | | -| 8 | | | | | | | -| 9 | | | | | | | -| 10 | | | | | | | -| 11 | | | | | | | -| 12 | | | | | | | -| 13 | | | | | | | -| 14 | | | | | | | -| 15 | | | | | | | -| 16 | | | | | | | -| 17 | | | | | | | -| 18 | | | | | | | -| 19 | | | | | | | -| 20 | | | | | | | +| 1 | completed | — | ok | — | vacuum | да | +| 2 | completed | — | ok | — | vacuum | да | +| 3 | completed | — | ok | — | vacuum | да | +| 4 | completed | — | ok | — | vacuum | да | +| 5 | completed | — | ok | — | vacuum | да | +| 6 | completed | — | — | ok | sealant_tool | да | +| 7 | completed | — | — | ok | sealant_tool | да | +| 8 | completed | — | — | ok | sealant_tool | да | +| 9 | completed | — | — | ok | sealant_tool | да | +| 10 | completed | — | ok | ok | sealant_tool, vacuum | да | +| 11 | completed | screw_1→hole_1 | ok | — | magnetic_tool | да | +| 12 | completed | screw_1→hole_1, screw_2→hole_3 | ok | — | magnetic_tool | да | +| 13 | completed | screw_1→hole_1, screw_2→hole_2, screw_5→hole_3 | ok | — | magnetic_tool | да | +| 14 | completed | screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4 | ok | — | magnetic_tool | да | +| 15 | completed | screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4 | ok | — | magnetic_tool | да | +| 16 | completed | screw_1→hole_1 | ok | ok | sealant_tool, vacuum, magnetic_tool | да | +| 17 | completed | screw_1→hole_1, screw_2→hole_3 | ok | ok | sealant_tool, vacuum, magnetic_tool | да | +| 18 | completed | screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4 | ok | ok | sealant_tool, vacuum, magnetic_tool | да | +| 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 | 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__.jsonl`. +- Логи запуска: `/tmp/opencode/logs_all/experiment_.log`. +- Сводка: `/tmp/opencode/summary.json`. +- В каждом успешном журнале последняя строка `event=result` содержит + `status=completed`, пары винт-отверстие, `cap.ok=true` (где люк есть), + `sealant.ok=true` (где контур есть), `returned_tools` и `home_reached=true`. + + ## Ветви отказов diff --git a/src/iiwa_experement/iiwa_experement/geometry.py b/src/iiwa_experement/iiwa_experement/geometry.py index 0128273..0f9d3ff 100644 --- a/src/iiwa_experement/iiwa_experement/geometry.py +++ b/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, ) diff --git a/src/iiwa_experement/iiwa_experement/motion.py b/src/iiwa_experement/iiwa_experement/motion.py index 085db78..8de01e3 100644 --- a/src/iiwa_experement/iiwa_experement/motion.py +++ b/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 diff --git a/src/iiwa_experement/iiwa_experement/operations.py b/src/iiwa_experement/iiwa_experement/operations.py index b11c947..893610e 100644 --- a/src/iiwa_experement/iiwa_experement/operations.py +++ b/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) - 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', + 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 retry.success: - self._emit('motion_retry', label=label, planner=planner, - reason=result.message) - return retry - result = retry - if result.success: - return result + 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 planning_failed: + break + if attempt + 1 < attempts: + # A failed search executed no motion; replan from the same + # measured, still-synchronized state. + self.motion.reset_stop() + continue 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, diff --git a/src/iiwa_experement/iiwa_experement/reference_scenarios.py b/src/iiwa_experement/iiwa_experement/reference_scenarios.py index f092f42..073ea2a 100644 --- a/src/iiwa_experement/iiwa_experement/reference_scenarios.py +++ b/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'): diff --git a/src/iiwa_experement/test/test_motion.py b/src/iiwa_experement/test/test_motion.py index 05ccc83..218be5d 100644 --- a/src/iiwa_experement/test/test_motion.py +++ b/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)