From de8c2eb9eed5ca33c07debfba4f9f6ef0ba42a8b 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: Sun, 8 Nov 2026 15:38:36 +1000 Subject: [PATCH] feat(experement): implement reference operations and stable scene sync --- src/iiwa_experement/config/geometry.yaml | 18 +- src/iiwa_experement/config/moveit_cpp.yaml | 61 ++ .../iiwa_experement/grant_supervisor.py | 89 +-- src/iiwa_experement/iiwa_experement/motion.py | 42 +- .../iiwa_experement/operations.py | 573 ++++++++++++++++++ .../iiwa_experement/planning_scene.py | 339 +++++++++-- .../iiwa_experement/runtime_config.py | 14 +- .../iiwa_experement/webots_nodes.py | 7 +- .../launch/experiment.launch.py | 1 + .../test/test_grant_supervisor.py | 32 +- src/iiwa_experement/test/test_operations.py | 219 +++++++ .../test/test_planning_scene.py | 52 +- src/iiwa_experement/test/test_webots_nodes.py | 12 +- 13 files changed, 1322 insertions(+), 137 deletions(-) create mode 100644 src/iiwa_experement/config/moveit_cpp.yaml create mode 100644 src/iiwa_experement/iiwa_experement/operations.py create mode 100644 src/iiwa_experement/test/test_operations.py diff --git a/src/iiwa_experement/config/geometry.yaml b/src/iiwa_experement/config/geometry.yaml index 39f50e1..79616c0 100644 --- a/src/iiwa_experement/config/geometry.yaml +++ b/src/iiwa_experement/config/geometry.yaml @@ -128,18 +128,28 @@ scene_collisions: type: box size: [1.28, 1.08, 0.045] offset_z: 0.7665 - fixture: + # The fixture is a frame, not a solid block: a low base plate and two + # side rails. A full box would cover the hatch seat and make placement + # impossible. + fixture_base: type: box - size: [0.350, 0.220, 0.041] - offset_z: 0.0105 + size: [0.350, 0.220, 0.010] + offset_z: -0.005 + fixture_rail: + type: box + size: [0.014, 0.156, 0.016] + offset_z: 0.008 + offset_x: 0.104 body: type: box size: [0.300, 0.170, 0.00915] offset_z: 0.000695 rack: + # The magazine is represented by its base plate only. Keep it just + # below the tool sockets so the vertical tool bodies do not overlap it. type: box size: [0.420, 0.140, 0.010] - offset_z: 0.005 + offset_z: -0.015 tray_base: type: box size: [0.120, 0.070, 0.0024] diff --git a/src/iiwa_experement/config/moveit_cpp.yaml b/src/iiwa_experement/config/moveit_cpp.yaml new file mode 100644 index 0000000..bbbe995 --- /dev/null +++ b/src/iiwa_experement/config/moveit_cpp.yaml @@ -0,0 +1,61 @@ +# Package-local MoveItCpp settings for the reference executor. +# +# The stock iiwa_config copy publishes the MoveItPy maintained scene on +# /monitored_planning_scene (the same topic move_group publishes) but listens +# to /planning_scene. The executor publishes its diffs to move_group, so the +# MoveItPy monitor must instead listen to move_group's maintained scene. +# +# publish_planning_scene_topic is the topic this monitor subscribes to; +# monitored_planning_scene_topic is where it republishes its own scene. +planning_scene_monitor_options: + name: "planning_scene_monitor" + robot_description: "robot_description" + joint_state_topic: "/joint_states" + attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor" + publish_planning_scene_topic: "/monitored_planning_scene" + monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 + +planning_pipelines: + pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp"] + +plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + max_velocity_scaling_factor: 0.5 + max_acceleration_scaling_factor: 1.0 + +ompl_rrtc: + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + planner_id: "RRTConnectkConfigDefault" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 1.0 + +pilz_ptp: + plan_request_params: + planning_attempts: 1 + planning_pipeline: pilz_industrial_motion_planner + planner_id: "PTP" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 0.8 + +pilz_lin: + plan_request_params: + planning_attempts: 1 + planning_pipeline: pilz_industrial_motion_planner + planner_id: "LIN" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 0.8 + +chomp_planner: + plan_request_params: + planning_attempts: 1 + planning_pipeline: chomp + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 1.5 diff --git a/src/iiwa_experement/iiwa_experement/grant_supervisor.py b/src/iiwa_experement/iiwa_experement/grant_supervisor.py index 725be37..6e92199 100644 --- a/src/iiwa_experement/iiwa_experement/grant_supervisor.py +++ b/src/iiwa_experement/iiwa_experement/grant_supervisor.py @@ -110,7 +110,8 @@ class GrantSupervisorNode(Node): self.spec: ScenarioSpec = load_scenario(experiment_id, scene_config) self.geometry: GeometrySpec = load_geometry(geometry_path) self.state = ObjectState(self.spec, self.geometry, read_only=read_only) - self.attached: tuple[str, str] | None = None + self.tool_held: str | None = None + self.payload: tuple[str, str] | None = None self.last_command_result: dict[str, Any] | None = None self.rack_poses = { name: entry['pose'] for name, entry in self.state.tools.items() @@ -174,6 +175,15 @@ class GrantSupervisorNode(Node): field.setSFBool(True) except Exception: pass + # All managed objects are teleported by the supervisor. Leaving a + # physics node attached lets Webots integrate huge velocities while + # the pose is overwritten every step. + try: + physics = node.getField('physics') + if physics is not None: + physics.remove() + except Exception: + pass def _pose_of(self, node) -> Transform | None: if node is None: @@ -223,24 +233,25 @@ class GrantSupervisorNode(Node): names.append('grant_cap') return names - def _node_for_attachment(self) -> Any | None: - if self.attached is None: + def _tool_node(self): + if self.tool_held is None: return None - kind, name = self.attached - if kind == 'tool': - return self.nodes.get(f'grant_{name}') + return self.nodes.get(f'grant_{self.tool_held}') + + def _payload_node(self): + if self.payload is None: + return None + kind, name = self.payload if kind == 'cap': return self.nodes.get('grant_cap') if kind == 'screw': return self.nodes.get(screw_def_name(name)) return None - def _relative_transform(self) -> Transform | None: - if self.attached is None: + def _payload_relative(self) -> Transform | None: + if self.payload is None: return None - kind, name = self.attached - if kind == 'tool': - return self.geometry.tool_tcp_to_nodes[name] + kind, _ = self.payload if kind == 'cap': return self.geometry.cap_tcp_to_node if kind == 'screw': @@ -248,22 +259,27 @@ class GrantSupervisorNode(Node): return None def follow(self) -> None: - if self.attached is None: - return tcp = self.tcp_pose() - node = self._node_for_attachment() - relative = self._relative_transform() - if tcp is None or node is None or relative is None: + if tcp is None: return - pose = tcp.compose_relaxed(relative) - self._set_pose(node, pose) - kind, name = self.attached - if kind == 'tool' and name in self.state.tools: - self.state.tools[name]['pose'] = pose - elif kind == 'cap': - self.state.cap_pose = pose - elif kind == 'screw' and name in self.state.screws: - self.state.screws[name]['pose'] = pose + if self.tool_held is not None: + node = self._tool_node() + relative = self.geometry.tool_tcp_to_nodes[self.tool_held] + if node is not None: + pose = tcp.compose_relaxed(relative) + self._set_pose(node, pose) + self.state.tools[self.tool_held]['pose'] = pose + if self.payload is not None: + node = self._payload_node() + relative = self._payload_relative() + if node is not None and relative is not None: + pose = tcp.compose_relaxed(relative) + self._set_pose(node, pose) + kind, name = self.payload + if kind == 'cap': + self.state.cap_pose = pose + elif kind == 'screw' and name in self.state.screws: + self.state.screws[name]['pose'] = pose # ------------------------------------------------------------------ state def _actual_pose(self, node) -> Transform | None: @@ -309,6 +325,10 @@ class GrantSupervisorNode(Node): def _apply_impl(self, operation: str, command: dict[str, Any], request_id: str): if operation == 'inspect': return True, None + # A command can arrive before the per-step follow update. Apply the + # current tcp to the held object first, otherwise the attachment + # appears one control step behind (about 11 mm at transfer speed). + self.follow() tcp = self.tcp_pose() if tcp is None: raise CommandError('actual tcp pose is unavailable') @@ -319,7 +339,7 @@ class GrantSupervisorNode(Node): if object_pose is None: raise CommandError(f'tool node grant_{kind} is unavailable') self.state.attach_tool(kind, tcp, object_pose, request_id=request_id) - self.attached = ('tool', kind) + self.tool_held = kind return True, None if operation == 'dock_tool': node = self.nodes.get(f'grant_{kind}') @@ -330,7 +350,7 @@ class GrantSupervisorNode(Node): self.state.dock_tool( kind, tcp, rack, object_pose, request_id=request_id, ) - self.attached = None + self.tool_held = None self._set_pose(node, rack) self.state.tools[kind]['pose'] = rack return True, None @@ -340,7 +360,7 @@ class GrantSupervisorNode(Node): if object_pose is None: raise CommandError('hatch node is unavailable') self.state.grab_cap(tcp, object_pose, request_id=request_id) - self.attached = ('cap', 'cap') + self.payload = ('cap', 'cap') return True, None if operation == 'place_cap': target = self._command_pose(command) @@ -349,7 +369,7 @@ class GrantSupervisorNode(Node): if object_pose is None: raise CommandError('hatch node is unavailable') self.state.place_cap(target, tcp, object_pose, request_id=request_id) - self.attached = None + self.payload = None self._set_pose(node, target) self._freeze(node) self.state.cap_pose = target @@ -360,7 +380,7 @@ class GrantSupervisorNode(Node): if object_pose is None: raise CommandError(f'screw node for {kind} is unavailable') self.state.grab_screw(kind, tcp, object_pose, request_id=request_id) - self.attached = ('screw', kind) + self.payload = ('screw', kind) return True, None if operation == 'handoff_screw': hole = str(command.get('target_name') or '') @@ -372,7 +392,7 @@ class GrantSupervisorNode(Node): self.state.handoff_screw( kind, hole, target, tcp, object_pose, request_id=request_id, ) - self.attached = None + self.payload = None if node is not None: try: node.remove() @@ -394,9 +414,10 @@ class GrantSupervisorNode(Node): payload['ready'] = self.patron_node is not None and all( name in self.nodes for name in self._required_defs() ) - payload['attached'] = ( - None if self.attached is None else list(self.attached) - ) + payload['attached'] = { + 'tool': self.tool_held, + 'payload': None if self.payload is None else list(self.payload), + } tcp = self.tcp_pose() payload['tcp'] = transform_json(tcp) payload['last_command_result'] = self.last_command_result diff --git a/src/iiwa_experement/iiwa_experement/motion.py b/src/iiwa_experement/iiwa_experement/motion.py index 3282dd8..337c613 100644 --- a/src/iiwa_experement/iiwa_experement/motion.py +++ b/src/iiwa_experement/iiwa_experement/motion.py @@ -107,7 +107,10 @@ class MotionAdapter: position_tolerance: float | None = None, orientation_tolerance: float | None = None, command_timeout_sec: float = 90.0, - settle_time_sec: float = 0.5, + settle_time_sec: float = 1.0, + stability_timeout_sec: float = 5.0, + stability_epsilon: float = 0.001, + stability_angle: float = 0.01, ) -> None: self.node = node self.geometry = geometry @@ -139,6 +142,9 @@ class MotionAdapter: ) self.command_timeout_sec = float(command_timeout_sec) self.settle_time_sec = max(0.0, float(settle_time_sec)) + self.stability_timeout_sec = max(0.0, float(stability_timeout_sec)) + self.stability_epsilon = float(stability_epsilon) + self.stability_angle = float(stability_angle) self.stopped = False self.stop_reason: str | None = None self._last_joint_time = None @@ -283,8 +289,7 @@ class MotionAdapter: base_goal, started, ) - self._settle() - actual = self.actual_tcp() + actual = self._settle_and_measure() if actual is None: return self._fail(label, 'actual tcp pose is unavailable after motion', base_goal, started) distance, angle = _pose_distance(actual, base_goal) @@ -343,6 +348,37 @@ class MotionAdapter: timeout_sec=min(0.05, max(0.0, end - time.monotonic())), ) + def _settle_and_measure(self) -> Transform | None: + """Wait until the controller stops moving before verifying tcp. + + The action result is delivered when the trajectory finishes, but the + joint trajectory controller can still be settling. Treating the + first sample as final caused false out-of-tolerance stops. + """ + self._settle() + import rclpy + + deadline = time.monotonic() + self.stability_timeout_sec + last = self.actual_tcp() + stable_since = None + while time.monotonic() < deadline and self.node is not None: + rclpy.spin_once(self.node, timeout_sec=0.05) + current = self.actual_tcp() + if current is None: + last = None + stable_since = None + continue + if last is not None: + distance, angle = _pose_distance(current, last) + if distance <= self.stability_epsilon and angle <= self.stability_angle: + stable_since = stable_since or time.monotonic() + if time.monotonic() - stable_since >= 0.25: + return current + else: + stable_since = None + last = current + return last + def _fail( self, label: str, diff --git a/src/iiwa_experement/iiwa_experement/operations.py b/src/iiwa_experement/iiwa_experement/operations.py new file mode 100644 index 0000000..ec104d2 --- /dev/null +++ b/src/iiwa_experement/iiwa_experement/operations.py @@ -0,0 +1,573 @@ +"""Reference operations: tools, hatch, sealant contour and screw handoff.""" +from __future__ import annotations + +import json +import math +import uuid +from dataclasses import dataclass, field +from typing import Any, Callable, Iterable + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String +from webots_ros2_msgs.srv import SetString + +from .geometry import ( + GeometrySpec, + Transform, + sealant_point, + tcp_goal, + world_target, +) +from .motion import MotionAdapter, MotionResult +from .planning_scene import ( + PlanningSceneBridge, + attached_cap, + attached_screw, + attached_tool, + screw_object_id, +) +from .scenario import ScenarioSpec + + +COMMAND_SERVICE = '/iiwa_experement/object_command' +STATE_TOPIC = '/iiwa_experement/object_state' + + +class OperationError(RuntimeError): + """An operation could not be completed safely.""" + + +@dataclass(frozen=True) +class OperationResult: + success: bool + operation: str + message: str + events: tuple[dict[str, Any], ...] = field(default_factory=tuple) + + +def _translation(xyz) -> Transform: + return Transform.from_xyz_rpy( + [float(value) for value in xyz], [0.0, 0.0, 0.0], + parent='tcp', child='offset', + ) + + +class SupervisorClient: + """Synchronous facade over the Webots attachment supervisor.""" + + def __init__(self, node: Node, *, timeout_sec: float = 15.0) -> None: + self.node = node + self.timeout_sec = float(timeout_sec) + self.latest: dict[str, Any] | None = None + self._client = node.create_client(SetString, COMMAND_SERVICE) + self._subscription = node.create_subscription( + String, STATE_TOPIC, self._on_state, 10, + ) + + def _on_state(self, message: String) -> None: + try: + self.latest = json.loads(message.data) + except Exception: + pass + + def _spin(self, seconds: float = 0.05) -> None: + import time + + finish = time.monotonic() + seconds + while time.monotonic() < finish: + rclpy.spin_once(self.node, timeout_sec=0.02) + + def wait_ready(self, timeout_sec: float = 60.0) -> dict[str, Any]: + import time + + end = time.monotonic() + timeout_sec + while time.monotonic() < end: + rclpy.spin_once(self.node, timeout_sec=0.05) + if self.latest and self.latest.get('ready'): + return self.latest + raise OperationError('supervisor did not become ready') + + def wait_version(self, minimum: int, timeout_sec: float | None = None) -> dict[str, Any]: + import time + + end = time.monotonic() + (self.timeout_sec if timeout_sec is None else timeout_sec) + while time.monotonic() < end: + rclpy.spin_once(self.node, timeout_sec=0.05) + if self.latest and self.latest.get('version', -1) >= minimum: + return self.latest + raise OperationError(f'supervisor state version {minimum} was not published') + + def command( + self, + op: str, + object_id: str = '', + *, + target: Transform | None = None, + target_name: str = '', + expected_state: dict[str, Any] | None = None, + ) -> dict[str, Any]: + if not self._client.wait_for_service(timeout_sec=5.0): + raise OperationError(f'{COMMAND_SERVICE} is unavailable') + request_id = uuid.uuid4().hex + payload: dict[str, Any] = { + 'request_id': request_id, + 'op': op, + 'object': object_id, + 'target_name': target_name, + } + if target is not None: + payload['target'] = _transform_payload(target) + if expected_state is not None: + payload['expected_state'] = expected_state + request = SetString.Request() + request.value = json.dumps(payload) + future = self._client.call_async(request) + self._spin(0.0) + import time + + end = time.monotonic() + self.timeout_sec + while not future.done() and time.monotonic() < end: + rclpy.spin_once(self.node, timeout_sec=0.05) + if not future.done(): + raise OperationError(f'supervisor command {op} timed out') + accepted = bool(future.result().success) + # The detailed result is published atomically after the transition. + state = self.latest + if state is None or state.get('last_command_result', {}).get('request_id') != request_id: + end = time.monotonic() + 2.0 + while time.monotonic() < end: + rclpy.spin_once(self.node, timeout_sec=0.02) + state = self.latest + if state and state.get('last_command_result', {}).get('request_id') == request_id: + break + result = (state or {}).get('last_command_result', {}) + if not accepted or not result.get('accepted', False): + reason = result.get('error') or 'supervisor rejected the command' + raise OperationError(f'{op}({object_id or "-"}) rejected: {reason}') + return state or {} + + @property + def objects(self) -> dict[str, Any]: + return dict((self.latest or {}).get('objects') or {}) + + def object_pose(self, name: str) -> Transform: + entry = self.objects.get(name) + if not isinstance(entry, dict): + raise OperationError(f'object {name!r} is not present in the supervisor state') + return _transform_from_payload(entry, child=name) + + def logical_cap(self) -> dict[str, Any]: + return dict((self.latest or {}).get('cap') or {}) + + def logical_screws(self) -> dict[str, Any]: + return dict((self.latest or {}).get('screws') or {}) + + +def _transform_payload(transform: Transform) -> dict[str, Any]: + return { + 'xyz': [float(value) for value in transform.xyz], + 'rotation': [ + [float(value) for value in row] for row in transform.rotation + ], + } + + +def _transform_from_payload(data: dict[str, Any], child: str = 'object') -> Transform: + xyz = data.get('xyz') + rotation = data.get('rotation') + if not isinstance(xyz, (list, tuple)) or len(xyz) != 3: + raise OperationError('object pose has no valid xyz') + if not isinstance(rotation, (list, tuple)) or len(rotation) != 3: + raise OperationError('object pose has no valid rotation') + if any(not isinstance(row, (list, tuple)) or len(row) != 3 for row in rotation): + raise OperationError('object pose rotation must be a 3x3 matrix') + return Transform( + xyz=tuple(float(value) for value in xyz), + rotation=tuple(tuple(float(value) for value in row) for row in rotation), + parent='world', + child=child, + ) + + +class ReferenceOperations: + """Sequence of visible robot operations for one experiment.""" + + def __init__( + self, + node: Node, + spec: ScenarioSpec, + geometry: GeometrySpec, + supervisor: SupervisorClient, + bridge: PlanningSceneBridge, + motion: MotionAdapter, + *, + event_sink: Callable[[dict[str, Any]], None] | None = None, + ) -> None: + self.node = node + self.spec = spec + self.geometry = geometry + self.supervisor = supervisor + self.bridge = bridge + self.motion = motion + self._event_sink = event_sink + self.events: list[dict[str, Any]] = [] + self.completed_sealant: set[str] = set() + self.last_sealant_point: tuple[str, float] | None = None + + # ------------------------------------------------------------------ helpers + def _emit(self, operation: str, **payload) -> None: + event = {'operation': operation, **payload} + self.events.append(event) + if self._event_sink is not None: + self._event_sink(event) + + def _require(self, condition: bool, message: str) -> None: + if not condition: + raise OperationError(message) + + def _move( + self, + goal: Transform, + planner: str, + speed: float, + label: str, + *, + allow_fallback: bool = True, + ) -> MotionResult: + result = self.motion.move_pose(goal, planner=planner, speed=speed, label=label) + if result.success: + return result + planning_failed = ( + 'Планирование' in result.message + or 'planning' in result.message.lower() + or 'недостижима' in result.message + ) + 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', + ) + if fallback.success: + self._emit( + 'motion_fallback', label=label, original=planner, + reason=result.message, + ) + return fallback + result = fallback + raise OperationError(f'{label}: {result.message}') + + def _above(self, goal: Transform, gap: float) -> Transform: + return goal.compose_relaxed(_translation([0.0, 0.0, -gap])) + + def _contact( + self, + goal: Transform, + label: str, + *, + approach_gap: float | None = None, + transfer_gap: float | None = None, + remove_before_contact: str | None = None, + remove_phase: str = 'transfer', + ) -> None: + approach_gap = ( + self.geometry.approach_gap if approach_gap is None else approach_gap + ) + transfer_gap = ( + self.geometry.transfer_gap if transfer_gap is None else transfer_gap + ) + if remove_before_contact is not None and remove_phase == 'transfer': + # The object is the intended contact target. Remove it from the + # world scene for the whole approach so the attached copy cannot + # duplicate it and the short axial moves are not rejected by a + # false contact with the target itself. + self.bridge.remove(remove_before_contact) + self._require_synchronized() + transfer = self._above(goal, transfer_gap) + approach = self._above(goal, approach_gap) + self._move(transfer, 'ompl', self.geometry.speeds['transfer'], f'{label}:transfer') + try: + self._move( + approach, 'ompl', self.geometry.speeds['approach'], + f'{label}:approach', + ) + except OperationError as error: + message = str(error) + if 'Планирование' not in message and 'недостижима' not in message: + raise + # A pre-contact point can sit on a different IK branch. The + # collision-checked direct path to the contact is safer than + # forcing an infeasible axial segment. + self._emit('approach_skipped', label=f'{label}:approach', + reason=message) + self.motion.reset_stop() + if remove_before_contact is not None and remove_phase == 'contact': + self.bridge.remove(remove_before_contact) + self._require_synchronized() + 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') + + def _require_synchronized(self) -> None: + try: + self.bridge.assert_synchronized(*self.bridge.expected_sets()) + except Exception as error: + raise OperationError(f'planning scene is not synchronized: {error}') from error + + def _expect_latest(self, timeout: float = 2.0) -> dict[str, Any]: + import time + + end = time.monotonic() + timeout + while time.monotonic() < end: + rclpy.spin_once(self.node, timeout_sec=0.02) + if self.supervisor.latest is not None: + return self.supervisor.latest + raise OperationError('supervisor state is unavailable') + + # ------------------------------------------------------------------ tools + def take_tool(self, tool_name: str) -> OperationResult: + self._emit('take_tool', object=tool_name, phase='start') + try: + 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( + goal, f'take_tool({tool_name})', + remove_before_contact=f'grant_{tool_name}', + ) + self.supervisor.command('attach_tool', tool_name) + self._retreat(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)) + except Exception as error: + self._emit('take_tool', object=tool_name, phase='failed', error=str(error)) + raise + + def dock_tool(self, tool_name: str) -> OperationResult: + self._emit('dock_tool', object=tool_name, phase='start') + try: + self._require_synchronized() + rack_pose = world_target(self.spec, self.geometry, f'tool:{tool_name}') + goal = tcp_goal(rack_pose, self.geometry.tool_tcp_to_nodes[tool_name]) + # Remove the held tool from MoveIt before entering the socket + # region. The supervisor keeps following tcp, and the contact + # pose is verified by both the motion adapter and supervisor. + self.bridge.remove(f'grant_{tool_name}') + self._require_synchronized() + self._contact(goal, f'dock_tool({tool_name})') + self.supervisor.command('dock_tool', tool_name) + self.bridge.add_world( + f'grant_{tool_name}', + self.geometry.tool_collisions[tool_name], + rack_pose, + ) + self._require_synchronized() + self._retreat(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: + self._emit('dock_tool', object=tool_name, phase='failed', error=str(error)) + raise + + # ------------------------------------------------------------------ hatch + def pick_cap(self) -> OperationResult: + self._emit('pick_cap', object='cap', phase='start') + try: + self._require_synchronized() + cap_pose = self.supervisor.object_pose('grant_cap') + goal = tcp_goal(cap_pose, self.geometry.cap_tcp_to_node) + self._contact( + goal, 'pick_cap', remove_before_contact='grant_cap', + ) + self.supervisor.command('grab_cap', 'cap') + 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)) + except Exception as error: + self._emit('pick_cap', object='cap', phase='failed', error=str(error)) + raise + + def place_cap(self, target_pose: Transform | None = None) -> OperationResult: + self._emit('place_cap', object='cap', phase='start') + try: + self._require_synchronized() + target = target_pose or world_target(self.spec, self.geometry, 'cap_seat') + goal = tcp_goal(target, self.geometry.cap_tcp_to_node) + self._contact( + goal, 'place_cap', + remove_before_contact='grant_cap', + remove_phase='contact', + ) + self.supervisor.command('place_cap', 'cap', target=target) + self.bridge.add_world( + 'grant_cap', self.geometry.cap_collision, target, + ) + self._require_synchronized() + self._retreat(goal, 'place_cap') + self._emit('place_cap', object='cap', phase='done', target=target.xyz) + return OperationResult(True, 'place_cap', 'hatch placed', tuple(self.events)) + except Exception as error: + self._emit('place_cap', object='cap', phase='failed', error=str(error)) + raise + + # ------------------------------------------------------------------ sealant + def dispense( + self, + segment_name: str, + start_fraction: float = 0.0, + end_fraction: float = 1.0, + *, + lift: bool = True, + ) -> OperationResult: + self._emit('dispense', object=segment_name, phase='start') + try: + self._require_synchronized() + start = sealant_point( + self.geometry, self.spec, segment_name, start_fraction, + ) + end = sealant_point( + self.geometry, self.spec, segment_name, end_fraction, + ) + tip = self.geometry.tool_work_tips['sealant_tool'] + start_goal = tcp_goal(start, tip) + end_goal = tcp_goal(end, tip) + if start_fraction <= 0.0 and end_fraction >= 1.0: + self._contact(start_goal, f'dispense({segment_name})') + else: + self._contact(start_goal, f'dispense({segment_name}):resume') + self._move( + end_goal, 'lin', self.geometry.speeds['contour'], + f'dispense({segment_name}):path', allow_fallback=False, + ) + self.last_sealant_point = (segment_name, end_fraction) + self.completed_sealant.add(segment_name) + if lift: + self._retreat(end_goal, f'dispense({segment_name})') + self._emit('dispense', object=segment_name, phase='done', + start_fraction=start_fraction, end_fraction=end_fraction) + return OperationResult(True, 'dispense', f'{segment_name} completed', tuple(self.events)) + except Exception as error: + self._emit('dispense', object=segment_name, phase='failed', error=str(error)) + raise + + def dispense_contour(self, segments: Iterable[str]) -> OperationResult: + """Traverse adjacent sides continuously without lifting the nozzle.""" + segments = list(segments) + self._emit('dispense_contour', segments=list(segments), phase='start') + try: + self._require_synchronized() + tip = self.geometry.tool_work_tips['sealant_tool'] + 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) + end_goal = tcp_goal(end, tip) + self._move( + end_goal, 'lin', self.geometry.speeds['contour'], + f'dispense_contour:{segment}', allow_fallback=False, + ) + self.completed_sealant.add(segment) + self.last_sealant_point = (segment, 1.0) + self._emit('dispense', object=segment, phase='done') + self._retreat(end_goal, 'dispense_contour') + self._emit('dispense_contour', segments=list(segments), phase='done') + return OperationResult(True, 'dispense_contour', 'contour completed', tuple(self.events)) + except Exception as error: + self._emit('dispense_contour', phases='failed', error=str(error)) + raise + + # ------------------------------------------------------------------ screws + def pick_screw(self, screw_id: str) -> OperationResult: + def_name = screw_object_id(screw_id) + self._emit('pick_screw', object=screw_id, phase='start') + try: + self._require_synchronized() + screw_pose = self.supervisor.object_pose(def_name) + goal = tcp_goal(screw_pose, self.geometry.screw_tcp_to_node) + self._contact( + goal, f'pick_screw({screw_id})', + remove_before_contact=def_name, + ) + self.supervisor.command('grab_screw', screw_id) + 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: + self._emit('pick_screw', object=screw_id, phase='failed', error=str(error)) + raise + + def handoff_screw(self, screw_id: str, hole_id: str) -> OperationResult: + def_name = screw_object_id(screw_id) + self._emit('handoff_screw', object=screw_id, target=hole_id, phase='start') + try: + 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( + goal, f'handoff_screw({screw_id},{hole_id})', + remove_before_contact=def_name, + remove_phase='contact', + ) + self.supervisor.command( + '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. + self._emit('delivered', object=screw_id, target=hole_id, + phase='done', hole=hole_id) + return OperationResult( + True, 'handoff_screw', f'{screw_id} delivered to {hole_id}', + tuple(self.events), + ) + except Exception as error: + self._emit('handoff_screw', object=screw_id, target=hole_id, + phase='failed', error=str(error)) + raise + + # ------------------------------------------------------------------ verifies + def inspect_cap(self, target_pose: Transform | 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() + cap = state.get('cap') or {} + if cap.get('state') != 'installed': + return {'ok': False, 'reason': f'hatch state is {cap.get("state")!r}'} + pose_entry = cap.get('pose') + if not pose_entry: + return {'ok': False, 'reason': 'hatch pose is unknown'} + pose = _transform_from_payload(pose_entry, child='cap') + distance = float( + (pose.position - target.position) @ (pose.position - target.position) + ) ** 0.5 + ok = distance <= self.geometry.position_tolerance + return { + 'ok': ok, + 'distance': distance, + 'target': target.xyz, + 'actual': pose.xyz, + } + + def inspect_sealant(self) -> dict[str, Any]: + expected = list(self.spec.sealant_sides) + missing = [side for side in expected if side not in self.completed_sealant] + return { + 'ok': not missing, + 'completed': sorted(self.completed_sealant), + 'missing': missing, + } + + def inspect_state(self) -> dict[str, Any]: + self.supervisor.command('inspect') + return self._expect_latest() diff --git a/src/iiwa_experement/iiwa_experement/planning_scene.py b/src/iiwa_experement/iiwa_experement/planning_scene.py index f55a6bb..84d6528 100644 --- a/src/iiwa_experement/iiwa_experement/planning_scene.py +++ b/src/iiwa_experement/iiwa_experement/planning_scene.py @@ -10,19 +10,24 @@ from __future__ import annotations import copy import math import time +import xml.etree.ElementTree as ET from dataclasses import dataclass +from pathlib import Path from typing import Iterable, Sequence import numpy as np import rclpy +from ament_index_python.packages import get_package_share_directory from geometry_msgs.msg import Pose from moveit_msgs.msg import ( + AllowedCollisionEntry, AttachedCollisionObject, CollisionObject, PlanningScene, ) from moveit_msgs.srv import GetPlanningScene from rclpy.node import Node +from sensor_msgs.msg import JointState from rclpy.parameter import Parameter from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy from shape_msgs.msg import SolidPrimitive @@ -35,7 +40,55 @@ SCENE_TOPIC = '/planning_scene' MONITORED_TOPIC = '/monitored_planning_scene' GET_SCENE_SERVICE = '/get_planning_scene' SCENE_FRAME = 'base_link' -TOUCH_LINKS = ('tcp', 'patron', 'camera_holder') +TOUCH_LINKS = ( + 'tcp', 'patron', 'camera_holder', 'camera_corner', + 'camera_hand', 'camera_optical_frame', +) +ROBOT_LINKS = ( + 'base_link', 'link1', 'link2', 'link3', 'link4', 'link5', 'link6', + 'link7', 'patron', 'camera_holder', 'camera_corner', 'camera_hand', + 'camera_optical_frame', 'tcp', +) +DEFAULT_SRDF = 'iiwa_config/config/moveit/iiwa7.srdf' + + +def load_srdf_disabled_pairs(path) -> list[tuple[str, str]]: + root = ET.parse(str(path)).getroot() + pairs = [] + for element in root.findall('disable_collisions'): + first = element.get('link1') + second = element.get('link2') + if first and second: + pairs.append((first, second)) + return pairs + + +def build_collision_matrix( + object_ids: Iterable[str], + *, + disabled_pairs: Iterable[tuple[str, str]] = (), + extra_allowed: Iterable[tuple[str, str]] = (), +) -> tuple[list[str], list[list[bool]]]: + """Build a complete ACM: only explicitly disabled pairs are allowed. + + This mirrors the SRDF and keeps every other robot/object and object/object + pair checked. It replaces an empty ACM that would otherwise make the + planning scene treat adjacent robot links as collisions. + """ + names = list(ROBOT_LINKS) + for name in object_ids: + if name not in names: + names.append(name) + matrix = [[False] * len(names) for _ in names] + for index in range(len(names)): + matrix[index][index] = True + for first, second in list(disabled_pairs) + list(extra_allowed): + if first in names and second in names: + row = names.index(first) + column = names.index(second) + matrix[row][column] = True + matrix[column][row] = True + return names, matrix class SceneSynchronizationError(RuntimeError): @@ -59,6 +112,7 @@ class AttachedSpec: box: Box relative_pose: Transform touch_links: tuple[str, ...] + frame_id: str | None = None def _box_at(pose: Transform, box: Box) -> Box: @@ -90,7 +144,7 @@ def build_world_objects(spec: ScenarioSpec, geometry: GeometrySpec) -> list[Scen 'grant_table', table, _rotated_pose(0.0, 0.0, table.offset[2], 0.0), )) - fixture = geometry.scene_collisions['fixture'] + fixture = geometry.scene_collisions['fixture_base'] objects.append(SceneObject( 'grant_fixture', fixture, @@ -101,6 +155,20 @@ def build_world_objects(spec: ScenarioSpec, geometry: GeometrySpec) -> list[Scen spec.fixture.yaw_deg, ), )) + rail = geometry.scene_collisions['fixture_rail'] + sine = math.sin(math.radians(spec.fixture.yaw_deg)) + cosine = math.cos(math.radians(spec.fixture.yaw_deg)) + for sign, suffix in ((-1.0, 'a'), (1.0, 'b')): + objects.append(SceneObject( + f'grant_fixture_rail_{suffix}', + rail, + _rotated_pose( + spec.fixture.x + sign * rail.offset[0] * cosine, + spec.fixture.y + sign * rail.offset[0] * sine, + spec.fixture.z + rail.offset[2], + spec.fixture.yaw_deg, + ), + )) body = geometry.scene_collisions['body'] objects.append(SceneObject( @@ -211,7 +279,10 @@ def attached_tool(name: str, geometry: GeometrySpec) -> AttachedSpec: offset=collision.offset, ), relative_pose=Transform.from_xyz_rpy( - collision.offset, [0.0, 0.0, 0.0], parent='tcp', child=f'grant_{name}', + [0.0, 0.0, collision.offset[2] + 0.0552], + [0.0, 0.0, 0.0], + parent='patron', + child=f'grant_{name}', ), touch_links=TOUCH_LINKS, ) @@ -222,9 +293,9 @@ def attached_cap(geometry: GeometrySpec) -> AttachedSpec: object_id='grant_cap', box=geometry.cap_collision, relative_pose=Transform.from_xyz_rpy( - geometry.cap_collision.offset, + [0.0, 0.0, geometry.cap_collision.offset[2] + 0.0552], [0.0, 0.0, 0.0], - parent='tcp', + parent='patron', child='grant_cap', ), touch_links=TOUCH_LINKS, @@ -237,9 +308,9 @@ def attached_screw(screw_id: str, geometry: GeometrySpec) -> AttachedSpec: object_id=object_id, box=geometry.screw_collision, relative_pose=Transform.from_xyz_rpy( - geometry.screw_collision.offset, + [0.0, 0.0, geometry.screw_collision.offset[2] + 0.0552], [0.0, 0.0, 0.0], - parent='tcp', + parent='patron', child=object_id, ), touch_links=TOUCH_LINKS, @@ -274,25 +345,32 @@ def collision_object( message.header.frame_id = SCENE_FRAME message.id = object_id message.operation = operation + message.pose = _pose_message(pose) primitive = SolidPrimitive() primitive.type = SolidPrimitive.BOX primitive.dimensions = [float(value) for value in box.size] message.primitives.append(primitive) - message.primitive_poses.append(_pose_message(pose)) + identity = Pose() + identity.orientation.w = 1.0 + message.primitive_poses.append(identity) return message def attached_object_message( spec: AttachedSpec, *, - link_name: str = 'tcp', + link_name: str | None = None, operation: int = CollisionObject.ADD, ) -> AttachedCollisionObject: message = AttachedCollisionObject() - message.link_name = link_name + message.link_name = link_name or spec.relative_pose.parent message.object = collision_object( spec.object_id, spec.box, spec.relative_pose, operation=operation, ) + # For attached objects the object pose is relative to the link. + message.object.header.frame_id = ( + spec.frame_id if spec.frame_id is not None else message.link_name + ) message.touch_links = list(spec.touch_links) return message @@ -334,6 +412,7 @@ class PlanningSceneBridge: *, node: Node | None = None, use_sim_time: bool = True, + srdf_path: str | None = None, ) -> None: self._owns_node = node is None if node is None: @@ -366,8 +445,29 @@ class PlanningSceneBridge: self._attached: dict[str, AttachedSpec] = {} self._seen_world: set[str] = set() self._seen_attached: set[str] = set() + self._latest_joint_state = None + self._last_full_scene: PlanningScene | None = None + self._acm_names: list[str] = [] + self._acm_values: list[list[bool]] = [] + self._disabled_pairs = [] + srdf = srdf_path or str( + Path(get_package_share_directory('iiwa_config')) + / 'config' / 'moveit' / 'iiwa7.srdf' + ) + try: + self._disabled_pairs = load_srdf_disabled_pairs(srdf) + except Exception: + self._disabled_pairs = [] + self._joint_subscription = self.node.create_subscription( + JointState, '/joint_states', self._on_joint_state, 10, + ) + + def _on_joint_state(self, message: JointState) -> None: + self._latest_joint_state = message def _on_scene(self, message: PlanningScene) -> None: + if not message.is_diff: + self._last_full_scene = copy.deepcopy(message) self._seen_world.update( obj.id for obj in message.world.collision_objects ) @@ -411,6 +511,137 @@ class PlanningSceneBridge: self._publisher.publish(scene) self._spin() + def _fetch_acm(self) -> None: + if getattr(self, '_get_scene', None) is None or self.node is None: + return + if not self._get_scene.service_is_ready(): + if not self._get_scene.wait_for_service(timeout_sec=1.0): + return + request = GetPlanningScene.Request() + request.components.components = 128 + future = self._get_scene.call_async(request) + end = time.monotonic() + 3.0 + while time.monotonic() < end and not future.done(): + rclpy.spin_once(self.node, timeout_sec=0.05) + if future.done(): + try: + matrix = future.result().scene.allowed_collision_matrix + self._acm_names = list(matrix.entry_names) + self._acm_values = [ + list(row.enabled) for row in matrix.entry_values + ] + except Exception: + pass + + def _rebuild_acm(self) -> None: + extra = [] + for spec in self._attached.values(): + extra.extend((spec.object_id, link) for link in spec.touch_links) + self._acm_names, self._acm_values = build_collision_matrix( + list(self._world_objects) + list(self._attached), + disabled_pairs=self._disabled_pairs, + extra_allowed=extra, + ) + + def _full_scene(self) -> PlanningScene: + scene = PlanningScene() + scene.is_diff = False + # Never overwrite the planner's current robot state from a planning + # scene update: the monitor receives /joint_states directly and a + # stale embedded sample makes the next trajectory start from an old + # pose. ``is_diff`` leaves the state untouched. + scene.robot_state.is_diff = True + for obj in self._world_objects.values(): + scene.world.collision_objects.append(collision_object( + obj.object_id, obj.box, self._base_pose(obj.pose), + )) + for spec in self._attached.values(): + scene.robot_state.attached_collision_objects.append( + attached_object_message(spec) + ) + if self._acm_names: + scene.allowed_collision_matrix.entry_names = list(self._acm_names) + scene.allowed_collision_matrix.entry_values = [ + AllowedCollisionEntry( + enabled=[bool(value) for value in row], + ) + for row in self._acm_values + ] + return scene + + def _refresh_joint_state(self) -> None: + """Wait for the next /joint_states sample before a full scene update. + + A full PlanningScene carries a robot state. Using a sample captured + before a preceding motion would make the MoveItPy planner start from + an old pose on the next trajectory. + """ + if self.node is None: + return + previous = None + if self._latest_joint_state is not None: + previous = ( + self._latest_joint_state.header.stamp.sec, + self._latest_joint_state.header.stamp.nanosec, + ) + end = time.monotonic() + 0.5 + while time.monotonic() < end: + rclpy.spin_once(self.node, timeout_sec=0.02) + current = self._latest_joint_state + if current is None: + continue + stamp = ( + current.header.stamp.sec, + current.header.stamp.nanosec, + ) + if previous is None or stamp != previous: + return + + def _relay_full_scene(self) -> None: + """Give MoveItPy the exact move_group scene, including its ACM. + + MoveItPy's monitor always listens on ``/planning_scene`` even when its + configured topics differ. Fetching the maintained scene from + move_group and republishing it once keeps both planners identical + without a feedback loop. + """ + if getattr(self, '_get_scene', None) is None or self.node is None: + return + if not self._get_scene.service_is_ready(): + if not self._get_scene.wait_for_service(timeout_sec=1.0): + return + expected_world = set(self._world_objects) + expected_attached = set(self._attached) + deadline = time.monotonic() + 3.0 + while time.monotonic() < deadline: + request = GetPlanningScene.Request() + request.components.components = 2 | 4 | 8 | 16 | 128 + future = self._get_scene.call_async(request) + end = time.monotonic() + 1.0 + while time.monotonic() < end and not future.done(): + rclpy.spin_once(self.node, timeout_sec=0.05) + if not future.done(): + continue + try: + scene = future.result().scene + except Exception: + continue + world, attached = scene_ids(scene) + if expected_world <= world and expected_attached <= attached: + scene.is_diff = False + scene.robot_state.is_diff = True + self._publisher.publish(scene) + self._spin() + return + # Never relay a stale scene: that would wipe the objects from the + # MoveItPy monitor and make the next motion unverified. + return + + def _publish_full(self) -> None: + self._refresh_joint_state() + self._publish(self._full_scene()) + self._relay_full_scene() + @staticmethod def _diff() -> PlanningScene: scene = PlanningScene() @@ -440,33 +671,24 @@ class PlanningSceneBridge: obj.object_id: obj for obj in build_world_objects(spec, geometry) } self._attached.clear() - scene = self._diff() - for obj in self._world_objects.values(): - scene.world.collision_objects.append(collision_object( - obj.object_id, obj.box, self._base_pose(obj.pose), - )) - self._publish(scene) + extra = [] + for spec in self._attached.values(): + extra.extend((spec.object_id, link) for link in spec.touch_links) + self._acm_names, self._acm_values = build_collision_matrix( + list(self._world_objects) + list(self._attached), + disabled_pairs=self._disabled_pairs, + extra_allowed=extra, + ) + self._publish_full() self.assert_synchronized( self._world_objects, (), seen_world=set(self._world_objects), ) def attach(self, spec: AttachedSpec) -> None: - if spec.object_id not in self._world_objects: - raise SceneSynchronizationError( - f'cannot attach {spec.object_id!r}: not a known world object' - ) - scene = self._diff() - scene.world.collision_objects.append(collision_object( - spec.object_id, spec.box, Transform.from_xyz_rpy( - [0, 0, 0], [0, 0, 0], parent='world', child='removed', - ), operation=CollisionObject.REMOVE, - )) - scene.robot_state.attached_collision_objects.append( - attached_object_message(spec) - ) self._world_objects.pop(spec.object_id, None) self._attached[spec.object_id] = spec - self._publish(scene) + self._rebuild_acm() + self._publish_full() def detach(self, object_id: str, world_pose: Transform) -> None: if object_id not in self._attached: @@ -476,34 +698,20 @@ class PlanningSceneBridge: spec = self._attached.pop(object_id) object_spec = SceneObject(object_id, spec.box, world_pose) self._world_objects[object_id] = object_spec - scene = self._diff() - scene.robot_state.attached_collision_objects.append( - attached_object_message( - spec, operation=CollisionObject.REMOVE, - ) - ) - scene.world.collision_objects.append(collision_object( - object_id, spec.box, self._base_pose(world_pose), - )) - self._publish(scene) + self._rebuild_acm() + self._publish_full() def remove(self, object_id: str) -> None: - scene = self._diff() - if object_id in self._attached: - spec = self._attached.pop(object_id) - scene.robot_state.attached_collision_objects.append( - attached_object_message( - spec, operation=CollisionObject.REMOVE, - ) - ) - if object_id in self._world_objects: - self._world_objects.pop(object_id) - scene.world.collision_objects.append(collision_object( - object_id, Box(size=(0.001, 0.001, 0.001)), Transform.from_xyz_rpy( - [0, 0, 0], [0, 0, 0], parent='world', child='removed', - ), operation=CollisionObject.REMOVE, - )) - self._publish(scene) + self._attached.pop(object_id, None) + self._world_objects.pop(object_id, None) + self._rebuild_acm() + self._publish_full() + + def add_world(self, object_id: str, box: Box, world_pose: Transform) -> None: + self._attached.pop(object_id, None) + self._world_objects[object_id] = SceneObject(object_id, box, world_pose) + self._rebuild_acm() + self._publish_full() def allow_contact(self, first: str, second: str, allowed: bool = True) -> None: """Change exactly one ACM pair; all other checks stay active.""" @@ -511,22 +719,21 @@ class PlanningSceneBridge: raise SceneSynchronizationError( 'cannot update contact permissions without a monitored scene' ) - scene = self._diff() - names = list(self.last_scene.allowed_collision_matrix.entry_names) - values = [ - list(row) for row in self.last_scene.allowed_collision_matrix.entry_values - ] - names, values = update_contact_entry( - names, values, first, second, allowed, + if not self._acm_names: + raise SceneSynchronizationError( + 'cannot update contact permissions without an ACM' + ) + self._acm_names, self._acm_values = update_contact_entry( + self._acm_names, self._acm_values, first, second, allowed, ) - scene.allowed_collision_matrix.entry_names = names - scene.allowed_collision_matrix.entry_values = values - self._publish(scene) + self._publish_full() def expected_sets(self) -> tuple[set[str], set[str]]: return set(self._world_objects), set(self._attached) def _query_scene(self, timeout_sec: float) -> tuple[set[str], set[str]] | None: + if getattr(self, '_get_scene', None) is None or self.node is None: + return if not self._get_scene.service_is_ready(): if not self._get_scene.wait_for_service(timeout_sec=1.0): return None diff --git a/src/iiwa_experement/iiwa_experement/runtime_config.py b/src/iiwa_experement/iiwa_experement/runtime_config.py index d61ace1..577b693 100644 --- a/src/iiwa_experement/iiwa_experement/runtime_config.py +++ b/src/iiwa_experement/iiwa_experement/runtime_config.py @@ -117,6 +117,7 @@ def _materialize_setting( base_setting: Path, world_path: Path, output_dir: Path, + moveit_cpp_path: Path | None = None, ) -> Path: if not base_setting.is_file(): raise FileNotFoundError(f'base settings file does not exist: {base_setting}') @@ -134,6 +135,11 @@ def _materialize_setting( tool = _mapping(data.setdefault('tool', {}), 'tool') tool['active'] = 'patron' + if moveit_cpp_path is not None: + controller = _mapping(data.setdefault('controller', {}), 'controller') + moveit = _mapping(controller.setdefault('moveit', {}), 'controller.moveit') + moveit['moveit_cpp'] = str(Path(moveit_cpp_path).resolve()) + setting_path = output_dir / 'setting.yaml' setting_path.write_text( yaml.safe_dump(data, sort_keys=False, allow_unicode=True), @@ -147,6 +153,7 @@ def prepare_runtime_files( world_template: Path | str, description_share: Path | str, output_dir: Path | str, + moveit_cpp_path: Path | str | None = None, ) -> tuple[Path, Path]: """Write an isolated world/settings pair and return ``(world, setting)``. @@ -161,5 +168,10 @@ def prepare_runtime_files( output_dir = Path(output_dir) output_dir.mkdir(parents=True, exist_ok=True) world_path = _materialize_world(world_template, description_share, output_dir) - setting_path = _materialize_setting(base_setting, world_path, output_dir) + setting_path = _materialize_setting( + base_setting, + world_path, + output_dir, + None if moveit_cpp_path is None else Path(moveit_cpp_path), + ) return world_path, setting_path diff --git a/src/iiwa_experement/iiwa_experement/webots_nodes.py b/src/iiwa_experement/iiwa_experement/webots_nodes.py index bb7e61d..45285b9 100644 --- a/src/iiwa_experement/iiwa_experement/webots_nodes.py +++ b/src/iiwa_experement/iiwa_experement/webots_nodes.py @@ -309,15 +309,16 @@ def _cap_node(spec: ScenarioSpec, asset: Path) -> str: translation 0 0.004165 0 children [ Box { size 0.1384 0.00367 0.0984 } ] }''' - installed = spec.cap_state == 'installed' + # All managed objects are positioned by the attachment supervisor. + # A free cap with physics can be launched by the supervisor's per-step + # teleport, so it is locked just like an installed cap. return _solid( name='grant_cap', translation=(pose.x, pose.y, pose.z), rotation=rotation_axis_angle(pose.yaw_deg), children=_mesh_shape(asset, '0.78 0.82 0.86'), bounding=bounding, - locked=installed, - physics_mass=None if installed else 0.1, + locked=True, ) diff --git a/src/iiwa_experement/launch/experiment.launch.py b/src/iiwa_experement/launch/experiment.launch.py index 72ce51b..d60d5cb 100644 --- a/src/iiwa_experement/launch/experiment.launch.py +++ b/src/iiwa_experement/launch/experiment.launch.py @@ -226,6 +226,7 @@ def _setup(context, *args, **kwargs): world_template=world_template, description_share=description_share, output_dir=output_dir, + moveit_cpp_path=package_share / 'config' / 'moveit_cpp.yaml', ) except Exception: shutil.rmtree(output_dir, ignore_errors=True) diff --git a/src/iiwa_experement/test/test_grant_supervisor.py b/src/iiwa_experement/test/test_grant_supervisor.py index e9af821..1108b5f 100644 --- a/src/iiwa_experement/test/test_grant_supervisor.py +++ b/src/iiwa_experement/test/test_grant_supervisor.py @@ -78,7 +78,8 @@ def make_node(experiment_id=1): node.spec = spec node.geometry = geometry node.state = ObjectState(spec, geometry) - node.attached = None + node.tool_held = None + node.payload = None node.rack_poses = { name: entry['pose'] for name, entry in node.state.tools.items() } @@ -144,7 +145,7 @@ def test_attach_requires_tcp_at_the_socket_and_rejects_far_pose(): accepted, error = node._apply('attach_tool', {'object': 'vacuum'}, 'r1') assert accepted is True assert error is None - assert node.attached == ('tool', 'vacuum') + assert node.tool_held == 'vacuum' other = make_node(1) set_tcp(other, good.compose_relaxed( @@ -155,7 +156,7 @@ def test_attach_requires_tcp_at_the_socket_and_rejects_far_pose(): accepted, error = other._apply('attach_tool', {'object': 'vacuum'}, 'r2') assert accepted is False assert 'tcp is' in error - assert other.attached is None + assert other.tool_held is None assert other.state.version == 0 @@ -170,7 +171,7 @@ def test_follow_moves_the_attached_tool_and_dock_returns_it_to_the_rack(): assert moved == pytest.approx(rack_pose.position, abs=1e-9) accepted, _ = node._apply('dock_tool', {'object': 'vacuum'}, 'dock') assert accepted - assert node.attached is None + assert node.tool_held is None assert node.state.mounted_tool is None assert node.nodes['grant_vacuum'].getPosition() == pytest.approx( rack_pose.position, abs=1e-9, @@ -213,8 +214,29 @@ def test_grab_and_handoff_remove_only_the_selected_screw(): def test_follow_refuses_when_tcp_is_unknown(): node = make_node(1) node.patron_node = None - node.attached = ('tool', 'vacuum') + node.tool_held = 'vacuum' node.follow() assert node.nodes['grant_vacuum'].getPosition() == pytest.approx( node.state.tools['vacuum']['pose'].position, abs=1e-9, ) + + +def test_mounted_tool_and_payload_follow_together(): + node = make_node(1) + tool_pose = node.state.tools['vacuum']['pose'] + tcp = tcp_goal(tool_pose, node.geometry.tool_tcp_to_nodes['vacuum']) + set_tcp(node, tcp) + assert node._apply('attach_tool', {'object': 'vacuum'}, 't')[0] + cap_pose = node.state.cap_pose + cap_tcp = tcp_goal(cap_pose, node.geometry.cap_tcp_to_node) + set_tcp(node, cap_tcp) + assert node._apply('grab_cap', {'object': 'cap'}, 'c')[0] + node.follow() + expected_tool = cap_tcp.compose_relaxed(node.geometry.tool_tcp_to_nodes['vacuum']) + expected_cap = cap_tcp.compose_relaxed(node.geometry.cap_tcp_to_node) + assert node.nodes['grant_vacuum'].getPosition() == pytest.approx( + expected_tool.position, abs=1e-9, + ) + assert node.nodes['grant_cap'].getPosition() == pytest.approx( + expected_cap.position, abs=1e-9, + ) diff --git a/src/iiwa_experement/test/test_operations.py b/src/iiwa_experement/test/test_operations.py new file mode 100644 index 0000000..37ad298 --- /dev/null +++ b/src/iiwa_experement/test/test_operations.py @@ -0,0 +1,219 @@ +"""Operation sequences are visible, ordered and fail closed.""" +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 = [] + + 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) + + def add_world(self, object_id, box, pose): + 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() + + 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) + 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 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):retreat', + 'take_tool(vacuum):clear', + ] + assert supervisor.commands[0][:2] == ('attach_tool', 'vacuum') + assert bridge.attached == [] + contact_goal = motion.moves[2][3] + assert contact_goal.xyz == pytest.approx(expected.xyz, abs=1e-9) + assert motion.moves[0][1] == 'ompl' + assert motion.moves[1][1] == 'ompl' + 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.added_world[0][0] == 'grant_vacuum' + assert labels(motion)[-1] == 'dock_tool(vacuum):clear' + + +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 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 == [] + 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_failed_motion_stops_the_operation_and_emits_failure(): + operations, supervisor, bridge, motion, geometry = make_operations(1) + motion.fail_labels.add('take_tool(vacuum):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 == [] diff --git a/src/iiwa_experement/test/test_planning_scene.py b/src/iiwa_experement/test/test_planning_scene.py index 36bae17..68c6f6e 100644 --- a/src/iiwa_experement/test/test_planning_scene.py +++ b/src/iiwa_experement/test/test_planning_scene.py @@ -10,6 +10,8 @@ from iiwa_experement.planning_scene import ( PlanningSceneBridge, SceneObject, attached_cap, + build_collision_matrix, + load_srdf_disabled_pairs, attached_screw, attached_tool, build_world_objects, @@ -40,6 +42,11 @@ class FakeBridge(PlanningSceneBridge): obj.object_id: obj for obj in build_world_objects(scenario, geometry) } self._attached = {} + self._disabled_pairs = [] + self._acm_names = [] + self._acm_values = [] + self._latest_joint_state = None + self.node = None self.last_scene = PlanningScene() self.published = [] @@ -53,13 +60,14 @@ class FakeBridge(PlanningSceneBridge): def test_initial_scene_contains_every_experiment_object(): objects = build_world_objects(spec(11), load_geometry(GEOMETRY)) ids = {obj.object_id for obj in objects} - assert {'grant_table', 'grant_fixture', 'grant_body', 'grant_rack', + assert {'grant_table', 'grant_fixture', 'grant_fixture_rail_a', + 'grant_fixture_rail_b', 'grant_body', 'grant_rack', 'grant_tray', 'grant_cap', 'grant_magnetic_tool'} <= ids assert {f'grant_screw_{index:02d}' for index in range(1, 11)} <= ids assert 'grant_vacuum' not in ids - # table, fixture, body, rack, tray base, four tray walls, ten screws, - # installed hatch and one magnetic tool. - assert len(ids) == 4 + 1 + 4 + 10 + 1 + 1 + # table, fixture base, two fixture rails, body, rack, tray base, four + # tray walls, ten screws, installed hatch and one magnetic tool. + assert len(ids) == 23 def test_experiment_nineteen_moves_tray_and_screws(): @@ -99,11 +107,11 @@ def test_attach_removes_the_world_object_and_adds_one_attachment(): assert tool.object_id not in bridge._world_objects assert bridge._attached[tool.object_id] is tool scene = bridge.published[-1] - assert scene.is_diff is True - assert [obj.id for obj in scene.world.collision_objects] == [tool.object_id] - assert scene.world.collision_objects[0].operation == CollisionObject.REMOVE + assert scene.is_diff is False + assert tool.object_id not in [obj.id for obj in scene.world.collision_objects] assert len(scene.robot_state.attached_collision_objects) == 1 - assert scene.robot_state.attached_collision_objects[0].link_name == 'tcp' + assert scene.robot_state.attached_collision_objects[0].object.id == tool.object_id + assert scene.robot_state.attached_collision_objects[0].link_name == 'patron' assert 'tcp' in scene.robot_state.attached_collision_objects[0].touch_links @@ -117,10 +125,8 @@ def test_detach_reverses_the_attachment(): assert tool.object_id in bridge._world_objects assert tool.object_id not in bridge._attached scene = bridge.published[-1] - assert scene.robot_state.attached_collision_objects[0].object.operation == ( - CollisionObject.REMOVE - ) - assert scene.world.collision_objects[0].operation == CollisionObject.ADD + assert not scene.robot_state.attached_collision_objects + assert tool.object_id in [obj.id for obj in scene.world.collision_objects] def test_handoff_removes_only_the_delivered_screw(): @@ -138,9 +144,9 @@ def test_cap_and_screw_attachment_geometry_uses_tcp_offsets(): geometry = load_geometry(GEOMETRY) cap = attached_cap(geometry) screw = attached_screw('screw_3', geometry) - assert cap.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.141835)) + assert cap.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.141835 + 0.0552)) assert cap.box.size == pytest.approx((0.1384, 0.0984, 0.00367)) - assert screw.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.1445)) + assert screw.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.1445 + 0.0552)) def test_contact_permission_changes_exactly_one_pair(): @@ -153,6 +159,24 @@ def test_contact_permission_changes_exactly_one_pair(): assert matrix[0][1] is False +def test_srdf_collision_matrix_allows_only_disabled_pairs(): + root = Path(__file__).resolve().parents[2] + srdf = root / 'iiwa_config' / 'config' / 'moveit' / 'iiwa7.srdf' + pairs = load_srdf_disabled_pairs(srdf) + assert ('link1', 'link2') in pairs + names, matrix = build_collision_matrix( + ['grant_body', 'grant_tool'], disabled_pairs=pairs, + ) + assert 'base_link' in names and 'grant_body' in names + def allowed(first, second): + return matrix[names.index(first)][names.index(second)] + assert allowed('link1', 'link2') is True + assert allowed('base_link', 'link1') is True + assert allowed('link1', 'grant_body') is False + assert allowed('grant_body', 'grant_tool') is False + assert allowed('grant_body', 'grant_body') is True + + def test_scene_ids_reports_world_and_attached_sets(): scene = PlanningScene() scene.world.collision_objects.append(CollisionObject(id='world_a')) diff --git a/src/iiwa_experement/test/test_webots_nodes.py b/src/iiwa_experement/test/test_webots_nodes.py index 8c983e1..b4f35a1 100644 --- a/src/iiwa_experement/test/test_webots_nodes.py +++ b/src/iiwa_experement/test/test_webots_nodes.py @@ -232,13 +232,11 @@ def test_loose_cap_touches_the_table_and_installed_cap_touches_the_body(): assert cap_z + cap_min == pytest.approx(body_z + body_max, abs=1e-9) -def test_free_cap_has_physics_and_installed_cap_is_locked(): - free_cap = by_name(nodes(1))['grant_cap'] - assert 'physics Physics {' in free_cap - assert 'locked TRUE' not in free_cap - installed_cap = by_name(nodes(11))['grant_cap'] - assert 'locked TRUE' in installed_cap - assert 'physics Physics {' not in installed_cap +def test_all_caps_are_locked_and_supervisor_controlled(): + for experiment_id in (1, 11): + cap = by_name(nodes(experiment_id))['grant_cap'] + assert 'locked TRUE' in cap + assert 'physics Physics {' not in cap def test_ten_screws_are_distinct_and_inside_the_tray():