Даниил Грабарь
cdda081b4f
- _contact transfer/approach and _retreat clear now use deterministic Pilz PTP joint interpolation; OMPL remains only the automatic fallback - move_to_joints on the motion server tries PTP first, OMPL on failure - verified in Webots execute mode: 1, 11, 14, 20 completed; no PTP failures in 1/11/14/20 server logs, OMPL used only as the fallback
320 lines
11 KiB
Python
320 lines
11 KiB
Python
"""Operation sequences are visible, ordered and fail closed."""
|
|
import math
|
|
from pathlib import Path
|
|
|
|
import pytest
|
|
|
|
from iiwa_experement.geometry import load_geometry, world_target
|
|
from iiwa_experement.motion import MotionResult
|
|
from iiwa_experement.operations import OperationError, ReferenceOperations
|
|
from iiwa_experement.scenario import load_scenario
|
|
|
|
|
|
PACKAGE = Path(__file__).resolve().parents[1]
|
|
SCENES = PACKAGE / 'config' / 'scenes.yaml'
|
|
GEOMETRY = PACKAGE / 'config' / 'geometry.yaml'
|
|
|
|
|
|
class FakeSupervisor:
|
|
def __init__(self, spec, geometry):
|
|
self.spec = spec
|
|
self.geometry = geometry
|
|
self.commands = []
|
|
self.latest = {
|
|
'cap': {'state': spec.cap_state},
|
|
'screws': {},
|
|
}
|
|
self.fail_on = None
|
|
|
|
def object_pose(self, name):
|
|
mapping = {
|
|
'grant_cap': 'cap',
|
|
**{f'grant_{tool}': f'tool:{tool}' for tool in self.spec.tool_names},
|
|
**{
|
|
f'grant_screw_{index:02d}': f'screw:screw_{index}'
|
|
for index in range(1, 11)
|
|
},
|
|
}
|
|
target = mapping[name]
|
|
return world_target(self.spec, self.geometry, target)
|
|
|
|
def command(self, op, object_id='', **kwargs):
|
|
if self.fail_on == op:
|
|
raise OperationError(f'{op} rejected')
|
|
self.commands.append((op, object_id, kwargs))
|
|
return self.latest
|
|
|
|
|
|
class FakeBridge:
|
|
def __init__(self):
|
|
self.attached = []
|
|
self.detached = []
|
|
self.removed = []
|
|
self.restored = []
|
|
|
|
def attach(self, spec):
|
|
self.attached.append(spec.object_id)
|
|
|
|
def detach(self, object_id, pose):
|
|
self.detached.append((object_id, pose))
|
|
|
|
def remove(self, object_id):
|
|
self.removed.append(object_id)
|
|
if object_id in self.attached:
|
|
self.attached.remove(object_id)
|
|
|
|
def restore_world(self, object_id):
|
|
if object_id in self.attached:
|
|
self.attached.remove(object_id)
|
|
self.restored.append(object_id)
|
|
|
|
def add_world(self, object_id, box, pose):
|
|
if object_id in self.attached:
|
|
self.attached.remove(object_id)
|
|
self.added_world = getattr(self, 'added_world', [])
|
|
self.added_world.append((object_id, pose))
|
|
|
|
def expected_sets(self):
|
|
return set(), set()
|
|
|
|
def assert_synchronized(self, *args, **kwargs):
|
|
return None
|
|
|
|
|
|
class FakeMotion:
|
|
def __init__(self):
|
|
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))
|
|
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 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)
|
|
geometry = load_geometry(GEOMETRY)
|
|
supervisor = FakeSupervisor(spec, geometry)
|
|
bridge = FakeBridge()
|
|
motion = FakeMotion()
|
|
operations = ReferenceOperations(
|
|
node=None,
|
|
spec=spec,
|
|
geometry=geometry,
|
|
supervisor=supervisor,
|
|
bridge=bridge,
|
|
motion=motion,
|
|
)
|
|
return operations, supervisor, bridge, motion, geometry
|
|
|
|
|
|
def labels(motion):
|
|
return [label for label, _, _, _ in motion.moves]
|
|
|
|
|
|
def test_take_tool_follows_the_visible_axial_route():
|
|
operations, supervisor, bridge, motion, geometry = make_operations(1)
|
|
tip = world_target(operations.spec, geometry, 'tool:vacuum')
|
|
expected = tip.compose_relaxed(geometry.tool_tcp_to_nodes['vacuum'].inverse())
|
|
result = operations.take_tool('vacuum')
|
|
assert result.success
|
|
assert labels(motion) == [
|
|
'take_tool(vacuum):transfer',
|
|
'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 == ['grant_vacuum']
|
|
contact_goal = motion.moves[2][3]
|
|
assert contact_goal.xyz == pytest.approx(expected.xyz, abs=1e-9)
|
|
assert motion.moves[0][1] == 'ptp'
|
|
assert motion.moves[1][1] == 'ptp'
|
|
assert motion.moves[2][1] == 'lin'
|
|
|
|
|
|
def test_dock_tool_returns_the_tool_to_its_socket():
|
|
operations, supervisor, bridge, motion, geometry = make_operations(1)
|
|
result = operations.dock_tool('vacuum')
|
|
assert result.success
|
|
assert supervisor.commands[0][:2] == ('dock_tool', 'vacuum')
|
|
assert bridge.restored == ['grant_vacuum']
|
|
assert labels(motion)[-1] == 'dock_tool(vacuum):exit'
|
|
|
|
|
|
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 == ['grant_cap']
|
|
assert supervisor.commands[0][:2] == ('grab_cap', 'cap')
|
|
assert operations.place_cap().success
|
|
assert bridge.added_world[0][0] == 'grant_cap'
|
|
assert supervisor.commands[1][0] == 'place_cap'
|
|
|
|
|
|
def test_single_sealant_side_lowers_and_lifts():
|
|
operations, supervisor, bridge, motion, geometry = make_operations(6)
|
|
assert operations.dispense('side_1').success
|
|
assert labels(motion) == [
|
|
'dispense(side_1):transfer',
|
|
'dispense(side_1):approach',
|
|
'dispense(side_1):contact',
|
|
'dispense(side_1):path',
|
|
'dispense(side_1):retreat',
|
|
'dispense(side_1):clear',
|
|
]
|
|
assert operations.completed_sealant == {'side_1'}
|
|
assert operations.inspect_sealant()['ok'] is True
|
|
|
|
|
|
def test_contour_does_not_lift_between_adjacent_sides():
|
|
operations, supervisor, bridge, motion, geometry = make_operations(8)
|
|
assert operations.dispense_contour(
|
|
['side_1', 'side_2', 'side_3', 'side_4']
|
|
).success
|
|
names = labels(motion)
|
|
assert names[:3] == [
|
|
'dispense_contour:start:transfer',
|
|
'dispense_contour:start:approach',
|
|
'dispense_contour:start:contact',
|
|
]
|
|
assert names[3:7] == [
|
|
'dispense_contour:side_1',
|
|
'dispense_contour:side_2',
|
|
'dispense_contour:side_3',
|
|
'dispense_contour:side_4',
|
|
]
|
|
assert 'dispense_contour:side_1' in names
|
|
assert names[-2:] == [
|
|
'dispense_contour:retreat', 'dispense_contour:clear',
|
|
]
|
|
assert operations.completed_sealant == {
|
|
'side_1', 'side_2', 'side_3', 'side_4',
|
|
}
|
|
|
|
|
|
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 == ['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'
|
|
assert supervisor.commands[1][2]['target_name'] == 'hole_1'
|
|
assert bridge.removed == ['grant_screw_01', 'grant_screw_01']
|
|
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',
|
|
]
|
|
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 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_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] == '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) == 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):recovery:contact',
|
|
})
|
|
with pytest.raises(OperationError):
|
|
operations.take_tool('vacuum')
|
|
assert operations.events[-1]['phase'] == 'failed'
|
|
assert bridge.attached == []
|
|
|
|
|
|
def test_failed_supervisor_command_is_not_ignored():
|
|
operations, supervisor, bridge, motion, geometry = make_operations(1)
|
|
supervisor.fail_on = 'attach_tool'
|
|
with pytest.raises(OperationError):
|
|
operations.take_tool('vacuum')
|
|
assert bridge.attached == []
|