mirror of
https://github.com/deepmodeling/Uni-Lab-OS
synced 2026-05-23 03:19:55 +00:00
111 lines
4.0 KiB
Python
111 lines
4.0 KiB
Python
"""Mock 检测器：无 ROS 依赖的简化碰撞与可达性检测。
|
||
|
||
碰撞检测基于 OBB SAT（O(n²) 两两比较）。
|
||
可达性检测基于最大臂展半径的欧几里得距离判断。
|
||
|
||
集成阶段由 ros_checkers.py 中的 MoveItCollisionChecker / IKFastReachabilityChecker 替代。
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import math
|
||
|
||
from .obb import obb_corners, obb_overlap
|
||
|
||
|
||
class MockCollisionChecker:
|
||
"""基于 OBB SAT 的碰撞检测。
|
||
|
||
输入格式与 CollisionChecker Protocol 一致：
|
||
placements: [{"id": str, "bbox": (w, d), "pos": (x, y, θ)}, ...]
|
||
"""
|
||
|
||
def check(self, placements: list[dict]) -> list[tuple[str, str]]:
|
||
"""返回所有碰撞的设备对。"""
|
||
collisions: list[tuple[str, str]] = []
|
||
n = len(placements)
|
||
for i in range(n):
|
||
for j in range(i + 1, n):
|
||
a, b = placements[i], placements[j]
|
||
corners_a = obb_corners(
|
||
a["pos"][0], a["pos"][1],
|
||
a["bbox"][0], a["bbox"][1],
|
||
a["pos"][2] if len(a["pos"]) > 2 else 0.0,
|
||
)
|
||
corners_b = obb_corners(
|
||
b["pos"][0], b["pos"][1],
|
||
b["bbox"][0], b["bbox"][1],
|
||
b["pos"][2] if len(b["pos"]) > 2 else 0.0,
|
||
)
|
||
if obb_overlap(corners_a, corners_b):
|
||
collisions.append((a["id"], b["id"]))
|
||
return collisions
|
||
|
||
def check_bounds(
|
||
self, placements: list[dict], lab_width: float, lab_depth: float
|
||
) -> list[str]:
|
||
"""返回超出实验室边界的设备 ID 列表。"""
|
||
out_of_bounds: list[str] = []
|
||
for p in placements:
|
||
hw, hd = self._rotated_half_extents(p)
|
||
x, y = p["pos"][:2]
|
||
if x - hw < 0 or x + hw > lab_width or y - hd < 0 or y + hd > lab_depth:
|
||
out_of_bounds.append(p["id"])
|
||
return out_of_bounds
|
||
|
||
@staticmethod
|
||
def _rotated_half_extents(p: dict) -> tuple[float, float]:
|
||
"""计算旋转后 AABB 的半宽和半深。"""
|
||
w, d = p["bbox"]
|
||
theta = p["pos"][2] if len(p["pos"]) > 2 else 0.0
|
||
cos_t = abs(math.cos(theta))
|
||
sin_t = abs(math.sin(theta))
|
||
half_w = (w * cos_t + d * sin_t) / 2
|
||
half_d = (w * sin_t + d * cos_t) / 2
|
||
return half_w, half_d
|
||
|
||
|
||
class MockReachabilityChecker:
|
||
"""基于最大臂展半径的简化可达性判断。
|
||
|
||
内置常见 Elite CS 系列机械臂的臂展参数。
|
||
自定义臂展可通过构造参数传入。
|
||
"""
|
||
|
||
# 默认臂展参数（单位：米）
|
||
DEFAULT_ARM_REACH: dict[str, float] = {
|
||
"elite_cs63": 0.624,
|
||
"elite_cs66": 0.914,
|
||
"elite_cs612": 1.304,
|
||
"elite_cs620": 1.800,
|
||
"arm_slider": 0.3, # 线性导轨臂：1.07 body 2.14m × 0.35m，reach ≈ half length
|
||
}
|
||
|
||
# 未知型号回退臂展：realistic default for lab-scale arms
|
||
DEFAULT_FALLBACK_REACH: float = 0.4
|
||
|
||
def __init__(self, arm_reach: dict[str, float] | None = None):
|
||
self.arm_reach = {**self.DEFAULT_ARM_REACH, **(arm_reach or {})}
|
||
|
||
def is_reachable(self, arm_id: str, arm_pose: dict, target: dict) -> bool:
|
||
"""判断目标点是否在机械臂最大臂展半径内。
|
||
|
||
Uses OBB edge-to-edge distance when available (passed as _obb_dist),
|
||
otherwise falls back to center-to-center Euclidean distance.
|
||
|
||
Args:
|
||
arm_id: 机械臂型号 ID（用于查臂展）
|
||
arm_pose: {"x": float, "y": float, "theta": float}
|
||
target: {"x": float, "y": float, "z": float, "_obb_dist": float (optional)}
|
||
|
||
Returns:
|
||
True 如果目标在臂展半径内
|
||
"""
|
||
max_reach = self.arm_reach.get(arm_id, self.DEFAULT_FALLBACK_REACH)
|
||
if "_obb_dist" in target:
|
||
return target["_obb_dist"] <= max_reach
|
||
dx = target["x"] - arm_pose["x"]
|
||
dy = target["y"] - arm_pose["y"]
|
||
dist_sq = dx**2 + dy**2
|
||
return dist_sq <= max_reach**2
|