diff --git a/src/iiwa_experement/config/geometry.yaml b/src/iiwa_experement/config/geometry.yaml index 4267c9b..39f50e1 100644 --- a/src/iiwa_experement/config/geometry.yaml +++ b/src/iiwa_experement/config/geometry.yaml @@ -122,9 +122,12 @@ speeds: # Webots bounding objects so MoveIt and the simulator agree. scene_collisions: table: + # Top at Z = 0.789, deliberately 1 mm below the robot base (Z = 0.790) + # and 4 mm below the work surface (Z = 0.793), so the base is not in + # collision while downward motion is still blocked. type: box size: [1.28, 1.08, 0.045] - center_z: 0.7705 + offset_z: 0.7665 fixture: type: box size: [0.350, 0.220, 0.041] diff --git a/src/iiwa_experement/iiwa_experement/geometry.py b/src/iiwa_experement/iiwa_experement/geometry.py index 9142a16..e0b0964 100644 --- a/src/iiwa_experement/iiwa_experement/geometry.py +++ b/src/iiwa_experement/iiwa_experement/geometry.py @@ -479,7 +479,10 @@ def world_target(spec: ScenarioSpec, geometry: GeometrySpec, name: str) -> Trans dx, dy = _rotate_xy( hole.dx, hole.dy, spec.body.yaw_deg - geometry.hole_reference_yaw_deg, ) - return _down_transform( + # The target is the screw node at the handoff point: upright, its + # lower point just above the hole. tcp_goal() turns it into a + # downward-facing tcp with the magnet above the screw head. + return object_pose_transform( spec.body.x + dx, spec.body.y + dy, spec.body.z + geometry.hole_z_above_body_origin + geometry.handoff_gap, diff --git a/src/iiwa_experement/iiwa_experement/planning_scene.py b/src/iiwa_experement/iiwa_experement/planning_scene.py new file mode 100644 index 0000000..f55a6bb --- /dev/null +++ b/src/iiwa_experement/iiwa_experement/planning_scene.py @@ -0,0 +1,601 @@ +"""Mirror the Webots scene into MoveIt and keep attached objects in sync. + +The pure builders in this module are used by the unit tests; the +``PlanningSceneBridge`` class is the thin ROS adapter used by the reference +runner. A trajectory is allowed only after ``assert_synchronized`` has seen +the expected object ids in the monitored planning scene. +""" +from __future__ import annotations + +import copy +import math +import time +from dataclasses import dataclass +from typing import Iterable, Sequence + +import numpy as np +import rclpy +from geometry_msgs.msg import Pose +from moveit_msgs.msg import ( + AttachedCollisionObject, + CollisionObject, + PlanningScene, +) +from moveit_msgs.srv import GetPlanningScene +from rclpy.node import Node +from rclpy.parameter import Parameter +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy +from shape_msgs.msg import SolidPrimitive + +from .geometry import Box, GeometrySpec, Transform, object_pose_transform +from .scenario import ScenarioSpec + + +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') + + +class SceneSynchronizationError(RuntimeError): + """The MoveIt scene does not match the requested object set.""" + + +def screw_object_id(screw_id: str) -> str: + return f'grant_screw_{int(screw_id.split("_")[1]):02d}' + + +@dataclass(frozen=True) +class SceneObject: + object_id: str + box: Box + pose: Transform + + +@dataclass(frozen=True) +class AttachedSpec: + object_id: str + box: Box + relative_pose: Transform + touch_links: tuple[str, ...] + + +def _box_at(pose: Transform, box: Box) -> Box: + del pose + return box + + +def _pose_with_offset(pose: Transform, offset) -> Transform: + translation = pose.position + pose.matrix @ np.asarray(offset, dtype=float) + return Transform( + xyz=tuple(float(value) for value in translation), + rotation=pose.rotation, + parent=pose.parent, + child=pose.child, + ) + + +def _rotated_pose(x: float, y: float, z: float, yaw_deg: float) -> Transform: + return Transform.from_xyz_rpy( + [x, y, z], [0.0, 0.0, yaw_deg], parent='world', child='scene_object', + ) + + +def build_world_objects(spec: ScenarioSpec, geometry: GeometrySpec) -> list[SceneObject]: + """Return the collision objects present in the initial Webots scene.""" + objects: list[SceneObject] = [] + table = geometry.scene_collisions['table'] + objects.append(SceneObject( + 'grant_table', table, _rotated_pose(0.0, 0.0, table.offset[2], 0.0), + )) + + fixture = geometry.scene_collisions['fixture'] + objects.append(SceneObject( + 'grant_fixture', + fixture, + _rotated_pose( + spec.fixture.x, + spec.fixture.y, + spec.fixture.z + fixture.offset[2], + spec.fixture.yaw_deg, + ), + )) + + body = geometry.scene_collisions['body'] + objects.append(SceneObject( + 'grant_body', + body, + _rotated_pose( + spec.body.x, + spec.body.y, + spec.body.z + body.offset[2], + spec.body.yaw_deg, + ), + )) + + rack = geometry.scene_collisions['rack'] + objects.append(SceneObject( + 'grant_rack', + rack, + _rotated_pose( + spec.rack.x, + spec.rack.y, + spec.rack.z + rack.offset[2], + spec.rack.yaw_deg, + ), + )) + + if spec.tray_pose is not None: + tray = spec.tray_pose + base = geometry.scene_collisions['tray_base'] + objects.append(SceneObject( + 'grant_tray', + base, + _rotated_pose( + tray.x, tray.y, tray.z + base.offset[2], tray.yaw_deg, + ), + )) + wall_x = geometry.scene_collisions['tray_wall_x'] + for sign, suffix in ((-1.0, 'a'), (1.0, 'b')): + dx, dy = math.cos(math.radians(tray.yaw_deg)) * sign * wall_x.offset[0], \ + math.sin(math.radians(tray.yaw_deg)) * sign * wall_x.offset[0] + objects.append(SceneObject( + f'grant_tray_wall_x_{suffix}', + wall_x, + _rotated_pose( + tray.x + dx, + tray.y + dy, + tray.z + wall_x.offset[2], + tray.yaw_deg, + ), + )) + wall_y = geometry.scene_collisions['tray_wall_y'] + for sign, suffix in ((-1.0, 'a'), (1.0, 'b')): + dx = -math.sin(math.radians(tray.yaw_deg)) * sign * wall_y.offset[1] + dy = math.cos(math.radians(tray.yaw_deg)) * sign * wall_y.offset[1] + objects.append(SceneObject( + f'grant_tray_wall_y_{suffix}', + wall_y, + _rotated_pose( + tray.x + dx, + tray.y + dy, + tray.z + wall_y.offset[2], + tray.yaw_deg, + ), + )) + for screw in spec.screws: + objects.append(SceneObject( + screw_object_id(screw.name), + geometry.screw_collision, + _rotated_pose( + screw.pose.x, + screw.pose.y, + screw.pose.z + 0.0045, + screw.pose.yaw_deg, + ), + )) + + for tool in spec.tools: + collision = geometry.tool_collisions[tool.name] + size = (collision.size[0], collision.size[1], 0.140) + objects.append(SceneObject( + f'grant_{tool.name}', + Box(size=size, offset=(0.0, 0.0, 0.070)), + _rotated_pose( + tool.pose.x, tool.pose.y, tool.pose.z + 0.070, + tool.pose.yaw_deg + 180.0, + ), + )) + + if spec.cap_state != 'absent': + cap_pose = spec.cap_pose + assert cap_pose is not None + objects.append(SceneObject( + 'grant_cap', + geometry.cap_collision, + _rotated_pose( + cap_pose.x, cap_pose.y, cap_pose.z + 0.004165, + cap_pose.yaw_deg, + ), + )) + return objects + + +def attached_tool(name: str, geometry: GeometrySpec) -> AttachedSpec: + collision = geometry.tool_collisions[name] + return AttachedSpec( + object_id=f'grant_{name}', + box=Box( + size=(collision.size[0], collision.size[1], 0.140), + offset=collision.offset, + ), + relative_pose=Transform.from_xyz_rpy( + collision.offset, [0.0, 0.0, 0.0], parent='tcp', child=f'grant_{name}', + ), + touch_links=TOUCH_LINKS, + ) + + +def attached_cap(geometry: GeometrySpec) -> AttachedSpec: + return AttachedSpec( + object_id='grant_cap', + box=geometry.cap_collision, + relative_pose=Transform.from_xyz_rpy( + geometry.cap_collision.offset, + [0.0, 0.0, 0.0], + parent='tcp', + child='grant_cap', + ), + touch_links=TOUCH_LINKS, + ) + + +def attached_screw(screw_id: str, geometry: GeometrySpec) -> AttachedSpec: + object_id = screw_object_id(screw_id) + return AttachedSpec( + object_id=object_id, + box=geometry.screw_collision, + relative_pose=Transform.from_xyz_rpy( + geometry.screw_collision.offset, + [0.0, 0.0, 0.0], + parent='tcp', + child=object_id, + ), + touch_links=TOUCH_LINKS, + ) + + +def _pose_message(transform: Transform) -> Pose: + from scipy.spatial.transform import Rotation + + quaternion = Rotation.from_matrix(transform.matrix).as_quat() + pose = Pose() + pose.position.x, pose.position.y, pose.position.z = ( + float(transform.xyz[0]), + float(transform.xyz[1]), + float(transform.xyz[2]), + ) + pose.orientation.x = float(quaternion[0]) + pose.orientation.y = float(quaternion[1]) + pose.orientation.z = float(quaternion[2]) + pose.orientation.w = float(quaternion[3]) + return pose + + +def collision_object( + object_id: str, + box: Box, + pose: Transform, + *, + operation: int = CollisionObject.ADD, +) -> CollisionObject: + message = CollisionObject() + message.header.frame_id = SCENE_FRAME + message.id = object_id + message.operation = operation + 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)) + return message + + +def attached_object_message( + spec: AttachedSpec, + *, + link_name: str = 'tcp', + operation: int = CollisionObject.ADD, +) -> AttachedCollisionObject: + message = AttachedCollisionObject() + message.link_name = link_name + message.object = collision_object( + spec.object_id, spec.box, spec.relative_pose, operation=operation, + ) + message.touch_links = list(spec.touch_links) + return message + + +def update_contact_entry( + names: Sequence[str], + values, + first: str, + second: str, + allowed: bool, +) -> tuple[list[str], list[list[bool]]]: + """Return a copy of an ACM with exactly one pair changed.""" + names = list(names) + matrix = [list(row) for row in values] + for name in (first, second): + if name not in names: + names.append(name) + for row in matrix: + row.append(allowed) + matrix.append([allowed] * len(names)) + first_index = names.index(first) + second_index = names.index(second) + matrix[first_index][second_index] = allowed + matrix[second_index][first_index] = allowed + return names, matrix + + +def scene_ids(scene: PlanningScene) -> tuple[set[str], set[str]]: + world = {obj.id for obj in scene.world.collision_objects} + attached = {obj.object.id for obj in scene.robot_state.attached_collision_objects} + return world, attached + + +class PlanningSceneBridge: + """Thin ROS adapter around a ``moveit_msgs/PlanningScene`` diff stream.""" + + def __init__( + self, + *, + node: Node | None = None, + use_sim_time: bool = True, + ) -> None: + self._owns_node = node is None + if node is None: + node = Node('planning_scene_bridge') + node.set_parameters([Parameter( + 'use_sim_time', value=use_sim_time, + )]) + self.node = node + self.geometry: GeometrySpec | None = None + self.spec: ScenarioSpec | None = None + self.base_to_world: Transform | None = None + self.last_scene: PlanningScene | None = None + self._publisher = self.node.create_publisher( + PlanningScene, SCENE_TOPIC, 10, + ) + self._subscription = self.node.create_subscription( + PlanningScene, + MONITORED_TOPIC, + self._on_scene, + QoSProfile( + depth=10, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.VOLATILE, + ), + ) + self._get_scene = self.node.create_client( + GetPlanningScene, GET_SCENE_SERVICE, + ) + self._world_objects: dict[str, SceneObject] = {} + self._attached: dict[str, AttachedSpec] = {} + self._seen_world: set[str] = set() + self._seen_attached: set[str] = set() + + def _on_scene(self, message: PlanningScene) -> None: + self._seen_world.update( + obj.id for obj in message.world.collision_objects + ) + self._seen_attached.update( + entry.object.id + for entry in message.robot_state.attached_collision_objects + ) + if not message.is_diff: + self.last_scene = copy.deepcopy(message) + return + if self.last_scene is None: + self.last_scene = PlanningScene() + self.last_scene.is_diff = False + scene = self.last_scene + world = {obj.id: obj for obj in scene.world.collision_objects} + for obj in message.world.collision_objects: + if obj.operation == CollisionObject.REMOVE: + world.pop(obj.id, None) + else: + world[obj.id] = copy.deepcopy(obj) + scene.world.collision_objects = list(world.values()) + attached = { + entry.object.id: entry + for entry in scene.robot_state.attached_collision_objects + } + for entry in message.robot_state.attached_collision_objects: + if entry.object.operation == CollisionObject.REMOVE: + attached.pop(entry.object.id, None) + else: + attached[entry.object.id] = copy.deepcopy(entry) + scene.robot_state.attached_collision_objects = list(attached.values()) + + def _spin(self, seconds: float = 0.05) -> None: + end = time.monotonic() + seconds + while time.monotonic() < end: + rclpy.spin_once(self.node, timeout_sec=0.01) + + def _publish(self, scene: PlanningScene) -> None: + self._seen_world.clear() + self._seen_attached.clear() + self._publisher.publish(scene) + self._spin() + + @staticmethod + def _diff() -> PlanningScene: + scene = PlanningScene() + scene.is_diff = True + scene.robot_state.is_diff = True + return scene + + def update_world_to_base(self, world_to_base: Transform) -> None: + self.base_to_world = world_to_base.inverse() + + def _base_pose(self, pose: Transform) -> Transform: + if self.base_to_world is None: + raise SceneSynchronizationError('base transform is not initialised') + return self.base_to_world.compose_relaxed(pose) + + def initialize( + self, + spec: ScenarioSpec, + geometry: GeometrySpec, + *, + world_to_base: Transform | None = None, + ) -> None: + self.spec = spec + self.geometry = geometry + self.base_to_world = (world_to_base or geometry.world_to_base).inverse() + self._world_objects = { + 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) + 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) + + def detach(self, object_id: str, world_pose: Transform) -> None: + if object_id not in self._attached: + raise SceneSynchronizationError( + f'cannot detach {object_id!r}: it is not attached' + ) + 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) + + 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) + + def allow_contact(self, first: str, second: str, allowed: bool = True) -> None: + """Change exactly one ACM pair; all other checks stay active.""" + if self.last_scene is None or not self.last_scene.allowed_collision_matrix.entry_names: + 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, + ) + scene.allowed_collision_matrix.entry_names = names + scene.allowed_collision_matrix.entry_values = values + self._publish(scene) + + 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 not self._get_scene.service_is_ready(): + if not self._get_scene.wait_for_service(timeout_sec=1.0): + return None + request = GetPlanningScene.Request() + request.components.components = 8 | 16 | 4 | 128 + future = self._get_scene.call_async(request) + end = time.monotonic() + timeout_sec + while time.monotonic() < end and not future.done(): + rclpy.spin_once(self.node, timeout_sec=0.05) + if not future.done(): + return None + try: + response = future.result() + except Exception: + return None + return scene_ids(response.scene) + + def assert_synchronized( + self, + expected_world: Iterable[str] | None = None, + expected_attached: Iterable[str] | None = None, + *, + seen_world: Iterable[str] | None = None, + seen_attached: Iterable[str] | None = None, + timeout_sec: float = 8.0, + ) -> None: + world_expected = set(expected_world) if expected_world is not None else set() + attached_expected = ( + set(expected_attached) if expected_attached is not None else set() + ) + world_seen_expected = set(seen_world) if seen_world is not None else set() + attached_seen_expected = ( + set(seen_attached) + if seen_attached is not None + else set(attached_expected) + ) + end = time.monotonic() + timeout_sec + last_world: set[str] = set() + last_attached: set[str] = set() + while time.monotonic() < end: + rclpy.spin_once(self.node, timeout_sec=0.05) + # 1. Every expected id must have appeared in a monitored scene + # (published by the MoveItPy or move_group monitor). + served = ( + world_seen_expected <= self._seen_world + and attached_seen_expected <= self._seen_attached + ) + # 2. The move_group service is the authoritative full snapshot. + snapshot = self._query_scene(timeout_sec=1.0) + if snapshot is not None: + last_world, last_attached = snapshot + if world_expected <= last_world and attached_expected <= last_attached: + if not served: + # The service proves move_group; still let the next + # monitored message confirm the second monitor. + continue + return + elif served: + if self.last_scene is not None: + last_world, last_attached = scene_ids(self.last_scene) + return + raise SceneSynchronizationError( + 'MoveIt scene is not synchronized: ' + f'missing world={sorted(world_expected - last_world)}, ' + f'missing attached={sorted(attached_expected - last_attached)}; ' + f'seen world={sorted(self._seen_world)}, ' + f'seen attached={sorted(self._seen_attached)}' + ) + + def destroy(self) -> None: + if self._owns_node: + self.node.destroy_node() diff --git a/src/iiwa_experement/test/test_planning_scene.py b/src/iiwa_experement/test/test_planning_scene.py new file mode 100644 index 0000000..36bae17 --- /dev/null +++ b/src/iiwa_experement/test/test_planning_scene.py @@ -0,0 +1,166 @@ +"""The MoveIt scene must mirror Webots and keep attachments exclusive.""" +from pathlib import Path + +import numpy as np +import pytest +from moveit_msgs.msg import CollisionObject, PlanningScene + +from iiwa_experement.geometry import Box, Transform, load_geometry +from iiwa_experement.planning_scene import ( + PlanningSceneBridge, + SceneObject, + attached_cap, + attached_screw, + attached_tool, + build_world_objects, + collision_object, + scene_ids, + update_contact_entry, +) +from iiwa_experement.scenario import load_scenario + + +PACKAGE = Path(__file__).resolve().parents[1] +SCENES = PACKAGE / 'config' / 'scenes.yaml' +GEOMETRY = PACKAGE / 'config' / 'geometry.yaml' + + +def spec(experiment_id): + return load_scenario(experiment_id, SCENES) + + +class FakeBridge(PlanningSceneBridge): + def __init__(self, experiment_id=11): + geometry = load_geometry(GEOMETRY) + scenario = spec(experiment_id) + self.geometry = geometry + self.spec = scenario + self.base_to_world = geometry.world_to_base.inverse() + self._world_objects = { + obj.object_id: obj for obj in build_world_objects(scenario, geometry) + } + self._attached = {} + self.last_scene = PlanningScene() + self.published = [] + + def _publish(self, scene): + self.published.append(scene) + + def assert_synchronized(self, *args, **kwargs): + return None + + +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', + '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 + + +def test_experiment_nineteen_moves_tray_and_screws(): + standard = {obj.object_id: obj for obj in build_world_objects( + spec(11), load_geometry(GEOMETRY))} + shifted = {obj.object_id: obj for obj in build_world_objects( + spec(19), load_geometry(GEOMETRY))} + for index in range(1, 11): + name = f'grant_screw_{index:02d}' + assert shifted[name].pose.position == pytest.approx( + standard[name].pose.position + np.array([-0.02, 0.02, 0.0]), + abs=1e-9, + ) + + +def test_collision_boxes_match_measured_meshes(): + objects = {obj.object_id: obj for obj in build_world_objects( + spec(16), load_geometry(GEOMETRY))} + assert objects['grant_body'].box.size == pytest.approx((0.30, 0.17, 0.00915)) + assert objects['grant_cap'].box.size == pytest.approx((0.1384, 0.0984, 0.00367)) + assert objects['grant_screw_01'].box.size == pytest.approx((0.0055, 0.0055, 0.009)) + assert objects['grant_vacuum'].box.size[2] == pytest.approx(0.140) + + +def test_collision_object_is_in_the_robot_model_frame(): + pose = Transform.from_xyz_rpy([0.0, 0.0, 0.0], [0, 0, 0], parent='world', child='x') + message = collision_object('obj', Box((0.1, 0.2, 0.3)), pose) + assert message.header.frame_id == 'base_link' + assert message.id == 'obj' + assert list(message.primitives[0].dimensions) == pytest.approx([0.1, 0.2, 0.3]) + + +def test_attach_removes_the_world_object_and_adds_one_attachment(): + bridge = FakeBridge(16) + tool = attached_tool('vacuum', bridge.geometry) + bridge.attach(tool) + 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 len(scene.robot_state.attached_collision_objects) == 1 + assert scene.robot_state.attached_collision_objects[0].link_name == 'tcp' + assert 'tcp' in scene.robot_state.attached_collision_objects[0].touch_links + + +def test_detach_reverses_the_attachment(): + bridge = FakeBridge(16) + tool = attached_tool('vacuum', bridge.geometry) + bridge.attach(tool) + bridge.detach(tool.object_id, bridge.geometry.world_to_base.compose_relaxed( + Transform.from_xyz_rpy([0, 0, 0], [0, 0, 0], parent='world', child='x') + )) + 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 + + +def test_handoff_removes_only_the_delivered_screw(): + bridge = FakeBridge(11) + screw = attached_screw('screw_1', bridge.geometry) + bridge.attach(screw) + assert 'grant_screw_02' in bridge._world_objects + bridge.remove(screw.object_id) + assert screw.object_id not in bridge._attached + assert screw.object_id not in bridge._world_objects + assert 'grant_screw_02' in bridge._world_objects + + +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.box.size == pytest.approx((0.1384, 0.0984, 0.00367)) + assert screw.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.1445)) + + +def test_contact_permission_changes_exactly_one_pair(): + names = ['link1', 'link2', 'tool'] + values = [[True, False, False], [False, True, False], [False, False, True]] + new_names, matrix = update_contact_entry(names, values, 'tool', 'link2', True) + assert new_names == names + assert matrix[2][1] is True and matrix[1][2] is True + assert matrix[2][0] is False and matrix[0][2] is False + assert matrix[0][1] is False + + +def test_scene_ids_reports_world_and_attached_sets(): + scene = PlanningScene() + scene.world.collision_objects.append(CollisionObject(id='world_a')) + from moveit_msgs.msg import AttachedCollisionObject + + attached = AttachedCollisionObject() + attached.object.id = 'attached_a' + scene.robot_state.attached_collision_objects.append(attached) + world, held = scene_ids(scene) + assert world == {'world_a'} + assert held == {'attached_a'}