From 6bd288dcaf4b18b32f47b60b99c92a7ef6c5470e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= Date: Mon, 9 Nov 2026 17:52:41 +1000 Subject: [PATCH] . --- cobot-setting.yaml | 2 +- src/iiwa_experement/config/geometry.yaml | 5 +- .../doc/reference-control-qa.md | 54 ++-- src/iiwa_experement/iiwa_experement/motion.py | 19 +- .../iiwa_experement/operations.py | 254 ++++++++++-------- .../iiwa_experement/planning_scene.py | 5 +- .../iiwa_experement/reference_scenarios.py | 69 +++-- src/iiwa_experement/test/test_motion.py | 44 +++ src/iiwa_experement/test/test_operations.py | 49 +++- .../test/test_reference_scenarios.py | 21 ++ 10 files changed, 322 insertions(+), 200 deletions(-) diff --git a/cobot-setting.yaml b/cobot-setting.yaml index fff134b..6f5f825 100644 --- a/cobot-setting.yaml +++ b/cobot-setting.yaml @@ -9,7 +9,7 @@ robot: digital_twin: tcp_gizmo: - enabled: true # TCP-гизма в Webots; только в режиме симуляции + enabled: false # TCP-гизма в Webots; только в режиме симуляции publish_period: 0.032 # период управления [с] command_timeout: 0.3 # максимальный возраст цели при отпускании мыши [с] state_timeout: 0.5 # допустимый возраст состояния робота [с] diff --git a/src/iiwa_experement/config/geometry.yaml b/src/iiwa_experement/config/geometry.yaml index 9f958ac..ea12934 100644 --- a/src/iiwa_experement/config/geometry.yaml +++ b/src/iiwa_experement/config/geometry.yaml @@ -126,10 +126,13 @@ speeds: # Calibrated safe Cartesian tcp poses (world frame). They are used as an # OMPL fallback when a short PTP named motion is rejected. +# Only the two poses the reference executor is allowed to use are mirrored +# here: 'work' is the neutral working pose, 'home' is the final pose of a +# route. The 'transport' pose from the global MoveIt config is deliberately +# not listed: it is forbidden in the reference routes. 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: diff --git a/src/iiwa_experement/doc/reference-control-qa.md b/src/iiwa_experement/doc/reference-control-qa.md index 76737fa..efd2996 100644 --- a/src/iiwa_experement/doc/reference-control-qa.md +++ b/src/iiwa_experement/doc/reference-control-qa.md @@ -61,19 +61,28 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \ ## Политика движений -- После захвата инструмент поднимается на 8 см, затем смещается на 10 см по - +Y от стойки, и только потом выполняется суставное перемещение. Полный - вертикальный подъём на 14–16 см упирается в предел запястья; боковой выход - даёт тот же зазор и не ломает достижимость `work`. -- Между операциями используется ближняя нейтральная поза `work`. `home` - остаётся финальной позой маршрута, позой перед установкой инструмента в - стойку и аварийной реконфигурацией. -- Если планировщик отклоняет подход из ветви `work` (дальние ячейки лотка, - отдельные гнёзда стойки), операция один раз переходит в `home` и повторяет - подход. Событие `reconfigure` с причиной пишется в журнал; в успешных - прогонах оно не появляется. +- После захвата инструмент поднимается строго вертикально (8 см, при отказе + планировщика — 6/3/1,5 см), затем смещается по +Y от стойки с достигнутой + высоты, и только потом выполняется суставное перемещение. Подъём + обязателен: если все осевые участки отклонены, операция останавливается, + а не продолжается боковым движением на глубине контакта. +- Маршрут начинается с перехода в `work` и заканчивается `work` → `home`. + `home` используется только как финальная поза. Между операциями робот + планирует прямо из текущей рабочей позы; повторные `work` не вставляются. +- Если планировщик отклоняет подход из текущей ветви (дальние ячейки лотка, + отдельные гнёзда стойки), операция один раз переходит в `work` и повторяет + подход. Событие `reconfigure` с причиной пишется в журнал. Поза + `transport` в эталонных маршрутах запрещена и не используется нигде, + включая аварийные ветви. +- После каждого контакта (передача винта, имитация затяжки, захват и посадка + люка, контур герметика) выполняется обязательный осевой подъём до + горизонтального перемещения. Захваченные инструмент, люк и винт + прикрепляются к роботу в сцене MoveIt (`bridge.attach`), поэтому перенос + планируется с учётом переносимого тела. - TCP при работе направлен вниз; затяжной инструмент вращается вокруг своей - оси, не меняя точку контакта. + оси, не меняя точку контакта. После поворота фланец возвращается в + исходную ориентацию (untwist), чтобы сустав 7 не накапливал закрутку + между отверстиями. - Положение после суставного перемещения проверяется по установившимся показаниям `/joint_states`, а не по первому отсчёту после ответа контроллера. При кратковременном отклонении цель повторяется один раз. @@ -86,6 +95,11 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \ ## Результаты 20 опытов +> Таблица ниже относится к предыдущей редакции маршрутов. После правок +> (запрет `transport`, обязательный осевой подъём после каждого контакта, +> untwist фланца, прикрепление переносимых объектов в сцене MoveIt) все 20 +> опытов требуют повторной верификации в Webots в режиме `execute`. + Каждый сценарий должен хотя бы один раз завершиться со статусом `completed`. При транзиентном отказе планировщика (`Планирование не удалось`) запуск повторялся с чистой сценой; таблица показывает подтверждённый успешный прогон. @@ -148,9 +162,10 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \ осевое опускание и поворот последнего сустава на 45°. Вращается только фланец, поэтому инструмент поворачивается вокруг своей оси, не уходя в другую ветвь IK. Если положительное направление упирается в предел, используется -отрицательное. После поворота робот уходит в `work`, как после передачи -винта. Отверстие остаётся пустым; винт не вставляется. Инструмент -возвращается в стойку, затем робот уходит домой. +отрицательное. После поворота фланец возвращается в исходную конфигурацию +(untwist), затем выполняется обязательный осевой подъём из отверстия, как +после передачи винта. Отверстие остаётся пустым; винт не вставляется. +Инструмент возвращается в стойку, затем робот уходит домой. ## Ветви отказов @@ -172,10 +187,11 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \ - Затяжка имитационная: `torque_tool` подходит к отверстию, поворачивается на 45° вокруг оси инструмента и отходит. Винт к этому моменту уже передан оператору и удалён из сцены. -- Прикреплённые объекты MoveItPy в установленной версии приводят к ложным - столкновениям. Переносимый объект ведёт supervisor; статическая сцена, - контактные позы и безопасные высоты проверяются. Это ограничение описано - в README. +- Прикреплённые объекты MoveIt моделируются: захваченные инструмент, люк и + винт переносятся из мировой сцены в `attached_collision_objects` с + разрешённым контактом звеньев `tcp`, `patron`, `link7` и держателя камеры. + Если в установленной версии MoveItPy снова появятся ложные касания, + первым делом проверяются touch-звенья и габариты боксов в `geometry.yaml`. - MoveIt/MoveItPy может аварийно завершиться после очень длинной серии планирований; один запуск выполняет один опыт, поэтому серия ограничена. - `Node.remove()` для винта в Webots R2025a иногда приводит к падению diff --git a/src/iiwa_experement/iiwa_experement/motion.py b/src/iiwa_experement/iiwa_experement/motion.py index f5b6d69..67efd4b 100644 --- a/src/iiwa_experement/iiwa_experement/motion.py +++ b/src/iiwa_experement/iiwa_experement/motion.py @@ -395,21 +395,10 @@ class MotionAdapter: # the named service once as the fallback path. self.reset_stop() result = self._move_named_service(name, speed, label) - if result.success or name != 'work': - return result - # The tool rack can leave the arm in a branch from which the close - # 'work' pose is unreachable. Fall back to the globally safe home - # configuration rather than aborting the route. - home = self.geometry.named_joints.get('home') - if home is not None: - self.reset_stop() - fallback = self.move_joints(home, speed, f'{label}:home') - if fallback.success: - if self.node is not None: - self.node.get_logger().warning( - f'{label}: work unreachable; reconfigured to home' - ) - return fallback + # 'home' and 'work' are the only named poses the reference executor + # may use; the forbidden 'transport' pose is never a fallback. A + # rejected 'work' already tried its calibrated safe Cartesian pose + # inside the service path, so a failure here is reported as-is. return result def move_joints( diff --git a/src/iiwa_experement/iiwa_experement/operations.py b/src/iiwa_experement/iiwa_experement/operations.py index a66c5cf..8a32ab0 100644 --- a/src/iiwa_experement/iiwa_experement/operations.py +++ b/src/iiwa_experement/iiwa_experement/operations.py @@ -331,36 +331,106 @@ class ReferenceOperations: self._move(goal, 'lin', self.geometry.speeds['contact'], f'{label}:contact') def _retreat(self, goal: Transform, label: str) -> None: - approach = self._above(goal, self.geometry.approach_gap) - transfer = self._above(goal, self.geometry.transfer_gap) - self._move(approach, 'lin', self.geometry.speeds['approach'], f'{label}:retreat') - self._move(transfer, 'ompl', self.geometry.speeds['transfer'], f'{label}:clear') + """Lift axially away from a contact pose before any lateral motion. - def _contact_with_home_retry( + On real hardware a sideways move straight out of a contact pose drags + the tool or the part across the fixture, so the lift is mandatory: + the gaps only shrink, and if every axial segment is rejected the + operation stops instead of continuing at contact depth. + """ + lifted = False + for index, gap in enumerate((self.geometry.approach_gap, 0.03, 0.015)): + lift_label = f'{label}:retreat' if index == 0 else f'{label}:retreat:{gap:.3f}' + try: + self._move( + self._above(goal, gap), 'lin', + self.geometry.speeds['approach'], lift_label, + ) + lifted = True + break + except OperationError as error: + if 'Планирование' not in str(error): + raise + self.motion.reset_stop() + if not lifted: + raise OperationError(f'{label}: the axial retreat was rejected') + self._move( + self._above(goal, self.geometry.transfer_gap), 'ompl', + self.geometry.speeds['transfer'], f'{label}:clear', + ) + + def _exit_rack(self, goal: Transform, label: str) -> None: + """Lift straight up out of the rack, then step sideways out of it. + + The lift is mandatory: a lateral move at socket depth would drag the + tool (or the empty patron) through the magazine on real hardware. + The sideways step starts from the height that was actually reached. + """ + height = None + for index, gap in enumerate(( + self.geometry.transfer_gap, self.geometry.approach_gap, 0.03, 0.015, + )): + lift_label = f'{label}:lift' if index == 0 else f'{label}:lift:{gap:.3f}' + try: + self._move( + self._above(goal, gap), 'lin', + self.geometry.speeds['approach'], lift_label, + ) + height = gap + break + except OperationError as error: + if 'Планирование' not in str(error): + raise + self.motion.reset_stop() + if height is None: + raise OperationError( + f'{label}: the axial lift out of the rack was rejected' + ) + base = self._above(goal, height) + for offset in (0.10, 0.06, 0.03): + exit_pose = Transform.from_xyz_rpy( + [0.0, offset, 0.0], [0.0, 0.0, 0.0], + parent='world', child='rack_exit_offset', + ).compose_relaxed(base) + exit_label = ( + f'{label}:exit' if offset == 0.10 else f'{label}:exit:{offset:.2f}' + ) + try: + self._move( + exit_pose, 'lin', self.geometry.speeds['transfer'], exit_label, + ) + return + except OperationError as error: + if 'Планирование' not in str(error): + raise + self.motion.reset_stop() + raise OperationError(f'{label}: no lateral rack exit was plannable') + + def _contact_with_recovery( self, goal: Transform, label: str, *, prepare=None, **kwargs, ) -> None: - """Contact a target, reconfiguring through home only if planning fails. + """Contact a target, reconfiguring through 'work' if planning fails. - The routes use the closer 'work' pose between tasks. A few tray and - rack cells are nevertheless only reachable from the home branch, so - the fallback is applied on demand instead of forcing a home detour on - every transition. + 'work' is the only neutral pose the routes are allowed to use: + 'transport' is forbidden entirely and 'home' is reserved for the end + of the route. A single retry from 'work' switches the IK branch + without leaving the working area. """ try: self._contact(goal, label, **kwargs) except OperationError as error: if 'Планирование' not in str(error): raise - self._emit('reconfigure', label=label, target='home', + self._emit('reconfigure', label=label, target='work', reason=str(error)) if prepare is not None: prepare() self.motion.reset_stop() - named = self.motion.move_named('home', 0.3) + named = self.motion.move_named('work', 0.3) if not getattr(named, 'success', False): raise error self.motion.reset_stop() - self._contact(goal, f'{label}:home_retry', **kwargs) + self._contact(goal, f'{label}:recovery', **kwargs) def _require_synchronized(self) -> None: try: @@ -394,49 +464,17 @@ class ReferenceOperations: self._require_synchronized() tool_pose = self.supervisor.object_pose(f'grant_{tool_name}') goal = tcp_goal(tool_pose, self.geometry.tool_tcp_to_nodes[tool_name]) - self._contact_with_home_retry( + self._contact_with_recovery( goal, f'take_tool({tool_name})', remove_before_contact=f'grant_{tool_name}', ) self.supervisor.command('attach_tool', tool_name) - # Lift a little, then translate along +Y away from the magazine - # before any large joint-space move. A full-height vertical lift - # runs into the wrist limit; the lateral exit is what actually - # keeps the tool body out of the rack textures. - transfer = self._above(goal, self.geometry.transfer_gap) - try: - self._move( - transfer, 'lin', self.geometry.speeds['approach'], - f'take_tool({tool_name}):lift', - ) - except OperationError as error: - # The tool is attached. Continue with the lateral exit (or - # let the route's 'work' move recover) rather than aborting - # with the tool in the rack. - if 'Планирование' not in str(error): - raise - self._emit('lift_skipped', label=f'take_tool({tool_name})', - reason=str(error)) - self.motion.reset_stop() - for offset in (0.10, 0.06, 0.03): - exit_pose = Transform.from_xyz_rpy( - [0.0, offset, 0.0], [0.0, 0.0, 0.0], - parent='world', child='rack_exit_offset', - ).compose_relaxed(transfer) - label = ( - f'take_tool({tool_name}):exit' if offset == 0.10 - else f'take_tool({tool_name}):exit:{offset:.2f}' - ) - try: - self._move( - exit_pose, 'lin', self.geometry.speeds['transfer'], - label, - ) - break - except OperationError as error: - if 'Планирование' not in str(error): - raise - self.motion.reset_stop() + # The held tool becomes part of the robot in the planning scene, + # so the lift, the rack exit and every later transfer are planned + # with the tool body instead of dragging it through the magazine. + self.bridge.attach(attached_tool(tool_name, self.geometry)) + self._require_synchronized() + self._exit_rack(goal, f'take_tool({tool_name})') self._emit('take_tool', object=tool_name, phase='done', tool=tool_name, tcp=goal.xyz) return OperationResult(True, 'take_tool', f'{tool_name} mounted', tuple(self.events)) @@ -455,7 +493,7 @@ class ReferenceOperations: # pose is verified by both the motion adapter and supervisor. self.bridge.remove(f'grant_{tool_name}') self._require_synchronized() - self._contact_with_home_retry(goal, f'dock_tool({tool_name})') + self._contact_with_recovery(goal, f'dock_tool({tool_name})') self.supervisor.command('dock_tool', tool_name) self.bridge.add_world( f'grant_{tool_name}', @@ -463,44 +501,7 @@ class ReferenceOperations: rack_pose, ) self._require_synchronized() - try: - self._retreat(goal, f'dock_tool({tool_name})') - except OperationError as error: - # The axial retreat can be rejected from the socket branch. - # The tool is already docked, so stepping out is still safe. - if 'Планирование' not in str(error): - raise - self.motion.reset_stop() - # Step out of the rack along +Y before any joint-space move; the - # axial retreat alone can leave the wrist against its limit. Use a - # lower height if the full retreat was rejected. - for gap in ( - self.geometry.transfer_gap, 0.05, 0.02, - ): - base = self._above(goal, gap) - reached = False - for offset in (0.10, 0.06, 0.03): - exit_pose = Transform.from_xyz_rpy( - [0.0, offset, 0.0], [0.0, 0.0, 0.0], - parent='world', child='rack_exit_offset', - ).compose_relaxed(base) - label = ( - f'dock_tool({tool_name}):exit' if offset == 0.10 - else f'dock_tool({tool_name}):exit:{offset:.2f}' - ) - try: - self._move( - exit_pose, 'lin', - self.geometry.speeds['transfer'], label, - ) - reached = True - break - except OperationError as error: - if 'Планирование' not in str(error): - raise - self.motion.reset_stop() - if reached: - break + self._exit_rack(goal, f'dock_tool({tool_name})') self._emit('dock_tool', object=tool_name, phase='done') return OperationResult(True, 'dock_tool', f'{tool_name} docked', tuple(self.events)) except Exception as error: @@ -526,6 +527,10 @@ class ReferenceOperations: goal, 'pick_cap', remove_before_contact='grant_cap', ) self.supervisor.command('grab_cap', 'cap') + # Plan the retreat and the transfer with the hatch attached to the + # robot, not as a blind spot removed from the scene. + self.bridge.attach(attached_cap(self.geometry)) + self._require_synchronized() self._retreat(goal, 'pick_cap') self._emit('pick_cap', object='cap', phase='done', tcp=goal.xyz) return OperationResult(True, 'pick_cap', 'hatch grasped', tuple(self.events)) @@ -607,9 +612,8 @@ class ReferenceOperations: first = sealant_point(self.geometry, self.spec, segments[0], 0.0) first_goal = tcp_goal(first, tip) self._contact(first_goal, 'dispense_contour:start') - for index, segment in enumerate(segments): - end_fraction = 1.0 if index < len(segments) - 1 else 1.0 - end = sealant_point(self.geometry, self.spec, segment, end_fraction) + for segment in segments: + end = sealant_point(self.geometry, self.spec, segment, 1.0) end_goal = tcp_goal(end, tip) self._move( end_goal, 'lin', self.geometry.speeds['contour'], @@ -641,24 +645,18 @@ class ReferenceOperations: ) self._require_synchronized() - self._contact_with_home_retry( + self._contact_with_recovery( goal, f'pick_screw({screw_id})', remove_before_contact=def_name, prepare=restore_screw, ) self.supervisor.command('grab_screw', screw_id) - try: - self._retreat(goal, f'pick_screw({screw_id})') - except OperationError as error: - # The screw is already attached. A rejected axial lift at a - # far tray cell can be recovered by the route's next 'work' - # move instead of aborting with the screw in hand. - if 'Планирование' not in str(error): - raise - self._emit('retreat_skipped', - label=f'pick_screw({screw_id})', - reason=str(error)) - self.motion.reset_stop() + self.bridge.attach(attached_screw(screw_id, self.geometry)) + self._require_synchronized() + # The axial lift is mandatory even at far tray cells: continuing + # laterally with the screw at tray depth would drag it across the + # tray walls on real hardware. + self._retreat(goal, f'pick_screw({screw_id})') self._emit('pick_screw', object=screw_id, phase='done') return OperationResult(True, 'pick_screw', f'{screw_id} grasped', tuple(self.events)) except Exception as error: @@ -672,7 +670,7 @@ class ReferenceOperations: self._require_synchronized() hole = world_target(self.spec, self.geometry, f'hole:{hole_id}') goal = tcp_goal(hole, self.geometry.screw_tcp_to_node) - self._contact( + self._contact_with_recovery( goal, f'handoff_screw({screw_id},{hole_id})', remove_before_contact=def_name, remove_phase='contact', @@ -681,9 +679,10 @@ class ReferenceOperations: 'handoff_screw', screw_id, target=hole, target_name=hole_id, ) self._require_synchronized() - # A Cartesian retreat from the hole can leave the wrist in a - # singular branch. The caller moves to 'work' in joint space - # immediately afterwards; that is the verified clearance move. + # Always lift axially out of the handoff pose before the route + # continues: a joint-space or lateral move at hole height would + # drag the magnet across the body on real hardware. + self._retreat(goal, f'handoff_screw({screw_id},{hole_id})') self._emit('delivered', object=screw_id, target=hole_id, phase='done', hole=hole_id) return OperationResult( @@ -717,10 +716,10 @@ class ReferenceOperations: ) tip = self.geometry.tool_tcp_to_nodes['torque_tool'] goal = tcp_goal(target, tip) - self._contact(goal, f'tighten_hole({hole_id})') + self._contact_with_recovery(goal, f'tighten_hole({hole_id})') twist_error = None - twisted = goal twist_done = False + joint_twist = False # Prefer a pure flange rotation: it twists the tool about its own # axis without moving the arm to another IK branch. A Cartesian # twist can push joint 6 into its limit, after which 'work' is no @@ -730,6 +729,7 @@ class ReferenceOperations: if provider is not None: joints = provider() if joints is not None and len(joints) == 7: + joints = tuple(float(value) for value in joints) for sign in (1.0, -1.0): target_joints = list(joints) target_joints[6] += math.radians(sign * twist_deg) @@ -740,6 +740,7 @@ class ReferenceOperations: ) if getattr(result, 'success', False): twist_done = True + joint_twist = True self._emit('tighten_twist', object=hole_id, joint=7, delta_deg=sign * twist_deg) break @@ -761,7 +762,6 @@ class ReferenceOperations: candidate, 'lin', self.geometry.speeds['contour'], f'tighten_hole({hole_id}):{suffix}', ) - twisted = candidate twist_done = True break except OperationError as error: @@ -774,9 +774,29 @@ class ReferenceOperations: twist_error = error if not twist_done: raise twist_error - # The caller's following 'work' move is the verified clearance - # move, as after a screw handoff. A separate Cartesian retreat - # from the twisted pose can be rejected by the wrist limits. + # Return the flange to the contact orientation before leaving. + # Without the untwist, joint 7 accumulates 45 deg per hole and the + # wrist winds up until every later plan starts from a twisted + # branch. + if joint_twist: + self.motion.wait_stationary() + untwist = self.motion.move_joints( + joints, self.geometry.speeds['contour'], + f'tighten_hole({hole_id}):joint_untwist', + ) + if not getattr(untwist, 'success', False): + raise OperationError( + f'tighten_hole({hole_id}): the joint untwist failed: ' + f'{untwist.message}' + ) + else: + self._move( + goal, 'lin', self.geometry.speeds['contour'], + f'tighten_hole({hole_id}):untwist', + ) + # Lift axially out of the hole before the route continues, exactly + # as after a screw handoff. + self._retreat(goal, f'tighten_hole({hole_id})') self._emit('tighten_simulated', object=hole_id, phase='done', twist_deg=twist_deg) return OperationResult( diff --git a/src/iiwa_experement/iiwa_experement/planning_scene.py b/src/iiwa_experement/iiwa_experement/planning_scene.py index d847f32..e8cefb3 100644 --- a/src/iiwa_experement/iiwa_experement/planning_scene.py +++ b/src/iiwa_experement/iiwa_experement/planning_scene.py @@ -40,8 +40,11 @@ SCENE_TOPIC = '/planning_scene' MONITORED_TOPIC = '/monitored_planning_scene' GET_SCENE_SERVICE = '/get_planning_scene' SCENE_FRAME = 'base_link' +# The tool is rigidly bolted to the flange, so contact between its collision +# box and the wrist/flange links is expected and allowed. Without 'link7' +# the wide vacuum box reads as a false collision against the wrist housing. TOUCH_LINKS = ( - 'tcp', 'patron', 'camera_holder', 'camera_corner', + 'tcp', 'patron', 'link7', 'camera_holder', 'camera_corner', 'camera_hand', 'camera_optical_frame', ) ROBOT_LINKS = ( diff --git a/src/iiwa_experement/iiwa_experement/reference_scenarios.py b/src/iiwa_experement/iiwa_experement/reference_scenarios.py index de57f78..b6ddbee 100644 --- a/src/iiwa_experement/iiwa_experement/reference_scenarios.py +++ b/src/iiwa_experement/iiwa_experement/reference_scenarios.py @@ -31,15 +31,13 @@ def _work(speed: float = 0.3) -> ReferenceStep: def _take(tool: str, *, first: bool = False) -> list[ReferenceStep]: + # The route starts at 'work'; later tool changes reconfigure there first + # so the approach starts from a known branch. The task itself follows + # directly from the tool exit. steps: list[ReferenceStep] = [] if not first: - # 'work' is the neutral transfer pose: closer to the task area than - # 'home' and already used by the verified routes between operations. steps.append(_work()) - steps.extend([ - ReferenceStep('take_tool', (tool,)), - _work(), - ]) + steps.append(ReferenceStep('take_tool', (tool,))) return steps @@ -49,7 +47,6 @@ def _cap_steps( grasp_failure: bool = False, verify_failure: bool = False, first_tool: bool = False, - finish_home: bool = True, ) -> list[ReferenceStep]: steps = _take('vacuum', first=first_tool) if grasp_failure: @@ -57,7 +54,7 @@ def _cap_steps( ReferenceStep('simulate_grasp_failure', ('cap',)), ReferenceStep('inspect_state'), ]) - steps.extend([ReferenceStep('pick_cap'), _work()]) + steps.append(ReferenceStep('pick_cap')) if verify_failure: shift = 0.003 for failure in spec.failures: @@ -68,7 +65,6 @@ def _cap_steps( ReferenceStep('place_cap_shifted', (), {'dx': shift}), ReferenceStep('inspect_cap', (), {'expect': 'not_ok', 'tolerance': 0.002}), ReferenceStep('pick_cap'), - _work(), ReferenceStep('place_cap'), ReferenceStep('inspect_cap', (), {'expect': 'ok'}), ]) @@ -77,13 +73,7 @@ def _cap_steps( ReferenceStep('place_cap'), ReferenceStep('inspect_cap', (), {'expect': 'ok'}), ]) - steps.extend([ - _home(), - ReferenceStep('dock_tool', ('vacuum',)), - ]) - # Always leave the rack through home: the dock retreat can sit against a - # wrist limit from which 'work' is not reachable. - steps.append(_home()) + steps.append(ReferenceStep('dock_tool', ('vacuum',))) return steps @@ -92,7 +82,6 @@ def _sealant_steps( *, 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) @@ -111,10 +100,8 @@ def _sealant_steps( steps.append(ReferenceStep('dispense_contour', (tuple(sides),))) steps.extend([ ReferenceStep('inspect_sealant', (), {'expect': 'ok'}), - _home(), ReferenceStep('dock_tool', ('sealant_tool',)), ]) - steps.append(_home()) return steps @@ -123,7 +110,6 @@ def _screw_steps( *, failure_index: int | None = None, first_tool: bool = False, - finish_home: bool = True, ) -> list[ReferenceStep]: steps = _take('magnetic_tool', first=first_tool) # Prefer the closer tray cells. The far +Y/+X corner can be outside the @@ -140,31 +126,23 @@ def _screw_steps( ReferenceStep('pick_screw', (screw,)), _work(), ReferenceStep('handoff_screw', (screw, hole)), - _work(), ]) - steps.append(_home()) + steps.append(_work()) steps.append(ReferenceStep('dock_tool', ('magnetic_tool',))) - steps.append(_home()) if 'torque_tool' in spec.tool_names: - steps.extend([ - ReferenceStep('take_tool', ('torque_tool',)), - _work(), - ]) + steps.append(_work()) + steps.append(ReferenceStep('take_tool', ('torque_tool',))) for hole in spec.screw_holes: - steps.extend([ - ReferenceStep('tighten_hole', (hole,)), - _work(), - ]) - steps.append(_home()) + steps.append(ReferenceStep('tighten_hole', (hole,))) + steps.append(_work()) steps.append(ReferenceStep('dock_tool', ('torque_tool',))) - steps.append(_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] = [] + steps: list[ReferenceStep] = [_work()] if experiment in (1, 2, 3, 4, 5): steps.extend(_cap_steps( @@ -179,7 +157,7 @@ def build_reference_sequence(spec: ScenarioSpec) -> tuple[ReferenceStep, ...]: )) elif experiment == 10: steps.extend(_sealant_steps( - spec, interrupted=False, first_tool=True, finish_home=False, + spec, interrupted=False, first_tool=True, )) steps.extend(_cap_steps(spec, first_tool=False)) elif 11 <= experiment <= 15: @@ -191,20 +169,35 @@ def build_reference_sequence(spec: ScenarioSpec) -> tuple[ReferenceStep, ...]: else: interrupted = experiment == 20 steps.extend(_sealant_steps( - spec, interrupted=interrupted, first_tool=True, finish_home=False, + spec, interrupted=interrupted, first_tool=True, )) steps.extend(_cap_steps( spec, verify_failure=(experiment == 20), first_tool=False, - finish_home=False, )) steps.extend(_screw_steps( spec, failure_index=0 if experiment == 20 else None, first_tool=False, )) - return tuple(steps) + + # The technical process always ends at 'work'; only the very last move is + # the final return to 'home'. + steps.append(_work()) + steps.append(_home()) + cleaned: list[ReferenceStep] = [] + for step in steps: + if ( + cleaned + and step.kind == 'move_named' + and step.args == ('work',) + and cleaned[-1].kind == 'move_named' + and cleaned[-1].args == ('work',) + ): + continue + cleaned.append(step) + return tuple(cleaned) def execute_sequence(sequence: tuple[ReferenceStep, ...], context) -> RunResult: diff --git a/src/iiwa_experement/test/test_motion.py b/src/iiwa_experement/test/test_motion.py index 218be5d..3415720 100644 --- a/src/iiwa_experement/test/test_motion.py +++ b/src/iiwa_experement/test/test_motion.py @@ -3,6 +3,8 @@ from pathlib import Path import pytest +from scipy.spatial.transform import Rotation + from iiwa_experement.geometry import Transform, load_geometry from iiwa_experement.motion import MotionAdapter @@ -245,3 +247,45 @@ def test_unavailable_server_is_reported_without_motion(): assert 'unavailable' in result.message assert action.requests == [] assert adapter.stopped is False + + +def test_work_fallback_uses_the_safe_pose_and_never_transport(): + planning_error = 'Планирование не удалось: injected failure' + joints = FakeActionClient(result=FakeResult(False, planning_error)) + named = FakeService(success=False, message=planning_error) + action = FakeActionClient() + + def echo_pose(): + request = action.requests[-1] + matrix = Rotation.from_euler( + 'ZYX', [request.a, request.b, request.c], + ).as_matrix() + return Transform( + xyz=(request.x, request.y, request.z), + rotation=tuple(tuple(float(v) for v in row) for row in matrix), + parent='base_link', child='tcp', + ) + + g = geometry() + # The forbidden pose is not mirrored into the executor at all. + assert 'transport' not in g.named_joints + adapter = MotionAdapter( + node=None, + geometry=g, + action_client=action, + joints_client=joints, + named_client=named, + stop_client=StopService(), + tf_buffer=object(), + scene_check=lambda: (True, 'ok'), + freshness_check=lambda: (True, 'ok'), + pose_provider=echo_pose, + spin_until=lambda future, timeout: future.done(), + ) + result = adapter.move_named('work', speed=0.2) + assert result.success, result.message + # Both named paths were rejected, so the calibrated safe Cartesian pose + # was planned with OMPL instead of reconfiguring through 'transport'. + assert action.requests[-1].planner == 'ompl' + for request in joints.requests: + assert list(request.joints) == list(g.named_joints['work']) diff --git a/src/iiwa_experement/test/test_operations.py b/src/iiwa_experement/test/test_operations.py index 1b5d8a3..d417405 100644 --- a/src/iiwa_experement/test/test_operations.py +++ b/src/iiwa_experement/test/test_operations.py @@ -59,6 +59,8 @@ class FakeBridge: def remove(self, object_id): self.removed.append(object_id) + if object_id in self.attached: + self.attached.remove(object_id) def add_world(self, object_id, box, pose): self.added_world = getattr(self, 'added_world', []) @@ -142,7 +144,7 @@ def test_take_tool_follows_the_visible_axial_route(): 'take_tool(vacuum):exit', ] assert supervisor.commands[0][:2] == ('attach_tool', 'vacuum') - assert bridge.attached == [] + assert bridge.attached == ['grant_vacuum'] contact_goal = motion.moves[2][3] assert contact_goal.xyz == pytest.approx(expected.xyz, abs=1e-9) assert motion.moves[0][1] == 'ompl' @@ -162,7 +164,7 @@ def test_dock_tool_returns_the_tool_to_its_socket(): def test_cap_pick_and_place_use_the_hatch_node_transform(): operations, supervisor, bridge, motion, geometry = make_operations(1) assert operations.pick_cap().success - assert bridge.attached == [] + assert bridge.attached == ['grant_cap'] assert supervisor.commands[0][:2] == ('grab_cap', 'cap') assert operations.place_cap().success assert bridge.added_world[0][0] == 'grant_cap' @@ -213,7 +215,7 @@ def test_contour_does_not_lift_between_adjacent_sides(): def test_screw_pick_and_handoff_are_one_motion_and_one_removal(): operations, supervisor, bridge, motion, geometry = make_operations(11) assert operations.pick_screw('screw_1').success - assert bridge.attached == [] + assert bridge.attached == ['grant_screw_01'] assert supervisor.commands[0][:2] == ('grab_screw', 'screw_1') assert operations.handoff_screw('screw_1', 'hole_1').success assert supervisor.commands[1][0] == 'handoff_screw' @@ -236,33 +238,64 @@ def test_tighten_hole_is_a_visible_revolution_without_a_screw(): assert label == 'tighten_hole(hole_1):joint_twist' assert joints[:6] == (0.0,) * 6 assert joints[6] == pytest.approx(math.radians(45.0)) + # The flange is untwisted before the axial retreat, so joint 7 does not + # accumulate twist across holes. + untwist_label, untwist_joints = motion.joint_moves[1] + assert untwist_label == 'tighten_hole(hole_1):joint_untwist' + assert untwist_joints == (0.0,) * 7 + assert names[-2:] == [ + 'tighten_hole(hole_1):retreat', 'tighten_hole(hole_1):clear', + ] # The operation is a simulation only: no supervisor command changes state. assert supervisor.commands == [] assert bridge.attached == [] -def test_screw_pick_reconfigures_to_home_when_work_is_blocked(): +def test_screw_pick_reconfigures_through_work_when_a_cell_is_blocked(): operations, supervisor, bridge, motion, geometry = make_operations(11) motion.fail_labels.add('pick_screw(screw_1):transfer') assert operations.pick_screw('screw_1').success - assert motion.named[-1][0] == 'home' - assert 'pick_screw(screw_1):home_retry:transfer' in labels(motion) + assert motion.named[-1][0] == 'work' + assert 'pick_screw(screw_1):recovery:transfer' in labels(motion) assert supervisor.commands[0][:2] == ('grab_screw', 'screw_1') +def test_screw_pick_stops_instead_of_dragging_when_the_lift_is_rejected(): + operations, supervisor, bridge, motion, geometry = make_operations(11) + motion.fail_labels.add('pick_screw(screw_1):retreat') + with pytest.raises(OperationError): + operations.pick_screw('screw_1') + assert operations.events[-1]['phase'] == 'failed' + + +def test_handoff_screw_lifts_axially_before_the_next_move(): + operations, supervisor, bridge, motion, geometry = make_operations(11) + assert operations.pick_screw('screw_1').success + motion.moves.clear() + assert operations.handoff_screw('screw_1', 'hole_1').success + names = labels(motion) + assert names[-2:] == [ + 'handoff_screw(screw_1,hole_1):retreat', + 'handoff_screw(screw_1,hole_1):clear', + ] + + def test_tighten_hole_reverses_when_the_flange_limit_blocks_one_direction(): operations, supervisor, bridge, motion, geometry = make_operations(14) motion.fail_positive_joint = True assert operations.tighten_hole('hole_1').success - assert len(motion.joint_moves) == 2 + assert len(motion.joint_moves) == 3 assert motion.joint_moves[1][1][6] == pytest.approx(math.radians(-45.0)) + # The untwist returns the flange to the contact configuration. + assert motion.joint_moves[2][0] == 'tighten_hole(hole_1):joint_untwist' + assert motion.joint_moves[2][1][6] == pytest.approx(0.0) def test_failed_motion_stops_the_operation_and_emits_failure(): operations, supervisor, bridge, motion, geometry = make_operations(1) motion.fail_labels.update({ 'take_tool(vacuum):contact', - 'take_tool(vacuum):home_retry:contact', + 'take_tool(vacuum):recovery:contact', }) with pytest.raises(OperationError): operations.take_tool('vacuum') diff --git a/src/iiwa_experement/test/test_reference_scenarios.py b/src/iiwa_experement/test/test_reference_scenarios.py index 0a46e7f..cdd0ad9 100644 --- a/src/iiwa_experement/test/test_reference_scenarios.py +++ b/src/iiwa_experement/test/test_reference_scenarios.py @@ -40,6 +40,27 @@ def test_every_route_is_nonempty_and_returns_home(): assert names[-2:] == ['dock_tool', 'move_named'] or names[-1] == 'move_named' or 'dock_tool' in names +def test_routes_start_at_work_and_only_end_at_home(): + for experiment_id in range(1, 21): + steps = route(experiment_id) + assert steps[0].kind == 'move_named' + assert steps[0].args == ('work',) + assert steps[-2].kind == 'move_named' + assert steps[-2].args == ('work',) + assert steps[-1].kind == 'move_named' + assert steps[-1].args == ('home',) + homes = [ + index for index, step in enumerate(steps) + if step.kind == 'move_named' and step.args == ('home',) + ] + assert homes == [len(steps) - 1] + # The 'transport' pose is forbidden in the reference routes. + assert not any( + step.kind == 'move_named' and step.args == ('transport',) + for step in steps + ) + + def test_cap_experiments_use_the_vacuum_tool(): for experiment_id in range(1, 6): names = kinds(experiment_id)