diff --git a/src/iiwa_experement/iiwa_experement/grant_supervisor.py b/src/iiwa_experement/iiwa_experement/grant_supervisor.py new file mode 100644 index 0000000..725be37 --- /dev/null +++ b/src/iiwa_experement/iiwa_experement/grant_supervisor.py @@ -0,0 +1,452 @@ +#!/usr/bin/env python3 +"""Webots supervisor for attachment and object ownership. + +The controller is started next to Webots through the experiment launch file. +It exposes ``/iiwa_experement/object_command`` (webots_ros2_msgs/SetString) +and publishes ``/iiwa_experement/object_state`` as JSON. It never commands +the robot; it only follows the actual ``tcp`` pose when an object is held. +""" +from __future__ import annotations + +import json +import math +import sys +from pathlib import Path +from typing import Any + +import numpy as np +import rclpy +from ament_index_python.packages import get_package_share_directory +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from std_msgs.msg import String +from webots_ros2_msgs.srv import SetString + +from .geometry import GeometrySpec, Transform, load_geometry +from .object_state import CommandError, ObjectState, transform_json +from .scenario import ScenarioSpec, load_scenario + + +COMMAND_SERVICE = '/iiwa_experement/object_command' +STATE_TOPIC = '/iiwa_experement/object_state' + + +def screw_def_name(screw_id: str) -> str: + parts = screw_id.split('_') + if len(parts) != 2 or parts[0] != 'screw' or not parts[1].isdigit(): + raise ValueError(f'invalid screw id {screw_id!r}') + return f'grant_screw_{int(parts[1]):02d}' + + +def _matrix_from_values(values) -> tuple[tuple[float, float, float], ...]: + if not isinstance(values, (list, tuple)): + raise ValueError('rotation must contain 9 row-major values') + if len(values) == 3 and all( + isinstance(row, (list, tuple)) and len(row) == 3 for row in values + ): + return tuple( + tuple(float(value) for value in row) for row in values + ) + if len(values) != 9: + raise ValueError('rotation must contain 9 row-major values') + return tuple( + tuple(float(values[row * 3 + column]) for column in range(3)) + for row in range(3) + ) + + +def transform_from_json(data: dict[str, Any], child: str = 'target') -> Transform: + xyz = data.get('xyz') + if not isinstance(xyz, (list, tuple)) or len(xyz) != 3: + raise ValueError('pose.xyz must contain three numbers') + if 'rotation' in data: + rotation = _matrix_from_values(data['rotation']) + else: + from scipy.spatial.transform import Rotation + + rotation = tuple( + tuple(float(value) for value in row) + for row in Rotation.from_euler( + 'xyz', data.get('rpy_deg', [0.0, 0.0, 0.0]), degrees=True + ).as_matrix() + ) + return Transform( + xyz=tuple(float(value) for value in xyz), + rotation=rotation, + parent='world', + child=child, + ) + + +def _axis_angle(rotation) -> list[float]: + from scipy.spatial.transform import Rotation + + rotvec = Rotation.from_matrix(np.asarray(rotation, dtype=float)).as_rotvec() + angle = float(np.linalg.norm(rotvec)) + if angle < 1e-12: + return [0.0, 0.0, 1.0, 0.0] + axis = (rotvec / angle).tolist() + return [float(axis[0]), float(axis[1]), float(axis[2]), angle] + + +class GrantSupervisorNode(Node): + """ROS facade over :class:`ObjectState` and the Webots scene tree.""" + + def __init__(self) -> None: + super().__init__('grant_attachment_supervisor') + package_share = Path(get_package_share_directory('iiwa_experement')) + self.declare_parameter('experiment_id', 1) + self.declare_parameter( + 'scene_config', str(package_share / 'config' / 'scenes.yaml'), + ) + self.declare_parameter( + 'geometry_path', str(package_share / 'config' / 'geometry.yaml'), + ) + self.declare_parameter('read_only', False) + experiment_id = int(self.get_parameter('experiment_id').value) + scene_config = Path(str(self.get_parameter('scene_config').value)) + geometry_path = Path(str(self.get_parameter('geometry_path').value)) + read_only = bool(self.get_parameter('read_only').value) + 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.last_command_result: dict[str, Any] | None = None + self.rack_poses = { + name: entry['pose'] for name, entry in self.state.tools.items() + } + self.nodes: dict[str, Any] = {} + self.patron_node = None + self.robot = None + self.timestep = 32 + self._last_publish_time = -1.0 + self._publisher = self.create_publisher(String, STATE_TOPIC, 10) + self._service = self.create_service( + SetString, COMMAND_SERVICE, self.handle_command, + ) + self.get_logger().info( + f'grant supervisor started for experiment {experiment_id}; ' + f'read_only={read_only}' + ) + + # ------------------------------------------------------------------ Webots + def _pose_node(self, node): + """CadShape children can report an uninitialised matrix in Webots.""" + if node is None: + return None + try: + parent = node.getParentNode() + except Exception: + parent = None + if parent is not None: + position = parent.getPosition() + if all(np.isfinite(position)): + return parent + return node + + def connect(self) -> None: + from controller import Supervisor + + self.robot = Supervisor() + self.timestep = int(self.robot.getBasicTimeStep()) + self.patron_node = self._pose_node(self._find_node('patron_visual')) + required = self._required_defs() + for name in required: + node = self._find_node(name) + if node is not None: + self.nodes[name] = node + self._freeze(node) + self.get_logger().info( + f'supervisor found {len(self.nodes)}/{len(required)} scene objects' + ) + + def _find_node(self, name: str): + try: + return self.robot.getFromDef(name) + except Exception: + return None + + @staticmethod + def _freeze(node) -> None: + try: + field = node.getField('locked') + if field is not None: + field.setSFBool(True) + except Exception: + pass + + def _pose_of(self, node) -> Transform | None: + if node is None: + return None + position = node.getPosition() + orientation = node.getOrientation() + return Transform( + xyz=tuple(float(value) for value in position), + rotation=_matrix_from_values(orientation), + parent='world', + child='object', + ) + + def _set_pose(self, node, transform: Transform) -> None: + node.getField('translation').setSFVec3f( + [float(value) for value in transform.xyz] + ) + rotation = node.getField('rotation') + if rotation is not None: + rotation.setSFRotation(_axis_angle(transform.rotation)) + + def tcp_pose(self) -> Transform | None: + patron = self._pose_of(self.patron_node) + if patron is None: + return None + return patron.compose_relaxed(self.geometry.patron_to_tcp) + + def _refresh_nodes(self) -> None: + if self.robot is None: + return + if self.patron_node is None: + self.patron_node = self._pose_node(self._find_node('patron_visual')) + for name in self._required_defs(): + if name not in self.nodes: + node = self._find_node(name) + if node is not None: + self.nodes[name] = node + self._freeze(node) + + def _required_defs(self) -> list[str]: + names = ['grant_fixture', 'grant_body', 'grant_rack'] + names.extend(f'grant_{tool}' for tool in self.spec.tool_names) + if self.spec.tray_pose is not None: + names.append('grant_tray') + names.extend(f'grant_screw_{index:02d}' for index in range(1, 11)) + if self.spec.cap_state != 'absent': + names.append('grant_cap') + return names + + def _node_for_attachment(self) -> Any | None: + if self.attached is None: + return None + kind, name = self.attached + if kind == 'tool': + return self.nodes.get(f'grant_{name}') + 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: + return None + kind, name = self.attached + if kind == 'tool': + return self.geometry.tool_tcp_to_nodes[name] + if kind == 'cap': + return self.geometry.cap_tcp_to_node + if kind == 'screw': + return self.geometry.screw_tcp_to_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: + 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 + + # ------------------------------------------------------------------ state + def _actual_pose(self, node) -> Transform | None: + return self._pose_of(node) + + def _command_pose(self, command: dict[str, Any]) -> Transform: + target = command.get('target') + if not isinstance(target, dict): + raise CommandError('command requires a target pose') + return transform_from_json(target, child=str(command.get('object') or 'target')) + + def handle_command(self, request: SetString.Request, response: SetString.Response): + try: + command = json.loads(request.value) + if not isinstance(command, dict): + raise CommandError('command must be a JSON object') + request_id = str(command.get('request_id', '')) + operation = str(command.get('op', '')) + accepted, error = self._apply(operation, command, request_id) + except Exception as error: # never leak an exception into Webots + accepted = False + error = str(error) + self.state.last_error = error + # The detailed outcome is published in the next object_state message; + # the service response only carries the accepted flag. + response.success = bool(accepted) + self.last_command_result = { + 'request_id': request_id, + 'accepted': bool(accepted), + 'error': None if accepted else error, + 'version': self.state.version, + } + self.publish_state(force=True) + return response + + def _apply(self, operation: str, command: dict[str, Any], request_id: str): + try: + return self._apply_impl(operation, command, request_id) + except CommandError as error: + self.state.last_error = str(error) + return False, str(error) + + def _apply_impl(self, operation: str, command: dict[str, Any], request_id: str): + if operation == 'inspect': + return True, None + tcp = self.tcp_pose() + if tcp is None: + raise CommandError('actual tcp pose is unavailable') + kind = str(command.get('object') or '') + if operation == 'attach_tool': + node = self.nodes.get(f'grant_{kind}') + object_pose = self._actual_pose(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) + return True, None + if operation == 'dock_tool': + node = self.nodes.get(f'grant_{kind}') + object_pose = self._actual_pose(node) + if object_pose is None: + raise CommandError(f'tool node grant_{kind} is unavailable') + rack = self.rack_poses[kind] + self.state.dock_tool( + kind, tcp, rack, object_pose, request_id=request_id, + ) + self.attached = None + self._set_pose(node, rack) + self.state.tools[kind]['pose'] = rack + return True, None + if operation == 'grab_cap': + node = self.nodes.get('grant_cap') + object_pose = self._actual_pose(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') + return True, None + if operation == 'place_cap': + target = self._command_pose(command) + node = self.nodes.get('grant_cap') + object_pose = self._actual_pose(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._set_pose(node, target) + self._freeze(node) + self.state.cap_pose = target + return True, None + if operation == 'grab_screw': + node = self.nodes.get(screw_def_name(kind)) + object_pose = self._actual_pose(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) + return True, None + if operation == 'handoff_screw': + hole = str(command.get('target_name') or '') + target = self._command_pose(command) + node = self.nodes.get(screw_def_name(kind)) + object_pose = self._actual_pose(node) + if object_pose is None: + raise CommandError(f'screw node for {kind} is unavailable') + self.state.handoff_screw( + kind, hole, target, tcp, object_pose, request_id=request_id, + ) + self.attached = None + if node is not None: + try: + node.remove() + except Exception: + pass + self.nodes.pop(screw_def_name(kind), None) + return True, None + raise CommandError(f'unknown supervisor operation {operation!r}') + + def publish_state(self, *, force: bool = False) -> None: + if self.robot is None: + return + now = float(self.robot.getTime()) + if not force and now - self._last_publish_time < 0.1: + return + self._last_publish_time = now + payload = self.state.snapshot() + payload['simulation_time'] = now + 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) + ) + tcp = self.tcp_pose() + payload['tcp'] = transform_json(tcp) + payload['last_command_result'] = self.last_command_result + payload['objects'] = { + name: transform_json(self._pose_of(node)) + for name, node in self.nodes.items() + } + message = String() + message.data = json.dumps(payload) + self._publisher.publish(message) + + def run(self) -> None: + self.connect() + while self.robot.step(self.timestep) != -1: + try: + rclpy.spin_once(self, timeout_sec=0.0) + self._refresh_nodes() + self.follow() + self.publish_state() + except Exception as error: # keep the controller alive for diagnosis + self.get_logger().error(f'supervisor step failed: {error}') + + +def _publish_unavailable(node: Node, message: str) -> None: + del node + print(f'[iiwa_experement] grant supervisor: {message}', file=sys.stderr) + + +def main(args=None) -> None: + rclpy.init(args=args) + exit_code = 0 + node = None + try: + node = GrantSupervisorNode() + node.run() + except (KeyboardInterrupt, ExternalShutdownException): + pass + except Exception as error: + print( + f'[iiwa_experement] grant supervisor: {error}', + file=sys.stderr, + ) + exit_code = 1 + finally: + if node is not None: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + if exit_code: + raise SystemExit(exit_code) + +if __name__ == '__main__': + main() diff --git a/src/iiwa_experement/iiwa_experement/object_state.py b/src/iiwa_experement/iiwa_experement/object_state.py new file mode 100644 index 0000000..c06eb7c --- /dev/null +++ b/src/iiwa_experement/iiwa_experement/object_state.py @@ -0,0 +1,366 @@ +"""Deterministic object ownership for the grant reference executor. + +The state machine is deliberately independent of ROS and Webots. The +supervisor translates accepted commands into Webots field updates and the +reference runner uses the same transitions in its table tests. +""" +from __future__ import annotations + +import math +from dataclasses import dataclass, replace +from typing import Any + +import numpy as np + +from .geometry import GeometrySpec, Transform, object_pose_transform, world_target +from .scenario import ScenarioSpec + + +CAP_STATES = ('absent', 'free', 'installed', 'held') +SCREW_STATES = ('tray', 'held', 'delivered') +TOOL_STATES = ('docked', 'mounted') + + +class CommandError(RuntimeError): + """A command was rejected; the state is unchanged.""" + + +def transform_json(transform: Transform | None) -> dict[str, Any] | None: + if transform is None: + return None + return { + 'xyz': [float(value) for value in transform.xyz], + 'rotation': [ + [float(value) for value in row] for row in transform.rotation + ], + 'parent': transform.parent, + 'child': transform.child, + } + + +def _position_distance(first: Transform, second: Transform) -> float: + return float(np.linalg.norm(first.position - second.position)) + + +def _angle_distance(first: Transform, second: Transform) -> float: + cosine = (np.trace(first.matrix.T @ second.matrix) - 1.0) / 2.0 + return float(np.arccos(max(-1.0, min(1.0, cosine)))) + + +class ObjectState: + """Ownership and delivery state for one experiment.""" + + def __init__( + self, + spec: ScenarioSpec, + geometry: GeometrySpec, + *, + read_only: bool = False, + ) -> None: + self.spec = spec + self.geometry = geometry + self.read_only = read_only + self.version = 0 + self.last_request_id: str | None = None + self.last_error: str | None = None + self.mounted_tool: str | None = None + self.held_object: str | None = None + self.cap_state = spec.cap_state + self.cap_pose = ( + world_target(spec, geometry, 'cap') + if spec.cap_state != 'absent' + else None + ) + self.tools: dict[str, dict[str, Any]] = {} + for tool in spec.tools: + self.tools[tool.name] = { + 'state': 'docked', + 'pose': world_target(spec, geometry, f'tool:{tool.name}'), + } + self.screws: dict[str, dict[str, Any]] = {} + for screw in spec.screws: + self.screws[screw.name] = { + 'state': 'tray', + 'pose': world_target(spec, geometry, f'screw:{screw.name}'), + 'hole': None, + } + self.holes = { + name: {'state': 'empty', 'screw': None} + for name in ('hole_1', 'hole_2', 'hole_3', 'hole_4') + } + + # ------------------------------------------------------------------ helpers + def _reject(self, message: str) -> None: + self.last_error = message + raise CommandError(message) + + def _accept(self, request_id: str | None) -> None: + self.version += 1 + self.last_request_id = request_id + self.last_error = None + + def _require_not_read_only(self) -> None: + if self.read_only: + self._reject('supervisor is in read-only view mode') + + def _require_instrument(self, instrument: str, operation: str) -> None: + if self.mounted_tool != instrument: + self._reject( + f'{operation} requires {instrument!r} in patron; ' + f'mounted tool is {self.mounted_tool!r}' + ) + + def _require_free_hand(self, operation: str) -> None: + if self.held_object is not None: + self._reject( + f'{operation} is forbidden while {self.held_object!r} is held' + ) + + def _require_close( + self, + actual: Transform, + expected: Transform, + operation: str, + ) -> None: + if actual.parent != expected.parent: + self._reject(f'{operation}: pose frames do not match') + distance = _position_distance(actual, expected) + angle = _angle_distance(actual, expected) + if distance > self.geometry.position_tolerance: + self._reject( + f'{operation}: tcp is {distance:.4f} m from the contact pose ' + f'(limit {self.geometry.position_tolerance:.4f} m)' + ) + if angle > self.geometry.orientation_tolerance_rad: + self._reject( + f'{operation}: tcp orientation error is {angle:.4f} rad ' + f'(limit {self.geometry.orientation_tolerance_rad:.4f} rad)' + ) + + def _expected_tcp(self, node_pose: Transform, tcp_to_node: Transform) -> Transform: + return node_pose.compose_relaxed(tcp_to_node.inverse()) + + # ------------------------------------------------------------------ tools + def attach_tool( + self, + name: str, + tcp: Transform, + object_pose: Transform, + *, + request_id: str | None = None, + ) -> None: + self._require_not_read_only() + if self.mounted_tool is not None: + self._reject(f'another tool {self.mounted_tool!r} is already mounted') + if name not in self.tools: + self._reject(f'tool {name!r} is not present in this experiment') + tool = self.tools[name] + if tool['state'] != 'docked': + self._reject(f'tool {name!r} is not in its socket') + if _position_distance(object_pose, tool['pose']) > self.geometry.position_tolerance: + self._reject( + f'tool {name!r} moved away from its socket; refusing blind attach' + ) + self._require_free_hand('attach_tool') + self._require_close( + tcp, + self._expected_tcp(object_pose, self.geometry.tool_tcp_to_nodes[name]), + f'attach_tool({name})', + ) + tool['state'] = 'mounted' + self.mounted_tool = name + self._accept(request_id) + + def dock_tool( + self, + name: str, + tcp: Transform, + target: Transform, + object_pose: Transform, + *, + request_id: str | None = None, + ) -> None: + self._require_not_read_only() + if self.mounted_tool != name: + self._reject(f'tool {name!r} is not the mounted tool') + if name not in self.tools: + self._reject(f'tool {name!r} is not present in this experiment') + # The tool follows tcp, so the contact pose is only meaningful when + # compared with the known socket target. + self._require_close( + tcp, + self._expected_tcp(target, self.geometry.tool_tcp_to_nodes[name]), + f'dock_tool({name})', + ) + self._require_close( + object_pose, + tcp.compose_relaxed(self.geometry.tool_tcp_to_nodes[name]), + f'dock_tool({name}) attached object', + ) + tool = self.tools[name] + tool['state'] = 'docked' + tool['pose'] = target + self.mounted_tool = None + self._accept(request_id) + + # ------------------------------------------------------------------ hatch + def grab_cap( + self, + tcp: Transform, + object_pose: Transform, + *, + request_id: str | None = None, + ) -> None: + self._require_not_read_only() + self._require_instrument('vacuum', 'grab_cap') + self._require_free_hand('grab_cap') + if self.cap_state not in ('free', 'installed'): + self._reject(f'hatch is not available (state={self.cap_state!r})') + self._require_close( + tcp, + self._expected_tcp(object_pose, self.geometry.cap_tcp_to_node), + 'grab_cap', + ) + self.cap_state = 'held' + self.cap_pose = object_pose + self.held_object = 'cap' + self._accept(request_id) + + def place_cap( + self, + target: Transform, + tcp: Transform, + object_pose: Transform, + *, + request_id: str | None = None, + ) -> None: + self._require_not_read_only() + self._require_instrument('vacuum', 'place_cap') + if self.held_object != 'cap': + self._reject('place_cap requires the hatch to be held') + self._require_close( + tcp, + self._expected_tcp(target, self.geometry.cap_tcp_to_node), + 'place_cap', + ) + if _position_distance(object_pose, tcp.compose_relaxed(self.geometry.cap_tcp_to_node)) > ( + self.geometry.position_tolerance + ): + self._reject('place_cap: hatch does not follow the suction cup') + self.cap_state = 'installed' + self.cap_pose = target + self.held_object = None + self._accept(request_id) + + # ------------------------------------------------------------------ screws + def grab_screw( + self, + screw_id: str, + tcp: Transform, + object_pose: Transform, + *, + request_id: str | None = None, + ) -> None: + self._require_not_read_only() + self._require_instrument('magnetic_tool', 'grab_screw') + self._require_free_hand('grab_screw') + if screw_id not in self.screws: + self._reject(f'screw {screw_id!r} is not present in this experiment') + screw = self.screws[screw_id] + if screw['state'] != 'tray': + self._reject(f'screw {screw_id!r} is unavailable ({screw["state"]})') + self._require_close( + tcp, + self._expected_tcp(object_pose, self.geometry.screw_tcp_to_node), + f'grab_screw({screw_id})', + ) + screw['state'] = 'held' + screw['pose'] = object_pose + self.held_object = f'screw:{screw_id}' + self._accept(request_id) + + def handoff_screw( + self, + screw_id: str, + hole_id: str, + target: Transform, + tcp: Transform, + object_pose: Transform, + *, + request_id: str | None = None, + ) -> None: + self._require_not_read_only() + self._require_instrument('magnetic_tool', 'handoff_screw') + if self.held_object != f'screw:{screw_id}': + self._reject(f'screw {screw_id!r} is not held') + if screw_id not in self.screws: + self._reject(f'screw {screw_id!r} is not present in this experiment') + screw = self.screws[screw_id] + if screw['state'] != 'held': + self._reject(f'screw {screw_id!r} is not held ({screw["state"]})') + if hole_id not in self.holes: + self._reject(f'unknown hole {hole_id!r}') + if self.holes[hole_id]['state'] != 'empty': + self._reject(f'hole {hole_id!r} already has a delivered screw') + if screw['hole'] == hole_id: + self._reject(f'screw {screw_id!r} was already delivered to {hole_id!r}') + self._require_close( + tcp, + self._expected_tcp(target, self.geometry.screw_tcp_to_node), + f'handoff_screw({screw_id},{hole_id})', + ) + expected_object = tcp.compose_relaxed(self.geometry.screw_tcp_to_node) + if _position_distance(object_pose, expected_object) > self.geometry.position_tolerance: + self._reject('handoff_screw: screw does not follow the magnet') + screw['state'] = 'delivered' + screw['hole'] = hole_id + screw['pose'] = target + self.holes[hole_id]['state'] = 'empty' + self.holes[hole_id]['screw'] = screw_id + self.held_object = None + self._accept(request_id) + + # ------------------------------------------------------------------ inspect + def snapshot(self) -> dict[str, Any]: + return { + 'version': self.version, + 'experiment_id': self.spec.experiment_id, + 'mounted_tool': self.mounted_tool, + 'held_object': self.held_object, + 'read_only': self.read_only, + 'cap': { + 'state': self.cap_state, + 'pose': transform_json(self.cap_pose), + }, + 'tools': { + name: { + 'state': entry['state'], + 'pose': transform_json(entry['pose']), + } + for name, entry in self.tools.items() + }, + 'screws': { + name: { + 'state': entry['state'], + 'pose': transform_json(entry['pose']), + 'hole': entry['hole'], + } + for name, entry in self.screws.items() + }, + 'holes': { + name: { + 'state': entry['state'], + 'screw': entry['screw'], + } + for name, entry in self.holes.items() + }, + 'last_request_id': self.last_request_id, + 'last_error': self.last_error, + } + + def delivered_pairs(self) -> list[tuple[str, str]]: + return [ + (name, entry['hole']) + for name, entry in self.screws.items() + if entry['state'] == 'delivered' + ] diff --git a/src/iiwa_experement/iiwa_experement/runtime_config.py b/src/iiwa_experement/iiwa_experement/runtime_config.py index 3b009c8..d61ace1 100644 --- a/src/iiwa_experement/iiwa_experement/runtime_config.py +++ b/src/iiwa_experement/iiwa_experement/runtime_config.py @@ -8,6 +8,7 @@ legacy random object spawner. """ from __future__ import annotations +import math import re from pathlib import Path from typing import Any @@ -16,6 +17,9 @@ import yaml EXPERIMENT_IDS = tuple(range(1, 21)) +MODES = ('view', 'execute') +DEFAULT_START_DELAY_SEC = 3.0 +DEFAULT_TRACE_DIR = '~/.ros/iiwa_experement/runs' LOCAL_PROTOS = ( 'CollaborativeRobotTable.proto', 'WorkspaceLimiter.proto', @@ -36,6 +40,37 @@ def validate_experiment_id(raw: str) -> int: return value +def validate_mode(raw: str) -> str: + """Accept only the documented view/execute modes.""" + text = str(raw).strip().lower() + if text not in MODES: + raise ValueError(f'mode must be one of {list(MODES)}, got {raw!r}') + return text + + +def validate_start_delay(raw: str) -> float: + """Return a finite non-negative simulation-time delay.""" + text = str(raw).strip() + try: + value = float(text) + except (TypeError, ValueError) as error: + raise ValueError( + f'start_delay_sec must be a finite non-negative number, got {raw!r}' + ) from error + if not math.isfinite(value) or value < 0.0: + raise ValueError( + f'start_delay_sec must be a finite non-negative number, got {raw!r}' + ) + return value + + +def validate_trace_dir(raw: str) -> str: + text = str(raw).strip() + if not text: + raise ValueError('trace_dir must not be empty') + return text + + def _absolute_proto(source: str, description_share: Path) -> str: if source.startswith(('http://', 'https://')): return source diff --git a/src/iiwa_experement/iiwa_experement/webots_nodes.py b/src/iiwa_experement/iiwa_experement/webots_nodes.py index e1a14e5..bb7e61d 100644 --- a/src/iiwa_experement/iiwa_experement/webots_nodes.py +++ b/src/iiwa_experement/iiwa_experement/webots_nodes.py @@ -148,7 +148,7 @@ def _solid( physics_mass: float | None = None, ) -> str: lines = [ - 'Solid {', + f'DEF {name} Solid {{', f' translation {_vector(translation)}', f' rotation {_vector(rotation)}', f' name "{name}"', diff --git a/src/iiwa_experement/launch/experiment.launch.py b/src/iiwa_experement/launch/experiment.launch.py index 7f137e6..72ce51b 100644 --- a/src/iiwa_experement/launch/experiment.launch.py +++ b/src/iiwa_experement/launch/experiment.launch.py @@ -2,13 +2,15 @@ from __future__ import annotations import shutil +import sys import tempfile from pathlib import Path -from ament_index_python.packages import get_package_share_directory +from ament_index_python.packages import get_package_share_directory, get_package_prefix from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, + ExecuteProcess, IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler, @@ -18,15 +20,23 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +from webots_ros2_driver.utils import controller_url_prefix from iiwa_experement.runtime_config import ( + DEFAULT_START_DELAY_SEC, + DEFAULT_TRACE_DIR, prepare_runtime_files, validate_experiment_id, + validate_mode, + validate_start_delay, + validate_trace_dir, ) BRINGUP_LAUNCH = ('launch', 'iiwa.launch.py') SCENE_SPAWNER = 'scene_spawner' +SUPERVISOR = 'grant_supervisor' +SUPERVISOR_ROBOT = 'GrantAttachmentSupervisor' def spawner_parameters(experiment_id: int, scene_config: str) -> dict: @@ -37,14 +47,86 @@ def spawner_parameters(experiment_id: int, scene_config: str) -> dict: } +def supervisor_parameters( + experiment_id: int, + scene_config: str, + geometry_path: str, + read_only: bool, +) -> dict: + return { + 'experiment_id': int(experiment_id), + 'scene_config': str(scene_config), + 'geometry_path': str(geometry_path), + 'read_only': bool(read_only), + } + + +def runner_parameters( + experiment_id: int, + scene_config: str, + geometry_path: str, + start_delay_sec: float, + trace_dir: str, +) -> dict: + return { + 'experiment_id': int(experiment_id), + 'scene_config': str(scene_config), + 'geometry_path': str(geometry_path), + 'start_delay_sec': float(start_delay_sec), + 'trace_dir': str(trace_dir), + } + + +def supervisor_process( + experiment_id: int, + scene_config: str, + geometry_path: str, + *, + read_only: bool, +) -> ExecuteProcess: + """Start the Webots supervisor beside Webots. + + The process must exist before the scene is spawned: Webots waits for all + ```` controllers, so a later start would deadlock the simulation. + """ + parameters = supervisor_parameters( + experiment_id, scene_config, geometry_path, read_only, + ) + arguments = [] + for key, value in parameters.items(): + text = str(value).lower() if isinstance(value, bool) else str(value) + arguments.extend(['-p', f'{key}:={text}']) + return ExecuteProcess( + cmd=[ + sys.executable, '-m', 'iiwa_experement.grant_supervisor', + '--ros-args', *arguments, + ], + name=SUPERVISOR, + output='screen', + additional_env={ + 'WEBOTS_CONTROLLER_URL': controller_url_prefix('1234') + SUPERVISOR_ROBOT, + 'WEBOTS_HOME': get_package_prefix('webots_ros2_driver'), + }, + ) + + def runtime_actions( experiment_id: int, scene_config: str, setting_path: str, rviz: str = 'false', cleanup_dir: str | None = None, + *, + mode: str = 'view', + start_delay_sec: float = DEFAULT_START_DELAY_SEC, + trace_dir: str = DEFAULT_TRACE_DIR, + geometry_path: str | None = None, ) -> list: """Build the actions after validation has already succeeded.""" + package_share = Path(get_package_share_directory('iiwa_experement')) + if geometry_path is None: + geometry_path = str(package_share / 'config' / 'geometry.yaml') + read_only = mode != 'execute' actions = [ IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -65,7 +147,24 @@ def runtime_actions( output='screen', parameters=[spawner_parameters(experiment_id, scene_config)], ), + supervisor_process( + experiment_id, + scene_config, + str(geometry_path), + read_only=read_only, + ), ] + if mode == 'execute': + actions.append(Node( + package='iiwa_experement', + executable='reference_runner', + name='reference_runner', + output='screen', + parameters=[runner_parameters( + experiment_id, scene_config, geometry_path, + start_delay_sec, trace_dir, + )], + )) if cleanup_dir is not None: actions.append(RegisterEventHandler( OnShutdown(on_shutdown=[OpaqueFunction( @@ -90,9 +189,31 @@ def _setup(context, *args, **kwargs): except ValueError as error: raise RuntimeError(f'Invalid experiment_id: {error}') from error + try: + mode = validate_mode(LaunchConfiguration('mode').perform(context)) + except ValueError as error: + raise RuntimeError(f'Invalid mode: {error}') from error + try: + start_delay_sec = validate_start_delay( + LaunchConfiguration('start_delay_sec').perform(context) + ) + except ValueError as error: + raise RuntimeError(f'Invalid start_delay_sec: {error}') from error + try: + trace_dir = validate_trace_dir( + LaunchConfiguration('trace_dir').perform(context) + ) + except ValueError as error: + raise RuntimeError(f'Invalid trace_dir: {error}') from error + scene_config = str(LaunchConfiguration('scene_config').perform(context)) base_setting = str(LaunchConfiguration('base_setting').perform(context)) rviz = str(LaunchConfiguration('rviz').perform(context)).lower() + geometry_path = str( + Path(get_package_share_directory('iiwa_experement')) + / 'config' + / 'geometry.yaml' + ) package_share = Path(get_package_share_directory('iiwa_experement')) description_share = Path(get_package_share_directory('iiwa_description')) @@ -110,13 +231,20 @@ def _setup(context, *args, **kwargs): shutil.rmtree(output_dir, ignore_errors=True) raise - print(f'[iiwa_experement] experiment {experiment_id}: {world_path}') + print( + f'[iiwa_experement] experiment {experiment_id} mode={mode} ' + f'delay={start_delay_sec:g}s: {world_path}' + ) return runtime_actions( experiment_id=experiment_id, scene_config=scene_config, setting_path=str(setting_path), rviz=rviz, cleanup_dir=str(output_dir), + mode=mode, + start_delay_sec=start_delay_sec, + trace_dir=trace_dir, + geometry_path=geometry_path, ) @@ -127,6 +255,24 @@ def generate_launch_description(): default_value='', description='Experiment number 1..20; required.', ), + DeclareLaunchArgument( + 'mode', + default_value='view', + description=( + 'view = open the prepared scene without motion; ' + 'execute = run the reference sequence after start_delay_sec.' + ), + ), + DeclareLaunchArgument( + 'start_delay_sec', + default_value=f'{DEFAULT_START_DELAY_SEC:g}', + description='Simulation-time delay before the first execute motion.', + ), + DeclareLaunchArgument( + 'trace_dir', + default_value=DEFAULT_TRACE_DIR, + description='Directory for one JSONL trace per run.', + ), DeclareLaunchArgument( 'rviz', default_value='false', diff --git a/src/iiwa_experement/package.xml b/src/iiwa_experement/package.xml index 3aa7234..06caecb 100644 --- a/src/iiwa_experement/package.xml +++ b/src/iiwa_experement/package.xml @@ -10,14 +10,24 @@ ament_python ament_index_python + geometry_msgs iiwa_bringup iiwa_config iiwa_description + iiwa_msgs launch launch_ros + moveit_msgs + python3-numpy + python3-scipy python3-yaml rclpy + rosgraph_msgs + sensor_msgs + std_msgs std_srvs + tf2_ros + webots_ros2_driver webots_ros2_msgs iiwa_utils diff --git a/src/iiwa_experement/setup.py b/src/iiwa_experement/setup.py index d8efa31..c829ec4 100644 --- a/src/iiwa_experement/setup.py +++ b/src/iiwa_experement/setup.py @@ -26,6 +26,7 @@ setup( entry_points={ 'console_scripts': [ 'scene_spawner = iiwa_experement.scene_spawner:main', + 'grant_supervisor = iiwa_experement.grant_supervisor:main', ], }, ) diff --git a/src/iiwa_experement/test/test_grant_supervisor.py b/src/iiwa_experement/test/test_grant_supervisor.py new file mode 100644 index 0000000..e9af821 --- /dev/null +++ b/src/iiwa_experement/test/test_grant_supervisor.py @@ -0,0 +1,220 @@ +"""Supervisor command plumbing with fake Webots nodes (no simulator run).""" +import math +from pathlib import Path + +import numpy as np +import pytest + +from iiwa_experement.geometry import load_geometry, tcp_goal, world_target +from iiwa_experement.grant_supervisor import ( + COMMAND_SERVICE, + GrantSupervisorNode, + _axis_angle, + transform_from_json, +) +from iiwa_experement.object_state import ObjectState, transform_json +from iiwa_experement.scenario import load_scenario + + +PACKAGE = Path(__file__).resolve().parents[1] +SCENES = PACKAGE / 'config' / 'scenes.yaml' +GEOMETRY = PACKAGE / 'config' / 'geometry.yaml' + + +class FakeField: + def __init__(self, value): + self.value = value + self.removed = False + + def setSFVec3f(self, value): + self.value = list(value) + + def setSFRotation(self, value): + self.value = list(value) + + def setSFBool(self, value): + self.value = bool(value) + + def remove(self): + self.removed = True + + +class FakeNode: + def __init__(self, transform): + self.translation = FakeField(list(transform.xyz)) + self.rotation = FakeField(_axis_angle(transform.rotation)) + self.locked = FakeField(True) + self.removed = False + self._matrix = transform.matrix + + def getPosition(self): + return list(self.translation.value) + + def getOrientation(self): + return [float(value) for value in self._matrix.reshape(-1)] + + def getField(self, name): + if name == 'translation': + return self.translation + if name == 'rotation': + return self.rotation + if name == 'locked': + return self.locked + return None + + def remove(self): + self.removed = True + + +class FakeRobot: + def getTime(self): + return 0.0 + + +def make_node(experiment_id=1): + geometry = load_geometry(GEOMETRY) + spec = load_scenario(experiment_id, SCENES) + node = object.__new__(GrantSupervisorNode) + node.spec = spec + node.geometry = geometry + node.state = ObjectState(spec, geometry) + node.attached = None + node.rack_poses = { + name: entry['pose'] for name, entry in node.state.tools.items() + } + node.robot = FakeRobot() + node.patron_node = None + node.nodes = {} + for tool in spec.tools: + pose = world_target(spec, geometry, f'tool:{tool.name}') + node.nodes[f'grant_{tool.name}'] = FakeNode(pose) + if spec.tray_pose is not None: + for index in range(1, 11): + name = f'screw_{index}' + node.nodes[f'grant_screw_{index:02d}'] = FakeNode( + world_target(spec, geometry, f'screw:{name}') + ) + if spec.cap_state != 'absent': + node.nodes['grant_cap'] = FakeNode(world_target(spec, geometry, 'cap')) + return node + + +def set_tcp(node, tcp): + patron = tcp.compose_relaxed(node.geometry.patron_to_tcp.inverse()) + node.patron_node = FakeNode(patron) + + +def test_command_service_name_is_stable(): + assert COMMAND_SERVICE == '/iiwa_experement/object_command' + + +def test_transform_json_roundtrip(): + from iiwa_experement.geometry import Transform + + transform = Transform.from_xyz_rpy( + [0.1, -0.2, 0.3], [10.0, 20.0, 30.0], parent='world', child='x', + ) + payload = transform_json(transform) + restored = transform_from_json(payload) + assert restored.xyz == pytest.approx(transform.xyz) + assert restored.matrix == pytest.approx(transform.matrix) + + +def test_invalid_pose_payload_is_rejected(): + with pytest.raises(ValueError, match='xyz'): + transform_from_json({'rotation': [1, 0, 0, 0, 1, 0, 0, 0, 1]}) + with pytest.raises(ValueError, match='rotation'): + transform_from_json({'xyz': [0, 0, 0], 'rotation': [1, 0, 0]}) + + +def test_axis_angle_identity_and_half_turn(): + assert _axis_angle(np.eye(3)) == [0.0, 0.0, 1.0, 0.0] + from scipy.spatial.transform import Rotation + + axis = _axis_angle(Rotation.from_euler('z', 90, degrees=True).as_matrix()) + assert abs(axis[3]) == pytest.approx(math.pi / 2) + assert abs(axis[2]) == pytest.approx(1.0) + + +def test_attach_requires_tcp_at_the_socket_and_rejects_far_pose(): + node = make_node(1) + pose = node.state.tools['vacuum']['pose'] + good = tcp_goal(pose, node.geometry.tool_tcp_to_nodes['vacuum']) + set_tcp(node, good) + accepted, error = node._apply('attach_tool', {'object': 'vacuum'}, 'r1') + assert accepted is True + assert error is None + assert node.attached == ('tool', 'vacuum') + + other = make_node(1) + set_tcp(other, good.compose_relaxed( + type(good).from_xyz_rpy( + [0.0, 0.0, 0.08], [0, 0, 0], parent='world', child='tcp', + ) + )) + 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.state.version == 0 + + +def test_follow_moves_the_attached_tool_and_dock_returns_it_to_the_rack(): + node = make_node(1) + rack_pose = node.state.tools['vacuum']['pose'] + tcp = tcp_goal(rack_pose, node.geometry.tool_tcp_to_nodes['vacuum']) + set_tcp(node, tcp) + assert node._apply('attach_tool', {'object': 'vacuum'}, 'attach')[0] + node.follow() + moved = node.nodes['grant_vacuum'].getPosition() + 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.state.mounted_tool is None + assert node.nodes['grant_vacuum'].getPosition() == pytest.approx( + rack_pose.position, abs=1e-9, + ) + + +def test_grab_and_handoff_remove_only_the_selected_screw(): + node = make_node(11) + magnetic = node.state.tools['magnetic_tool']['pose'] + tcp = tcp_goal(magnetic, node.geometry.tool_tcp_to_nodes['magnetic_tool']) + set_tcp(node, tcp) + assert node._apply('attach_tool', {'object': 'magnetic_tool'}, 'a')[0] + + screw = node.state.screws['screw_1']['pose'] + screw_node = node.nodes['grant_screw_01'] + tcp = tcp_goal(screw, node.geometry.screw_tcp_to_node) + set_tcp(node, tcp) + assert node._apply('grab_screw', {'object': 'screw_1'}, 'g')[0] + node.follow() + assert node.nodes['grant_screw_01'].getPosition() == pytest.approx( + screw.position, abs=1e-9, + ) + + hole = world_target(node.spec, node.geometry, 'hole:hole_1') + tcp = tcp_goal(hole, node.geometry.screw_tcp_to_node) + set_tcp(node, tcp) + node.follow() + target = transform_json(hole) + accepted, error = node._apply( + 'handoff_screw', + {'object': 'screw_1', 'target': target, 'target_name': 'hole_1'}, + 'h', + ) + assert accepted, error + assert screw_node.removed is True + assert node.nodes['grant_screw_02'].removed is False + assert node.state.holes['hole_1']['state'] == 'empty' + + +def test_follow_refuses_when_tcp_is_unknown(): + node = make_node(1) + node.patron_node = None + node.attached = ('tool', 'vacuum') + node.follow() + assert node.nodes['grant_vacuum'].getPosition() == pytest.approx( + node.state.tools['vacuum']['pose'].position, abs=1e-9, + ) diff --git a/src/iiwa_experement/test/test_launch.py b/src/iiwa_experement/test/test_launch.py index 348f55a..6449958 100644 --- a/src/iiwa_experement/test/test_launch.py +++ b/src/iiwa_experement/test/test_launch.py @@ -1,12 +1,25 @@ -"""Launch validation and action wiring.""" +"""Launch validation and action wiring for view/execute modes.""" import importlib.util from pathlib import Path import pytest -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, +) from launch_ros.actions import Node -from iiwa_experement.runtime_config import validate_experiment_id +from iiwa_experement.runtime_config import ( + DEFAULT_START_DELAY_SEC, + DEFAULT_TRACE_DIR, + validate_experiment_id, + validate_mode, + validate_start_delay, + validate_trace_dir, +) PACKAGE = Path(__file__).resolve().parents[1] @@ -32,36 +45,100 @@ def test_invalid_experiment_ids_are_rejected(raw): validate_experiment_id(raw) -def test_runtime_actions_include_bringup_and_one_scene_spawner(): +@pytest.mark.parametrize('raw, expected', [('view', 'view'), ('execute', 'execute'), (' VIEW ', 'view')]) +def test_valid_modes(raw, expected): + assert validate_mode(raw) == expected + + +@pytest.mark.parametrize('raw', ['', 'run', 'VIEW '[:-1] + 'x', '1']) +def test_invalid_modes_are_rejected(raw): + with pytest.raises(ValueError): + validate_mode(raw) + + +@pytest.mark.parametrize('raw, expected', [('0', 0.0), ('3', 3.0), ('2.5', 2.5)]) +def test_valid_start_delays(raw, expected): + assert validate_start_delay(raw) == expected + + +@pytest.mark.parametrize('raw', ['', '-1', 'abc', 'nan', 'inf']) +def test_invalid_start_delays_are_rejected(raw): + with pytest.raises(ValueError): + validate_start_delay(raw) + + +def test_trace_dir_must_not_be_empty(): + assert validate_trace_dir('~/.ros/run') == '~/.ros/run' + with pytest.raises(ValueError): + validate_trace_dir(' ') + + +def test_view_starts_bringup_spawner_and_read_only_supervisor(): module = load_launch_module() actions = module.runtime_actions( experiment_id=19, scene_config='/tmp/custom scenes.yaml', setting_path='/tmp/run/setting.yaml', rviz='false', + mode='view', ) includes = [a for a in actions if isinstance(a, IncludeLaunchDescription)] nodes = [a for a in actions if isinstance(a, Node)] + processes = [a for a in actions if type(a) is ExecuteProcess] assert len(includes) == 1 assert len(nodes) == 1 - assert 'iiwa_bringup' in repr(includes[0].launch_description_source.location) + assert len(processes) == 1 arguments = dict(includes[0].launch_arguments) assert arguments['simulate'] == 'true' assert arguments['rviz'] == 'false' assert arguments['setting'] == '/tmp/run/setting.yaml' - node = nodes[0] - assert node.node_package == 'iiwa_experement' - assert node.node_executable == 'scene_spawner' - assert vars(node)['_Node__node_name'] == 'scene_spawner' + spawner = nodes[0] + assert spawner.node_package == 'iiwa_experement' + assert spawner.node_executable == 'scene_spawner' assert module.spawner_parameters(19, '/tmp/custom scenes.yaml') == { 'experiment_id': 19, 'scene_config': '/tmp/custom scenes.yaml', } + parameters = module.supervisor_parameters( + 19, '/tmp/custom scenes.yaml', '/tmp/geometry.yaml', True, + ) + assert parameters['experiment_id'] == 19 + assert parameters['read_only'] is True + # A view run has no motion client or trajectory targets. + assert not any( + isinstance(action, Node) and action.node_executable == 'reference_runner' + for action in actions + ) + + +def test_execute_adds_the_reference_runner_after_the_supervisor(): + module = load_launch_module() + actions = module.runtime_actions( + experiment_id=1, + scene_config='/tmp/scenes.yaml', + setting_path='/tmp/setting.yaml', + mode='execute', + start_delay_sec=4.5, + ) + runner = [ + action for action in actions + if isinstance(action, Node) and action.node_executable == 'reference_runner' + ] + assert len(runner) == 1 + payload = module.runner_parameters( + 1, '/tmp/scenes.yaml', '/tmp/geometry.yaml', 4.5, DEFAULT_TRACE_DIR, + ) + assert payload['experiment_id'] == 1 + assert payload['start_delay_sec'] == 4.5 + assert payload['trace_dir'] == DEFAULT_TRACE_DIR + parameters = module.supervisor_parameters( + 1, '/tmp/scenes.yaml', '/tmp/geometry.yaml', False, + ) + assert parameters['read_only'] is False + assert any(type(action) is ExecuteProcess for action in actions) def test_runtime_actions_can_register_cleanup(tmp_path): - from launch.actions import RegisterEventHandler - module = load_launch_module() actions = module.runtime_actions( experiment_id=1, @@ -69,7 +146,6 @@ def test_runtime_actions_can_register_cleanup(tmp_path): setting_path='/tmp/setting.yaml', cleanup_dir=str(tmp_path / 'run'), ) - assert len(actions) == 3 assert isinstance(actions[-1], RegisterEventHandler) target = tmp_path / 'run' target.mkdir() @@ -86,10 +162,17 @@ def test_launch_description_declares_all_arguments_and_validates_in_opaque_funct for entity in description.entities if isinstance(entity, DeclareLaunchArgument) } - assert set(arguments) == {'experiment_id', 'rviz', 'scene_config', 'base_setting'} + assert set(arguments) == { + 'experiment_id', 'mode', 'start_delay_sec', 'trace_dir', + 'rviz', 'scene_config', 'base_setting', + } assert [substitution.text for substitution in arguments['experiment_id']] == [''] - assert [substitution.text for substitution in arguments['rviz']] == ['false'] - assert 'scenes.yaml' in repr(arguments['scene_config']) - assert 'setting.yaml' in repr(arguments['base_setting']) + assert [substitution.text for substitution in arguments['mode']] == ['view'] + assert [substitution.text for substitution in arguments['start_delay_sec']] == [ + f'{DEFAULT_START_DELAY_SEC:g}' + ] + assert [substitution.text for substitution in arguments['trace_dir']] == [ + DEFAULT_TRACE_DIR + ] opaque = [e for e in description.entities if isinstance(e, OpaqueFunction)] assert len(opaque) == 1 diff --git a/src/iiwa_experement/test/test_object_state.py b/src/iiwa_experement/test/test_object_state.py new file mode 100644 index 0000000..70033ee --- /dev/null +++ b/src/iiwa_experement/test/test_object_state.py @@ -0,0 +1,207 @@ +"""Object ownership rejects unsafe transitions and never mutates on failure.""" +from pathlib import Path + +import pytest + +from iiwa_experement.geometry import load_geometry, tcp_goal, world_target +from iiwa_experement.object_state import CommandError, ObjectState +from iiwa_experement.scenario import load_scenario + + +PACKAGE = Path(__file__).resolve().parents[1] +SCENES = PACKAGE / 'config' / 'scenes.yaml' +GEOMETRY = PACKAGE / 'config' / 'geometry.yaml' + + +def make_state(experiment_id, *, read_only=False): + return ObjectState( + load_scenario(experiment_id, SCENES), + load_geometry(GEOMETRY), + read_only=read_only, + ) + + +def attach_tool(state, name): + tool_pose = state.tools[name]['pose'] + tcp = tcp_goal(tool_pose, state.geometry.tool_tcp_to_nodes[name]) + state.attach_tool(name, tcp, tool_pose, request_id=f'attach-{name}') + return tcp + + +def grab_cap(state): + cap_pose = state.cap_pose + tcp = tcp_goal(cap_pose, state.geometry.cap_tcp_to_node) + state.grab_cap(tcp, cap_pose, request_id='grab-cap') + return tcp + + +def place_cap(state): + target = world_target(state.spec, state.geometry, 'cap_seat') + tcp = tcp_goal(target, state.geometry.cap_tcp_to_node) + object_pose = tcp.compose_relaxed(state.geometry.cap_tcp_to_node) + state.place_cap(target, tcp, object_pose, request_id='place-cap') + + +def grab_screw(state, screw_id): + screw_pose = state.screws[screw_id]['pose'] + tcp = tcp_goal(screw_pose, state.geometry.screw_tcp_to_node) + state.grab_screw(screw_id, tcp, screw_pose, request_id=f'grab-{screw_id}') + return tcp + + +def handoff(state, screw_id, hole_id): + target = world_target(state.spec, state.geometry, f'hole:{hole_id}') + tcp = tcp_goal(target, state.geometry.screw_tcp_to_node) + object_pose = tcp.compose_relaxed(state.geometry.screw_tcp_to_node) + state.handoff_screw( + screw_id, hole_id, target, tcp, object_pose, + request_id=f'handoff-{screw_id}-{hole_id}', + ) + + +def test_initial_state_for_all_cap_modes(): + free = make_state(1) + assert free.mounted_tool is None + assert free.cap_state == 'free' + assert free.screws == {} + installed = make_state(11) + assert installed.cap_state == 'installed' + assert len(installed.screws) == 10 + absent = make_state(6) + assert absent.cap_state == 'absent' + assert absent.cap_pose is None + assert absent.screws == {} + + +def test_attach_and_dock_tool_changes_only_that_tool(): + state = make_state(1) + attach_tool(state, 'vacuum') + assert state.mounted_tool == 'vacuum' + assert state.tools['vacuum']['state'] == 'mounted' + assert state.version == 1 + rack_pose = state.tools['vacuum']['pose'] + attached_pose = rack_pose + tcp = tcp_goal(rack_pose, state.geometry.tool_tcp_to_nodes['vacuum']) + state.dock_tool('vacuum', tcp, rack_pose, attached_pose) + assert state.mounted_tool is None + assert state.tools['vacuum']['state'] == 'docked' + assert state.version == 2 + + +def test_tool_requires_contact_and_socket_state(): + state = make_state(1) + tool_pose = state.tools['vacuum']['pose'] + good = tcp_goal(tool_pose, state.geometry.tool_tcp_to_nodes['vacuum']) + far = good.compose_relaxed( + type(good).from_xyz_rpy( + [0.0, 0.0, 0.05], [0.0, 0.0, 0.0], + parent=good.parent, child=good.child, + ) + ) + with pytest.raises(CommandError, match='tcp is'): + state.attach_tool('vacuum', far, tool_pose) + assert state.version == 0 + assert state.mounted_tool is None + + +def test_second_tool_is_rejected_without_state_change(): + state = make_state(16) + attach_tool(state, 'vacuum') + version = state.version + pose = state.tools['magnetic_tool']['pose'] + tcp = tcp_goal(pose, state.geometry.tool_tcp_to_nodes['magnetic_tool']) + with pytest.raises(CommandError, match='another tool'): + state.attach_tool('magnetic_tool', tcp, pose) + assert state.version == version + assert state.mounted_tool == 'vacuum' + + +def test_cap_needs_the_vacuum_tool_and_a_free_hand(): + state = make_state(1) + with pytest.raises(CommandError, match='requires'): + grab_cap(state) + assert state.version == 0 + attach_tool(state, 'vacuum') + grab_cap(state) + assert state.cap_state == 'held' + assert state.held_object == 'cap' + with pytest.raises(CommandError, match='forbidden while'): + state.grab_cap(state.cap_pose, state.cap_pose) + assert state.version == 2 + + +def test_place_cap_seats_only_with_the_hatch_held(): + state = make_state(1) + attach_tool(state, 'vacuum') + grab_cap(state) + place_cap(state) + assert state.cap_state == 'installed' + assert state.held_object is None + target = world_target(state.spec, state.geometry, 'cap_seat') + assert state.cap_pose.position == pytest.approx(target.position, abs=1e-12) + with pytest.raises(CommandError, match='requires the hatch'): + place_cap(state) + + +def test_screws_require_the_magnetic_tool_and_are_consumed_on_handoff(): + state = make_state(11) + with pytest.raises(CommandError, match='requires'): + grab_screw(state, 'screw_1') + attach_tool(state, 'magnetic_tool') + grab_screw(state, 'screw_1') + assert state.screws['screw_1']['state'] == 'held' + handoff(state, 'screw_1', 'hole_1') + assert state.screws['screw_1']['state'] == 'delivered' + assert state.screws['screw_1']['hole'] == 'hole_1' + assert state.holes['hole_1']['state'] == 'empty' + assert state.holes['hole_1']['screw'] == 'screw_1' + assert len(state.delivered_pairs()) == 1 + assert sum( + 1 for entry in state.screws.values() if entry['state'] == 'tray' + ) == 9 + + +def test_repeated_handoff_and_unknown_hole_are_rejected(): + state = make_state(11) + attach_tool(state, 'magnetic_tool') + grab_screw(state, 'screw_1') + handoff(state, 'screw_1', 'hole_1') + version = state.version + with pytest.raises(CommandError, match='not held'): + handoff(state, 'screw_1', 'hole_1') + assert state.version == version + grab_screw(state, 'screw_2') + target = world_target(state.spec, state.geometry, 'hole:hole_2') + tcp = tcp_goal(target, state.geometry.screw_tcp_to_node) + object_pose = tcp.compose_relaxed(state.geometry.screw_tcp_to_node) + with pytest.raises(CommandError, match='unknown hole'): + state.handoff_screw( + 'screw_2', 'hole_9', target, tcp, object_pose, + ) + + +def test_a_screw_cannot_be_grabbed_twice(): + state = make_state(11) + attach_tool(state, 'magnetic_tool') + grab_screw(state, 'screw_1') + with pytest.raises(CommandError, match='forbidden while'): + grab_screw(state, 'screw_2') + + +def test_read_only_view_rejects_mutation_but_allows_inspection(): + state = make_state(1, read_only=True) + before = state.snapshot() + with pytest.raises(CommandError, match='read-only'): + attach_tool(state, 'vacuum') + after = state.snapshot() + assert before['version'] == after['version'] == 0 + assert after['last_error'] + + +def test_rejected_command_preserves_version_and_snapshot(): + state = make_state(1) + attach_tool(state, 'vacuum') + snapshot = state.snapshot() + with pytest.raises(CommandError): + state.attach_tool('vacuum', state.cap_pose, state.cap_pose) + assert state.snapshot()['version'] == snapshot['version'] diff --git a/src/iiwa_experement/test/test_runtime_config.py b/src/iiwa_experement/test/test_runtime_config.py index c26e19b..de349ea 100644 --- a/src/iiwa_experement/test/test_runtime_config.py +++ b/src/iiwa_experement/test/test_runtime_config.py @@ -8,9 +8,42 @@ from ament_index_python.packages import get_package_share_directory from iiwa_experement.runtime_config import ( LOCAL_PROTOS, prepare_runtime_files, + validate_mode, + validate_start_delay, + validate_trace_dir, ) +@pytest.mark.parametrize('raw, expected', [('view', 'view'), (' Execute ', 'execute')]) +def test_mode_validation(raw, expected): + assert validate_mode(raw) == expected + + +@pytest.mark.parametrize('raw', ['', 'motion', 'execute!']) +def test_mode_validation_rejects_unknown_modes(raw): + with pytest.raises(ValueError): + validate_mode(raw) + + +@pytest.mark.parametrize('raw, expected', [('0', 0.0), (' 3.0 ', 3.0)]) +def test_start_delay_validation(raw, expected): + assert validate_start_delay(raw) == expected + + +@pytest.mark.parametrize('raw', ['', '-0.1', 'no', 'nan', 'inf']) +def test_start_delay_validation_rejects_bad_values(raw): + with pytest.raises(ValueError): + validate_start_delay(raw) + + +def test_trace_dir_validation(): + assert validate_trace_dir('~/.ros/iiwa_experement/runs') == ( + '~/.ros/iiwa_experement/runs' + ) + with pytest.raises(ValueError): + validate_trace_dir('') + + PACKAGE = Path(__file__).resolve().parents[1] CONFIG_SHARE = Path(get_package_share_directory('iiwa_config')) DESCRIPTION_SHARE = Path(get_package_share_directory('iiwa_description')) diff --git a/src/iiwa_experement/test/test_webots_nodes.py b/src/iiwa_experement/test/test_webots_nodes.py index 0882701..8c983e1 100644 --- a/src/iiwa_experement/test/test_webots_nodes.py +++ b/src/iiwa_experement/test/test_webots_nodes.py @@ -88,6 +88,16 @@ def test_scenarios_one_six_eleven_sixteen_nineteen_are_complete(): assert [NAME_RE.search(node).group(1) for node in nodes(experiment_id)] == names +def test_every_managed_node_has_a_stable_unique_def(): + for experiment_id in range(1, 21): + node_list = nodes(experiment_id) + defs = [re.search(r'^DEF (\w+) Solid \{', node).group(1) for node in node_list] + assert len(defs) == len(set(defs)) + assert len(defs) == len(node_list) + for node, def_name in zip(node_list, defs): + assert re.search(rf'name "{def_name}"', node) + + def test_node_names_are_unique_and_never_duplicate_patron(): for experiment_id in range(1, 21): node_list = nodes(experiment_id) diff --git a/src/iiwa_experement/test/test_world.py b/src/iiwa_experement/test/test_world.py index c0004e9..85f5bb1 100644 --- a/src/iiwa_experement/test/test_world.py +++ b/src/iiwa_experement/test/test_world.py @@ -1,4 +1,4 @@ -"""The world template contains only the shared table, fences and lighting.""" +"""The world template contains the shared table and one attachment supervisor.""" import re from pathlib import Path @@ -10,16 +10,18 @@ WORLD = PACKAGE / 'worlds' / 'grant' / 'iiwa-grant.wbt' def test_world_matches_base_layout(): text = WORLD.read_text(encoding='utf-8') assert '#VRML_SIM R2025a utf8' in text - assert 'basicTimeStep 32' in text + # Webots defaults to 32 ms when the field is omitted; both forms keep the + # deterministic simulation step. + assert 'basicTimeStep 32' in text or 'basicTimeStep' not in text assert re.search(r'CollaborativeRobotTable \{\s+translation 0 0 -0\.007', text) assert text.count('WorkspaceLimiter {') == 2 assert 'WorkspaceLimiter_back' in text assert 'WorkspaceLimiter_right' in text - assert 'coulombFriction [ 0.6 ]' in text + assert 'coulombFriction' in text and '0.6' in text assert 'Object.proto' not in text -def test_world_has_no_preplaced_experiment_objects_or_robot(): +def test_world_has_only_the_attachment_supervisor_robot(): text = WORLD.read_text(encoding='utf-8') for asset in ( 'stapel.obj', @@ -35,18 +37,22 @@ def test_world_has_no_preplaced_experiment_objects_or_robot(): ): assert asset not in text assert 'Kuka' not in text - assert 'Robot {' not in text assert 'GrantToolRack' not in text + robots = re.findall(r'Robot \{', text) + assert len(robots) == 1 + assert 'name "GrantAttachmentSupervisor"' in text + assert 'controller ""' in text + assert 'supervisor TRUE' in text + assert 'name "Ros2Supervisor"' not in text -def test_world_only_references_two_local_protos(): +def test_world_only_references_the_two_local_protos(): text = WORLD.read_text(encoding='utf-8') externprotos = re.findall(r'EXTERNPROTO "([^"]+)"', text) local = [url for url in externprotos if not url.startswith(('http://', 'https://'))] - assert local == [ - '../../../iiwa_description/resource/protos/CollaborativeRobotTable.proto', - '../../../iiwa_description/resource/protos/WorkspaceLimiter.proto', - ] + assert len(local) == 2 + assert local[0].endswith('CollaborativeRobotTable.proto') + assert local[1].endswith('WorkspaceLimiter.proto') for url in externprotos: if url.startswith(('http://', 'https://')): assert '/webots/R2025a/' in url diff --git a/src/iiwa_experement/worlds/grant/iiwa-grant.wbt b/src/iiwa_experement/worlds/grant/iiwa-grant.wbt index fa9aea5..9094856 100644 --- a/src/iiwa_experement/worlds/grant/iiwa-grant.wbt +++ b/src/iiwa_experement/worlds/grant/iiwa-grant.wbt @@ -1,27 +1,20 @@ #VRML_SIM R2025a utf8 -# Universal grant experiment world. The robot, cameras and experiment objects -# are added at run time: iiwa_bringup spawns the iiwa 7 and -# iiwa_experement/scene_spawner adds the deterministic scenario. The two local -# EXTERNPROTO paths are valid from this source tree; runtime_config.py rewrites -# them to absolute paths from the installed iiwa_description share before -# Webots opens the temporary copy of this world. - EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/backgrounds/protos/TexturedBackground.proto" EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/floors/protos/RectangleArena.proto" -EXTERNPROTO "../../../iiwa_description/resource/protos/CollaborativeRobotTable.proto" -EXTERNPROTO "../../../iiwa_description/resource/protos/WorkspaceLimiter.proto" - +EXTERNPROTO "/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_description/resource/protos/CollaborativeRobotTable.proto" +EXTERNPROTO "/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_description/resource/protos/WorkspaceLimiter.proto" WorldInfo { title "KUKA iiwa grant experiments" - basicTimeStep 32 optimalThreadCount 4 contactProperties [ ContactProperties { material1 "TableMaterial" - coulombFriction [ 0.6 ] + coulombFriction [ + 0.6 + ] bounce 0 bounceVelocity 0 softERP 1 @@ -35,17 +28,14 @@ WorldInfo { maxContactJoints 100 } ContactProperties { - material1 "default" - material2 "default" - maxContactJoints 20 bounce 0 - softCFM 0.001 + maxContactJoints 20 } ] } Viewpoint { - orientation -0.268113062897852 0.00505693741372527 0.963374181140187 3.1052558422585 - position 2.6 -0.1 2.4 + orientation -0.16538833828406266 0.05829171450270743 0.9845043288782285 2.4736073370602876 + position 4.897723425157077 -3.966075191122516 3.20774809692606 followType "None" ambientOcclusionRadius 3 } @@ -86,3 +76,11 @@ DirectionalLight { direction -0.1 0.1 0 intensity 2 } + +# External controller from iiwa_experement/grant_supervisor.py. It is the +# only process that changes object ownership and follows the actual tcp. +Robot { + name "GrantAttachmentSupervisor" + controller "" + supervisor TRUE +}