diff --git a/src/iiwa_experement/doc/README.md b/src/iiwa_experement/doc/README.md index 30f52e7..0adf081 100644 --- a/src/iiwa_experement/doc/README.md +++ b/src/iiwa_experement/doc/README.md @@ -90,7 +90,7 @@ Webots. В `execute` первое движение начинается не р инструментами и признаком `home_reached`. - Винты не вставляются. После подтверждённой передачи у отверстия винт убирается из сцены, отверстие остаётся пустым. В опытах 14–20 берётся - `torque_tool`, подходит к каждому отверстию, выполняет поворот на 60° и + `torque_tool`, подходит к каждому отверстию, выполняет поворот на 45° и возвращается в стойку. Это видимая имитация затяжки без установки винта. Управляемые ветви: опыт 4 — один неудачный захват люка; 5 — первая посадка diff --git a/src/iiwa_experement/doc/reference-control-qa.md b/src/iiwa_experement/doc/reference-control-qa.md index 4add569..8fa74af 100644 --- a/src/iiwa_experement/doc/reference-control-qa.md +++ b/src/iiwa_experement/doc/reference-control-qa.md @@ -61,9 +61,10 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \ ## Политика движений -- После захвата инструмент поднимается вертикально на 16 см, и только затем - выполняется боковое или суставное перемещение. Так корпус инструмента не - проходит сквозь текстуры стойки. +- После захвата инструмент поднимается на 8 см, затем смещается на 10 см по + +Y от стойки, и только потом выполняется суставное перемещение. Полный + вертикальный подъём на 14–16 см упирается в предел запястья; боковой выход + даёт тот же зазор и не ломает достижимость `work`. - Между операциями используется ближняя нейтральная поза `work`. `home` остаётся финальной позой маршрута, позой перед установкой инструмента в стойку и аварийной реконфигурацией. @@ -139,9 +140,11 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \ В опытах 14, 15 и 16–20 после передач винтов берётся `torque_tool`. Для каждого отверстия выполняется `tighten_simulated`: подъём над отверстием, -осевое опускание, поворот на 60°, возврат и осевой отход. Если у дальнего -отверстия запястье упирается в предел, поворот выполняется в противоположную -сторону. Отверстие остаётся пустым; винт не вставляется. Инструмент +осевое опускание и поворот последнего сустава на 45°. Вращается только +фланец, поэтому инструмент поворачивается вокруг своей оси, не уходя в другую +ветвь IK. Если положительное направление упирается в предел, используется +отрицательное. После поворота робот уходит в `work`, как после передачи +винта. Отверстие остаётся пустым; винт не вставляется. Инструмент возвращается в стойку, затем робот уходит домой. ## Ветви отказов @@ -162,7 +165,7 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \ - Только симуляция Webots; FRI и физический робот не запускаются. - Винты не вставляются; после передачи отверстие остаётся пустым. - Затяжка имитационная: `torque_tool` подходит к отверстию, поворачивается на - 60° вокруг оси инструмента и отходит. Винт к этому моменту уже передан + 45° вокруг оси инструмента и отходит. Винт к этому моменту уже передан оператору и удалён из сцены. - Прикреплённые объекты MoveItPy в установленной версии приводят к ложным столкновениям. Переносимый объект ведёт supervisor; статическая сцена, diff --git a/src/iiwa_experement/iiwa_experement/motion.py b/src/iiwa_experement/iiwa_experement/motion.py index d33e681..f5b6d69 100644 --- a/src/iiwa_experement/iiwa_experement/motion.py +++ b/src/iiwa_experement/iiwa_experement/motion.py @@ -157,6 +157,7 @@ class MotionAdapter: self.joint_correction_attempts = max(1, int(joint_correction_attempts)) self.joint_position_tolerance = float(joint_position_tolerance) self._joint_positions: tuple[float, ...] | None = None + self._joint_velocities: tuple[float, ...] | None = None self.stopped = False self.stop_reason: str | None = None self._last_joint_time = None @@ -168,6 +169,45 @@ class MotionAdapter: 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) + if len(message.velocity) == 7: + self._joint_velocities = tuple( + float(value) for value in message.velocity + ) + + def joint_positions(self) -> tuple[float, ...] | None: + return self._joint_positions + + def wait_stationary( + self, timeout_sec: float = 3.0, tolerance: float = 0.01, + ) -> bool: + """Wait for fresh, near-zero joint velocities before planning. + + The planning pipeline rejects a motion whose recorded start state still + carries velocity. The controller action can finish before the next + zero-velocity sample reaches the planning server, so gate the next + plan on a short stable window. + """ + if self.node is None or self._freshness_check is not None: + return True + import rclpy + + deadline = time.monotonic() + max(0.0, float(timeout_sec)) + stable_since = None + while time.monotonic() < deadline: + rclpy.spin_once(self.node, timeout_sec=0.02) + velocities = self._joint_velocities + if velocities is None or len(velocities) != 7 or any( + abs(value) > tolerance for value in velocities + ): + stable_since = None + continue + stable_since = stable_since or time.monotonic() + if time.monotonic() - stable_since >= 0.20: + return True + self.node.get_logger().warning( + 'joint_states did not settle before planning' + ) + return False def _default_freshness(self) -> tuple[bool, str]: if self._last_joint_time is None: @@ -338,6 +378,7 @@ class MotionAdapter: def move_named(self, name: str, speed: float = 0.2, label: str = '') -> MotionResult: label = label or f'move_named({name})' + self.wait_stationary() 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) @@ -353,7 +394,23 @@ class MotionAdapter: # 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) + 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 + return result def move_joints( self, diff --git a/src/iiwa_experement/iiwa_experement/operations.py b/src/iiwa_experement/iiwa_experement/operations.py index 5ff347b..58da731 100644 --- a/src/iiwa_experement/iiwa_experement/operations.py +++ b/src/iiwa_experement/iiwa_experement/operations.py @@ -304,6 +304,7 @@ class ReferenceOperations: # false contact with the target itself. self.bridge.remove(remove_before_contact) self._require_synchronized() + self.motion.wait_stationary() transfer = self._above(goal, transfer_gap) approach = self._above(goal, approach_gap) self._move(transfer, 'ompl', self.geometry.speeds['transfer'], f'{label}:transfer') @@ -333,7 +334,9 @@ class ReferenceOperations: self._move(approach, 'lin', self.geometry.speeds['approach'], f'{label}:retreat') self._move(transfer, 'ompl', self.geometry.speeds['transfer'], f'{label}:clear') - def _contact_with_home_retry(self, goal: Transform, label: str, **kwargs) -> None: + def _contact_with_home_retry( + self, goal: Transform, label: str, *, prepare=None, **kwargs, + ) -> None: """Contact a target, reconfiguring through home only if planning fails. The routes use the closer 'work' pose between tasks. A few tray and @@ -348,6 +351,8 @@ class ReferenceOperations: raise self._emit('reconfigure', label=label, target='home', reason=str(error)) + if prepare is not None: + prepare() self.motion.reset_stop() named = self.motion.move_named('home', 0.3) if not getattr(named, 'success', False): @@ -392,30 +397,34 @@ class ReferenceOperations: remove_before_contact=f'grant_{tool_name}', ) self.supervisor.command('attach_tool', tool_name) - # Lift straight up, clear of the magazine, before any lateral or - # joint-space move. Without this the tool body sweeps through the - # rack obstacles while the arm travels to 'work'. The sockets sit - # at slightly different reachable heights, so accept the highest - # clearance that plans. - lift_error = None - for gap in (0.16, 0.14, 0.12): + # 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) + self._move( + transfer, 'lin', self.geometry.speeds['approach'], + f'take_tool({tool_name}):lift', + ) + 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}):lift' if gap == 0.16 - else f'take_tool({tool_name}):lift:{gap:.2f}' + f'take_tool({tool_name}):exit' if offset == 0.10 + else f'take_tool({tool_name}):exit:{offset:.2f}' ) try: self._move( - self._above(goal, gap), 'lin', - self.geometry.speeds['approach'], label, + exit_pose, 'lin', self.geometry.speeds['transfer'], + label, ) break except OperationError as error: if 'Планирование' not in str(error): raise self.motion.reset_stop() - lift_error = error - else: - raise lift_error 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)) @@ -443,6 +452,28 @@ class ReferenceOperations: ) self._require_synchronized() self._retreat(goal, f'dock_tool({tool_name})') + # Step out of the rack along +Y before any joint-space move; the + # axial retreat alone can leave the wrist against its limit. + transfer = self._above(goal, self.geometry.transfer_gap) + 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'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, + ) + break + except OperationError as error: + if 'Планирование' not in str(error): + raise + self.motion.reset_stop() 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: @@ -575,9 +606,18 @@ class ReferenceOperations: self._require_synchronized() screw_pose = self.supervisor.object_pose(def_name) goal = tcp_goal(screw_pose, self.geometry.screw_tcp_to_node) + def restore_screw() -> None: + # The first attempt removed the screw from the planning scene + # before its transfer was rejected. Restore it for the retry. + self.bridge.add_world( + def_name, self.geometry.screw_collision, screw_pose, + ) + self._require_synchronized() + self._contact_with_home_retry( goal, f'pick_screw({screw_id})', remove_before_contact=def_name, + prepare=restore_screw, ) self.supervisor.command('grab_screw', screw_id) self._retreat(goal, f'pick_screw({screw_id})') @@ -618,7 +658,7 @@ class ReferenceOperations: raise # ------------------------------------------------------------------ tightening - def tighten_hole(self, hole_id: str, twist_deg: float = 60.0) -> OperationResult: + def tighten_hole(self, hole_id: str, twist_deg: float = 45.0) -> OperationResult: """Simulate tightening at an empty hole with the torque tool. The screw has already been handed to the virtual operator and removed @@ -641,35 +681,64 @@ class ReferenceOperations: goal = tcp_goal(target, tip) self._contact(goal, f'tighten_hole({hole_id})') twist_error = None - for suffix, offset in ( - ('twist:positive', twist_deg), - ('twist:negative', -twist_deg), - ): - twisted = goal.compose_relaxed(Transform.from_xyz_rpy( - [0.0, 0.0, 0.0], [0.0, 0.0, offset], - parent='tcp', child='tighten_twist', - )) - try: - self._move( - twisted, 'lin', self.geometry.speeds['contour'], - f'tighten_hole({hole_id}):{suffix}', + twisted = goal + twist_done = 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 + # longer reachable. + joints = None + provider = getattr(self.motion, 'joint_positions', None) + if provider is not None: + joints = provider() + if joints is not None and len(joints) == 7: + for sign in (1.0, -1.0): + target_joints = list(joints) + target_joints[6] += math.radians(sign * twist_deg) + self.motion.wait_stationary() + result = self.motion.move_joints( + target_joints, self.geometry.speeds['contour'], + f'tighten_hole({hole_id}):joint_twist', ) - break - except OperationError as error: - # Holes on the far side of the body can sit against the - # wrist limit in one direction. A failed plan executes no - # motion, so the opposite twist is safe to try. - if 'Планирование' not in str(error): - raise - self.motion.reset_stop() - twist_error = error - else: - raise twist_error - self._move( - goal, 'lin', self.geometry.speeds['contour'], - f'tighten_hole({hole_id}):return', - ) - self._retreat(goal, f'tighten_hole({hole_id})') + if getattr(result, 'success', False): + twist_done = True + self._emit('tighten_twist', object=hole_id, + joint=7, delta_deg=sign * twist_deg) + break + reset = getattr(self.motion, 'reset_stop', None) + if reset is not None: + reset() + if not twist_done: + for suffix, offset in ( + ('twist:positive', twist_deg), + ('twist:negative', -twist_deg), + ): + candidate = goal.compose_relaxed(Transform.from_xyz_rpy( + [0.0, 0.0, 0.0], [0.0, 0.0, offset], + parent='tcp', child='tighten_twist', + )) + self.motion.wait_stationary() + try: + self._move( + candidate, 'lin', self.geometry.speeds['contour'], + f'tighten_hole({hole_id}):{suffix}', + ) + twisted = candidate + twist_done = True + break + except OperationError as error: + # Holes on the far side of the body can sit against + # the wrist limit in one direction. A failed plan + # executes no motion, so the opposite twist is safe. + if 'Планирование' not in str(error): + raise + self.motion.reset_stop() + 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. self._emit('tighten_simulated', object=hole_id, phase='done', twist_deg=twist_deg) return OperationResult( diff --git a/src/iiwa_experement/iiwa_experement/reference_scenarios.py b/src/iiwa_experement/iiwa_experement/reference_scenarios.py index a4d2ed0..de57f78 100644 --- a/src/iiwa_experement/iiwa_experement/reference_scenarios.py +++ b/src/iiwa_experement/iiwa_experement/reference_scenarios.py @@ -81,8 +81,9 @@ def _cap_steps( _home(), ReferenceStep('dock_tool', ('vacuum',)), ]) - if finish_home: - steps.append(_home()) + # Always leave the rack through home: the dock retreat can sit against a + # wrist limit from which 'work' is not reachable. + steps.append(_home()) return steps @@ -113,8 +114,7 @@ def _sealant_steps( _home(), ReferenceStep('dock_tool', ('sealant_tool',)), ]) - if finish_home: - steps.append(_home()) + steps.append(_home()) return steps @@ -144,9 +144,9 @@ def _screw_steps( ]) steps.append(_home()) steps.append(ReferenceStep('dock_tool', ('magnetic_tool',))) + steps.append(_home()) if 'torque_tool' in spec.tool_names: steps.extend([ - _work(), ReferenceStep('take_tool', ('torque_tool',)), _work(), ]) @@ -157,7 +157,6 @@ def _screw_steps( ]) steps.append(_home()) steps.append(ReferenceStep('dock_tool', ('torque_tool',))) - if finish_home: steps.append(_home()) return steps diff --git a/src/iiwa_experement/test/test_operations.py b/src/iiwa_experement/test/test_operations.py index 6382573..1b5d8a3 100644 --- a/src/iiwa_experement/test/test_operations.py +++ b/src/iiwa_experement/test/test_operations.py @@ -1,4 +1,5 @@ """Operation sequences are visible, ordered and fail closed.""" +import math from pathlib import Path import pytest @@ -75,6 +76,9 @@ class FakeMotion: self.moves = [] self.named = [] self.fail_labels = set() + self.joint_moves = [] + self.joint_values = (0.0,) * 7 + self.fail_positive_joint = False def move_pose(self, goal, planner='ptp', speed=0.2, label=''): self.moves.append((label, planner, speed, goal)) @@ -86,9 +90,22 @@ class FakeMotion: self.named.append((name, speed)) return MotionResult(True, 'ok', f'move_named({name})') + def joint_positions(self): + return self.joint_values + + def move_joints(self, joints, speed=0.2, label=''): + self.joint_moves.append((label, tuple(joints))) + if self.fail_positive_joint and joints[6] > 0: + return MotionResult(False, 'injected joint failure', label) + self.joint_values = tuple(joints) + return MotionResult(True, 'ok', label) + def reset_stop(self): return None + def wait_stationary(self, *args, **kwargs): + return True + def make_operations(experiment_id): spec = load_scenario(experiment_id, SCENES) @@ -122,6 +139,7 @@ def test_take_tool_follows_the_visible_axial_route(): 'take_tool(vacuum):approach', 'take_tool(vacuum):contact', 'take_tool(vacuum):lift', + 'take_tool(vacuum):exit', ] assert supervisor.commands[0][:2] == ('attach_tool', 'vacuum') assert bridge.attached == [] @@ -138,7 +156,7 @@ def test_dock_tool_returns_the_tool_to_its_socket(): assert result.success assert supervisor.commands[0][:2] == ('dock_tool', 'vacuum') assert bridge.added_world[0][0] == 'grant_vacuum' - assert labels(motion)[-1] == 'dock_tool(vacuum):clear' + assert labels(motion)[-1] == 'dock_tool(vacuum):exit' def test_cap_pick_and_place_use_the_hatch_node_transform(): @@ -213,11 +231,11 @@ def test_tighten_hole_is_a_visible_revolution_without_a_screw(): 'tighten_hole(hole_1):approach', 'tighten_hole(hole_1):contact', ] - twist = motion.moves[names.index('tighten_hole(hole_1):twist:positive')][3] - contact = motion.moves[names.index('tighten_hole(hole_1):contact')][3] - assert twist.xyz == pytest.approx(contact.xyz) - relative = contact.inverse().compose_relaxed(twist) - assert abs(relative.rotation[2][2] - 1.0) < 1e-9 + assert motion.joint_moves + label, joints = motion.joint_moves[0] + assert label == 'tighten_hole(hole_1):joint_twist' + assert joints[:6] == (0.0,) * 6 + assert joints[6] == pytest.approx(math.radians(45.0)) # The operation is a simulation only: no supervisor command changes state. assert supervisor.commands == [] assert bridge.attached == [] @@ -232,13 +250,12 @@ def test_screw_pick_reconfigures_to_home_when_work_is_blocked(): assert supervisor.commands[0][:2] == ('grab_screw', 'screw_1') -def test_tighten_hole_reverses_the_twist_when_one_direction_is_blocked(): +def test_tighten_hole_reverses_when_the_flange_limit_blocks_one_direction(): operations, supervisor, bridge, motion, geometry = make_operations(14) - motion.fail_labels.add('tighten_hole(hole_1):twist:positive') + motion.fail_positive_joint = True assert operations.tighten_hole('hole_1').success - names = labels(motion) - assert 'tighten_hole(hole_1):twist:positive' in names - assert 'tighten_hole(hole_1):twist:negative' in names + assert len(motion.joint_moves) == 2 + assert motion.joint_moves[1][1][6] == pytest.approx(math.radians(-45.0)) def test_failed_motion_stops_the_operation_and_emits_failure():