feat(experement): simulate tightening and smooth tool transfers

Add a torque-tool tightening simulation for experiments 14-20: the tool approaches each empty hole, twists 60 degrees (reversing at the wrist limit), returns and retracts while the screw stays handed off. Lift attached tools vertically clear of the magazine before lateral motion, prefer the close 'work' pose between operations, and reconfigure through home only when planning fails. Settle measured joint positions before verifying joint moves.

src/iiwa_experement/test/test_operations.py

@@ -78,14 +78,17 @@ class FakeMotion:
 
     def move_pose(self, goal, planner='ptp', speed=0.2, label=''):
         self.moves.append((label, planner, speed, goal))
-        if label in self.fail_labels:
-            return MotionResult(False, 'injected failure', label)
+        if any(label.startswith(prefix) for prefix in self.fail_labels):
+            return MotionResult(False, 'Планирование не удалось: injected failure', label)
         return MotionResult(True, 'ok', label, True, goal, goal)
 
     def move_named(self, name, speed=0.2):
         self.named.append((name, speed))
         return MotionResult(True, 'ok', f'move_named({name})')
 
+    def reset_stop(self):
+        return None
+
 
 def make_operations(experiment_id):
     spec = load_scenario(experiment_id, SCENES)
@@ -118,8 +121,7 @@ def test_take_tool_follows_the_visible_axial_route():
         'take_tool(vacuum):transfer',
         'take_tool(vacuum):approach',
         'take_tool(vacuum):contact',
-        'take_tool(vacuum):retreat',
-        'take_tool(vacuum):clear',
+        'take_tool(vacuum):lift',
     ]
     assert supervisor.commands[0][:2] == ('attach_tool', 'vacuum')
     assert bridge.attached == []
@@ -202,9 +204,49 @@ def test_screw_pick_and_handoff_are_one_motion_and_one_removal():
     assert bridge.attached == []
 
 
+def test_tighten_hole_is_a_visible_revolution_without_a_screw():
+    operations, supervisor, bridge, motion, geometry = make_operations(14)
+    assert operations.tighten_hole('hole_1').success
+    names = labels(motion)
+    assert names[:3] == [
+        'tighten_hole(hole_1):transfer',
+        '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
+    # 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():
+    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 supervisor.commands[0][:2] == ('grab_screw', 'screw_1')
+
+
+def test_tighten_hole_reverses_the_twist_when_one_direction_is_blocked():
+    operations, supervisor, bridge, motion, geometry = make_operations(14)
+    motion.fail_labels.add('tighten_hole(hole_1):twist:positive')
+    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
+
+
 def test_failed_motion_stops_the_operation_and_emits_failure():
     operations, supervisor, bridge, motion, geometry = make_operations(1)
-    motion.fail_labels.add('take_tool(vacuum):contact')
+    motion.fail_labels.update({
+        'take_tool(vacuum):contact',
+        'take_tool(vacuum):home_retry:contact',
+    })
     with pytest.raises(OperationError):
         operations.take_tool('vacuum')
     assert operations.events[-1]['phase'] == 'failed'

src/iiwa_experement/test/test_reference_scenarios.py

@@ -63,8 +63,27 @@ def test_sealant_experiments_use_only_the_sealant_tool():
 def test_full_assembly_tool_order_is_sealant_cap_screws():
     for experiment_id in range(16, 21):
         takes = [step.args[0] for step in route(experiment_id) if step.kind == 'take_tool']
-        assert takes == ['sealant_tool', 'vacuum', 'magnetic_tool']
-        assert 'torque_tool' not in takes
+        assert takes == ['sealant_tool', 'vacuum', 'magnetic_tool', 'torque_tool']
+
+
+def test_tightening_simulation_matches_the_delivered_holes():
+    assert not any(
+        step.kind == 'tighten_hole' for step in route(11)
+    )
+    for experiment_id in range(14, 21):
+        tighten = [
+            step for step in route(experiment_id) if step.kind == 'tighten_hole'
+        ]
+        handoffs = [
+            step for step in route(experiment_id) if step.kind == 'handoff_screw'
+        ]
+        assert [step.args[0] for step in tighten] == [
+            step.args[1] for step in handoffs
+        ]
+    for experiment_id in (12, 13):
+        assert not any(
+            step.kind == 'tighten_hole' for step in route(experiment_id)
+        )
 
 
 def test_screw_counts_and_holes_match_the_contract():

src/iiwa_experement/test/test_scenario.py

@@ -88,21 +88,24 @@ def test_screw_experiments_have_installed_hatch_and_ten_screws():
         assert spec.cap_state == 'installed'
         assert spec.tray_pose is not None
         assert len(spec.screws) == 10
-        assert spec.tool_names == ('magnetic_tool',)
-        assert 'torque_tool' not in spec.tool_names
+        if experiment_id in (14, 15):
+            assert set(spec.tool_names) == {'magnetic_tool', 'torque_tool'}
+        else:
+            assert spec.tool_names == ('magnetic_tool',)
         assert len(spec.screw_holes) == {
             11: 1, 12: 2, 13: 3, 14: 4, 15: 4,
         }[experiment_id]
 
 
-def test_full_assembly_experiments_have_loose_hatch_tray_and_no_torque_tool():
+def test_full_assembly_experiments_have_loose_hatch_tray_and_all_tools():
     for experiment_id in range(16, 21):
         spec = scenario(experiment_id)
         assert spec.cap_state == 'free'
         assert spec.tray_pose is not None
         assert len(spec.screws) == 10
-        assert set(spec.tool_names) == {'vacuum', 'magnetic_tool', 'sealant_tool'}
-        assert 'torque_tool' not in spec.tool_names
+        assert set(spec.tool_names) == {
+            'vacuum', 'magnetic_tool', 'sealant_tool', 'torque_tool',
+        }
         assert len(spec.screw_holes) == {16: 1, 17: 2, 18: 4, 19: 4, 20: 4}[experiment_id]
 
 

src/iiwa_experement/test/test_webots_nodes.py

@@ -74,13 +74,13 @@ def test_scenarios_one_six_eleven_sixteen_nineteen_are_complete():
         16: [
             'grant_fixture', 'grant_body', 'grant_rack',
             'grant_vacuum', 'grant_magnetic_tool', 'grant_sealant_tool',
-            'grant_tray',
+            'grant_torque_tool', 'grant_tray',
             *[f'grant_screw_{i:02d}' for i in range(1, 11)], 'grant_cap',
         ],
         19: [
             'grant_fixture', 'grant_body', 'grant_rack',
             'grant_vacuum', 'grant_magnetic_tool', 'grant_sealant_tool',
-            'grant_tray',
+            'grant_torque_tool', 'grant_tray',
             *[f'grant_screw_{i:02d}' for i in range(1, 11)], 'grant_cap',
         ],
     }
@@ -161,8 +161,8 @@ def test_working_tools_are_in_their_own_sockets():
     positions = {
         translation(parsed[TOOL_NODE_NAMES[name]]) for name in scenario.tool_names
     }
-    assert len(positions) == len(scenario.tool_names) == 3
-    assert 'grant_torque_tool' not in parsed
+    assert len(positions) == len(scenario.tool_names) == 4
+    assert 'grant_torque_tool' in parsed
 
 
 def test_experiment_nineteen_shifts_the_tray_and_all_ten_screws():