From f4c0e40a2542ade103fa5f4e9c84e455cd72cc19 Mon Sep 17 00:00:00 2001 From: yexiaozhou Date: Thu, 2 Apr 2026 13:33:38 +0800 Subject: [PATCH] feat(layout_optimizer): crossing penalty weighted by intersection length MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replace _line_of_sight_penalty (flat per-blocker) with _crossing_penalty (DEFAULT_WEIGHT_DISTANCE * crossing_length). Uses opening→arm-OBB endpoints. Applied regardless of reachability pass/fail. Co-Authored-By: Claude Opus 4.6 --- unilabos/layout_optimizer/constraints.py | 48 +++++------ .../tests/test_constraints.py | 84 ++++++++++++++++++- 2 files changed, 105 insertions(+), 27 deletions(-) diff --git a/unilabos/layout_optimizer/constraints.py b/unilabos/layout_optimizer/constraints.py index 08929cac..6dd74000 100644 --- a/unilabos/layout_optimizer/constraints.py +++ b/unilabos/layout_optimizer/constraints.py @@ -16,7 +16,7 @@ from .obb import ( obb_corners, obb_min_distance, obb_penetration_depth, - segment_intersects_obb, + segment_obb_intersection_length, ) if TYPE_CHECKING: @@ -298,10 +298,7 @@ def _evaluate_single( arm_dev = device_map.get(arm_id) target_dev = device_map.get(target_device_id) - # Distance from target's opening surface center to nearest point on arm OBB. - # This naturally enforces orientation: a device facing away has its opening - # far from the arm, so it fails reachability without needing a separate - # facing penalty. + # opening surface center → nearest point on arm OBB if arm_dev and target_dev: opening_pt = _opening_surface_center(target_dev, target_p) arm_corners = obb_corners( @@ -310,27 +307,30 @@ def _evaluate_single( nearest = nearest_point_on_obb(opening_pt[0], opening_pt[1], arm_corners) dist = math.sqrt((opening_pt[0] - nearest[0])**2 + (opening_pt[1] - nearest[1])**2) else: + opening_pt = (target_p.x, target_p.y) + nearest = (arm_p.x, arm_p.y) dist = _device_distance_center(arm_p, target_p) or 0.0 + # 交叉惩罚始终计算（soft, 不依赖可达性结果） + crossing_cost = _crossing_penalty( + opening_pt, nearest, + arm_id, target_device_id, + device_map, placement_map, + ) + arm_pose = {"x": arm_p.x, "y": arm_p.y, "theta": arm_p.theta} target_point = {"x": target_p.x, "y": target_p.y, "z": 0.0} target_point["_obb_dist"] = dist if not reachability_checker.is_reachable(arm_id, arm_pose, target_point): if is_hard and not graduated: return math.inf - # Graduated: penalty proportional to overshoot + # Graduated: overshoot penalty + crossing cost max_reach = reachability_checker.arm_reach.get(arm_id, 2.0) overshoot = max(0.0, dist - max_reach) w = effective_weight * (HARD_MULTIPLIER if is_hard else 1.0) - return w * overshoot * 10.0 + return w * overshoot * 10.0 + crossing_cost - # Line-of-sight penalty: penalize if any other device OBB blocks - # the path from opening to arm - los_cost = _line_of_sight_penalty( - arm_id, arm_p, target_device_id, target_p, - device_map, placement_map, effective_weight, - ) - return los_cost + return crossing_cost if rule == "prefer_aligned": alignment_cost = sum( @@ -576,23 +576,19 @@ def _constraint_display_name(c: Constraint) -> str: return c.rule_name -def _line_of_sight_penalty( +def _crossing_penalty( + opening_pt: tuple[float, float], + arm_nearest_pt: tuple[float, float], arm_id: str, - arm_p: Placement, target_id: str, - target_p: Placement, device_map: dict[str, Device], placement_map: dict[str, Placement], - weight: float, ) -> float: - """Penalty for other devices blocking the line from target to arm center. + """交叉惩罚：其他设备 OBB 遮挡 opening→arm 路径的长度加权 penalty。 - For each other device whose OBB intersects the segment (target_center → arm_center), - adds a penalty proportional to the weight. This encourages layouts where - the arm has a clear path to each target. + Soft penalty，权重 = DEFAULT_WEIGHT_DISTANCE * 穿过各遮挡设备 OBB 的线段长度之和。 + 始终生效（不论可达性是否通过），为 DE 提供清晰的梯度信号。 """ - p1 = (target_p.x, target_p.y) - p2 = (arm_p.x, arm_p.y) cost = 0.0 for dev_id, p in placement_map.items(): if dev_id == arm_id or dev_id == target_id: @@ -601,6 +597,6 @@ def _line_of_sight_penalty( if dev is None: continue corners = obb_corners(p.x, p.y, dev.bbox[0], dev.bbox[1], p.theta) - if segment_intersects_obb(p1, p2, corners): - cost += weight * 2.0 # penalty per blocking device + crossing_len = segment_obb_intersection_length(opening_pt, arm_nearest_pt, corners) + cost += DEFAULT_WEIGHT_DISTANCE * crossing_len return cost diff --git a/unilabos/layout_optimizer/tests/test_constraints.py b/unilabos/layout_optimizer/tests/test_constraints.py index ac8b3a68..68c73f9a 100644 --- a/unilabos/layout_optimizer/tests/test_constraints.py +++ b/unilabos/layout_optimizer/tests/test_constraints.py @@ -5,11 +5,15 @@ import math import pytest from ..constraints import ( + _crossing_penalty, + _opening_surface_center, + DEFAULT_WEIGHT_DISTANCE, evaluate_constraints, evaluate_default_hard_constraints, ) from ..mock_checkers import MockCollisionChecker, MockReachabilityChecker -from ..models import Constraint, Device, Lab, Placement +from ..models import Constraint, Device, Opening, Placement, Lab +from ..obb import nearest_point_on_obb, obb_corners def _make_devices(): @@ -421,3 +425,81 @@ class TestGraduatedHardConstraints: devices, placements, _make_lab(), constraints, checker, ) assert not math.isinf(cost) + + +class TestCrossingPenalty: + """_crossing_penalty: 交叉长度加权的 soft penalty。""" + + def _make_device(self, dev_id, bbox=(0.5, 0.5), direction=(0.0, -1.0)): + return Device( + id=dev_id, name=dev_id, device_type="static", + bbox=bbox, height=0.3, + openings=[Opening(direction=direction, label="front")], + ) + + def test_no_blockers_returns_zero(self): + """arm 与 target 之间无遮挡设备 → 交叉代价为 0。""" + arm = self._make_device("arm", bbox=(2.14, 0.35)) + target = self._make_device("target") + arm_p = Placement(device_id="arm", x=2.0, y=1.0, theta=0.0) + target_p = Placement(device_id="target", x=0.5, y=1.0, theta=3.14159) + device_map = {"arm": arm, "target": target} + placement_map = {"arm": arm_p, "target": target_p} + + opening_pt = _opening_surface_center(target, target_p) + arm_corners = obb_corners(arm_p.x, arm_p.y, arm.bbox[0], arm.bbox[1], arm_p.theta) + nearest = nearest_point_on_obb(opening_pt[0], opening_pt[1], arm_corners) + + cost = _crossing_penalty( + opening_pt, nearest, + "arm", "target", + device_map, placement_map, + ) + assert cost == 0.0 + + def test_one_blocker_proportional_to_length(self): + """一个遮挡设备 → cost = DEFAULT_WEIGHT_DISTANCE * 穿过长度。""" + arm = self._make_device("arm", bbox=(2.14, 0.35)) + target = self._make_device("target") + blocker = self._make_device("blocker", bbox=(0.5, 0.5)) + arm_p = Placement(device_id="arm", x=3.0, y=1.0, theta=0.0) + target_p = Placement(device_id="target", x=0.0, y=1.0, theta=0.0) + blocker_p = Placement(device_id="blocker", x=1.5, y=1.0, theta=0.0) + device_map = {"arm": arm, "target": target, "blocker": blocker} + placement_map = {"arm": arm_p, "target": target_p, "blocker": blocker_p} + + opening_pt = _opening_surface_center(target, target_p) + arm_corners = obb_corners(arm_p.x, arm_p.y, arm.bbox[0], arm.bbox[1], arm_p.theta) + nearest = nearest_point_on_obb(opening_pt[0], opening_pt[1], arm_corners) + + cost = _crossing_penalty( + opening_pt, nearest, + "arm", "target", + device_map, placement_map, + ) + # blocker 宽 0.5m，theta=0，路径水平 → 穿过长度 ≈ 0.5m + # cost = DEFAULT_WEIGHT_DISTANCE * 0.5 = 100 * 0.5 = 50 + assert cost > 0 + assert abs(cost - DEFAULT_WEIGHT_DISTANCE * 0.5) < DEFAULT_WEIGHT_DISTANCE * 0.1 + + def test_blocker_off_path_returns_zero(self): + """不在路径上的设备 → 交叉代价为 0。""" + arm = self._make_device("arm", bbox=(2.14, 0.35)) + target = self._make_device("target") + bystander = self._make_device("bystander", bbox=(0.5, 0.5)) + arm_p = Placement(device_id="arm", x=3.0, y=1.0, theta=0.0) + target_p = Placement(device_id="target", x=0.0, y=1.0, theta=0.0) + bystander_p = Placement(device_id="bystander", x=1.5, y=3.0, theta=0.0) + device_map = {"arm": arm, "target": target, "bystander": bystander} + placement_map = {"arm": arm_p, "target": target_p, "bystander": bystander_p} + + opening_pt = _opening_surface_center(target, target_p) + arm_corners = obb_corners(arm_p.x, arm_p.y, arm.bbox[0], arm.bbox[1], arm_p.theta) + nearest = nearest_point_on_obb(opening_pt[0], opening_pt[1], arm_corners) + + cost = _crossing_penalty( + opening_pt, nearest, + "arm", "target", + device_map, placement_map, + ) + assert cost == 0.0