mirror of
https://github.com/deepmodeling/Uni-Lab-OS
synced 2026-05-23 21:29:59 +00:00
431 lines
16 KiB
Python
431 lines
16 KiB
Python
"""MoveItCollisionChecker 和 IKFastReachabilityChecker 测试。
|
||
|
||
使用 unittest.mock 模拟 MoveIt2 实例，验证适配器逻辑，
|
||
无需 ROS2 / MoveIt2 运行环境。
|
||
"""
|
||
|
||
import math
|
||
import tempfile
|
||
from pathlib import Path
|
||
from unittest.mock import MagicMock, patch
|
||
|
||
import numpy as np
|
||
import pytest
|
||
|
||
from ..ros_checkers import (
|
||
IKFastReachabilityChecker,
|
||
MoveItCollisionChecker,
|
||
_transform_to_arm_frame,
|
||
_yaw_to_quat,
|
||
_yaw_to_rotation_matrix,
|
||
create_checkers,
|
||
)
|
||
|
||
|
||
# ---------- 辅助函数测试 ----------
|
||
|
||
|
||
class TestYawToQuat:
|
||
def test_zero_rotation(self):
|
||
"""零旋转 → 单位四元数。"""
|
||
q = _yaw_to_quat(0.0)
|
||
assert q == pytest.approx((0.0, 0.0, 0.0, 1.0))
|
||
|
||
def test_90_degrees(self):
|
||
"""90° → (0, 0, sin(π/4), cos(π/4))。"""
|
||
q = _yaw_to_quat(math.pi / 2)
|
||
expected = (0.0, 0.0, math.sin(math.pi / 4), math.cos(math.pi / 4))
|
||
assert q == pytest.approx(expected)
|
||
|
||
def test_180_degrees(self):
|
||
"""180° → (0, 0, 1, 0)。"""
|
||
q = _yaw_to_quat(math.pi)
|
||
assert q == pytest.approx((0.0, 0.0, 1.0, 0.0), abs=1e-10)
|
||
|
||
|
||
class TestTransformToArmFrame:
|
||
def test_identity_transform(self):
|
||
"""臂在原点无旋转，目标在 (1, 0, 0.5)。"""
|
||
arm_pose = {"x": 0.0, "y": 0.0, "theta": 0.0}
|
||
target = {"x": 1.0, "y": 0.0, "z": 0.5}
|
||
local = _transform_to_arm_frame(arm_pose, target)
|
||
assert local == pytest.approx((1.0, 0.0, 0.5))
|
||
|
||
def test_translation_only(self):
|
||
"""臂在 (2, 3) 无旋转，目标在 (3, 4, 0)。"""
|
||
arm_pose = {"x": 2.0, "y": 3.0, "theta": 0.0}
|
||
target = {"x": 3.0, "y": 4.0, "z": 0.0}
|
||
local = _transform_to_arm_frame(arm_pose, target)
|
||
assert local == pytest.approx((1.0, 1.0, 0.0))
|
||
|
||
def test_rotation_90(self):
|
||
"""臂旋转 90°，目标在臂前方。"""
|
||
arm_pose = {"x": 0.0, "y": 0.0, "theta": math.pi / 2}
|
||
target = {"x": 0.0, "y": 1.0, "z": 0.0}
|
||
local = _transform_to_arm_frame(arm_pose, target)
|
||
# 世界 Y+ 在臂坐标系中变成 X+
|
||
assert local[0] == pytest.approx(1.0, abs=1e-10)
|
||
assert local[1] == pytest.approx(0.0, abs=1e-10)
|
||
|
||
|
||
class TestYawToRotationMatrix:
|
||
def test_identity(self):
|
||
"""零旋转 → 单位矩阵。"""
|
||
R = _yaw_to_rotation_matrix(0.0)
|
||
np.testing.assert_allclose(R, np.eye(3), atol=1e-10)
|
||
|
||
def test_90_degrees(self):
|
||
"""90° 旋转矩阵。"""
|
||
R = _yaw_to_rotation_matrix(math.pi / 2)
|
||
expected = np.array([
|
||
[0.0, -1.0, 0.0],
|
||
[1.0, 0.0, 0.0],
|
||
[0.0, 0.0, 1.0],
|
||
])
|
||
np.testing.assert_allclose(R, expected, atol=1e-10)
|
||
|
||
|
||
# ---------- MoveItCollisionChecker 测试 ----------
|
||
|
||
|
||
class TestMoveItCollisionChecker:
|
||
def setup_method(self):
|
||
self.moveit2 = MagicMock()
|
||
# 禁用 FCL，使用 OBB 回退（测试环境无需 python-fcl）
|
||
self.checker = MoveItCollisionChecker(
|
||
self.moveit2, sync_to_scene=True,
|
||
)
|
||
self.checker._fcl_available = False
|
||
|
||
def test_no_collision_far_apart(self):
|
||
"""两个设备距离足够远，不碰撞。"""
|
||
placements = [
|
||
{"id": "a", "bbox": (0.5, 0.5), "pos": (1.0, 1.0, 0.0)},
|
||
{"id": "b", "bbox": (0.5, 0.5), "pos": (3.0, 3.0, 0.0)},
|
||
]
|
||
assert self.checker.check(placements) == []
|
||
|
||
def test_collision_overlapping(self):
|
||
"""两个设备重叠，应检测到碰撞。"""
|
||
placements = [
|
||
{"id": "a", "bbox": (1.0, 1.0), "pos": (1.0, 1.0, 0.0)},
|
||
{"id": "b", "bbox": (1.0, 1.0), "pos": (1.5, 1.0, 0.0)},
|
||
]
|
||
collisions = self.checker.check(placements)
|
||
assert ("a", "b") in collisions
|
||
|
||
def test_collision_with_rotation(self):
|
||
"""旋转后的碰撞检测。"""
|
||
placements = [
|
||
{"id": "a", "bbox": (1.0, 0.2), "pos": (1.0, 1.0, math.pi / 4)},
|
||
{"id": "b", "bbox": (0.5, 0.5), "pos": (1.4, 1.0, 0.0)},
|
||
]
|
||
collisions = self.checker.check(placements)
|
||
assert ("a", "b") in collisions
|
||
|
||
def test_syncs_collision_objects(self):
|
||
"""验证 check() 调用 add_collision_box 同步到 MoveIt2。"""
|
||
placements = [
|
||
{"id": "dev_a", "bbox": (0.6, 0.8), "pos": (1.0, 2.0, 0.5)},
|
||
]
|
||
self.checker.check(placements)
|
||
|
||
self.moveit2.add_collision_box.assert_called_once()
|
||
call_kwargs = self.moveit2.add_collision_box.call_args
|
||
# 验证使用 {device_id}_ 前缀
|
||
assert call_kwargs.kwargs["id"] == "dev_a_"
|
||
# 验证 size = (w, d, h)
|
||
assert call_kwargs.kwargs["size"] == (0.6, 0.8, 0.4)
|
||
|
||
def test_device_id_prefix(self):
|
||
"""碰撞对象名称使用 {device_id}_ 前缀。"""
|
||
placements = [
|
||
{"id": "robot_arm", "bbox": (0.3, 0.3), "pos": (1.0, 1.0, 0.0)},
|
||
{"id": "centrifuge", "bbox": (0.5, 0.5), "pos": (3.0, 3.0, 0.0)},
|
||
]
|
||
self.checker.check(placements)
|
||
|
||
calls = self.moveit2.add_collision_box.call_args_list
|
||
ids = [c.kwargs["id"] for c in calls]
|
||
assert "robot_arm_" in ids
|
||
assert "centrifuge_" in ids
|
||
|
||
def test_sync_failure_does_not_crash(self):
|
||
"""add_collision_box 异常不影响碰撞检测结果。"""
|
||
self.moveit2.add_collision_box.side_effect = RuntimeError("service unavailable")
|
||
placements = [
|
||
{"id": "a", "bbox": (0.5, 0.5), "pos": (1.0, 1.0, 0.0)},
|
||
{"id": "b", "bbox": (0.5, 0.5), "pos": (3.0, 3.0, 0.0)},
|
||
]
|
||
# 不应抛异常
|
||
collisions = self.checker.check(placements)
|
||
assert collisions == []
|
||
|
||
def test_check_bounds_within(self):
|
||
"""设备在边界内。"""
|
||
placements = [
|
||
{"id": "a", "bbox": (0.5, 0.5), "pos": (1.0, 1.0, 0.0)},
|
||
]
|
||
assert self.checker.check_bounds(placements, 5.0, 5.0) == []
|
||
|
||
def test_check_bounds_outside(self):
|
||
"""设备超出边界。"""
|
||
placements = [
|
||
{"id": "a", "bbox": (1.0, 1.0), "pos": (0.2, 0.2, 0.0)},
|
||
]
|
||
oob = self.checker.check_bounds(placements, 5.0, 5.0)
|
||
assert "a" in oob
|
||
|
||
def test_no_sync_mode(self):
|
||
"""sync_to_scene=False 时不调用 add_collision_box。"""
|
||
checker = MoveItCollisionChecker(
|
||
self.moveit2, sync_to_scene=False,
|
||
)
|
||
checker._fcl_available = False
|
||
placements = [
|
||
{"id": "a", "bbox": (0.5, 0.5), "pos": (1.0, 1.0, 0.0)},
|
||
]
|
||
checker.check(placements)
|
||
self.moveit2.add_collision_box.assert_not_called()
|
||
|
||
def test_touching_edges_no_collision(self):
|
||
"""恰好边缘接触，不算碰撞。"""
|
||
placements = [
|
||
{"id": "a", "bbox": (1.0, 1.0), "pos": (0.5, 0.5, 0.0)},
|
||
{"id": "b", "bbox": (1.0, 1.0), "pos": (1.5, 0.5, 0.0)},
|
||
]
|
||
collisions = self.checker.check(placements)
|
||
assert collisions == []
|
||
|
||
def test_three_devices_multiple_collisions(self):
|
||
"""三个设备，相邻碰撞。"""
|
||
placements = [
|
||
{"id": "a", "bbox": (1.0, 1.0), "pos": (1.0, 1.0, 0.0)},
|
||
{"id": "b", "bbox": (1.0, 1.0), "pos": (1.3, 1.0, 0.0)},
|
||
{"id": "c", "bbox": (1.0, 1.0), "pos": (1.6, 1.0, 0.0)},
|
||
]
|
||
collisions = self.checker.check(placements)
|
||
assert ("a", "b") in collisions
|
||
assert ("b", "c") in collisions
|
||
|
||
|
||
# ---------- IKFastReachabilityChecker 测试 ----------
|
||
|
||
|
||
class TestIKFastReachabilityCheckerVoxel:
|
||
"""体素图模式测试。"""
|
||
|
||
def _create_voxel_dir(self, tmp_path: Path, arm_id: str = "elite_cs66") -> Path:
|
||
"""创建包含体素图的临时目录。"""
|
||
# 创建一个简单的体素网格：中心区域可达
|
||
grid = np.zeros((100, 100, 50), dtype=bool)
|
||
# 标记中心 60x60x30 区域为可达
|
||
grid[20:80, 20:80, 10:40] = True
|
||
|
||
origin = np.array([-0.5, -0.5, 0.0])
|
||
resolution = 0.01
|
||
|
||
npz_path = tmp_path / f"{arm_id}.npz"
|
||
np.savez(str(npz_path), grid=grid, origin=origin, resolution=resolution)
|
||
return tmp_path
|
||
|
||
def test_reachable_in_voxel(self, tmp_path):
|
||
"""目标在体素图可达区域内。"""
|
||
voxel_dir = self._create_voxel_dir(tmp_path)
|
||
checker = IKFastReachabilityChecker(voxel_dir=voxel_dir)
|
||
|
||
arm_pose = {"x": 0.0, "y": 0.0, "theta": 0.0}
|
||
# 中心区域：local = (0.0, 0.0, 0.2) → ix=50, iy=50, iz=20 → 可达
|
||
target = {"x": 0.0, "y": 0.0, "z": 0.2}
|
||
assert checker.is_reachable("elite_cs66", arm_pose, target)
|
||
|
||
def test_not_reachable_outside_voxel(self, tmp_path):
|
||
"""目标在体素图不可达区域。"""
|
||
voxel_dir = self._create_voxel_dir(tmp_path)
|
||
checker = IKFastReachabilityChecker(voxel_dir=voxel_dir)
|
||
|
||
arm_pose = {"x": 0.0, "y": 0.0, "theta": 0.0}
|
||
# 边缘区域：local = (-0.45, -0.45, 0.0) → ix=5, iy=5, iz=0 → 不可达
|
||
target = {"x": -0.45, "y": -0.45, "z": 0.0}
|
||
assert not checker.is_reachable("elite_cs66", arm_pose, target)
|
||
|
||
def test_out_of_bounds_not_reachable(self, tmp_path):
|
||
"""目标超出体素图范围。"""
|
||
voxel_dir = self._create_voxel_dir(tmp_path)
|
||
checker = IKFastReachabilityChecker(voxel_dir=voxel_dir)
|
||
|
||
arm_pose = {"x": 0.0, "y": 0.0, "theta": 0.0}
|
||
target = {"x": 5.0, "y": 5.0, "z": 0.0}
|
||
assert not checker.is_reachable("elite_cs66", arm_pose, target)
|
||
|
||
def test_arm_rotation_transforms_target(self, tmp_path):
|
||
"""臂旋转后目标变换到臂坐标系。"""
|
||
voxel_dir = self._create_voxel_dir(tmp_path)
|
||
checker = IKFastReachabilityChecker(voxel_dir=voxel_dir)
|
||
|
||
# 臂旋转 90°，目标在世界 Y+ 方向 → 臂坐标系 X+ 方向
|
||
arm_pose = {"x": 0.0, "y": 0.0, "theta": math.pi / 2}
|
||
# 世界 (0, 0.1, 0.2) → 臂坐标系 (0.1, 0, 0.2) → 在可达范围
|
||
target = {"x": 0.0, "y": 0.1, "z": 0.2}
|
||
assert checker.is_reachable("elite_cs66", arm_pose, target)
|
||
|
||
def test_unknown_arm_no_voxel_no_moveit(self, tmp_path):
|
||
"""未知臂型且无 MoveIt2，乐观返回 True。"""
|
||
voxel_dir = self._create_voxel_dir(tmp_path)
|
||
checker = IKFastReachabilityChecker(voxel_dir=voxel_dir)
|
||
|
||
arm_pose = {"x": 0.0, "y": 0.0, "theta": 0.0}
|
||
target = {"x": 0.5, "y": 0.0, "z": 0.0}
|
||
assert checker.is_reachable("unknown_arm", arm_pose, target)
|
||
|
||
def test_missing_voxel_dir(self):
|
||
"""体素目录不存在不报错。"""
|
||
checker = IKFastReachabilityChecker(voxel_dir="/nonexistent/path")
|
||
assert len(checker._voxel_maps) == 0
|
||
|
||
|
||
class TestIKFastReachabilityCheckerLiveIK:
|
||
"""实时 IK 模式测试。"""
|
||
|
||
def test_reachable_via_ik(self):
|
||
"""compute_ik 返回 JointState → 可达。"""
|
||
moveit2 = MagicMock()
|
||
moveit2.compute_ik.return_value = MagicMock() # 非 None → 成功
|
||
|
||
checker = IKFastReachabilityChecker(moveit2)
|
||
arm_pose = {"x": 0.0, "y": 0.0, "theta": 0.0}
|
||
target = {"x": 0.5, "y": 0.0, "z": 0.3}
|
||
assert checker.is_reachable("elite_cs66", arm_pose, target)
|
||
|
||
def test_not_reachable_via_ik(self):
|
||
"""compute_ik 返回 None → 不可达。"""
|
||
moveit2 = MagicMock()
|
||
moveit2.compute_ik.return_value = None
|
||
|
||
checker = IKFastReachabilityChecker(moveit2)
|
||
arm_pose = {"x": 0.0, "y": 0.0, "theta": 0.0}
|
||
target = {"x": 5.0, "y": 5.0, "z": 0.0}
|
||
assert not checker.is_reachable("elite_cs66", arm_pose, target)
|
||
|
||
def test_ik_exception_returns_false(self):
|
||
"""compute_ik 抛异常 → 不可达。"""
|
||
moveit2 = MagicMock()
|
||
moveit2.compute_ik.side_effect = RuntimeError("service timeout")
|
||
|
||
checker = IKFastReachabilityChecker(moveit2)
|
||
arm_pose = {"x": 0.0, "y": 0.0, "theta": 0.0}
|
||
target = {"x": 0.5, "y": 0.0, "z": 0.0}
|
||
assert not checker.is_reachable("elite_cs66", arm_pose, target)
|
||
|
||
def test_ik_called_with_correct_position(self):
|
||
"""验证 compute_ik 接收正确的臂坐标系位置。"""
|
||
moveit2 = MagicMock()
|
||
moveit2.compute_ik.return_value = MagicMock()
|
||
|
||
checker = IKFastReachabilityChecker(moveit2)
|
||
arm_pose = {"x": 1.0, "y": 2.0, "theta": 0.0}
|
||
target = {"x": 1.5, "y": 2.3, "z": 0.4}
|
||
checker.is_reachable("elite_cs66", arm_pose, target)
|
||
|
||
call_kwargs = moveit2.compute_ik.call_args.kwargs
|
||
assert call_kwargs["position"] == pytest.approx((0.5, 0.3, 0.4))
|
||
|
||
def test_voxel_takes_priority_over_live_ik(self, tmp_path):
|
||
"""有体素图时优先使用体素查询，不调用 compute_ik。"""
|
||
# 创建体素图
|
||
grid = np.ones((10, 10, 10), dtype=bool)
|
||
origin = np.array([-0.05, -0.05, 0.0])
|
||
np.savez(
|
||
str(tmp_path / "test_arm.npz"),
|
||
grid=grid, origin=origin, resolution=0.01,
|
||
)
|
||
|
||
moveit2 = MagicMock()
|
||
checker = IKFastReachabilityChecker(moveit2, voxel_dir=tmp_path)
|
||
|
||
arm_pose = {"x": 0.0, "y": 0.0, "theta": 0.0}
|
||
target = {"x": 0.0, "y": 0.0, "z": 0.05}
|
||
checker.is_reachable("test_arm", arm_pose, target)
|
||
|
||
moveit2.compute_ik.assert_not_called()
|
||
|
||
|
||
# ---------- create_checkers 工厂函数测试 ----------
|
||
|
||
|
||
class TestCreateCheckers:
|
||
def test_mock_mode(self):
|
||
"""mock 模式返回 Mock 检测器。"""
|
||
from ..mock_checkers import (
|
||
MockCollisionChecker,
|
||
MockReachabilityChecker,
|
||
)
|
||
|
||
collision, reachability = create_checkers(mode="mock")
|
||
assert isinstance(collision, MockCollisionChecker)
|
||
assert isinstance(reachability, MockReachabilityChecker)
|
||
|
||
def test_moveit_mode(self):
|
||
"""moveit 模式返回 MoveIt2 检测器。"""
|
||
moveit2 = MagicMock()
|
||
collision, reachability = create_checkers(moveit2, mode="moveit")
|
||
assert isinstance(collision, MoveItCollisionChecker)
|
||
assert isinstance(reachability, IKFastReachabilityChecker)
|
||
|
||
def test_moveit_mode_requires_instance(self):
|
||
"""moveit 模式无实例时抛异常。"""
|
||
with pytest.raises(ValueError, match="MoveIt2 instance required"):
|
||
create_checkers(mode="moveit")
|
||
|
||
def test_default_mode_is_mock(self):
|
||
"""默认使用 mock 模式。"""
|
||
from ..mock_checkers import MockCollisionChecker
|
||
|
||
collision, _ = create_checkers()
|
||
assert isinstance(collision, MockCollisionChecker)
|
||
|
||
def test_env_var_override(self, monkeypatch):
|
||
"""LAYOUT_CHECKER_MODE 环境变量覆盖默认值。"""
|
||
moveit2 = MagicMock()
|
||
monkeypatch.setenv("LAYOUT_CHECKER_MODE", "moveit")
|
||
collision, _ = create_checkers(moveit2)
|
||
assert isinstance(collision, MoveItCollisionChecker)
|
||
|
||
|
||
# ---------- Protocol 兼容性测试 ----------
|
||
|
||
|
||
class TestProtocolConformance:
|
||
"""验证适配器满足 Protocol 接口签名。"""
|
||
|
||
def test_collision_checker_has_check(self):
|
||
"""MoveItCollisionChecker 实现 check(placements) 方法。"""
|
||
moveit2 = MagicMock()
|
||
checker = MoveItCollisionChecker(moveit2, sync_to_scene=False)
|
||
checker._fcl_available = False
|
||
placements = [
|
||
{"id": "a", "bbox": (0.5, 0.5), "pos": (1.0, 1.0, 0.0)},
|
||
]
|
||
result = checker.check(placements)
|
||
assert isinstance(result, list)
|
||
|
||
def test_reachability_checker_has_is_reachable(self):
|
||
"""IKFastReachabilityChecker 实现 is_reachable(arm_id, arm_pose, target) 方法。"""
|
||
checker = IKFastReachabilityChecker()
|
||
result = checker.is_reachable(
|
||
"arm_id",
|
||
{"x": 0.0, "y": 0.0, "theta": 0.0},
|
||
{"x": 0.5, "y": 0.0, "z": 0.0},
|
||
)
|
||
assert isinstance(result, bool)
|
||
|
||
def test_collision_checker_has_check_bounds(self):
|
||
"""MoveItCollisionChecker 实现 check_bounds 方法。"""
|
||
moveit2 = MagicMock()
|
||
checker = MoveItCollisionChecker(moveit2, sync_to_scene=False)
|
||
placements = [
|
||
{"id": "a", "bbox": (0.5, 0.5), "pos": (1.0, 1.0, 0.0)},
|
||
]
|
||
result = checker.check_bounds(placements, 5.0, 5.0)
|
||
assert isinstance(result, list)
|