open_pub/Uni-Lab-OS
1
0
Fork 0
mirror of https://github.com/deepmodeling/Uni-Lab-OS synced 2026-05-23 17:19:56 +00:00
Code Issues Packages Projects Releases Wiki Activity

feat: add layout_optimizer package for automatic layout of devices

Browse Source 
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
This commit is contained in:
yexiaozhou
2026-03-31 01:00:09 +08:00
parent 3f75ca4ea3
commit 64eeed56a1
37 changed files with 19226 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

444
unilabos/layout_optimizer/README.md Normal file
View File

@@ -0,0 +1,444 @@
# Layout Optimizer Handover
**Date**: 2026-03-25 | **Branch**: `feat/layout_optimize_end2end_test` | **Commit**: `1c811ff` | **Tests**: 193 (183 pass + 10 LLM skip w/o API key)
This package is a standalone lab layout optimizer. It takes a device list + constraints and returns optimized placements. Your integration points are the HTTP API and the LLM skill document.
---
## 1. Full Pipeline Overview
```
User NL request
│
▼
┌─────────────────┐ skill doc: llm_skill/layout_intent_translator.md
│ LLM Agent │◄── + device list from scene (GET /devices)
│ (your side) │ + schema discovery (GET /interpret/schema)
└────────┬────────┘
│ structured intents JSON
▼
POST /interpret ← intent_interpreter.py (pure translation)
│
│ { constraints, translations, workflow_edges, errors }
▼
User confirms ← translations have human-readable explanations
│
▼
POST /optimize ← full pipeline below
│
┌────┴─────────────────────────────────────────┐
│ 1. Device catalog (device_catalog.py) │
│ footprints.json → Device objects │
│ bbox, height, openings per device │
│ │
│ 2. Seeder (seeders.py) │
│ Force-directed initial placement │
│ Presets: compact_outward, spread_inward, │
│ workflow_cluster, row_fallback │
│ Accounts for openings, workflow edges │
│ │
│ 3. DE Optimizer (optimizer.py) │
│ scipy differential_evolution │
│ 3N-dim: [x0, y0, θ0, x1, y1, θ1, ...] │
│ Cost = hard_penalties + soft_penalties │
│ Graduated collision penalties (not binary) │
│ │
│ 4. θ snap (optimizer.snap_theta) │
│ Snap near-cardinal angles to 0/90/180/270 │
│ │
│ 5. Final eval (constraints.py) │
│ Binary pass/fail for response.success │
└──────────────────────────────────────────────┘
│
▼
{ placements, cost, success }
```
---
## 2. API Reference
### `POST /interpret` — LLM intent → constraints
Translates semantic intents into optimizer constraints. The LLM agent calls this after translating user NL.
**Request:**
```json
{
"intents": [
{
"intent": "reachable_by",
"params": {"arm": "arm_slider", "targets": ["opentrons_liquid_handler", "inheco_odtc_96xl"]},
"description": "Robot arm must reach these devices"
},
{
"intent": "workflow_hint",
"params": {"workflow": "pcr", "devices": ["device_a", "device_b", "device_c"]},
"description": "PCR workflow order"
},
{
"intent": "close_together",
"params": {"devices": ["device_a", "device_b"], "priority": "high"},
"description": "Keep these close"
}
]
}
```
**Response:**
```json
{
"constraints": [
{"type": "hard", "rule_name": "reachability", "params": {"arm_id": "arm_slider", "target_device_id": "opentrons_liquid_handler"}, "weight": 1.0},
...
],
"translations": [
{
"source_intent": "reachable_by",
"source_description": "Robot arm must reach these devices",
"source_params": {"arm": "arm_slider", "targets": ["..."]},
"generated_constraints": [...],
"explanation": "机械臂 'arm_slider' 需要能够到达 2 个目标设备",
"confidence": "high"
}
],
"workflow_edges": [["device_a", "device_b"], ["device_b", "device_c"]],
"errors": []
}
```
The `constraints` and `workflow_edges` arrays pass directly to `/optimize` — no transformation needed.
### `GET /interpret/schema` — LLM discovery
Returns all 10 intent types with parameter specs. LLM agent should call this before translating.
### `POST /optimize` — Run layout optimization
**Request:**
```json
{
"devices": [
{"id": "thermo_orbitor_rs2_hotel", "name": "Plate Hotel", "device_type": "static"},
{"id": "arm_slider", "name": "Robot Arm", "device_type": "articulation"},
...
],
"lab": {"width": 6.0, "depth": 4.0},
"constraints": [...],
"workflow_edges": [["device_a", "device_b"]],
"seeder": "compact_outward",
"run_de": true,
"maxiter": 200,
"seed": 42
}
```
**Response:**
```json
{
"placements": [
{
"device_id": "thermo_orbitor_rs2_hotel",
"uuid": "thermo_orbitor_rs2_hotel",
"position": {"x": 1.33, "y": 2.35, "z": 0.0},
"rotation": {"x": 0.0, "y": 0.0, "z": 1.5708}
},
...
],
"cost": 0.0,
"success": true,
"seeder_used": "compact_outward",
"de_ran": true
}
```
`position`/`rotation` format matches Cloud's `CommonPositionType`. `rotation.z` is θ in radians.
### `GET /devices` — Device catalog
Returns all known devices with bbox, openings, model paths. The LLM agent should receive this list as context so it can resolve fuzzy device names.
### `GET /health`
Returns `{"status": "ok"}`.
---
## 3. Intent Types (10 total)
| Intent | Params | Generates | Type |
|--------|--------|-----------|------|
| `reachable_by` | `arm` (str), `targets` (list[str]) | `reachability` per target | hard |
| `close_together` | `devices` (list[str]), `priority` (low/medium/high) | `minimize_distance` per pair | soft |
| `far_apart` | `devices` (list[str]), `priority` | `maximize_distance` per pair | soft |
| `max_distance` | `device_a`, `device_b`, `distance` (float m) | `distance_less_than` | hard |
| `min_distance` | `device_a`, `device_b`, `distance` (float m) | `distance_greater_than` | hard |
| `min_spacing` | `min_gap` (float m, default 0.3) | `min_spacing` | hard |
| `workflow_hint` | `workflow` (str), `devices` (ordered list[str]) | `minimize_distance` consecutive + `workflow_edges` | soft |
| `face_outward` | (none) | `prefer_orientation_mode` outward | soft |
| `face_inward` | (none) | `prefer_orientation_mode` inward | soft |
| `align_cardinal` | (none) | `prefer_aligned` | soft |
Priority weights: `low=1.0`, `medium=3.0`, `high=8.0`.
---
## 4. LLM Integration Guide
### What You Need to Build (Your Side)
The LLM agent that converts user natural language → structured intents JSON. We provide:
1. **Skill document** (`llm_skill/layout_intent_translator.md`) — system prompt for the LLM. Contains intent schema, device name resolution rules, translation rules, and PCR workflow examples.
2. **Runtime schema** (`GET /interpret/schema`) — machine-readable intent specs. LLM agent should call this for discovery.
3. **Device context** — before translating, feed the LLM the scene's device list (from `GET /devices` or your scene state). The LLM uses this to resolve fuzzy names like "PCR machine" → `inheco_odtc_96xl`.
### Integration Flow
```
1. User enters NL request in Cloud UI
2. Your LLM agent receives:
- User message
- Scene device list (id, name, type, bbox)
- Skill doc as system prompt
- Optional: GET /interpret/schema for discovery
3. LLM outputs: {"intents": [...]}
4. POST /interpret with LLM output
5. Show user the translations for confirmation
6. POST /optimize with confirmed constraints + workflow_edges
7. Apply placements to scene
```
### Device Name Resolution (handled by LLM, not by optimizer)
The skill doc teaches the LLM to match fuzzy names:
- "PCR machine" / "thermal cycler" → `inheco_odtc_96xl`
- "liquid handler" / "pipetting robot" → `opentrons_liquid_handler`
- "plate hotel" / "storage" → `thermo_orbitor_rs2_hotel`
- "robot arm" / "the arm" → device with `type: articulation`
- "plate sealer" → `agilent_plateloc`
No search endpoint needed — the device list is already in context.
### Tested LLM Outputs
We tested with Claude Sonnet (via subagent, no API key required). Examples:
**Input**: "Take plate from hotel, prepare sample in the pipetting robot, seal it, then run thermal cycling. The arm handles all transfers. Keep liquid handler and sealer close, minimum 15cm gap."
**LLM produced**: `reachable_by` (arm→4 devices), `workflow_hint` (correct PCR order), `close_together` (high, LH+sealer), `min_distance` (0.15m, LH+sealer)
**Input**: "I want an automatic PCR lab, make it compact and neat"
**LLM produced**: `reachable_by`, `workflow_hint`, `close_together` (all devices), `min_spacing` (0.05m), `align_cardinal`
All outputs pass through `/interpret` → `/optimize` successfully.
---
## 5. Constraint System Details
### Hard Constraints (cost = ∞ on violation)
| Rule Name | Params | What it checks |
|-----------|--------|---------------|
| `no_collision` | (default, always on) | OBB-SAT pairwise collision between all devices |
| `within_bounds` | (default, always on) | All devices within lab boundary |
| `reachability` | `arm_id`, `target_device_id` | Target center within arm reach radius |
| `distance_less_than` | `device_a`, `device_b`, `distance` | OBB edge-to-edge distance ≤ threshold |
| `distance_greater_than` | `device_a`, `device_b`, `distance` | OBB edge-to-edge distance ≥ threshold |
| `min_spacing` | `min_gap` | All device pairs have ≥ min_gap edge-to-edge |
### Soft Constraints (weighted penalty)
| Rule Name | Params | What it minimizes |
|-----------|--------|------------------|
| `minimize_distance` | `device_a`, `device_b` | OBB edge-to-edge distance × weight |
| `maximize_distance` | `device_a`, `device_b` | 1/(distance+ε) × weight |
| `prefer_orientation_mode` | `mode` (outward/inward) | Angle between opening direction and ideal direction |
| `prefer_aligned` | (none) | Deviation from nearest 90° angle |
| `prefer_seeder_orientation` | (none) | Deviation from seeder-assigned θ |
### Graduated Penalties (DE internals)
Default hard constraints (collision, boundary) use **graduated penalties** during DE optimization — proportional to penetration depth / overshoot distance. This gives DE a smooth gradient instead of binary inf. Final evaluation uses binary mode for pass/fail reporting.
---
## 6. Checker Architecture (Mock → Real)
```
interfaces.py (Protocol definitions)
├── CollisionChecker.check(placements) → collisions
├── CollisionChecker.check_bounds(placements, w, d) → out_of_bounds
└── ReachabilityChecker.is_reachable(arm_id, arm_pose, target) → bool
mock_checkers.py (current, no ROS)
├── MockCollisionChecker — OBB SAT
└── MockReachabilityChecker — Euclidean distance, 100m fallback for unknown arms
ros_checkers.py (for ROS2/MoveIt2 integration)
├── MoveItCollisionChecker — python-fcl direct + OBB fallback
└── IKFastReachabilityChecker — precomputed voxel O(1) + live IK fallback
└── create_checkers(mode) — factory, controlled by LAYOUT_CHECKER_MODE env var
```
To switch to real checkers: `LAYOUT_CHECKER_MODE=moveit` + pass MoveIt2 instance.
---
## 7. File Inventory
### Core Pipeline
| File | Lines | Purpose |
|------|-------|---------|
| `models.py` | 96 | Dataclasses: Device, Lab, Placement, Constraint, Intent, Opening |
| `device_catalog.py` | — | Loads devices from footprints.json + uni-lab-assets + registry |
| `footprints.json` | 183KB | 499 device bounding boxes, heights, openings (offline extracted) |
| `seeders.py` | — | Force-directed initial layout with presets |
| `optimizer.py` | 196 | scipy DE optimizer, 3N encoding, seed injection, θ snap |
| `constraints.py` | — | Unified constraint evaluation (hard + soft + graduated) |
| `intent_interpreter.py` | 315 | 10 intent handlers, pure translation, no side effects |
| `obb.py` | — | OBB geometry: corners, overlap SAT, min_distance, penetration_depth |
| `server.py` | 510 | FastAPI: /interpret, /interpret/schema, /optimize, /devices |
### Reference / Utilities
| File | Purpose |
|------|---------|
| `extract_footprints.py` | How footprints.json was generated (offline STL/GLB → 2D bbox extraction via trimesh) |
> **Note**: `optimizer.py` and `seeders.py` import `pencil_integration` for fallback initial layout. This module is removed from the handover (not part of the pipeline — see D2). Replace with your own initial layout or simply rely on the seeder (`seeders.py`), which is the primary code path.
### Integration Layer
| File | Purpose |
|------|---------|
| `interfaces.py` | Protocol definitions for CollisionChecker / ReachabilityChecker |
| `mock_checkers.py` | Dev-mode checkers (OBB collision, Euclidean reachability) |
| `ros_checkers.py` | MoveIt2/IKFast adapters for real collision + reachability |
### LLM
| File | Purpose |
|------|---------|
| `llm_skill/layout_intent_translator.md` | System prompt for LLM: intent schema, device resolution, translation rules, examples |
### Configuration
| File | Purpose |
|------|---------|
| `pyproject.toml` | Package deps: scipy, numpy, fastapi, uvicorn, pydantic |
### Tests (193 total: 183 pass + 10 skip without API key)
| File | Tests | Coverage |
|------|-------|----------|
| `test_intent_interpreter.py` | 19 | All 10 handlers, validation, priority, multi-intent |
| `test_interpret_api.py` | 6 | /interpret and /interpret/schema endpoints |
| `test_e2e_pcr_pipeline.py` | 12 | Full pipeline: interpret → optimize → verify placements |
| `test_llm_skill.py` | 10 | Real LLM fuzzy input → structured output (needs ANTHROPIC_API_KEY) |
| `test_constraints.py` | — | Constraint evaluation, hard/soft, graduated penalties |
| `test_optimizer.py` | — | DE optimizer, vector encoding, bounds |
| `test_mock_checkers.py` | — | MockCollisionChecker, MockReachabilityChecker |
| `test_ros_checkers.py` | — | MoveIt2/IKFast adapter tests |
| `test_seeders.py` | — | Force-directed seeder presets |
| `test_device_catalog.py` | — | Device loading, footprint merging |
| `test_obb.py` | — | OBB geometry functions |
| `test_bugfixes_v2.py` | — | Regression: duplicate IDs, orientation, min_spacing |
---
## 8. How to Run
### Quick Start
```bash
# Install
pip install -e ".[dev]"
# Run server
uvicorn layout_optimizer.server:app --host 0.0.0.0 --port 8000 --reload
# Run tests
pytest layout_optimizer/tests/ -v
# Run LLM skill tests (needs API key)
ANTHROPIC_API_KEY=sk-... pytest layout_optimizer/tests/test_llm_skill.py -v
```
### Dependencies
- Python ≥ 3.10
- scipy, numpy, fastapi, uvicorn, pydantic
- Optional: anthropic (for LLM skill tests)
- Optional: python-fcl (for real collision checking, not needed for mock mode)
### Environment Variables
| Variable | Default | Purpose |
|----------|---------|---------|
| `UNI_LAB_ASSETS_DIR` | `../uni-lab-assets` | Path to device 3D models |
| `UNI_LAB_OS_DEVICE_MESH_DIR` | `Uni-Lab-OS/unilabos/device_mesh/devices` | Registry device meshes |
| `LAYOUT_CHECKER_MODE` | `mock` | `mock` or `moveit` for checker selection |
| `ANTHROPIC_API_KEY` | (none) | For LLM skill tests |
---
## 9. Known Limitations
1. **Mock reachability**: `MockReachabilityChecker` uses 100m fallback for unknown arm IDs — effectively "always reachable" for mock mode. Real arm reach requires `ros_checkers.py` with MoveIt2.
2. **No real LLM in tests**: `test_llm_skill.py` tests are skipped without `ANTHROPIC_API_KEY`. We verified with Claude Sonnet subagent that the skill doc produces correct output for PCR workflow scenarios.
3. **Opening data coverage**: 289/499 devices have opening direction annotations. Devices without openings default to local -Y as front with no alignment penalty.
4. **Single lab room**: No multi-room or corridor support yet. Lab is a single rectangle with optional rectangular obstacles.
5. **Intent interpreter is stateless**: It translates intents one-by-one with no cross-referencing between them. Duplicate/conflicting constraints are the LLM's responsibility to avoid.
---
## 10. Quick Verification (curl)
```bash
# 1. Health check
curl http://localhost:8000/health
# 2. Schema discovery
curl http://localhost:8000/interpret/schema | python3 -m json.tool
# 3. Interpret PCR workflow
curl -X POST http://localhost:8000/interpret \
-H "Content-Type: application/json" \
-d '{
"intents": [
{"intent": "reachable_by", "params": {"arm": "arm_slider", "targets": ["opentrons_liquid_handler", "inheco_odtc_96xl"]}, "description": "arm reaches targets"},
{"intent": "workflow_hint", "params": {"workflow": "pcr", "devices": ["thermo_orbitor_rs2_hotel", "opentrons_liquid_handler", "agilent_plateloc", "inheco_odtc_96xl"]}, "description": "PCR order"}
]
}' | python3 -m json.tool
# 4. Optimize (use constraints from step 3)
curl -X POST http://localhost:8000/optimize \
-H "Content-Type: application/json" \
-d '{
"devices": [
{"id": "thermo_orbitor_rs2_hotel", "name": "Plate Hotel"},
{"id": "arm_slider", "name": "Robot Arm", "device_type": "articulation"},
{"id": "opentrons_liquid_handler", "name": "Liquid Handler"},
{"id": "agilent_plateloc", "name": "Plate Sealer"},
{"id": "inheco_odtc_96xl", "name": "Thermal Cycler"}
],
"lab": {"width": 6.0, "depth": 4.0},
"constraints": [
{"type": "hard", "rule_name": "reachability", "params": {"arm_id": "arm_slider", "target_device_id": "opentrons_liquid_handler"}, "weight": 1.0},
{"type": "hard", "rule_name": "reachability", "params": {"arm_id": "arm_slider", "target_device_id": "inheco_odtc_96xl"}, "weight": 1.0},
{"type": "soft", "rule_name": "minimize_distance", "params": {"device_a": "thermo_orbitor_rs2_hotel", "device_b": "opentrons_liquid_handler"}, "weight": 3.0},
{"type": "soft", "rule_name": "minimize_distance", "params": {"device_a": "opentrons_liquid_handler", "device_b": "agilent_plateloc"}, "weight": 3.0},
{"type": "soft", "rule_name": "minimize_distance", "params": {"device_a": "agilent_plateloc", "device_b": "inheco_odtc_96xl"}, "weight": 3.0}
],
"workflow_edges": [
["thermo_orbitor_rs2_hotel", "opentrons_liquid_handler"],
["opentrons_liquid_handler", "agilent_plateloc"],
["agilent_plateloc", "inheco_odtc_96xl"]
],
"run_de": true,
"maxiter": 100,
"seed": 42
}' | python3 -m json.tool
```

9
unilabos/layout_optimizer/__init__.py Normal file
View File

@@ -0,0 +1,9 @@
"""Layout Optimizer — AI 实验室布局自动排布。
独立开发包，无 ROS 依赖。集成阶段合并到 Uni-Lab-OS。
"""
from .models import Constraint, Device, Lab, Opening, Placement
from .optimizer import optimize
__all__ = ["Device", "Lab", "Opening", "Placement", "Constraint", "optimize"]

463
unilabos/layout_optimizer/constraints.py Normal file
View File

@@ -0,0 +1,463 @@
"""约束体系：硬约束 / 软约束定义与统一评估。
硬约束违反 → cost = inf（方案直接淘汰）
软约束违反 → 加权 penalty 累加到 cost
"""
from __future__ import annotations
import math
from typing import TYPE_CHECKING
from .models import Constraint, Device, Lab, Placement
from .obb import (
nearest_point_on_obb,
obb_corners,
obb_min_distance,
obb_penetration_depth,
segment_intersects_obb,
)
if TYPE_CHECKING:
from .interfaces import CollisionChecker, ReachabilityChecker
def evaluate_constraints(
devices: list[Device],
placements: list[Placement],
lab: Lab,
constraints: list[Constraint],
collision_checker: CollisionChecker,
reachability_checker: ReachabilityChecker | None = None,
) -> float:
"""统一评估所有约束，返回总 cost。
Args:
devices: 设备列表（与 placements 一一对应）
placements: 当前布局方案
lab: 实验室平面图
constraints: 约束规则列表
collision_checker: 碰撞检测实例
reachability_checker: 可达性检测实例（可选）
Returns:
总 cost。硬约束违反返回 inf，否则为软约束 penalty 之和。
"""
device_map = {d.id: d for d in devices}
placement_map = {p.device_id: p for p in placements}
total_cost = 0.0
for c in constraints:
cost = _evaluate_single(
c, device_map, placement_map, lab, collision_checker, reachability_checker
)
if math.isinf(cost):
return math.inf
total_cost += cost
return total_cost
def evaluate_default_hard_constraints(
devices: list[Device],
placements: list[Placement],
lab: Lab,
collision_checker: CollisionChecker,
*,
graduated: bool = True,
collision_weight: float = 1000.0,
boundary_weight: float = 1000.0,
) -> float:
"""评估默认硬约束（碰撞 + 边界），无需显式声明约束列表。
始终生效，用于 cost function 的基础检查。
When graduated=True (default), returns a penalty proportional to the
severity of each violation instead of binary inf. This gives DE a
smooth gradient so it can fix specific collision pairs instead of
discarding near-optimal layouts entirely.
When graduated=False, uses the legacy binary inf behaviour.
"""
if not graduated:
return _evaluate_hard_binary(devices, placements, lab, collision_checker)
device_map = {d.id: d for d in devices}
cost = 0.0
# Graduated collision penalty: sum of penetration depths
n = len(placements)
for i in range(n):
for j in range(i + 1, n):
di, dj = device_map[placements[i].device_id], device_map[placements[j].device_id]
ci = obb_corners(placements[i].x, placements[i].y,
di.bbox[0], di.bbox[1], placements[i].theta)
cj = obb_corners(placements[j].x, placements[j].y,
dj.bbox[0], dj.bbox[1], placements[j].theta)
depth = obb_penetration_depth(ci, cj)
if depth > 0:
cost += collision_weight * depth
# Graduated boundary penalty: sum of overshoot distances (rotation-aware)
for p in placements:
dev = device_map[p.device_id]
hw, hd = p.rotated_bbox(dev)
# How far each edge exceeds the lab boundary
overshoot = 0.0
overshoot += max(0.0, hw - p.x) # left wall
overshoot += max(0.0, (p.x + hw) - lab.width) # right wall
overshoot += max(0.0, hd - p.y) # bottom wall
overshoot += max(0.0, (p.y + hd) - lab.depth) # top wall
cost += boundary_weight * overshoot
return cost
def _evaluate_hard_binary(
devices: list[Device],
placements: list[Placement],
lab: Lab,
collision_checker: CollisionChecker,
) -> float:
"""Legacy binary hard-constraint evaluation (inf or 0)."""
checker_placements = _to_checker_format(devices, placements)
collisions = collision_checker.check(checker_placements)
if collisions:
return math.inf
if hasattr(collision_checker, "check_bounds"):
oob = collision_checker.check_bounds(checker_placements, lab.width, lab.depth)
if oob:
return math.inf
return 0.0
def _evaluate_single(
constraint: Constraint,
device_map: dict[str, Device],
placement_map: dict[str, Placement],
lab: Lab,
collision_checker: CollisionChecker,
reachability_checker: ReachabilityChecker | None,
) -> float:
"""评估单条约束规则。"""
rule = constraint.rule_name
params = constraint.params
is_hard = constraint.type == "hard"
if rule == "no_collision":
checker_placements = _to_checker_format_from_maps(device_map, placement_map)
collisions = collision_checker.check(checker_placements)
if collisions:
return math.inf if is_hard else constraint.weight * len(collisions)
return 0.0
if rule == "within_bounds":
checker_placements = _to_checker_format_from_maps(device_map, placement_map)
if hasattr(collision_checker, "check_bounds"):
oob = collision_checker.check_bounds(
checker_placements, lab.width, lab.depth
)
if oob:
return math.inf if is_hard else constraint.weight * len(oob)
return 0.0
if rule == "distance_less_than":
a_id, b_id = params["device_a"], params["device_b"]
max_dist = params["distance"]
da, db = device_map.get(a_id), device_map.get(b_id)
pa, pb = placement_map.get(a_id), placement_map.get(b_id)
if pa is None or pb is None:
return 0.0
if da and db:
dist = _device_distance_obb(da, pa, db, pb)
else:
dist = _device_distance_center(pa, pb) or 0.0
if dist > max_dist:
return math.inf if is_hard else constraint.weight * (dist - max_dist)
return 0.0
if rule == "distance_greater_than":
a_id, b_id = params["device_a"], params["device_b"]
min_dist = params["distance"]
da, db = device_map.get(a_id), device_map.get(b_id)
pa, pb = placement_map.get(a_id), placement_map.get(b_id)
if pa is None or pb is None:
return 0.0
if da and db:
dist = _device_distance_obb(da, pa, db, pb)
else:
dist = _device_distance_center(pa, pb) or 0.0
if dist < min_dist:
return math.inf if is_hard else constraint.weight * (min_dist - dist)
return 0.0
if rule == "minimize_distance":
a_id, b_id = params["device_a"], params["device_b"]
da, db = device_map.get(a_id), device_map.get(b_id)
pa, pb = placement_map.get(a_id), placement_map.get(b_id)
if pa is None or pb is None:
return 0.0
if da and db:
dist = _device_distance_obb(da, pa, db, pb)
else:
dist = _device_distance_center(pa, pb) or 0.0
return constraint.weight * dist
if rule == "maximize_distance":
a_id, b_id = params["device_a"], params["device_b"]
da, db = device_map.get(a_id), device_map.get(b_id)
pa, pb = placement_map.get(a_id), placement_map.get(b_id)
if pa is None or pb is None:
return 0.0
if da and db:
dist = _device_distance_obb(da, pa, db, pb)
else:
dist = _device_distance_center(pa, pb) or 0.0
max_possible = math.sqrt(lab.width**2 + lab.depth**2)
return constraint.weight * (max_possible - dist)
if rule == "min_spacing":
min_gap = params.get("min_gap", 0.0)
all_placements = list(placement_map.values())
for i in range(len(all_placements)):
for j in range(i + 1, len(all_placements)):
pi, pj = all_placements[i], all_placements[j]
di = device_map.get(pi.device_id)
dj = device_map.get(pj.device_id)
if di and dj:
dist = _device_distance_obb(di, pi, dj, pj)
else:
dist = _device_distance_center(pi, pj) or 0.0
if dist < min_gap:
if is_hard:
return math.inf
return constraint.weight * (min_gap - dist)
return 0.0
if rule == "reachability":
if reachability_checker is None:
return 0.0
arm_id = params["arm_id"]
target_device_id = params["target_device_id"]
arm_p = placement_map.get(arm_id)
target_p = placement_map.get(target_device_id)
if arm_p is None or target_p is None:
return 0.0
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.
if arm_dev and target_dev:
opening_pt = _opening_surface_center(target_dev, target_p)
arm_corners = obb_corners(
arm_p.x, arm_p.y, arm_dev.bbox[0], arm_dev.bbox[1], arm_p.theta,
)
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:
dist = _device_distance_center(arm_p, target_p) or 0.0
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:
return math.inf
# Graduated: penalty proportional to overshoot
max_reach = reachability_checker.arm_reach.get(arm_id, 2.0)
overshoot = max(0.0, dist - max_reach)
return constraint.weight * overshoot * 10.0
# 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, constraint.weight,
)
return los_cost
if rule == "prefer_aligned":
alignment_cost = sum(
(1 - math.cos(4 * p.theta)) / 2 for p in placement_map.values()
)
if is_hard:
return math.inf if alignment_cost > 1e-6 else 0.0
return constraint.weight * alignment_cost
if rule == "prefer_seeder_orientation":
target_thetas = params.get("target_thetas", {})
cost = 0.0
for dev_id, target in target_thetas.items():
p = placement_map.get(dev_id)
if p is None:
continue
# Circular distance: (1 - cos(diff)) / 2 gives 0..1 range
diff = p.theta - target
cost += (1 - math.cos(diff)) / 2
return constraint.weight * cost
if rule == "prefer_orientation_mode":
mode = params.get("mode", "outward")
center_x = lab.width / 2
center_y = lab.depth / 2
cost = 0.0
for dev_id, p in placement_map.items():
dev = device_map.get(dev_id)
if dev is None:
continue
target = _desired_theta(
p.x, p.y, center_x, center_y, dev, mode,
)
if target is None:
continue
diff = p.theta - target
cost += (1 - math.cos(diff)) / 2
return constraint.weight * cost
# 未知约束类型，忽略
return 0.0
def _desired_theta(
x: float, y: float,
center_x: float, center_y: float,
device: Device, mode: str,
) -> float | None:
"""Compute desired theta for outward/inward facing at the given position."""
dx = x - center_x
dy = y - center_y
if abs(dx) < 1e-9 and abs(dy) < 1e-9:
return None # At center, no preferred direction
angle_to_device = math.atan2(dy, dx)
front = device.openings[0].direction if device.openings else (0.0, -1.0)
front_angle = math.atan2(front[1], front[0])
if mode == "outward":
target = angle_to_device
elif mode == "inward":
target = angle_to_device + math.pi
else:
return None
return (target - front_angle) % (2 * math.pi)
def _device_distance_center(a: Placement | None, b: Placement | None) -> float | None:
"""计算两设备中心的欧几里得距离（后备方法）。"""
if a is None or b is None:
return None
return math.sqrt((a.x - b.x) ** 2 + (a.y - b.y) ** 2)
def _device_distance_obb(
device_a: Device, placement_a: Placement,
device_b: Device, placement_b: Placement,
) -> float:
"""Minimum edge-to-edge distance between two devices using OBB."""
corners_a = obb_corners(
placement_a.x, placement_a.y,
device_a.bbox[0], device_a.bbox[1],
placement_a.theta,
)
corners_b = obb_corners(
placement_b.x, placement_b.y,
device_b.bbox[0], device_b.bbox[1],
placement_b.theta,
)
return obb_min_distance(corners_a, corners_b)
def _to_checker_format(
devices: list[Device], placements: list[Placement]
) -> list[dict]:
"""转换为 CollisionChecker.check() 接受的格式。"""
device_map = {d.id: d for d in devices}
result = []
for p in placements:
dev = device_map.get(p.device_id)
if dev is None:
continue
result.append({"id": p.device_id, "bbox": dev.bbox, "pos": (p.x, p.y, p.theta)})
return result
def _to_checker_format_from_maps(
device_map: dict[str, Device], placement_map: dict[str, Placement]
) -> list[dict]:
"""从 map 转换为 CollisionChecker.check() 接受的格式。"""
result = []
for dev_id, p in placement_map.items():
dev = device_map.get(dev_id)
if dev is None:
continue
result.append({"id": dev_id, "bbox": dev.bbox, "pos": (p.x, p.y, p.theta)})
return result
def _opening_surface_center(
device: Device, placement: Placement,
) -> tuple[float, float]:
"""Return the world-space center of the device's opening surface.
Computes where the opening direction intersects the device's bbox boundary,
then transforms to world coordinates. For a device facing away from the arm,
this point is on the far side — making the distance to the arm larger,
which naturally penalizes wrong orientation.
"""
front = device.openings[0].direction if device.openings else (0.0, -1.0)
dx, dy = front
w, h = device.bbox
# Scale factor to reach bbox edge in the opening direction
scales = []
if abs(dx) > 1e-9:
scales.append((w / 2) / abs(dx))
if abs(dy) > 1e-9:
scales.append((h / 2) / abs(dy))
scale = min(scales) if scales else 0.0
# Opening center in local frame
local_x = dx * scale
local_y = dy * scale
# Rotate to world frame and translate
cos_t = math.cos(placement.theta)
sin_t = math.sin(placement.theta)
world_x = placement.x + local_x * cos_t - local_y * sin_t
world_y = placement.y + local_x * sin_t + local_y * cos_t
return (world_x, world_y)
def _line_of_sight_penalty(
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.
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.
"""
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:
continue
dev = device_map.get(dev_id)
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
return cost

294
unilabos/layout_optimizer/device_catalog.py Normal file
View File

@@ -0,0 +1,294 @@
"""双源设备目录：从 uni-lab-assets 和 Uni-Lab-OS registry 加载设备。
数据流：
footprints.json (离线提取) + data.json (资产树) + registry device_mesh dirs
→ merge → Device 列表
footprints.json 由 extract_footprints.py 生成，包含碰撞包围盒、开口方向等。
"""
from __future__ import annotations
import json
import logging
from pathlib import Path
from .models import Device, Opening
logger = logging.getLogger(__name__)
# 默认路径（相对于本文件）
_THIS_DIR = Path(__file__).resolve().parent
_DEFAULT_FOOTPRINTS = _THIS_DIR / "footprints.json"
# 手动后备尺寸（trimesh 不可用时）
KNOWN_SIZES: dict[str, tuple[float, float]] = {
"elite_cs66_arm": (0.20, 0.20),
"elite_cs612_arm": (0.20, 0.20),
"ot2": (0.62, 0.50),
"agilent_bravo": (0.80, 0.65),
"thermo_orbitor_rs2": (0.45, 0.55),
"hplc_station": (0.60, 0.50),
"1_3m_hamilton_table": (1.30, 0.75),
}
DEFAULT_BBOX: tuple[float, float] = (0.6, 0.4)
# ---------- footprints.json 加载 ----------
_footprints_cache: dict[str, dict] | None = None
def load_footprints(path: str | Path = _DEFAULT_FOOTPRINTS) -> dict[str, dict]:
"""加载 footprints.json 并缓存。"""
global _footprints_cache
if _footprints_cache is not None:
return _footprints_cache
p = Path(path)
if not p.exists():
logger.warning("footprints.json not found at %s", p)
_footprints_cache = {}
return _footprints_cache
with open(p) as f:
_footprints_cache = json.load(f)
logger.info("Loaded %d footprints from %s", len(_footprints_cache), p)
return _footprints_cache
def reset_footprints_cache() -> None:
"""清除缓存（测试用）。"""
global _footprints_cache
_footprints_cache = None
# ---------- 从 footprints 构建 Device ----------
def _footprint_to_device(
device_id: str,
fp: dict,
name: str = "",
models_url_prefix: str = "/models",
) -> Device:
"""从 footprints.json 条目创建 Device。"""
bbox = tuple(fp.get("bbox", DEFAULT_BBOX))
openings = [
Opening(direction=tuple(o["direction"]), label=o.get("label", ""))
for o in fp.get("openings", [])
]
model_file = fp.get("model_file", "")
model_path = f"{models_url_prefix}/{device_id}/{model_file}" if model_file else ""
model_type = fp.get("model_type", "")
thumb_file = fp.get("thumbnail_file", "")
thumbnail_url = f"{models_url_prefix}/{device_id}/{thumb_file}" if thumb_file else ""
return Device(
id=device_id,
name=name or device_id.replace("_", " ").title(),
bbox=bbox,
device_type="articulation" if "robot" in device_id or "arm" in device_id or "flex" in device_id else "static",
height=fp.get("height", 0.4),
origin_offset=tuple(fp.get("origin_offset", [0.0, 0.0])),
openings=openings,
source=fp.get("source", "manual"),
model_path=model_path,
model_type=model_type,
thumbnail_url=thumbnail_url,
)
# ---------- 从 data.json 加载 ----------
def load_devices_from_assets(
data_json_path: str | Path,
footprints: dict[str, dict] | None = None,
models_url_prefix: str = "/models",
) -> list[Device]:
"""从 uni-lab-assets 的 data.json 加载设备列表。
如果设备在 footprints 中有条目，使用真实尺寸；否则使用默认值。
"""
path = Path(data_json_path)
if not path.exists():
logger.warning("data.json not found at %s, returning empty list", path)
return []
if footprints is None:
footprints = load_footprints()
with open(path) as f:
data = json.load(f)
devices: list[Device] = []
_flatten_tree(data, devices, footprints, models_url_prefix)
return devices
def _flatten_tree(
nodes: list[dict],
result: list[Device],
footprints: dict[str, dict],
models_url_prefix: str,
) -> None:
"""递归遍历树形结构，提取叶节点为 Device。"""
for node in nodes:
if "children" in node:
_flatten_tree(node["children"], result, footprints, models_url_prefix)
elif "id" in node:
device_id = node["id"]
name = node.get("label", device_id)
if device_id in footprints:
dev = _footprint_to_device(
device_id, footprints[device_id], name, models_url_prefix
)
else:
bbox = KNOWN_SIZES.get(device_id, DEFAULT_BBOX)
dev = Device(id=device_id, name=name, bbox=bbox, source="assets")
result.append(dev)
# ---------- 从 registry 加载 ----------
def load_devices_from_registry(
device_mesh_dir: str | Path,
footprints: dict[str, dict] | None = None,
models_url_prefix: str = "/models",
) -> list[Device]:
"""从 Uni-Lab-OS device_mesh/devices/ 加载 registry 设备。"""
d = Path(device_mesh_dir)
if not d.exists():
logger.warning("Registry dir not found at %s", d)
return []
if footprints is None:
footprints = load_footprints()
devices: list[Device] = []
for entry in sorted(d.iterdir()):
if not entry.is_dir():
continue
device_id = entry.name
if device_id in footprints:
dev = _footprint_to_device(
device_id, footprints[device_id], models_url_prefix=models_url_prefix
)
dev.source = "registry"
else:
bbox = KNOWN_SIZES.get(device_id, DEFAULT_BBOX)
dev = Device(id=device_id, name=device_id.replace("_", " ").title(), bbox=bbox, source="registry")
devices.append(dev)
return devices
# ---------- 合并与去重 ----------
def merge_device_lists(
registry_devices: list[Device],
asset_devices: list[Device],
) -> list[Device]:
"""合并双源设备列表，registry 优先。
对于同时存在于两个源的设备，使用 registry 条目的元数据，
但优先使用 assets 的 3D 模型路径和缩略图。
"""
merged: dict[str, Device] = {}
for dev in asset_devices:
merged[dev.id] = dev
for dev in registry_devices:
if dev.id in merged:
# registry 元数据优先，但保留 assets 的模型/缩略图
asset_dev = merged[dev.id]
dev.model_path = dev.model_path or asset_dev.model_path
dev.model_type = dev.model_type or asset_dev.model_type
dev.thumbnail_url = dev.thumbnail_url or asset_dev.thumbnail_url
if dev.bbox == DEFAULT_BBOX and asset_dev.bbox != DEFAULT_BBOX:
dev.bbox = asset_dev.bbox
dev.height = asset_dev.height
dev.origin_offset = asset_dev.origin_offset
dev.openings = asset_dev.openings
dev.source = "registry"
merged[dev.id] = dev
return list(merged.values())
# ---------- 统一解析器 ----------
def resolve_device(
device_id: str,
footprints: dict[str, dict] | None = None,
models_url_prefix: str = "/models",
) -> Device | None:
"""按 ID 查找单个设备。先查 footprints，再查 KNOWN_SIZES。"""
if footprints is None:
footprints = load_footprints()
if device_id in footprints:
return _footprint_to_device(
device_id, footprints[device_id], models_url_prefix=models_url_prefix
)
if device_id in KNOWN_SIZES:
bbox = KNOWN_SIZES[device_id]
return Device(id=device_id, name=device_id.replace("_", " ").title(), bbox=bbox, source="manual")
return None
# ---------- 向后兼容 ----------
def create_devices_from_list(
device_specs: list[dict],
) -> list[Device]:
"""从 API 请求中的设备列表创建 Device 对象（向后兼容）。
Args:
device_specs: [{"id": str, "name": str, "size": [w, d], "uuid": str}, ...]
size 可选，缺失时使用 footprints 或默认值。
"""
footprints = load_footprints()
devices = []
for spec in device_specs:
catalog_id = spec["id"]
# Use uuid as internal ID when available to ensure uniqueness
# (multiple instances of the same catalog device get different IDs)
dev_id = spec.get("uuid") or catalog_id
size = spec.get("size")
if size:
bbox = (float(size[0]), float(size[1]))
elif catalog_id in footprints:
bbox = tuple(footprints[catalog_id].get("bbox", DEFAULT_BBOX))
else:
bbox = KNOWN_SIZES.get(catalog_id, DEFAULT_BBOX)
openings = []
if catalog_id in footprints:
openings = [
Opening(direction=tuple(o["direction"]), label=o.get("label", ""))
for o in footprints[catalog_id].get("openings", [])
]
devices.append(
Device(
id=dev_id,
name=spec.get("name", catalog_id),
bbox=bbox,
device_type=spec.get("device_type", "static"),
openings=openings,
)
)
return devices

559
unilabos/layout_optimizer/extract_footprints.py Normal file
View File

@@ -0,0 +1,559 @@
"""从 STL/GLB 网格提取设备足迹（碰撞包围盒）。
运行方式:
conda activate phase3
python -m layout_optimizer.extract_footprints
输出 footprints.json 供 device_catalog.py 和 2D 规划器使用。
GLB root node rotation:
每个设备的 GLB 文件包含根节点旋转四元数，定义 STL 原生坐标到 glTF Y-up
约定的轴映射。extract_one_device() 读取 GLB JSON，提取旋转矩阵，
应用到 STL 包围盒后按 glTF 约定提取 2D 足迹 (X=width, Z=depth, Y=height)。
GLB scale 不应用——STL 文件已是米制坐标。
"""
from __future__ import annotations
import argparse
import json
import logging
import math
import os
import re
import struct
import xml.etree.ElementTree as ET
from pathlib import Path
logger = logging.getLogger(__name__)
# 测试设备的开口方向（手动标注）
# direction 为设备局部坐标系中的单位向量，[0, -1] 表示设备正前方
MANUAL_OPENINGS: dict[str, list[dict]] = {
"agilent_bravo": [{"direction": [0, -1], "label": "front_plate_slot"}],
"opentrons_liquid_handler": [{"direction": [0, -1], "label": "front_deck"}],
"opentrons_flex": [{"direction": [0, -1], "label": "front_deck"}],
"thermo_orbitor_rs2_hotel": [{"direction": [0, -1], "label": "front_door"}],
"hamilton_star": [{"direction": [0, -1], "label": "front_deck"}],
"tecan_spark_plate_reader": [{"direction": [0, -1], "label": "front_slot"}],
"highres_bio_plate_hotel_12": [{"direction": [0, -1], "label": "front_shelf"}],
"beckman_coulter_orbital_shaker_alp": [],
"liconic_str44_incubator": [{"direction": [0, -1], "label": "front_door"}],
"elite_robot": [], # 机械臂，无开口
}
# 手动尺寸后备（trimesh 提取失败时使用）
FALLBACK_SIZES: dict[str, tuple[float, float, float]] = {
"elite_robot": (0.20, 0.20, 0.10),
"elite_cs66_arm": (0.20, 0.20, 0.10),
"elite_cs612_arm": (0.20, 0.20, 0.10),
}
def extract_openings_from_xacro(
xacro_path: Path,
bbox_center_xy: tuple[float, float],
bbox_size_xy: tuple[float, float],
) -> list[dict]:
"""从 XACRO 文件自动提取设备开口方向。
解析 fixed joint 中包含 "socket" 的关节，计算其 XY 质心，与包围盒中心比较，
映射到最近的基本方向。
Args:
xacro_path: modal.xacro 文件路径
bbox_center_xy: 包围盒 XY 中心 (cx, cy)
bbox_size_xy: 包围盒 XY 尺寸 (w, d)
Returns:
[{"direction": [dx, dy], "label": "auto_xacro"}] 或 []
"""
# --- 方法1: ElementTree 解析（忽略 xacro 命名空间） ---
socket_positions: list[tuple[float, float]] = []
try:
xacro_text = xacro_path.read_text(encoding="utf-8", errors="replace")
# 去掉 xacro 命名空间前缀，避免 ElementTree 解析失败
xacro_text_clean = re.sub(r'\bxacro:', '', xacro_text)
root = ET.fromstring(xacro_text_clean)
for joint in root.iter("joint"):
joint_name = joint.get("name", "")
joint_type = joint.get("type", "")
if "socket" not in joint_name.lower():
continue
if joint_type != "fixed":
continue
origin = joint.find("origin")
if origin is None:
continue
xyz_str = origin.get("xyz", "")
if not xyz_str:
continue
parts = xyz_str.split()
if len(parts) < 2:
continue
try:
x = float(parts[0])
y = float(parts[1])
socket_positions.append((x, y))
except ValueError:
continue
except ET.ParseError as e:
logger.debug("ElementTree parse error for %s: %s — falling back to regex", xacro_path, e)
# --- 方法2: 正则表达式后备（当 ElementTree 失败或无结果时） ---
if not socket_positions:
try:
xacro_text = xacro_path.read_text(encoding="utf-8", errors="replace")
# 匹配包含 "socket" 的 joint 块，提取 origin xyz
joint_blocks = re.findall(
r'<joint\s[^>]*name=["\'][^"\']*socket[^"\']*["\'][^>]*>.*?</joint>',
xacro_text,
flags=re.IGNORECASE | re.DOTALL,
)
for block in joint_blocks:
# 只处理 fixed 类型
if 'type="fixed"' not in block and "type='fixed'" not in block:
continue
xyz_match = re.search(r'<origin[^>]*xyz=["\']([^"\']+)["\']', block)
if not xyz_match:
continue
parts = xyz_match.group(1).split()
if len(parts) < 2:
continue
try:
x = float(parts[0])
y = float(parts[1])
socket_positions.append((x, y))
except ValueError:
continue
except Exception as e:
logger.debug("Regex fallback also failed for %s: %s", xacro_path, e)
if not socket_positions:
return []
# 计算 socket XY 质心
cx_sock = sum(p[0] for p in socket_positions) / len(socket_positions)
cy_sock = sum(p[1] for p in socket_positions) / len(socket_positions)
# 方向向量：从包围盒中心指向 socket 质心
dx = cx_sock - bbox_center_xy[0]
dy = cy_sock - bbox_center_xy[1]
# 如果 socket 质心非常靠近包围盒中心（<5% 尺寸），判断为顶部装载
threshold = 0.05 * max(bbox_size_xy[0], bbox_size_xy[1], 1e-6)
if math.hypot(dx, dy) < threshold:
logger.debug(
"%s: socket centroid too close to bbox center (dist=%.4f, threshold=%.4f) → top-loading",
xacro_path.parent.name,
math.hypot(dx, dy),
threshold,
)
return []
# 映射到最近基本方向
# socket 质心指示交互区在设备哪一侧，而 opening direction 是从该面
# 向外的法线方向（与质心偏移同向），这里的 dx/dy 已经是从包围盒中心
# 指向 socket 区域的方向，即 opening 朝外的方向
# 注意：在 uni-lab-assets 中，大多数设备 front 在 Y=0 而 body 在 -Y，
# 所以 socket 集中在 +Y 侧（靠近 Y=0 前端），bbox 中心在 -Y/2。
# 方向 center→socket = +Y，但 "opening faces front" 在手动标注中
# 写作 [0, -1]（法线向外=向操作者方向）。
# 因此需要取反：opening direction = -(center→socket)
if abs(dx) >= abs(dy):
cardinal = [-1, 0] if dx > 0 else [1, 0]
else:
cardinal = [0, -1] if dy > 0 else [0, 1]
logger.debug(
"%s: %d socket joints → centroid=(%.3f, %.3f) dir=%s",
xacro_path.parent.name,
len(socket_positions),
cx_sock,
cy_sock,
cardinal,
)
return [{"direction": cardinal, "label": "auto_xacro"}]
def _find_mesh_files(device_dir: Path) -> list[Path]:
"""查找设备目录中的所有 STL/GLB 网格文件。"""
mesh_files: list[Path] = []
meshes_dir = device_dir / "meshes"
if not meshes_dir.exists():
return mesh_files
# uni-lab-assets 结构: meshes/*.stl, meshes/*.glb
for f in meshes_dir.iterdir():
if f.suffix.lower() in (".stl", ".glb"):
mesh_files.append(f)
# registry 结构: meshes/<variant>/collision/*.stl
if not mesh_files:
for variant_dir in meshes_dir.iterdir():
if variant_dir.is_dir():
collision_dir = variant_dir / "collision"
if collision_dir.exists():
for f in collision_dir.iterdir():
if f.suffix.lower() == ".stl":
mesh_files.append(f)
if mesh_files:
break # 使用找到的第一个变体
return sorted(mesh_files)
def _find_best_model_file(device_dir: Path) -> tuple[str, str]:
"""找到最佳可展示的模型文件。优先 GLB > STL。
Returns:
(relative_path, model_type) e.g. ("meshes/0_base.glb", "gltf")
"""
meshes_dir = device_dir / "meshes"
if not meshes_dir.exists():
return "", ""
glbs = sorted(meshes_dir.glob("*.glb"))
if glbs:
return f"meshes/{glbs[0].name}", "gltf"
stls = sorted(f for f in meshes_dir.glob("*.stl") if f.suffix == ".stl")
if not stls:
stls = sorted(f for f in meshes_dir.glob("*.STL"))
if stls:
return f"meshes/{stls[0].name}", "stl"
return "", ""
def _find_thumbnail(device_dir: Path) -> str:
"""查找设备目录中的第一个 PNG 缩略图。"""
pngs = sorted(device_dir.glob("*.png"))
if pngs:
return pngs[0].name
return ""
def _read_glb_json(glb_path: Path) -> dict | None:
"""Read the JSON chunk from a GLB (Binary glTF) file.
GLB structure: 12-byte header + chunks. Chunk 0 is JSON.
Returns parsed dict or None on failure.
"""
try:
with open(glb_path, "rb") as f:
header = f.read(12)
if len(header) < 12:
return None
magic, version, length = struct.unpack("<III", header)
if magic != 0x46546C67: # 'glTF'
return None
chunk_header = f.read(8)
if len(chunk_header) < 8:
return None
chunk_length, chunk_type = struct.unpack("<II", chunk_header)
if chunk_type != 0x4E4F534A: # 'JSON'
return None
json_bytes = f.read(chunk_length)
return json.loads(json_bytes)
except Exception as e:
logger.debug("Failed to read GLB JSON from %s: %s", glb_path, e)
return None
def _quat_to_matrix(q: list[float]) -> list[list[float]]:
"""Convert quaternion [x, y, z, w] to 3×3 rotation matrix."""
x, y, z, w = q
return [
[1 - 2*(y*y + z*z), 2*(x*y - z*w), 2*(x*z + y*w)],
[ 2*(x*y + z*w), 1 - 2*(x*x + z*z), 2*(y*z - x*w)],
[ 2*(x*z - y*w), 2*(y*z + x*w), 1 - 2*(x*x + y*y)],
]
def _get_glb_root_rotation(device_dir: Path) -> list[list[float]] | None:
"""Extract root node rotation matrix from the first GLB in device_dir/meshes/.
Only rotation is extracted — GLB scale is NOT applied because STL files
are already in meters while GLB scale converts GLB mesh units (often mm)
to scene units. Since we read STL directly, scale is irrelevant.
Returns 3×3 rotation matrix or None if no GLB or no rotation found.
"""
meshes_dir = device_dir / "meshes"
if not meshes_dir.exists():
return None
glbs = sorted(meshes_dir.glob("*.glb"))
if not glbs:
return None
gltf = _read_glb_json(glbs[0])
if gltf is None:
return None
nodes = gltf.get("nodes", [])
if not nodes:
return None
root = nodes[0]
rotation = root.get("rotation")
if rotation is None:
return None
# Skip identity quaternion [0,0,0,1]
x, y, z, w = rotation
if abs(x) < 1e-9 and abs(y) < 1e-9 and abs(z) < 1e-9 and abs(w - 1.0) < 1e-9:
return None
return _quat_to_matrix(rotation)
def _apply_rotation_to_bbox(
stl_min: list[float], stl_max: list[float],
rot: list[list[float]],
) -> tuple[list[float], list[float]]:
"""Apply rotation to an axis-aligned bounding box.
Transforms all 8 corners of the STL AABB through rotation,
then computes the new AABB in glTF space.
"""
corners = []
for x in (stl_min[0], stl_max[0]):
for y in (stl_min[1], stl_max[1]):
for z in (stl_min[2], stl_max[2]):
tx = rot[0][0]*x + rot[0][1]*y + rot[0][2]*z
ty = rot[1][0]*x + rot[1][1]*y + rot[1][2]*z
tz = rot[2][0]*x + rot[2][1]*y + rot[2][2]*z
corners.append((tx, ty, tz))
xs = [c[0] for c in corners]
ys = [c[1] for c in corners]
zs = [c[2] for c in corners]
return [min(xs), min(ys), min(zs)], [max(xs), max(ys), max(zs)]
def extract_one_device(device_dir: Path) -> dict | None:
"""提取单个设备的足迹信息。"""
try:
import trimesh
except ImportError:
logger.error("trimesh not installed. Run: pip install trimesh")
return None
mesh_files = _find_mesh_files(device_dir)
if not mesh_files:
return None
# 加载所有网格部件并计算联合包围盒
meshes = []
for f in mesh_files:
try:
m = trimesh.load(str(f), force="mesh")
if hasattr(m, "bounds") and m.bounds is not None:
meshes.append(m)
except Exception as e:
logger.warning("Failed to load %s: %s", f, e)
if not meshes:
return None
if len(meshes) == 1:
combined = meshes[0]
else:
combined = trimesh.util.concatenate(meshes)
bounds = combined.bounds
stl_min = [float(bounds[0][i]) for i in range(3)]
stl_max = [float(bounds[1][i]) for i in range(3)]
# 应用 GLB 根节点旋转到 STL 包围盒（scale 不应用 — STL 已是米制）
# glTF 约定: X=right, Y=up, Z=forward → 2D 足迹取 X 和 Z, 高度取 Y
rot = _get_glb_root_rotation(device_dir)
if rot is not None:
t_min, t_max = _apply_rotation_to_bbox(stl_min, stl_max, rot)
t_size = [t_max[i] - t_min[i] for i in range(3)]
t_center = [(t_min[i] + t_max[i]) / 2 for i in range(3)]
# glTF Y-up: X=width, Z=depth, Y=height
bbox_w = round(t_size[0], 4)
bbox_d = round(t_size[2], 4)
height = round(t_size[1], 4)
origin_offset = [round(t_center[0], 4), round(t_center[2], 4)]
logger.debug(
"%s: GLB rotation applied → bbox=[%.3f, %.3f] height=%.3f",
device_dir.name, bbox_w, bbox_d, height,
)
else:
# 无 GLB 或 identity rotation → 沿用原始 STL 坐标 (X=width, Y=depth, Z=height)
size = [stl_max[i] - stl_min[i] for i in range(3)]
center = [(stl_min[i] + stl_max[i]) / 2 for i in range(3)]
bbox_w = round(size[0], 4)
bbox_d = round(size[1], 4)
height = round(size[2], 4)
origin_offset = [round(center[0], 4), round(center[1], 4)]
model_file, model_type = _find_best_model_file(device_dir)
thumbnail_file = _find_thumbnail(device_dir)
device_id = device_dir.name
# 确定 openings：手动标注优先，否则尝试从 XACRO 自动提取
# 注意：XACRO socket 坐标是 STL 原生坐标系，这里传入变换后的 bbox
if device_id in MANUAL_OPENINGS:
openings = MANUAL_OPENINGS[device_id]
else:
xacro_path = device_dir / "modal.xacro"
if xacro_path.exists():
openings = extract_openings_from_xacro(
xacro_path,
bbox_center_xy=(origin_offset[0], origin_offset[1]),
bbox_size_xy=(bbox_w, bbox_d),
)
else:
openings = []
result: dict = {
"bbox": [bbox_w, bbox_d],
"height": height,
"origin_offset": origin_offset,
"model_file": model_file,
"model_type": model_type,
"thumbnail_file": thumbnail_file,
"openings": openings,
}
return result
def extract_all(
assets_dir: Path | None = None,
registry_dir: Path | None = None,
device_ids: list[str] | None = None,
) -> dict[str, dict]:
"""提取所有（或指定）设备的足迹。
Args:
assets_dir: uni-lab-assets/device_models/ 路径
registry_dir: Uni-Lab-OS/unilabos/device_mesh/devices/ 路径
device_ids: 仅提取指定设备（None = 全部扫描）
Returns:
{device_id: footprint_dict}
"""
results: dict[str, dict] = {}
dirs_to_scan: list[tuple[Path, str]] = []
if assets_dir and assets_dir.exists():
for d in sorted(assets_dir.iterdir()):
if d.is_dir() and (device_ids is None or d.name in device_ids):
dirs_to_scan.append((d, "assets"))
if registry_dir and registry_dir.exists():
for d in sorted(registry_dir.iterdir()):
if d.is_dir() and (device_ids is None or d.name in device_ids):
if d.name not in results: # assets 已有的不重复扫描
dirs_to_scan.append((d, "registry"))
for device_dir, source in dirs_to_scan:
device_id = device_dir.name
if device_id in results:
continue
footprint = extract_one_device(device_dir)
if footprint:
footprint["source"] = source
results[device_id] = footprint
logger.info(
"Extracted %s: bbox=%s height=%.3f source=%s",
device_id,
footprint["bbox"],
footprint["height"],
source,
)
# 统计自动提取的 openings 数量
auto_xacro_count = sum(
1
for fp in results.values()
if any(o.get("label") == "auto_xacro" for o in fp.get("openings", []))
)
logger.info(
"Auto-extracted openings from XACRO for %d / %d devices",
auto_xacro_count,
len(results),
)
# 手动后备
for dev_id, (w, d, h) in FALLBACK_SIZES.items():
if dev_id not in results:
results[dev_id] = {
"bbox": [w, d],
"height": h,
"origin_offset": [0.0, 0.0],
"model_file": "",
"model_type": "",
"thumbnail_file": "",
"openings": MANUAL_OPENINGS.get(dev_id, []),
"source": "manual",
}
return results
def main() -> None:
parser = argparse.ArgumentParser(
description="Extract device footprints from STL/GLB meshes"
)
parser.add_argument(
"--assets-dir",
type=Path,
default=Path(__file__).resolve().parent.parent / "uni-lab-assets" / "device_models",
help="Path to uni-lab-assets/device_models/",
)
parser.add_argument(
"--registry-dir",
type=Path,
default=Path(__file__).resolve().parent / "Uni-Lab-OS" / "unilabos" / "device_mesh" / "devices",
help="Path to Uni-Lab-OS device_mesh/devices/",
)
parser.add_argument(
"--output",
type=Path,
default=Path(__file__).resolve().parent / "footprints.json",
help="Output JSON path",
)
parser.add_argument(
"--devices",
nargs="*",
default=None,
help="Only extract these device IDs (default: all)",
)
parser.add_argument("-v", "--verbose", action="store_true")
args = parser.parse_args()
logging.basicConfig(
level=logging.DEBUG if args.verbose else logging.INFO,
format="%(levelname)s: %(message)s",
)
logger.info("Assets dir: %s (exists=%s)", args.assets_dir, args.assets_dir.exists())
logger.info("Registry dir: %s (exists=%s)", args.registry_dir, args.registry_dir.exists())
results = extract_all(
assets_dir=args.assets_dir,
registry_dir=args.registry_dir,
device_ids=args.devices,
)
with open(args.output, "w") as f:
json.dump(results, f, indent=2, ensure_ascii=False)
logger.info("Wrote %d devices to %s", len(results), args.output)
if __name__ == "__main__":
main()

10274
unilabos/layout_optimizer/footprints.json Normal file
View File

File diff suppressed because it is too large Load Diff

187
unilabos/layout_optimizer/generate_asset_registry.py Normal file
View File

@@ -0,0 +1,187 @@
"""
Generate a YAML registry file for all devices in uni-lab-assets that don't
already have a registry entry (identified by model.mesh value).
Output: Uni-Lab-OS/unilabos/registry/devices/asset_models.yaml
"""
import json
from pathlib import Path
import yaml
# ---------------------------------------------------------------------------
# Paths (resolved relative to this script's location)
# ---------------------------------------------------------------------------
REPO_ROOT = Path(__file__).parent.parent
ASSETS_DIR = REPO_ROOT.parent / "uni-lab-assets" / "device_models"
REGISTRY_DIR = REPO_ROOT / "Uni-Lab-OS" / "unilabos" / "registry" / "devices"
OUTPUT_FILE = REGISTRY_DIR / "asset_models.yaml"
OSS_BASE = (
"https://uni-lab.oss-cn-zhangjiakou.aliyuncs.com/uni-lab/devices"
)
CONTAINER_CLASS = (
"unilabos.devices.resource_container.container:HotelContainer"
)
# ---------------------------------------------------------------------------
# Step 1 — collect mesh names already present in the registry
# ---------------------------------------------------------------------------
def collect_registered_meshes() -> set[str]:
"""Return the set of mesh values found in all existing registry YAML files."""
registered: set[str] = set()
for yaml_file in REGISTRY_DIR.glob("*.yaml"):
# Skip the output file itself so the script is idempotent
if yaml_file == OUTPUT_FILE:
continue
try:
with yaml_file.open("r", encoding="utf-8") as fh:
data = yaml.safe_load(fh)
except Exception as exc:
print(f" [warn] Could not parse {yaml_file.name}: {exc}")
continue
if not isinstance(data, dict):
continue
for _key, entry in data.items():
if not isinstance(entry, dict):
continue
model = entry.get("model")
if isinstance(model, dict):
mesh = model.get("mesh")
if mesh:
registered.add(str(mesh))
return registered
# ---------------------------------------------------------------------------
# Step 2 — scan uni-lab-assets/device_models/
# ---------------------------------------------------------------------------
def scan_asset_devices() -> list[dict]:
"""
Return a list of device dicts for every subfolder that has a modal.xacro.
Each dict has keys: folder_name, description.
"""
devices = []
if not ASSETS_DIR.is_dir():
raise FileNotFoundError(f"Assets directory not found: {ASSETS_DIR}")
for device_dir in sorted(ASSETS_DIR.iterdir()):
if not device_dir.is_dir():
continue
folder_name = device_dir.name
# modal.xacro is required
if not (device_dir / "modal.xacro").exists():
continue
# Read optional meta.json
description = folder_name
meta_path = device_dir / "meta.json"
if meta_path.exists():
try:
with meta_path.open("r", encoding="utf-8") as fh:
meta = json.load(fh)
# Use name field if present; otherwise fall back to folder name
description = meta.get("name", folder_name)
except Exception as exc:
print(f" [warn] Could not parse {meta_path}: {exc}")
devices.append(
{
"folder_name": folder_name,
"description": description,
}
)
return devices
# ---------------------------------------------------------------------------
# Step 3 — build registry entry for a single device
# ---------------------------------------------------------------------------
def build_entry(folder_name: str, description: str) -> dict:
return {
"category": ["asset_model"],
"class": {
"action_value_mappings": {},
"module": CONTAINER_CLASS,
"status_types": {},
"type": "python",
},
"config_info": [],
"description": description,
"handles": [],
"icon": "",
"init_param_schema": {
"config": {
"properties": {},
"required": [],
"type": "object",
},
"data": {
"properties": {},
"required": [],
"type": "object",
},
},
"model": {
"mesh": folder_name,
"path": f"{OSS_BASE}/{folder_name}/macro_device.xacro",
"type": "device",
},
"version": "1.0.0",
}
# ---------------------------------------------------------------------------
# Main
# ---------------------------------------------------------------------------
def main() -> None:
print("Scanning existing registry for registered meshes...")
registered_meshes = collect_registered_meshes()
print(f" Found {len(registered_meshes)} already-registered mesh(es).")
print(f"\nScanning asset devices in: {ASSETS_DIR}")
all_devices = scan_asset_devices()
print(f" Found {len(all_devices)} device folder(s) with modal.xacro.")
registry: dict[str, dict] = {}
skipped = 0
generated = 0
for device in all_devices:
folder_name = device["folder_name"]
if folder_name in registered_meshes:
skipped += 1
continue
key = f"asset_model.{folder_name}"
registry[key] = build_entry(folder_name, device["description"])
generated += 1
print(f"\nWriting {generated} new entr(ies) to: {OUTPUT_FILE}")
OUTPUT_FILE.parent.mkdir(parents=True, exist_ok=True)
with OUTPUT_FILE.open("w", encoding="utf-8") as fh:
yaml.dump(
registry,
fh,
default_flow_style=False,
allow_unicode=True,
sort_keys=False,
)
print("\n--- Summary ---")
print(f" Total devices found (with modal.xacro): {len(all_devices)}")
print(f" Already registered (skipped): {skipped}")
print(f" Newly generated: {generated}")
if __name__ == "__main__":
main()

314
unilabos/layout_optimizer/intent_interpreter.py Normal file
View File

@@ -0,0 +1,314 @@
"""意图解释器：将语义化意图翻译为 Constraint 列表。"""
from __future__ import annotations
import itertools
from collections.abc import Callable
from dataclasses import dataclass, field
from .models import Constraint, Intent
# 优先级权重映射
_PRIORITY_WEIGHTS: dict[str, float] = {"low": 1.0, "medium": 3.0, "high": 8.0}
_DEFAULT_WEIGHT = _PRIORITY_WEIGHTS["medium"]
@dataclass
class InterpretResult:
"""意图解释结果。"""
constraints: list[Constraint] = field(default_factory=list)
translations: list[dict] = field(default_factory=list)
errors: list[str] = field(default_factory=list)
workflow_edges: list[list[str]] = field(default_factory=list)
def _handle_reachable_by(intent: Intent, result: InterpretResult) -> None:
"""reachable_by：机械臂必须能到达指定设备列表。"""
arm = intent.params.get("arm")
targets = intent.params.get("targets", [])
if arm is None:
result.errors.append(f"reachable_by: 缺少必要参数 'arm'")
return
if not targets:
result.errors.append(f"reachable_by: 参数 'targets' 不能为空")
return
generated: list[dict] = []
for target in targets:
c = Constraint(
type="hard",
rule_name="reachability",
params={"arm_id": arm, "target_device_id": target},
)
result.constraints.append(c)
generated.append({"type": c.type, "rule_name": c.rule_name, "params": c.params, "weight": c.weight})
result.translations.append({
"source_intent": intent.intent,
"source_description": intent.description,
"source_params": intent.params,
"generated_constraints": generated,
"explanation": f"机械臂 '{arm}' 需要能够到达 {len(targets)} 个目标设备",
})
def _handle_close_together(intent: Intent, result: InterpretResult) -> None:
"""close_together：设备组内两两最小化距离。"""
devices: list[str] = intent.params.get("devices", [])
priority: str = intent.params.get("priority", "medium")
if len(devices) < 2:
result.errors.append(f"close_together: 参数 'devices' 至少需要 2 个设备，当前 {len(devices)} 个")
return
weight = _PRIORITY_WEIGHTS.get(priority, _DEFAULT_WEIGHT)
generated: list[dict] = []
for dev_a, dev_b in itertools.combinations(devices, 2):
c = Constraint(
type="soft",
rule_name="minimize_distance",
params={"device_a": dev_a, "device_b": dev_b},
weight=weight,
)
result.constraints.append(c)
generated.append({"type": c.type, "rule_name": c.rule_name, "params": c.params, "weight": c.weight})
result.translations.append({
"source_intent": intent.intent,
"source_description": intent.description,
"source_params": intent.params,
"generated_constraints": generated,
"explanation": f"设备组 {devices} 应尽量靠近（优先级: {priority}）",
})
def _handle_far_apart(intent: Intent, result: InterpretResult) -> None:
"""far_apart：设备组内两两最大化距离。"""
devices: list[str] = intent.params.get("devices", [])
priority: str = intent.params.get("priority", "medium")
if len(devices) < 2:
result.errors.append(f"far_apart: 参数 'devices' 至少需要 2 个设备，当前 {len(devices)} 个")
return
weight = _PRIORITY_WEIGHTS.get(priority, _DEFAULT_WEIGHT)
generated: list[dict] = []
for dev_a, dev_b in itertools.combinations(devices, 2):
c = Constraint(
type="soft",
rule_name="maximize_distance",
params={"device_a": dev_a, "device_b": dev_b},
weight=weight,
)
result.constraints.append(c)
generated.append({"type": c.type, "rule_name": c.rule_name, "params": c.params, "weight": c.weight})
result.translations.append({
"source_intent": intent.intent,
"source_description": intent.description,
"source_params": intent.params,
"generated_constraints": generated,
"explanation": f"设备组 {devices} 应尽量分散（优先级: {priority}）",
})
def _handle_max_distance(intent: Intent, result: InterpretResult) -> None:
"""max_distance：两设备间距不超过指定值。"""
device_a = intent.params.get("device_a")
device_b = intent.params.get("device_b")
distance = intent.params.get("distance")
if device_a is None or device_b is None or distance is None:
result.errors.append(
f"max_distance: 缺少必要参数，需要 'device_a'、'device_b' 和 'distance'，"
f"当前: device_a={device_a}, device_b={device_b}, distance={distance}"
)
return
c = Constraint(
type="hard",
rule_name="distance_less_than",
params={"device_a": device_a, "device_b": device_b, "distance": distance},
)
result.constraints.append(c)
result.translations.append({
"source_intent": intent.intent,
"source_description": intent.description,
"source_params": intent.params,
"generated_constraints": [{"type": c.type, "rule_name": c.rule_name, "params": c.params, "weight": c.weight}],
"explanation": f"设备 '{device_a}' 与 '{device_b}' 之间的距离不得超过 {distance} 米",
})
def _handle_min_distance(intent: Intent, result: InterpretResult) -> None:
"""min_distance：两设备间距不小于指定值。"""
device_a = intent.params.get("device_a")
device_b = intent.params.get("device_b")
distance = intent.params.get("distance")
if device_a is None or device_b is None or distance is None:
result.errors.append(
f"min_distance: 缺少必要参数，需要 'device_a'、'device_b' 和 'distance'，"
f"当前: device_a={device_a}, device_b={device_b}, distance={distance}"
)
return
c = Constraint(
type="hard",
rule_name="distance_greater_than",
params={"device_a": device_a, "device_b": device_b, "distance": distance},
)
result.constraints.append(c)
result.translations.append({
"source_intent": intent.intent,
"source_description": intent.description,
"source_params": intent.params,
"generated_constraints": [{"type": c.type, "rule_name": c.rule_name, "params": c.params, "weight": c.weight}],
"explanation": f"设备 '{device_a}' 与 '{device_b}' 之间的距离不得小于 {distance} 米",
})
def _handle_min_spacing(intent: Intent, result: InterpretResult) -> None:
"""min_spacing：所有设备之间的最小间隙。"""
min_gap: float = intent.params.get("min_gap", 0.3)
c = Constraint(
type="hard",
rule_name="min_spacing",
params={"min_gap": min_gap},
)
result.constraints.append(c)
result.translations.append({
"source_intent": intent.intent,
"source_description": intent.description,
"source_params": intent.params,
"generated_constraints": [{"type": c.type, "rule_name": c.rule_name, "params": c.params, "weight": c.weight}],
"explanation": f"所有设备之间至少保持 {min_gap} 米的间隙",
})
def _handle_face_outward(intent: Intent, result: InterpretResult) -> None:
"""face_outward：设备朝向偏好为向外。"""
c = Constraint(
type="soft",
rule_name="prefer_orientation_mode",
params={"mode": "outward"},
)
result.constraints.append(c)
result.translations.append({
"source_intent": intent.intent,
"source_description": intent.description,
"source_params": intent.params,
"generated_constraints": [{"type": c.type, "rule_name": c.rule_name, "params": c.params, "weight": c.weight}],
"explanation": "设备开口偏好朝向实验室外侧",
})
def _handle_face_inward(intent: Intent, result: InterpretResult) -> None:
"""face_inward：设备朝向偏好为向内。"""
c = Constraint(
type="soft",
rule_name="prefer_orientation_mode",
params={"mode": "inward"},
)
result.constraints.append(c)
result.translations.append({
"source_intent": intent.intent,
"source_description": intent.description,
"source_params": intent.params,
"generated_constraints": [{"type": c.type, "rule_name": c.rule_name, "params": c.params, "weight": c.weight}],
"explanation": "设备开口偏好朝向实验室内侧",
})
def _handle_align_cardinal(intent: Intent, result: InterpretResult) -> None:
"""align_cardinal：设备偏好对齐到主轴方向。"""
c = Constraint(
type="soft",
rule_name="prefer_aligned",
params={},
)
result.constraints.append(c)
result.translations.append({
"source_intent": intent.intent,
"source_description": intent.description,
"source_params": intent.params,
"generated_constraints": [{"type": c.type, "rule_name": c.rule_name, "params": c.params, "weight": c.weight}],
"explanation": "设备偏好与实验室主轴对齐（0°/90°/180°/270°）",
})
def _handle_workflow_hint(intent: Intent, result: InterpretResult) -> None:
"""workflow_hint：工作流顺序暗示，相邻步骤设备靠近。"""
workflow: str = intent.params.get("workflow", "")
devices: list[str] = intent.params.get("devices", [])
if len(devices) < 2:
result.errors.append(
f"workflow_hint: 参数 'devices' 至少需要 2 个设备，当前 {len(devices)} 个"
)
return
generated: list[dict] = []
for dev_a, dev_b in zip(devices[:-1], devices[1:]):
c = Constraint(
type="soft",
rule_name="minimize_distance",
params={"device_a": dev_a, "device_b": dev_b},
)
result.constraints.append(c)
generated.append({"type": c.type, "rule_name": c.rule_name, "params": c.params, "weight": c.weight})
result.workflow_edges.append([dev_a, dev_b])
result.translations.append({
"source_intent": intent.intent,
"source_description": intent.description,
"source_params": intent.params,
"generated_constraints": generated,
"explanation": f"工作流 '{workflow}' 中相邻步骤设备应靠近",
"confidence": "low",
})
# 意图处理器分发表
_HANDLERS: dict[str, Callable[[Intent, InterpretResult], None]] = {
"reachable_by": _handle_reachable_by,
"close_together": _handle_close_together,
"far_apart": _handle_far_apart,
"max_distance": _handle_max_distance,
"min_distance": _handle_min_distance,
"min_spacing": _handle_min_spacing,
"face_outward": _handle_face_outward,
"face_inward": _handle_face_inward,
"align_cardinal": _handle_align_cardinal,
"workflow_hint": _handle_workflow_hint,
}
def interpret_intents(intents: list[Intent]) -> InterpretResult:
"""将意图列表翻译为约束列表。
Args:
intents: 语义化意图列表（通常由 LLM 生成）
Returns:
InterpretResult，包含约束、翻译记录、错误信息和工作流边
"""
result = InterpretResult()
for intent in intents:
handler = _HANDLERS.get(intent.intent)
if handler is None:
result.errors.append(f"未知意图类型: '{intent.intent}'，跳过处理")
continue
handler(intent, result)
return result

41
unilabos/layout_optimizer/interfaces.py Normal file
View File

@@ -0,0 +1,41 @@
"""Protocol 接口定义，隔离 ROS 依赖。
开发阶段使用 mock_checkers.py 中的 Mock 实现，
集成阶段替换为 ros_checkers.py 中的 MoveIt2 / IKFast 实现。
"""
from __future__ import annotations
from typing import Protocol
class CollisionChecker(Protocol):
"""碰撞检测接口。"""
def check(self, placements: list[dict]) -> list[tuple[str, str]]:
"""返回碰撞设备对列表。
Args:
placements: [{"id": str, "bbox": (w, d), "pos": (x, y, θ)}, ...]
Returns:
[("device_a", "device_b"), ...] 存在碰撞的设备对
"""
...
class ReachabilityChecker(Protocol):
"""可达性检测接口。"""
def is_reachable(self, arm_id: str, arm_pose: dict, target: dict) -> bool:
"""判断机械臂在给定位姿下能否到达目标点。
Args:
arm_id: 机械臂设备 ID
arm_pose: {"x": float, "y": float, "theta": float}
target: {"x": float, "y": float, "z": float}
Returns:
True 如果可达
"""
...

49
unilabos/layout_optimizer/lab_parser.py Normal file
View File

@@ -0,0 +1,49 @@
"""解析实验室平面图 JSON。
简单格式：
{
"width": 6.0,
"depth": 4.0,
"obstacles": [
{"x": 2.0, "y": 0.0, "width": 0.1, "depth": 1.0}
]
}
"""
from __future__ import annotations
import json
from pathlib import Path
from .models import Lab, Obstacle
def parse_lab(data: dict) -> Lab:
"""从字典解析实验室平面图。"""
obstacles = []
for obs in data.get("obstacles", []):
obstacles.append(
Obstacle(
x=float(obs["x"]),
y=float(obs["y"]),
width=float(obs["width"]),
depth=float(obs["depth"]),
)
)
return Lab(
width=float(data["width"]),
depth=float(data["depth"]),
obstacles=obstacles,
)
def load_lab_from_file(path: str | Path) -> Lab:
"""从 JSON 文件加载实验室平面图。"""
with open(path) as f:
data = json.load(f)
return parse_lab(data)
def create_simple_lab(width: float, depth: float) -> Lab:
"""创建一个无障碍物的简单矩形实验室。"""
return Lab(width=width, depth=depth)

186
unilabos/layout_optimizer/llm_skill/demo_agent.md Normal file
View File

@@ -0,0 +1,186 @@
# Demo Agent — Lab Layout Orchestrator
You are a lab layout agent for a recorded demo. Your job is to take a natural language lab request, translate it into optimizer constraints, run the optimization, and push results to the 3D frontend — all while outputting only concise, readable status lines.
## CRITICAL OUTPUT RULES
- Output ONLY short status lines. No markdown fences. No raw JSON. No explanations.
- Every HTTP call uses `curl -s` (silent). Never show curl output to the user.
- Parse responses internally. Extract only the fields needed for your status lines.
- Server base URL: `http://localhost:8000`
## Pipeline
Execute these steps in order. Print the status line shown after each step.
### Step 1 — Retrieve devices
Run:
```
curl -s http://localhost:8000/devices
```
Filter to `is_standalone: true` entries. Count them. Build an id→name lookup.
Print:
```
retrieving devices... N standalone devices found
```
Then print an id mapping table showing the user-friendly name → device_id for devices relevant to the user's request:
```
id mapping:
plate hotel → thermo_orbitor_rs2_hotel
robot arm → arm_slider
liquid handler → opentrons_liquid_handler
plate sealer → agilent_plateloc
pcr machine → inheco_odtc_96xl
```
Only include devices that are relevant to the user's request, not the full catalog.
### Step 2 — Translate intent to constraints
Using the rules in `layout_intent_translator.md` (which you have already read), translate the user's natural language request into an intents JSON structure.
Do NOT print the JSON. Instead, print a human-readable constraint summary:
```
translating intent to constraints...
constraints:
hard: arm_slider must reach 4 devices
hard: min spacing 0.05m between all devices
soft: workflow order hotel → liquid handler → sealer → pcr
soft: all devices close together (high priority)
soft: align to cardinal directions
```
### Step 3 — Interpret intents
Send the intents JSON to the interpret endpoint:
```
curl -s -X POST http://localhost:8000/interpret \
-H "Content-Type: application/json" \
-d '{ "intents": [...] }'
```
Capture the `constraints` and `workflow_edges` arrays from the response. Do NOT print anything for this step — it's a silent validation.
If `errors` is non-empty, print:
```
warning: N intents failed to translate
```
### Step 3.5 — Read lab dimensions
```
curl -s http://localhost:8000/scene/lab
```
Returns `{"width": W, "depth": D}`. Use these values for the optimize request. Do NOT print anything for this step.
### Step 4 — Optimize layout
Build the optimize request using:
- `devices`: the relevant devices from Step 1 (id, name, device_type)
- `lab`: the `{"width": W, "depth": D}` from Step 3.5
- `constraints`: from Step 3 interpret response
- `workflow_edges`: from Step 3 interpret response
- `seeder`: `"compact_outward"` (default)
- `seeder_overrides`: `{"align_weight": W}` — if user explicitly says "align cardinal", "lock to horizontal/vertical", or "angle locked", use `align_weight: 15.0` instead of default 2.0
- `run_de`: `true`
- `maxiter`: `200`
- `seed`: `42`
Run:
```
curl -s -X POST http://localhost:8000/optimize \
-H "Content-Type: application/json" \
-d '{ ... }'
```
Print:
```
optimizing layout (DE, 200 iterations)...
optimization complete — cost: X.XX, success: true/false
```
If `success` is false, print:
```
error: optimization failed (cost: inf) — constraints may conflict
```
And stop.
### Step 5 — Apply placements
Take the `placements` array from the optimize response and POST them. Do NOT add a `location` field — the backend schema only accepts `device_id`, `uuid`, `position`, and `rotation`. Extra fields will cause validation errors.
```
curl -s -X POST http://localhost:8000/scene/placements \
-H "Content-Type: application/json" \
-d '{ "placements": [
{
"device_id": "...",
"uuid": "...",
"position": {"x": ..., "y": ..., "z": ...},
"rotation": {"x": ..., "y": ..., "z": ...}
}
] }'
```
**Important — version-based polling:** The frontend polls `GET /scene/placements` every 1 second and uses a version number to detect changes. On the **first poll**, it captures the current version as a baseline and does **not** apply placements. It only renders placements when the version **increases beyond** that baseline. This means if you POST placements before the frontend has polled once, the frontend will silently skip that update.
**Solution:** After the initial POST, send the **same request a second time** to bump the version. This guarantees the frontend sees a version increase after its baseline poll and applies the placements.
Print:
```
applying placements to scene...
layout applied — N devices positioned
```
## Follow-up Requests
If the user gives a follow-up request (e.g., "now move the sealer farther from the thermal cycler"):
1. Print a `---` separator
2. Keep the same device list (no need to re-fetch)
3. Translate the NEW request into intents — these REPLACE the previous constraints entirely
4. Run Steps 3–5 again with the new constraints
5. Same output format
## Error Handling
- Server unreachable: `error: server unreachable at localhost:8000`
- Optimize fails: `error: optimization failed (cost: inf) — constraints may conflict`
- After any error, stop and wait for user input.
## Device Name Resolution
You have `layout_intent_translator.md` loaded as context. Use its device name resolution rules to match user's informal names (e.g., "PCR machine", "the arm", "liquid handler") to exact device IDs from the catalog retrieved in Step 1.
## Example Full Output
For input: "Set up a PCR workflow — hotel, liquid handler, sealer, thermal cycler. The arm handles all transfers. Keep it compact."
```
retrieving devices... 47 standalone devices found
id mapping:
plate hotel → thermo_orbitor_rs2_hotel
robot arm → arm_slider
liquid handler → opentrons_liquid_handler
plate sealer → agilent_plateloc
pcr machine → inheco_odtc_96xl
translating intent to constraints...
constraints:
hard: arm_slider must reach 4 devices
soft: workflow order hotel → liquid handler → sealer → pcr
soft: all devices close together (high priority)
soft: align to cardinal directions
optimizing layout (DE, 200 iterations)...
optimization complete — cost: 0.00, success: true
applying placements to scene...
layout applied — 5 devices positioned
```

284
unilabos/layout_optimizer/llm_skill/layout_intent_translator.md Normal file
View File

@@ -0,0 +1,284 @@
# Layout Intent Translator — LLM Skill
You are a lab layout intent translator. Your job is to convert natural language descriptions of lab layout requirements into structured JSON intents that the layout optimizer can understand.
## Your Role
Users describe their lab needs in natural language. You must:
1. Identify devices by their IDs from the provided device list
2. Infer spatial relationships, workflow order, and physical constraints
3. Output structured intents (JSON) that map to the optimizer's intent schema
4. Provide clear `description` fields so users can verify the translation
## Output Format
You MUST output a JSON object with an `intents` array. Each intent has:
```json
{
"intents": [
{
"intent": "<intent_type>",
"params": { ... },
"description": "Human-readable explanation of what this intent means"
}
]
}
```
## Available Intent Types
### `reachable_by` — Robot arm must reach devices
```json
{
"intent": "reachable_by",
"params": {
"arm": "arm_device_id",
"targets": ["device_a", "device_b"]
},
"description": "Robot arm must be able to reach device A and device B"
}
```
**When to use:** Any time a robot arm transfers items between devices, all those devices must be reachable.
### `close_together` — Devices should be near each other
```json
{
"intent": "close_together",
"params": {
"devices": ["device_a", "device_b", "device_c"],
"priority": "high"
},
"description": "These devices are used frequently together and should be close"
}
```
**Priority:** `"low"` (nice-to-have), `"medium"` (default), `"high"` (critical for workflow speed)
### `far_apart` — Devices should be separated
```json
{
"intent": "far_apart",
"params": {
"devices": ["heat_source", "reagent_storage"],
"priority": "medium"
}
}
```
**When to use:** Thermal interference, contamination risk, safety separation.
### `max_distance` — Hard limit on maximum distance
```json
{
"intent": "max_distance",
"params": {
"device_a": "device_a_id",
"device_b": "device_b_id",
"distance": 1.5
}
}
```
**When to use:** Physical constraints like tube length, cable reach, arm range.
### `min_distance` — Hard limit on minimum distance
```json
{
"intent": "min_distance",
"params": {
"device_a": "device_a_id",
"device_b": "device_b_id",
"distance": 0.5
}
}
```
**When to use:** Safety clearance, thermal isolation, vibration separation.
### `min_spacing` — Global minimum gap between all devices
```json
{
"intent": "min_spacing",
"params": { "min_gap": 0.3 }
}
```
**When to use:** General accessibility, maintenance clearance.
### `workflow_hint` — Workflow step ordering
```json
{
"intent": "workflow_hint",
"params": {
"workflow": "pcr",
"devices": ["liquid_handler", "thermal_cycler", "plate_sealer", "storage"]
}
}
```
**When to use:** When user describes a sequential process. Devices are listed in workflow order. Consecutive devices will be placed near each other.
### `face_outward` / `face_inward` / `align_cardinal`
```json
{"intent": "face_outward"}
{"intent": "face_inward"}
{"intent": "align_cardinal"}
```
**When to use:** User mentions accessibility from outside, central robot, or neat alignment.
## Device Name Resolution
You will receive the current scene's device list as context. This is the **only** source of valid device IDs. Users will refer to devices using informal names — you must match them to exact IDs from this list.
### Input Context Format
Before each translation request, you receive the scene's device list:
```
Devices in scene:
- thermo_orbitor_rs2_hotel: Thermo Orbitor RS2 Hotel (type: static, bbox: 0.68×0.52m)
- arm_slider: Arm Slider (type: articulation, bbox: 1.20×0.30m)
- opentrons_liquid_handler: Opentrons Liquid Handler (type: static, bbox: 0.65×0.60m)
- agilent_plateloc: Agilent PlateLoc (type: static, bbox: 0.35×0.40m)
- inheco_odtc_96xl: Inheco ODTC 96XL (type: static, bbox: 0.30×0.35m)
```
### Matching Rules
1. **Exact match first**: If user says "arm_slider", match directly
2. **Name/brand match**: "opentrons" → `opentrons_liquid_handler`, "plateloc" → `agilent_plateloc`
3. **Function match**: "PCR machine" / "thermal cycler" → `inheco_odtc_96xl`; "liquid handler" / "pipetting robot" → `opentrons_liquid_handler`; "plate hotel" / "storage" → `thermo_orbitor_rs2_hotel`; "plate sealer" → `agilent_plateloc`
4. **Type match**: "robot arm" / "the arm" → look for `device_type: articulation`
5. **Ambiguous**: If multiple devices could match, list candidates in the `description` field and pick the most likely one. If truly ambiguous, return an error intent asking the user to clarify.
### Example Resolution
User says: "the robot should reach the PCR machine and the liquid handler"
Scene devices: `arm_slider` (articulation), `inheco_odtc_96xl`, `opentrons_liquid_handler`, ...
Resolution:
- "the robot" → `arm_slider` (only articulation-type device)
- "PCR machine" → `inheco_odtc_96xl` (thermal cycler = PCR)
- "liquid handler" → `opentrons_liquid_handler`
## Translation Rules
### 1. Robot Arm Inference
If any robot arm is in the device list and the workflow involves plate/sample transfer between devices, ALL devices that exchange plates/samples with each other via the arm must be in `reachable_by.targets`.
### 2. Workflow Order
When a user describes a process (e.g., "prepare samples, then run PCR, then seal"), extract the device order and create a `workflow_hint`. The device order follows the sample processing path.
### 3. Implicit Constraints
- If devices frequently exchange items → `close_together` (high priority)
- If a robot arm is mentioned "in between" → `reachable_by` for all involved devices
- If user says "short transit" or "fast transfer" → `close_together` with `"priority": "high"`
- If user says "keep X away from Y" → `far_apart` or `min_distance`
### 4. Don't Over-Constrain
- Only add constraints the user's description implies
- When unsure about priority, use `"medium"`
- For workflow_hint, confidence is inherently `"low"` — the optimizer notes this
## Example: PCR Workflow
**User input:**
> "Take plate from hotel, prepare sample in opentrons, seal plate then pcr cycle, arm_slider handles all transfers"
**Device list provided:**
- `thermo_orbitor_rs2_hotel` (plate hotel/storage)
- `arm_slider` (robot arm on linear rail)
- `opentrons_liquid_handler` (liquid handling/pipetting)
- `agilent_plateloc` (plate sealer)
- `inheco_odtc_96xl` (thermal cycler for PCR)
**Your output:**
```json
{
"intents": [
{
"intent": "reachable_by",
"params": {
"arm": "arm_slider",
"targets": [
"thermo_orbitor_rs2_hotel",
"opentrons_liquid_handler",
"agilent_plateloc",
"inheco_odtc_96xl"
]
},
"description": "arm_slider must reach all devices since it handles all plate transfers"
},
{
"intent": "workflow_hint",
"params": {
"workflow": "pcr",
"devices": [
"thermo_orbitor_rs2_hotel",
"opentrons_liquid_handler",
"agilent_plateloc",
"inheco_odtc_96xl"
]
},
"description": "PCR workflow order: hotel → liquid handler → plate sealer → thermal cycler"
},
{
"intent": "close_together",
"params": {
"devices": ["opentrons_liquid_handler", "agilent_plateloc"],
"priority": "high"
},
"description": "Sealing happens immediately after sample prep — minimize transit time"
}
]
}
```
**Reasoning:**
- The arm handles ALL transfers → all 4 devices in reachable_by targets
- User described a clear sequence → workflow_hint in that order
- "seal plate then pcr" implies sealing is immediately after prep → close_together for the pair with high priority
## Example: Simple Proximity Request
**User input:**
> "Keep the thermal cycler close to the plate sealer, at most 1 meter apart"
**Your output:**
```json
{
"intents": [
{
"intent": "max_distance",
"params": {
"device_a": "inheco_odtc_96xl",
"device_b": "agilent_plateloc",
"distance": 1.0
},
"description": "Thermal cycler and plate sealer must be within 1 meter"
}
]
}
```
## API Integration
### Discovery
Call `GET /interpret/schema` to get the current list of available intent types and their parameter specifications. Always check this before translating, as new intent types may be added.
### Translation
Send your output to `POST /interpret`:
```
POST /interpret
Content-Type: application/json
{
"intents": [ ... your translated intents ... ]
}
```
### Response
The endpoint returns:
- `constraints` — ready to pass to `/optimize`
- `translations` — human-readable mapping of each intent to generated constraints
- `workflow_edges` — extracted workflow connections
- `errors` — any intents that failed to translate
### Optimization
After user confirms the translation, pass `constraints` and `workflow_edges` to `POST /optimize` along with the device list and lab dimensions.

109
unilabos/layout_optimizer/mock_checkers.py Normal file
View File

@@ -0,0 +1,109 @@
"""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,
}
# 未知型号回退臂展：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

95
unilabos/layout_optimizer/models.py Normal file
View File

@@ -0,0 +1,95 @@
"""数据模型定义：Device, Lab, Placement, Constraint 及 API 请求/响应。"""
from __future__ import annotations
import math
from dataclasses import dataclass, field
from typing import Literal
@dataclass
class Opening:
"""设备的访问开口（用于方向约束）。"""
# 设备局部坐标系中的方向单位向量，如 (0, -1) = 正前方
direction: tuple[float, float] = (0.0, -1.0)
label: str = ""
@dataclass
class Device:
"""设备描述。"""
id: str
name: str
# 碰撞包围盒 (width along X, depth along Y)，单位：米
bbox: tuple[float, float] = (0.6, 0.4)
device_type: Literal["static", "articulation", "rigid"] = "static"
# 以下为可选扩展字段（向后兼容）
height: float = 0.4
origin_offset: tuple[float, float] = (0.0, 0.0)
openings: list[Opening] = field(default_factory=list)
source: Literal["registry", "assets", "manual"] = "manual"
model_path: str = ""
model_type: str = ""
thumbnail_url: str = ""
@dataclass
class Obstacle:
"""实验室内固定障碍物（矩形）。"""
x: float
y: float
width: float
depth: float
@dataclass
class Lab:
"""实验室平面图。"""
width: float # X 方向，单位：米
depth: float # Y 方向，单位：米
obstacles: list[Obstacle] = field(default_factory=list)
@dataclass
class Placement:
"""单个设备的布局位姿。"""
device_id: str
x: float
y: float
theta: float # 旋转角，弧度
uuid: str = "" # 前端分配的唯一标识，透传不生成
def rotated_bbox(self, device: Device) -> tuple[float, float]:
"""返回旋转后的 AABB 尺寸 (half_w, half_h)。"""
w, d = device.bbox
cos_t = abs(math.cos(self.theta))
sin_t = abs(math.sin(self.theta))
half_w = (w * cos_t + d * sin_t) / 2
half_h = (w * sin_t + d * cos_t) / 2
return half_w, half_h
@dataclass
class Constraint:
"""约束规则。"""
type: Literal["hard", "soft"]
rule_name: str
# 规则参数，含义因 rule_name 而异
params: dict = field(default_factory=dict)
# 仅 soft 约束使用
weight: float = 1.0
@dataclass
class Intent:
"""LLM 可生成的语义化意图，由 interpreter 翻译为 Constraint 列表。"""
intent: str # 意图类型，如 "reachable_by", "close_together"
params: dict = field(default_factory=dict)
description: str = "" # 可选的自然语言描述（用于审计/调试）

207
unilabos/layout_optimizer/obb.py Normal file
View File

@@ -0,0 +1,207 @@
"""OBB (Oriented Bounding Box) geometry: corners, overlap (SAT), minimum distance."""
from __future__ import annotations
import math
def obb_corners(
cx: float, cy: float, w: float, h: float, theta: float
) -> list[tuple[float, float]]:
"""Return 4 corners of the OBB as (x, y) tuples.
Args:
cx, cy: center position
w, h: full width and height (not half-extents)
theta: rotation angle in radians
"""
hw, hh = w / 2, h / 2
cos_t, sin_t = math.cos(theta), math.sin(theta)
dx_w, dy_w = hw * cos_t, hw * sin_t # half-width vector
dx_h, dy_h = -hh * sin_t, hh * cos_t # half-height vector
return [
(cx + dx_w + dx_h, cy + dy_w + dy_h),
(cx - dx_w + dx_h, cy - dy_w + dy_h),
(cx - dx_w - dx_h, cy - dy_w - dy_h),
(cx + dx_w - dx_h, cy + dy_w - dy_h),
]
def _get_axes(corners: list[tuple[float, float]]) -> list[tuple[float, float]]:
"""Return 2 edge-normal axes for a rectangle (4 corners)."""
axes = []
for i in range(2): # Only need 2 axes for a rectangle
edge_x = corners[i + 1][0] - corners[i][0]
edge_y = corners[i + 1][1] - corners[i][1]
length = math.sqrt(edge_x**2 + edge_y**2)
if length > 0:
axes.append((-edge_y / length, edge_x / length))
return axes
def _project(corners: list[tuple[float, float]], axis: tuple[float, float]) -> tuple[float, float]:
"""Project all corners onto axis, return (min, max) scalar projections."""
dots = [c[0] * axis[0] + c[1] * axis[1] for c in corners]
return min(dots), max(dots)
def obb_overlap(corners_a: list[tuple[float, float]], corners_b: list[tuple[float, float]]) -> bool:
"""Return True if two OBBs overlap using Separating Axis Theorem.
Uses strict inequality (touching edges = no overlap).
"""
for axis in _get_axes(corners_a) + _get_axes(corners_b):
min_a, max_a = _project(corners_a, axis)
min_b, max_b = _project(corners_b, axis)
if max_a <= min_b or max_b <= min_a:
return False
return True
def _point_to_segment_dist_sq(
px: float, py: float,
ax: float, ay: float,
bx: float, by: float,
) -> float:
"""Squared distance from point (px,py) to line segment (ax,ay)-(bx,by)."""
dx, dy = bx - ax, by - ay
len_sq = dx * dx + dy * dy
if len_sq == 0:
return (px - ax) ** 2 + (py - ay) ** 2
t = max(0.0, min(1.0, ((px - ax) * dx + (py - ay) * dy) / len_sq))
proj_x, proj_y = ax + t * dx, ay + t * dy
return (px - proj_x) ** 2 + (py - proj_y) ** 2
def obb_penetration_depth(
corners_a: list[tuple[float, float]],
corners_b: list[tuple[float, float]],
) -> float:
"""Minimum penetration depth between two OBBs (SAT-based).
Returns 0.0 if not overlapping. Otherwise returns the minimum overlap
along any separating axis — the smallest push needed to separate them.
"""
min_overlap = float("inf")
for axis in _get_axes(corners_a) + _get_axes(corners_b):
min_a, max_a = _project(corners_a, axis)
min_b, max_b = _project(corners_b, axis)
overlap = min(max_a - min_b, max_b - min_a)
if overlap <= 0:
return 0.0 # Separated on this axis
if overlap < min_overlap:
min_overlap = overlap
return min_overlap
def nearest_point_on_obb(
px: float, py: float,
corners: list[tuple[float, float]],
) -> tuple[float, float]:
"""Return the nearest point on an OBB's boundary to point (px, py).
If the point is inside the OBB, returns the nearest edge point.
"""
best_x, best_y = corners[0]
best_dist_sq = float("inf")
n = len(corners)
for i in range(n):
ax, ay = corners[i]
bx, by = corners[(i + 1) % n]
dx, dy = bx - ax, by - ay
len_sq = dx * dx + dy * dy
if len_sq == 0:
proj_x, proj_y = ax, ay
else:
t = max(0.0, min(1.0, ((px - ax) * dx + (py - ay) * dy) / len_sq))
proj_x, proj_y = ax + t * dx, ay + t * dy
d_sq = (px - proj_x) ** 2 + (py - proj_y) ** 2
if d_sq < best_dist_sq:
best_dist_sq = d_sq
best_x, best_y = proj_x, proj_y
return best_x, best_y
def segment_intersects_obb(
p1: tuple[float, float],
p2: tuple[float, float],
corners: list[tuple[float, float]],
) -> bool:
"""Return True if line segment p1-p2 intersects the OBB (convex polygon).
Uses separating axis theorem on the Minkowski difference:
test segment against each edge of the polygon + segment normal.
"""
# Quick: test each edge of the OBB against the segment
n = len(corners)
for i in range(n):
ax, ay = corners[i]
bx, by = corners[(i + 1) % n]
if _segments_intersect(p1[0], p1[1], p2[0], p2[1], ax, ay, bx, by):
return True
# Also check if segment is fully inside the OBB
if _point_in_convex(p1, corners) or _point_in_convex(p2, corners):
return True
return False
def _segments_intersect(
ax: float, ay: float, bx: float, by: float,
cx: float, cy: float, dx: float, dy: float,
) -> bool:
"""Return True if segment AB intersects segment CD (proper or endpoint)."""
def cross(ox: float, oy: float, px: float, py: float, qx: float, qy: float) -> float:
return (px - ox) * (qy - oy) - (py - oy) * (qx - ox)
d1 = cross(cx, cy, dx, dy, ax, ay)
d2 = cross(cx, cy, dx, dy, bx, by)
d3 = cross(ax, ay, bx, by, cx, cy)
d4 = cross(ax, ay, bx, by, dx, dy)
if ((d1 > 0 and d2 < 0) or (d1 < 0 and d2 > 0)) and \
((d3 > 0 and d4 < 0) or (d3 < 0 and d4 > 0)):
return True
# Collinear cases — skip for simplicity (near-zero probability in DE)
return False
def _point_in_convex(
p: tuple[float, float], corners: list[tuple[float, float]]
) -> bool:
"""Return True if point is inside a convex polygon (corners in order)."""
n = len(corners)
sign = None
for i in range(n):
ax, ay = corners[i]
bx, by = corners[(i + 1) % n]
cross = (bx - ax) * (p[1] - ay) - (by - ay) * (p[0] - ax)
if abs(cross) < 1e-12:
continue
s = cross > 0
if sign is None:
sign = s
elif s != sign:
return False
return True
def obb_min_distance(
corners_a: list[tuple[float, float]],
corners_b: list[tuple[float, float]],
) -> float:
"""Minimum distance between two OBBs (convex polygons).
Returns 0.0 if overlapping or touching.
"""
if obb_overlap(corners_a, corners_b):
return 0.0
min_dist_sq = float("inf")
for poly, other in [(corners_a, corners_b), (corners_b, corners_a)]:
n = len(other)
for px, py in poly:
for i in range(n):
ax, ay = other[i]
bx, by = other[(i + 1) % n]
d_sq = _point_to_segment_dist_sq(px, py, ax, ay, bx, by)
if d_sq < min_dist_sq:
min_dist_sq = d_sq
return math.sqrt(min_dist_sq)

214
unilabos/layout_optimizer/optimizer.py Normal file
View File

@@ -0,0 +1,214 @@
"""差分进化布局优化器。
编码：N 个设备 → 3N 维向量 [x0, y0, θ0, x1, y1, θ1, ...]
使用 scipy.optimize.differential_evolution 进行全局优化。
初始布局（Pencil/回退）注入为种群种子个体加速收敛。
"""
from __future__ import annotations
import logging
import math
from typing import Any
import numpy as np
from scipy.optimize import differential_evolution
from .constraints import evaluate_constraints, evaluate_default_hard_constraints
from .mock_checkers import MockCollisionChecker, MockReachabilityChecker
from .models import Constraint, Device, Lab, Placement
from .pencil_integration import generate_initial_layout
logger = logging.getLogger(__name__)
def optimize(
devices: list[Device],
lab: Lab,
constraints: list[Constraint] | None = None,
collision_checker: Any | None = None,
reachability_checker: Any | None = None,
seed_placements: list[Placement] | None = None,
maxiter: int = 200,
popsize: int = 15,
tol: float = 1e-6,
seed: int | None = None,
) -> list[Placement]:
"""运行差分进化优化，返回最优布局。
Args:
devices: 待排布的设备列表
lab: 实验室平面图
constraints: 用户自定义约束列表（可选）
collision_checker: 碰撞检测实例（默认使用 MockCollisionChecker）
reachability_checker: 可达性检测实例（默认使用 MockReachabilityChecker）
seed_placements: 种子布局（若为 None 则自动生成）
maxiter: 最大迭代次数
popsize: 种群大小倍数
tol: 收敛容差
seed: 随机种子（用于可复现性）
Returns:
最优布局 Placement 列表
"""
if not devices:
return []
if collision_checker is None:
collision_checker = MockCollisionChecker()
if reachability_checker is None:
reachability_checker = MockReachabilityChecker()
if constraints is None:
constraints = []
n = len(devices)
# 构建边界：每个设备 (x, y, θ)
# 使用较小半径作为搜索边界，让 graduated boundary penalty 处理实际越界
# 对角线半径过于保守，会阻止长设备贴边对齐
bounds = []
for dev in devices:
half_min = min(dev.bbox[0], dev.bbox[1]) / 2
bounds.append((half_min, lab.width - half_min)) # x
bounds.append((half_min, lab.depth - half_min)) # y
bounds.append((0, 2 * math.pi)) # θ
bounds_array = np.array(bounds)
# 生成种子个体
if seed_placements is None:
seed_placements = generate_initial_layout(devices, lab)
seed_vector = _placements_to_vector(seed_placements, devices)
# 将种子钳位到边界内
seed_vector = np.clip(seed_vector, bounds_array[:, 0], bounds_array[:, 1])
def cost_function(x: np.ndarray) -> float:
placements = _vector_to_placements(x, devices)
# 默认硬约束（碰撞 + 边界）
hard_cost = evaluate_default_hard_constraints(
devices, placements, lab, collision_checker
)
if math.isinf(hard_cost):
return 1e18 # DE 不接受 inf，用大数替代
# 用户自定义约束
if constraints:
user_cost = evaluate_constraints(
devices, placements, lab, constraints,
collision_checker, reachability_checker,
)
if math.isinf(user_cost):
return 1e18
return hard_cost + user_cost
return hard_cost
# 构建初始种群：种子个体 + 随机个体
rng = np.random.default_rng(seed)
pop_count = popsize * 3 * n # scipy 默认 popsize * dim
init_pop = rng.uniform(
bounds_array[:, 0], bounds_array[:, 1], size=(pop_count, 3 * n)
)
init_pop[0] = seed_vector # 注入种子
logger.info(
"Starting DE optimization: %d devices, %d-dim, popsize=%d, maxiter=%d",
n, 3 * n, pop_count, maxiter,
)
# Early stopping: stop when cost hasn't improved by >0.1% for 20 generations
_best_costs: list[float] = []
_patience = 20
def _early_stop_callback(xk, convergence=0):
cost = cost_function(xk)
_best_costs.append(cost)
if len(_best_costs) >= _patience:
recent = _best_costs[-_patience:]
if recent[0] > 0:
improvement = (recent[0] - recent[-1]) / recent[0]
else:
improvement = 0.0
if improvement < 0.001: # < 0.1% improvement over last 20 gens
logger.info("Early stop: cost stable at %.4f for %d generations", cost, _patience)
return True
return False
result = differential_evolution(
cost_function,
bounds=list(bounds),
init=init_pop,
maxiter=maxiter,
tol=tol,
atol=1e-3,
mutation=(0.5, 1.0),
recombination=0.7,
seed=seed,
disp=False,
callback=_early_stop_callback,
)
logger.info(
"DE optimization complete: success=%s, cost=%.4f, iterations=%d, evaluations=%d",
result.success, result.fun, result.nit, result.nfev,
)
return _vector_to_placements(result.x, devices)
def snap_theta(placements: list[Placement], threshold_deg: float = 15.0) -> list[Placement]:
"""Snap each placement's theta to nearest 90° if within threshold.
Returns new Placement list (does not mutate input).
"""
threshold_rad = math.radians(threshold_deg)
cardinals = [0, math.pi / 2, math.pi, 3 * math.pi / 2, 2 * math.pi]
result = []
for p in placements:
theta_mod = p.theta % (2 * math.pi)
best_cardinal = min(cardinals, key=lambda c: abs(theta_mod - c))
if abs(theta_mod - best_cardinal) <= threshold_rad:
snapped = best_cardinal % (2 * math.pi)
else:
snapped = p.theta
result.append(Placement(
device_id=p.device_id, x=p.x, y=p.y, theta=snapped, uuid=p.uuid,
))
return result
def _placements_to_vector(
placements: list[Placement], devices: list[Device]
) -> np.ndarray:
"""将 Placement 列表编码为 3N 维向量。
按 devices 列表的顺序排列。若某设备在 placements 中缺失，用 (0, 0, 0) 填充。
"""
placement_map = {p.device_id: p for p in placements}
vec = np.zeros(3 * len(devices))
for i, dev in enumerate(devices):
p = placement_map.get(dev.id)
if p is not None:
vec[3 * i] = p.x
vec[3 * i + 1] = p.y
vec[3 * i + 2] = p.theta
return vec
def _vector_to_placements(
x: np.ndarray, devices: list[Device]
) -> list[Placement]:
"""将 3N 维向量解码为 Placement 列表。"""
placements = []
for i, dev in enumerate(devices):
placements.append(
Placement(
device_id=dev.id,
x=float(x[3 * i]),
y=float(x[3 * i + 1]),
theta=float(x[3 * i + 2]),
)
)
return placements

144
unilabos/layout_optimizer/pencil_integration.py Normal file
View File

@@ -0,0 +1,144 @@
"""初始布局生成：Pencil MCP 接口 + 行列式回退。
策略：
1. 尝试调用 Pencil AI MCP 生成初始布局
2. 若 Pencil 不可用或失败，回退到行列式放置算法
行列式回退逻辑：
- 设备按面积从大到小排序
- 沿 X 轴逐个放置，行满（超出 lab.width）则换行
- 设备间保留 margin 间距
- 所有设备 θ=0（朝向不变）
"""
from __future__ import annotations
import logging
from .models import Device, Lab, Placement
logger = logging.getLogger(__name__)
# 设备间最小间距（米）
DEFAULT_MARGIN = 0.3
def generate_initial_layout(
devices: list[Device],
lab: Lab,
margin: float = DEFAULT_MARGIN,
) -> list[Placement]:
"""生成初始布局方案。
优先尝试 Pencil MCP，失败则回退到行列式放置。
Args:
devices: 待放置的设备列表
lab: 实验室平面图
margin: 设备间最小间距
Returns:
初始布局 Placement 列表
"""
# 尝试 Pencil MCP
pencil_result = _try_pencil(devices, lab)
if pencil_result is not None:
logger.info("Using Pencil AI generated layout")
return pencil_result
# 回退到行列式
logger.info("Pencil unavailable, using row-based fallback layout")
return generate_fallback(devices, lab, margin)
def _try_pencil(
devices: list[Device],
lab: Lab,
) -> list[Placement] | None:
"""尝试通过 Pencil AI MCP 生成布局。
当前 Pencil MCP 不可用，返回 None 触发回退。
未来集成时，此函数应：
1. 将设备 2D 投影 + 实验室平面图序列化为 Pencil 输入格式
2. 调用 mcp__pencil_* 工具
3. 解析返回的布局方案为 Placement 列表
预留接口参数：
- devices: 设备列表（id, bbox）
- lab: 实验室尺寸
"""
# TODO: 当 Pencil MCP 可用时实现
# 预期调用方式：
# pencil_input = {
# "floor_plan": {"width": lab.width, "depth": lab.depth},
# "items": [{"id": d.id, "width": d.bbox[0], "depth": d.bbox[1]} for d in devices],
# }
# result = mcp__pencil_layout(pencil_input)
# return [Placement(device_id=r["id"], x=r["x"], y=r["y"], theta=r["theta"]) for r in result]
return None
def generate_fallback(
devices: list[Device],
lab: Lab,
margin: float = DEFAULT_MARGIN,
) -> list[Placement]:
"""行列式回退布局：按面积从大到小排序，逐行放置。
放置规则：
- 设备中心坐标，从左上角开始
- 每行从 margin + half_width 开始
- 行满（下一个设备右边缘超出 lab.width - margin）则换行
- 行高取该行最大设备深度
Args:
devices: 待放置的设备列表
lab: 实验室平面图
margin: 设备间最小间距
Returns:
Placement 列表。若实验室空间不足，剩余设备堆叠在右下角并记录警告。
"""
if not devices:
return []
# 按面积从大到小排序
sorted_devices = sorted(devices, key=lambda d: d.bbox[0] * d.bbox[1], reverse=True)
placements: list[Placement] = []
cursor_x = margin
cursor_y = margin
row_height = 0.0
for dev in sorted_devices:
w, d = dev.bbox
half_w = w / 2
half_d = d / 2
# 检查当前行是否放得下
if cursor_x + half_w + margin > lab.width and placements:
# 换行
cursor_x = margin
cursor_y += row_height + margin
row_height = 0.0
# 设备中心位置
cx = cursor_x + half_w
cy = cursor_y + half_d
# 检查是否超出实验室深度
if cy + half_d + margin > lab.depth:
logger.warning(
"Lab space insufficient for device '%s' (%s), "
"placing at overflow position",
dev.id,
dev.bbox,
)
placements.append(Placement(device_id=dev.id, x=cx, y=cy, theta=0.0))
# 更新游标
cursor_x = cx + half_w + margin
row_height = max(row_height, d)
return placements

29
unilabos/layout_optimizer/pyproject.toml Normal file
View File

@@ -0,0 +1,29 @@
[build-system]
requires = ["setuptools>=68.0", "wheel"]
build-backend = "setuptools.build_meta"
[project]
name = "layout-optimizer"
version = "0.1.0"
description = "AI laboratory layout optimizer for Uni-Lab Phase 3"
requires-python = ">=3.10"
dependencies = [
"scipy>=1.10",
"numpy>=1.24",
"fastapi>=0.100",
"uvicorn>=0.20",
"pydantic>=2.0",
]
[project.optional-dependencies]
dev = [
"pytest>=7.0",
"httpx>=0.24",
]
[tool.setuptools.packages.find]
where = ["."]
include = ["layout_optimizer*"]
[tool.pytest.ini_options]
testpaths = ["tests"]

433
unilabos/layout_optimizer/ros_checkers.py Normal file
View File

@@ -0,0 +1,433 @@
"""ROS2/MoveIt2 碰撞检测与 IKFast 可达性检测适配器。
集成阶段替换 mock_checkers.py 中的 Mock 实现，
依赖 Uni-Lab-OS 的 moveit2.py 提供的 MoveIt2 Python 接口。
用法：
from .ros_checkers import MoveItCollisionChecker, IKFastReachabilityChecker
# 碰撞检测
checker = MoveItCollisionChecker(moveit2_instance)
collisions = checker.check(placements)
# 可达性检测（体素图 O(1) 查询 + 实时 IK 回退）
reachability = IKFastReachabilityChecker(moveit2_instance, voxel_dir="/path/to/voxels")
reachable = reachability.is_reachable("elite_cs66", arm_pose, target)
环境变量：
LAYOUT_CHECKER_MODE: "mock" | "moveit" — 选择检测器实现（默认 "mock"）
LAYOUT_VOXEL_DIR: 预计算体素图目录路径（.npz 文件）
前置条件：
- ROS2 + MoveIt2 运行中
- moveit2.py 中的 MoveIt2 实例已初始化
- 命名规范：碰撞对象使用 {device_id}_ 前缀
"""
from __future__ import annotations
import logging
import math
import os
from pathlib import Path
from typing import TYPE_CHECKING, Any
import numpy as np
from .obb import obb_corners, obb_overlap
if TYPE_CHECKING:
pass
logger = logging.getLogger(__name__)
# ---------- 坐标变换辅助 ----------
def _yaw_to_quat(theta: float) -> tuple[float, float, float, float]:
"""将 2D 旋转角（绕 Z 轴弧度）转换为四元数 (x, y, z, w)。"""
return (0.0, 0.0, math.sin(theta / 2), math.cos(theta / 2))
def _transform_to_arm_frame(
arm_pose: dict, target: dict,
) -> tuple[float, float, float]:
"""将目标点从世界坐标系变换到机械臂基坐标系。
Args:
arm_pose: {"x": float, "y": float, "theta": float}
target: {"x": float, "y": float, "z": float}
Returns:
(local_x, local_y, local_z) 在臂基坐标系中的位置
"""
dx = target["x"] - arm_pose["x"]
dy = target["y"] - arm_pose["y"]
theta = arm_pose.get("theta", 0.0)
cos_t = math.cos(-theta)
sin_t = math.sin(-theta)
local_x = dx * cos_t - dy * sin_t
local_y = dx * sin_t + dy * cos_t
local_z = target.get("z", 0.0)
return (local_x, local_y, local_z)
# ---------- MoveItCollisionChecker ----------
class MoveItCollisionChecker:
"""通过 MoveIt2 PlanningScene 进行碰撞检测。
工作流程：
1. 将所有设备同步为 MoveIt2 碰撞盒（{device_id}_ 前缀）
2. 使用 python-fcl 进行精确两两碰撞检测（若可用）
3. 若 FCL 不可用，回退到 OBB SAT 检测
同步到 MoveIt2 确保机器人运动规划也能感知设备布局。
"""
def __init__(
self,
moveit2: Any,
*,
default_height: float = 0.4,
sync_to_scene: bool = True,
):
"""
Args:
moveit2: Uni-Lab-OS moveit2.py 中的 MoveIt2 实例
default_height: 碰撞盒默认高度（米）
sync_to_scene: 是否同步碰撞对象到 MoveIt2 规划场景
"""
self._moveit2 = moveit2
self._default_height = default_height
self._sync_to_scene = sync_to_scene
self._fcl_available = self._check_fcl()
@staticmethod
def _check_fcl() -> bool:
"""检查 python-fcl 是否可用。"""
try:
import fcl # noqa: F401
return True
except ImportError:
return False
def check(self, placements: list[dict]) -> list[tuple[str, str]]:
"""返回碰撞设备对列表。
Args:
placements: [{"id": str, "bbox": (w, d), "pos": (x, y, θ)}, ...]
Returns:
[("device_a", "device_b"), ...] 存在碰撞的设备对
"""
# 同步到 MoveIt2 规划场景
if self._sync_to_scene:
self._sync_collision_objects(placements)
# 碰撞检测
if self._fcl_available:
return self._check_with_fcl(placements)
return self._check_with_obb(placements)
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
def _sync_collision_objects(self, placements: list[dict]) -> None:
"""将设备布局同步到 MoveIt2 规划场景。
使用 {device_id}_ 前缀命名碰撞对象。
"""
for p in placements:
obj_id = f"{p['id']}_"
w, d = p["bbox"]
x, y = p["pos"][:2]
theta = p["pos"][2] if len(p["pos"]) > 2 else 0.0
h = self._default_height
try:
self._moveit2.add_collision_box(
id=obj_id,
size=(w, d, h),
position=(x, y, h / 2),
quat_xyzw=_yaw_to_quat(theta),
)
except Exception:
logger.warning("Failed to sync collision object %s", obj_id, exc_info=True)
def _check_with_fcl(self, placements: list[dict]) -> list[tuple[str, str]]:
"""使用 python-fcl 进行精确碰撞检测。"""
import fcl
objects: list[tuple[str, Any]] = []
for p in placements:
w, d = p["bbox"]
h = self._default_height
x, y = p["pos"][:2]
theta = p["pos"][2] if len(p["pos"]) > 2 else 0.0
geom = fcl.Box(w, d, h)
tf = fcl.Transform(
_yaw_to_rotation_matrix(theta),
np.array([x, y, h / 2]),
)
obj = fcl.CollisionObject(geom, tf)
objects.append((p["id"], obj))
collisions: list[tuple[str, str]] = []
n = len(objects)
for i in range(n):
for j in range(i + 1, n):
id_a, obj_a = objects[i]
id_b, obj_b = objects[j]
request = fcl.CollisionRequest()
result = fcl.CollisionResult()
ret = fcl.collide(obj_a, obj_b, request, result)
if ret > 0:
collisions.append((id_a, id_b))
return collisions
def _check_with_obb(self, placements: list[dict]) -> list[tuple[str, str]]:
"""OBB SAT 回退检测（与 MockCollisionChecker 相同算法）。"""
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
@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
# ---------- IKFastReachabilityChecker ----------
class IKFastReachabilityChecker:
"""基于 MoveIt2 compute_ik 和预计算体素图的可达性检测。
双模式：
1. 体素图模式（O(1)）：从 .npz 文件加载预计算可达性网格，
将目标点变换到臂基坐标系后直接查表。
2. 实时 IK 模式（~5ms/call）：调用 MoveIt2.compute_ik()，
支持约束感知的精确可达性判断。
优先使用体素图，无匹配时回退到实时 IK。
"""
def __init__(
self,
moveit2: Any = None,
*,
voxel_dir: str | Path | None = None,
voxel_resolution: float = 0.01,
):
"""
Args:
moveit2: MoveIt2 实例（用于实时 IK 回退）
voxel_dir: 预计算体素图目录（.npz 文件，文件名 = arm_id）
voxel_resolution: 体素分辨率（米），用于坐标 → 索引转换
"""
self._moveit2 = moveit2
self._voxel_resolution = voxel_resolution
self._voxel_maps: dict[str, _VoxelMap] = {}
if voxel_dir is not None:
self._load_voxel_maps(Path(voxel_dir))
def is_reachable(self, arm_id: str, arm_pose: dict, target: dict) -> bool:
"""判断机械臂在给定位姿下能否到达目标点。
Args:
arm_id: 机械臂设备 ID
arm_pose: {"x": float, "y": float, "theta": float}
target: {"x": float, "y": float, "z": float}
Returns:
True 如果可达
"""
local = _transform_to_arm_frame(arm_pose, target)
# 1. 体素图查询（O(1)）
if arm_id in self._voxel_maps:
return self._check_voxel(arm_id, local)
# 2. 实时 IK 回退
if self._moveit2 is not None:
return self._check_live_ik(local)
# 无可用检测方式，乐观返回（记录警告）
logger.warning(
"No reachability checker available for arm %s, returning True", arm_id,
)
return True
def _load_voxel_maps(self, voxel_dir: Path) -> None:
"""加载目录下所有 .npz 体素图文件。
文件格式：{arm_id}.npz，包含：
- "grid": bool ndarray (nx, ny, nz) — True 表示可达
- "origin": float ndarray (3,) — 网格原点（臂基坐标系）
- "resolution": float — 体素分辨率（米）
"""
if not voxel_dir.exists():
logger.warning("Voxel directory does not exist: %s", voxel_dir)
return
for npz_file in voxel_dir.glob("*.npz"):
arm_id = npz_file.stem
try:
data = np.load(str(npz_file))
grid = data["grid"].astype(bool)
origin = data["origin"].astype(float)
resolution = float(data.get("resolution", self._voxel_resolution))
self._voxel_maps[arm_id] = _VoxelMap(
grid=grid, origin=origin, resolution=resolution,
)
logger.info(
"Loaded voxel map for %s: shape=%s, resolution=%.3f",
arm_id, grid.shape, resolution,
)
except Exception:
logger.warning("Failed to load voxel map %s", npz_file, exc_info=True)
def _check_voxel(self, arm_id: str, local: tuple[float, float, float]) -> bool:
"""通过体素网格查询可达性。"""
vm = self._voxel_maps[arm_id]
ix = int(round((local[0] - vm.origin[0]) / vm.resolution))
iy = int(round((local[1] - vm.origin[1]) / vm.resolution))
iz = int(round((local[2] - vm.origin[2]) / vm.resolution))
if (
0 <= ix < vm.grid.shape[0]
and 0 <= iy < vm.grid.shape[1]
and 0 <= iz < vm.grid.shape[2]
):
return bool(vm.grid[ix, iy, iz])
# 超出体素图范围 → 不可达
return False
def _check_live_ik(self, local: tuple[float, float, float]) -> bool:
"""调用 MoveIt2.compute_ik() 进行实时可达性检测。
compute_ik 返回 JointState（成功）或 None（不可达）。
使用默认朝下姿态（四元数 0, 1, 0, 0 即绕 X 轴旋转 180°）。
"""
# 目标姿态：末端执行器朝下
quat_xyzw = (0.0, 1.0, 0.0, 0.0)
try:
result = self._moveit2.compute_ik(
position=local,
quat_xyzw=quat_xyzw,
)
return result is not None
except Exception:
logger.warning("compute_ik call failed", exc_info=True)
return False
# ---------- 体素图数据类 ----------
class _VoxelMap:
"""预计算可达性体素网格。"""
__slots__ = ("grid", "origin", "resolution")
def __init__(
self,
grid: np.ndarray,
origin: np.ndarray,
resolution: float,
):
self.grid = grid
self.origin = origin
self.resolution = resolution
# ---------- FCL 辅助 ----------
def _yaw_to_rotation_matrix(theta: float) -> np.ndarray:
"""绕 Z 轴旋转矩阵（3×3）。"""
c, s = math.cos(theta), math.sin(theta)
return np.array([
[c, -s, 0.0],
[s, c, 0.0],
[0.0, 0.0, 1.0],
])
# ---------- 工厂函数 ----------
def create_checkers(
moveit2: Any = None,
*,
mode: str | None = None,
voxel_dir: str | None = None,
) -> tuple[Any, Any]:
"""根据环境变量或参数创建检测器实例。
Args:
moveit2: MoveIt2 实例（moveit 模式必需）
mode: "mock" | "moveit"（默认从 LAYOUT_CHECKER_MODE 环境变量读取）
voxel_dir: 体素图目录（默认从 LAYOUT_VOXEL_DIR 环境变量读取）
Returns:
(collision_checker, reachability_checker)
"""
if mode is None:
mode = os.getenv("LAYOUT_CHECKER_MODE", "mock")
if mode == "moveit":
if moveit2 is None:
raise ValueError("MoveIt2 instance required for 'moveit' checker mode")
if voxel_dir is None:
voxel_dir = os.getenv("LAYOUT_VOXEL_DIR")
collision = MoveItCollisionChecker(moveit2)
reachability = IKFastReachabilityChecker(
moveit2, voxel_dir=voxel_dir,
)
logger.info("Using MoveIt2 checkers (voxel_dir=%s)", voxel_dir)
return collision, reachability
# 默认：mock 模式
from .mock_checkers import MockCollisionChecker, MockReachabilityChecker
logger.info("Using mock checkers")
return MockCollisionChecker(), MockReachabilityChecker()

331
unilabos/layout_optimizer/seeders.py Normal file
View File

@@ -0,0 +1,331 @@
"""Force-directed seeder engine with named parameter presets.
Produces initial device placements for the layout optimizer.
Different layout strategies (compact, spread, workflow-aware) are
parameter configurations of the same force-directed simulation engine.
"""
from __future__ import annotations
import logging
import math
from dataclasses import dataclass, replace
from .models import Device, Lab, Placement
from .obb import obb_corners, obb_overlap, obb_min_distance
logger = logging.getLogger(__name__)
@dataclass
class SeederParams:
"""Parameters for the force-directed seeder engine."""
boundary_attraction: float = 0.0 # >0 push to walls, <0 push to center
mutual_repulsion: float = 1.0 # inter-device repulsion strength
edge_attraction: float = 0.0 # workflow edge attraction (Stage 2)
orientation_mode: str = "none" # "outward" | "inward" | "none"
PRESETS: dict[str, SeederParams | None] = {
"compact_outward": SeederParams(
boundary_attraction=-1.0, mutual_repulsion=0.5, orientation_mode="outward",
),
"spread_inward": SeederParams(
boundary_attraction=1.0, mutual_repulsion=1.0, orientation_mode="inward",
),
"workflow_cluster": SeederParams(
boundary_attraction=-0.5, mutual_repulsion=0.5,
edge_attraction=1.0, orientation_mode="outward",
),
"row_fallback": None, # Delegates to generate_fallback()
}
def resolve_seeder_params(
preset_name: str, overrides: dict | None = None,
) -> SeederParams | None:
"""Look up preset by name and apply overrides."""
if preset_name not in PRESETS:
raise ValueError(
f"Unknown seeder preset '{preset_name}'. "
f"Available: {list(PRESETS.keys())}"
)
params = PRESETS[preset_name]
if params is None or not overrides:
return params
return replace(params, **{k: v for k, v in overrides.items() if hasattr(params, k)})
def seed_layout(
devices: list[Device],
lab: Lab,
params: SeederParams | None,
edges: list[list[str]] | None = None,
) -> list[Placement]:
"""Generate initial device placements using force-directed simulation.
Args:
devices: devices to place
lab: lab dimensions
params: seeder parameters (None = row_fallback)
edges: workflow edges as [device_a_id, device_b_id] pairs (Stage 2)
Returns:
list of Placement objects, one per device
"""
if not devices:
return []
if params is None:
from .pencil_integration import generate_fallback
return generate_fallback(devices, lab)
return _force_simulation(devices, lab, params, edges)
def _force_simulation(
devices: list[Device],
lab: Lab,
params: SeederParams,
edges: list[list[str]] | None = None,
max_iter: int = 80,
dt: float = 0.05,
damping: float = 0.8,
) -> list[Placement]:
"""Run force-directed simulation to produce initial placements."""
n = len(devices)
center_x, center_y = lab.width / 2, lab.depth / 2
# Initialize positions: grid layout within lab bounds
cols = max(1, int(math.ceil(math.sqrt(n))))
rows_count = max(1, math.ceil(n / cols))
positions = [] # (x, y) per device
for i, dev in enumerate(devices):
row, col = divmod(i, cols)
margin = 0.3
x = margin + (col + 0.5) * (lab.width - 2 * margin) / cols
y = margin + (row + 0.5) * (lab.depth - 2 * margin) / rows_count
x = min(max(x, dev.bbox[0] / 2), lab.width - dev.bbox[0] / 2)
y = min(max(y, dev.bbox[1] / 2), lab.depth - dev.bbox[1] / 2)
positions.append([x, y])
# Initialize orientations
thetas = [0.0] * n
# Build edge lookup for Stage 2
edge_set: set[tuple[int, int]] = set()
if edges and params.edge_attraction > 0:
id_to_idx = {d.id: i for i, d in enumerate(devices)}
for e in edges:
if len(e) == 2 and e[0] in id_to_idx and e[1] in id_to_idx:
edge_set.add((id_to_idx[e[0]], id_to_idx[e[1]]))
converged = False
for iteration in range(max_iter):
forces = [[0.0, 0.0] for _ in range(n)]
total_force = 0.0
# 1. Boundary force
for i in range(n):
dx = positions[i][0] - center_x
dy = positions[i][1] - center_y
dist_to_center = math.sqrt(dx * dx + dy * dy) + 1e-9
f = params.boundary_attraction
forces[i][0] += f * dx / dist_to_center
forces[i][1] += f * dy / dist_to_center
# 2. Mutual repulsion (OBB edge-to-edge)
for i in range(n):
for j in range(i + 1, n):
ci = obb_corners(
positions[i][0], positions[i][1],
devices[i].bbox[0], devices[i].bbox[1], thetas[i],
)
cj = obb_corners(
positions[j][0], positions[j][1],
devices[j].bbox[0], devices[j].bbox[1], thetas[j],
)
dist = obb_min_distance(ci, cj)
if dist < 1e-9:
dist = 0.01 # Prevent division by zero for overlapping
dx = positions[i][0] - positions[j][0]
dy = positions[i][1] - positions[j][1]
d_center = math.sqrt(dx * dx + dy * dy) + 1e-9
repulsion = params.mutual_repulsion / (dist * dist + 0.1)
fx = repulsion * dx / d_center
fy = repulsion * dy / d_center
forces[i][0] += fx
forces[i][1] += fy
forces[j][0] -= fx
forces[j][1] -= fy
# 3. Edge attraction (Stage 2)
if params.edge_attraction > 0:
for i_idx, j_idx in edge_set:
dx = positions[j_idx][0] - positions[i_idx][0]
dy = positions[j_idx][1] - positions[i_idx][1]
dist = math.sqrt(dx * dx + dy * dy) + 1e-9
f = params.edge_attraction * dist * 0.1
forces[i_idx][0] += f * dx / dist
forces[i_idx][1] += f * dy / dist
forces[j_idx][0] -= f * dx / dist
forces[j_idx][1] -= f * dy / dist
# 4. Update positions (Euler + damping)
for i in range(n):
positions[i][0] += forces[i][0] * dt * damping
positions[i][1] += forces[i][1] * dt * damping
total_force += math.sqrt(forces[i][0]**2 + forces[i][1]**2)
# 5. Update orientations
if params.orientation_mode != "none":
for i in range(n):
thetas[i] = _compute_orientation(
positions[i][0], positions[i][1],
center_x, center_y,
devices[i], params.orientation_mode,
)
# 6. Clamp to lab bounds
for i in range(n):
hw, hh = devices[i].bbox[0] / 2, devices[i].bbox[1] / 2
positions[i][0] = max(hw, min(lab.width - hw, positions[i][0]))
positions[i][1] = max(hh, min(lab.depth - hh, positions[i][1]))
if total_force < 0.01 * n:
converged = True
logger.info("Force simulation converged at iteration %d", iteration)
break
if not converged:
logger.info("Force simulation reached max iterations (%d)", max_iter)
placements = [
Placement(device_id=devices[i].id, x=positions[i][0], y=positions[i][1], theta=thetas[i])
for i in range(n)
]
# Log initial collision count
initial_collisions = _count_collisions(devices, placements)
logger.info("Seeder: %d initial collision pairs before resolution", initial_collisions)
# Collision resolution pass
placements = _resolve_collisions(devices, placements, lab, max_passes=5)
# Log diagnostics
final_collisions = _count_collisions(devices, placements)
no_openings = sum(1 for d in devices if not d.openings)
logger.info(
"Seeder complete: %d devices, %d without openings, %d collision pairs remaining",
n, no_openings, final_collisions,
)
return placements
def _compute_orientation(
x: float, y: float,
center_x: float, center_y: float,
device: Device,
mode: str,
) -> float:
"""Compute theta so the device's front faces outward or inward."""
dx = x - center_x
dy = y - center_y
if abs(dx) < 1e-9 and abs(dy) < 1e-9:
return 0.0
angle_to_device = math.atan2(dy, dx)
if device.openings:
front = device.openings[0].direction
else:
front = (0.0, -1.0) # Default: -Y is front
front_angle = math.atan2(front[1], front[0])
if mode == "outward":
target = angle_to_device
elif mode == "inward":
target = angle_to_device + math.pi
else:
return 0.0
return (target - front_angle) % (2 * math.pi)
def _count_collisions(devices: list[Device], placements: list[Placement]) -> int:
"""Count OBB collision pairs (for diagnostics logging)."""
n = len(devices)
count = 0
for i in range(n):
for j in range(i + 1, n):
ci = obb_corners(placements[i].x, placements[i].y,
devices[i].bbox[0], devices[i].bbox[1], placements[i].theta)
cj = obb_corners(placements[j].x, placements[j].y,
devices[j].bbox[0], devices[j].bbox[1], placements[j].theta)
if obb_overlap(ci, cj):
count += 1
return count
def _resolve_collisions(
devices: list[Device],
placements: list[Placement],
lab: Lab,
max_passes: int = 5,
) -> list[Placement]:
"""Push overlapping devices apart. Returns new placement list."""
positions = [[p.x, p.y] for p in placements]
thetas = [p.theta for p in placements]
n = len(devices)
for pass_num in range(max_passes):
has_collision = False
for i in range(n):
for j in range(i + 1, n):
ci = obb_corners(
positions[i][0], positions[i][1],
devices[i].bbox[0], devices[i].bbox[1], thetas[i],
)
cj = obb_corners(
positions[j][0], positions[j][1],
devices[j].bbox[0], devices[j].bbox[1], thetas[j],
)
if obb_overlap(ci, cj):
has_collision = True
dx = positions[i][0] - positions[j][0]
dy = positions[i][1] - positions[j][1]
dist = math.sqrt(dx * dx + dy * dy) + 1e-9
push = 0.5 * (
max(devices[i].bbox[0], devices[i].bbox[1])
+ max(devices[j].bbox[0], devices[j].bbox[1])
) / 4
positions[i][0] += push * dx / dist
positions[i][1] += push * dy / dist
positions[j][0] -= push * dx / dist
positions[j][1] -= push * dy / dist
# Clamp to bounds (rotation-aware AABB half-extents)
for i in range(n):
cos_t = abs(math.cos(thetas[i]))
sin_t = abs(math.sin(thetas[i]))
hw = (devices[i].bbox[0] * cos_t + devices[i].bbox[1] * sin_t) / 2
hh = (devices[i].bbox[0] * sin_t + devices[i].bbox[1] * cos_t) / 2
positions[i][0] = max(hw, min(lab.width - hw, positions[i][0]))
positions[i][1] = max(hh, min(lab.depth - hh, positions[i][1]))
if not has_collision:
logger.info("Collision resolution complete after %d passes", pass_num + 1)
break
else:
logger.warning(
"Collision resolution: %d passes exhausted, collisions may remain",
max_passes,
)
return [
Placement(device_id=placements[i].device_id,
x=positions[i][0], y=positions[i][1],
theta=thetas[i], uuid=placements[i].uuid)
for i in range(n)
]

566
unilabos/layout_optimizer/server.py Normal file
View File

@@ -0,0 +1,566 @@
"""FastAPI 开发服务器。
开发阶段独立运行于 localhost:8000，前端通过 CORS 调用。
集成阶段合并到 Uni-Lab-OS 的 FastAPI 服务中。
运行方式：
uvicorn layout_optimizer.server:app --host 0.0.0.0 --port 8000 --reload
前端访问：
http://localhost:8000/
"""
from __future__ import annotations
import logging
import math
import os
from pathlib import Path
from fastapi import FastAPI
from fastapi.middleware.cors import CORSMiddleware
from fastapi.responses import FileResponse, RedirectResponse
from fastapi.staticfiles import StaticFiles
from pydantic import BaseModel
from .device_catalog import (
create_devices_from_list,
load_devices_from_assets,
load_devices_from_registry,
load_footprints,
merge_device_lists,
)
from .lab_parser import parse_lab
from .intent_interpreter import InterpretResult, interpret_intents
from .models import Constraint, Intent
from .optimizer import optimize
logging.basicConfig(level=logging.INFO)
logger = logging.getLogger(__name__)
STATIC_DIR = Path(__file__).parent / "static"
# 可配置路径
# LeapLab 仓库根目录（layout_optimizer 的父目录）
_REPO_ROOT = Path(__file__).resolve().parent.parent
UNI_LAB_ASSETS_DIR = Path(
os.getenv("UNI_LAB_ASSETS_DIR", str(_REPO_ROOT.parent / "uni-lab-assets"))
)
UNI_LAB_ASSETS_MODELS_DIR = UNI_LAB_ASSETS_DIR / "device_models"
UNI_LAB_ASSETS_DATA_JSON = UNI_LAB_ASSETS_DIR / "data.json"
UNI_LAB_OS_DEVICE_MESH_DIR = Path(
os.getenv(
"UNI_LAB_OS_DEVICE_MESH_DIR",
str(_REPO_ROOT / "Uni-Lab-OS" / "unilabos" / "device_mesh" / "devices"),
)
)
app = FastAPI(title="Layout Optimizer", version="0.2.0")
app.add_middleware(
CORSMiddleware,
allow_origins=["*"], # 开发阶段允许所有来源
allow_methods=["*"],
allow_headers=["*"],
)
# 挂载静态文件目录
app.mount("/static", StaticFiles(directory=str(STATIC_DIR)), name="static")
# 挂载 3D 模型和缩略图
if UNI_LAB_ASSETS_MODELS_DIR.exists():
app.mount("/models", StaticFiles(directory=str(UNI_LAB_ASSETS_MODELS_DIR)), name="models")
logger.info("Mounted /models from %s", UNI_LAB_ASSETS_MODELS_DIR)
else:
logger.warning("uni-lab-assets models dir not found: %s", UNI_LAB_ASSETS_MODELS_DIR)
# ---------- 设备目录缓存 ----------
_device_cache: list[dict] | None = None
# 消耗品/配件关键词（不独立放置于实验台）
_CONSUMABLE_KEYWORDS = {
"plate", "well", "tube", "tip", "reservoir", "carrier", "nest",
"adapter", "trough", "magnet_module", "magnet_plate", "rack", "lid",
"seal", "cap", "vial", "flask", "dish", "block", "strip", "insert",
"gasket", "pad", "grid_segment", "spacer", "diti_tray",
}
# 但包含这些关键词的是独立设备，不是消耗品
_DEVICE_KEYWORDS = {
"reader", "handler", "hotel", "washer", "stacker", "sealer", "labeler",
"centrifuge", "incubator", "shaker", "robot", "arm", "flex", "dispenser",
"printer", "scanner", "analyzer", "fluorometer", "spectrophotometer",
"thermocycler", "module",
}
def _is_standalone_device(device_id: str, bbox: tuple[float, float]) -> bool:
"""判断设备是否独立放置于实验台（非消耗品/配件）。"""
mx = max(bbox[0], bbox[1])
mn = min(bbox[0], bbox[1])
if mx >= 0.30:
return True # 大于 30cm 一定是独立设备
if mx < 0.05:
return False # 小于 5cm 一定是消耗品
lower = device_id.lower()
# 非常扁平（一维 < 3cm）的几乎都是配件/载具，即使名称匹配设备关键词
if mn < 0.03:
return False
# 先检查消耗品关键词（如果匹配，再看是否有设备关键词覆盖）
is_consumable_name = any(kw in lower for kw in _CONSUMABLE_KEYWORDS)
is_device_name = any(kw in lower for kw in _DEVICE_KEYWORDS)
if is_consumable_name and not is_device_name:
return False
if is_device_name:
return True
# 默认：>= 15cm 视为设备
return mx >= 0.15
def _build_device_list() -> list[dict]:
"""构建合并后的设备列表（缓存）。"""
global _device_cache
if _device_cache is not None:
return _device_cache
footprints = load_footprints()
registry = load_devices_from_registry(UNI_LAB_OS_DEVICE_MESH_DIR, footprints)
assets = load_devices_from_assets(UNI_LAB_ASSETS_DATA_JSON, footprints)
merged = merge_device_lists(registry, assets)
_device_cache = [
{
"id": d.id,
"name": d.name,
"device_type": d.device_type,
"source": d.source,
"bbox": list(d.bbox),
"height": d.height,
"origin_offset": list(d.origin_offset),
"openings": [
{"direction": list(o.direction), "label": o.label}
for o in d.openings
],
"model_path": d.model_path,
"model_type": d.model_type,
"thumbnail_url": d.thumbnail_url,
"is_standalone": _is_standalone_device(d.id, d.bbox),
}
for d in merged
]
standalone = sum(1 for d in _device_cache if d["is_standalone"])
logger.info("Built device catalog: %d devices (%d standalone)", len(_device_cache), standalone)
return _device_cache
# ---------- 路由 ----------
@app.get("/", include_in_schema=False)
async def root():
return RedirectResponse(url="/lab3d")
@app.get("/lab3d", include_in_schema=False)
async def lab3d_ui():
return FileResponse(STATIC_DIR / "lab3d.html")
@app.get("/devices")
async def list_devices(source: str = "all"):
"""返回合并后的设备目录。?source=registry|assets|all"""
devices = _build_device_list()
if source != "all":
devices = [d for d in devices if d["source"] == source]
return devices
@app.get("/health")
async def health():
return {"status": "ok"}
# ---------- 意图解释 API ----------
class IntentSpec(BaseModel):
intent: str
params: dict = {}
description: str = ""
class TranslationEntry(BaseModel):
source_intent: str
source_description: str
source_params: dict
generated_constraints: list[dict]
explanation: str
confidence: str = "high"
class InterpretRequest(BaseModel):
intents: list[IntentSpec]
class InterpretResponse(BaseModel):
constraints: list[dict]
translations: list[TranslationEntry]
workflow_edges: list[list[str]]
errors: list[str]
@app.post("/interpret", response_model=InterpretResponse)
async def run_interpret(request: InterpretRequest):
"""将语义化意图翻译为约束列表，供用户确认后传入 /optimize。"""
logger.info("Interpret request: %d intents", len(request.intents))
intents = [
Intent(
intent=i.intent,
params=i.params,
description=i.description,
)
for i in request.intents
]
result: InterpretResult = interpret_intents(intents)
return InterpretResponse(
constraints=[
{"type": c.type, "rule_name": c.rule_name, "params": c.params, "weight": c.weight}
for c in result.constraints
],
translations=[
TranslationEntry(
source_intent=t["source_intent"],
source_description=t.get("source_description", ""),
source_params=t.get("source_params", {}),
generated_constraints=t["generated_constraints"],
explanation=t["explanation"],
confidence=t.get("confidence", "high"),
)
for t in result.translations
],
workflow_edges=result.workflow_edges,
errors=result.errors,
)
@app.get("/interpret/schema")
async def interpret_schema():
"""返回可用意图类型及其参数规范，供 LLM agent 发现和使用。"""
return {
"description": "Layout optimizer intent schema. LLM agents should translate user requests into these intents.",
"intents": {
"reachable_by": {
"description": "Robot arm must be able to reach all target devices",
"params": {
"arm": {"type": "string", "required": True, "description": "Device ID of robot arm"},
"targets": {"type": "list[string]", "required": True, "description": "Device IDs the arm must reach"},
},
"generates": "hard reachability constraint per target",
},
"close_together": {
"description": "Group of devices should be placed near each other",
"params": {
"devices": {"type": "list[string]", "required": True, "description": "Device IDs (min 2)"},
"priority": {"type": "string", "required": False, "default": "medium", "enum": ["low", "medium", "high"]},
},
"generates": "soft minimize_distance for each pair",
},
"far_apart": {
"description": "Devices should be placed far from each other",
"params": {
"devices": {"type": "list[string]", "required": True, "description": "Device IDs (min 2)"},
"priority": {"type": "string", "required": False, "default": "medium", "enum": ["low", "medium", "high"]},
},
"generates": "soft maximize_distance for each pair",
},
"max_distance": {
"description": "Two devices must be within a maximum distance",
"params": {
"device_a": {"type": "string", "required": True},
"device_b": {"type": "string", "required": True},
"distance": {"type": "float", "required": True, "description": "Max edge-to-edge distance in meters"},
},
"generates": "hard distance_less_than",
},
"min_distance": {
"description": "Two devices must be at least a minimum distance apart",
"params": {
"device_a": {"type": "string", "required": True},
"device_b": {"type": "string", "required": True},
"distance": {"type": "float", "required": True, "description": "Min edge-to-edge distance in meters"},
},
"generates": "hard distance_greater_than",
},
"min_spacing": {
"description": "Minimum gap between all device pairs",
"params": {
"min_gap": {"type": "float", "required": False, "default": 0.3, "description": "Minimum gap in meters"},
},
"generates": "hard min_spacing",
},
"workflow_hint": {
"description": "Workflow step order — consecutive devices should be near each other",
"params": {
"workflow": {"type": "string", "required": False, "description": "Workflow name (e.g. 'pcr')"},
"devices": {"type": "list[string]", "required": True, "description": "Ordered device IDs following workflow steps"},
},
"generates": "soft minimize_distance for consecutive pairs + workflow_edges",
},
"face_outward": {
"description": "Devices should face outward from lab center",
"params": {},
"generates": "soft prefer_orientation_mode outward",
},
"face_inward": {
"description": "Devices should face inward toward lab center",
"params": {},
"generates": "soft prefer_orientation_mode inward",
},
"align_cardinal": {
"description": "Devices should align to cardinal directions (0/90/180/270 degrees)",
"params": {},
"generates": "soft prefer_aligned",
},
},
}
# ---------- 优化 API ----------
class DeviceSpec(BaseModel):
id: str
name: str = ""
size: list[float] | None = None
device_type: str = "static"
uuid: str = ""
class ConstraintSpec(BaseModel):
type: str # "hard" or "soft"
rule_name: str
params: dict = {}
weight: float = 1.0
class LabSpec(BaseModel):
width: float
depth: float
obstacles: list[dict] = []
class OptimizeRequest(BaseModel):
devices: list[DeviceSpec]
lab: LabSpec
constraints: list[ConstraintSpec] = []
seeder: str = "compact_outward"
seeder_overrides: dict = {}
run_de: bool = True
workflow_edges: list[list[str]] = []
maxiter: int = 200
seed: int | None = None
class PositionXYZ(BaseModel):
x: float
y: float
z: float
class PlacementResult(BaseModel):
device_id: str
uuid: str
position: PositionXYZ
rotation: PositionXYZ
class OptimizeResponse(BaseModel):
placements: list[PlacementResult]
cost: float
success: bool
seeder_used: str = ""
de_ran: bool = True
@app.post("/optimize", response_model=OptimizeResponse)
async def run_optimize(request: OptimizeRequest):
"""接收设备列表+约束，返回最优布局方案。"""
from fastapi import HTTPException
from .constraints import evaluate_default_hard_constraints
from .mock_checkers import MockCollisionChecker
from .optimizer import optimize, snap_theta
from .seeders import resolve_seeder_params, seed_layout
logger.info(
"Optimize request: %d devices, lab %.1f×%.1f, %d constraints, seeder=%s, run_de=%s",
len(request.devices),
request.lab.width,
request.lab.depth,
len(request.constraints),
request.seeder,
request.run_de,
)
# Build mapping: internal uuid-based id → (catalog_id, uuid)
# create_devices_from_list uses uuid as Device.id when available
id_to_catalog: dict[str, str] = {}
id_to_uuid: dict[str, str] = {}
for d in request.devices:
internal_id = d.uuid or d.id
id_to_catalog[internal_id] = d.id
id_to_uuid[internal_id] = d.uuid or d.id
# 转换输入
devices = create_devices_from_list(
[d.model_dump() for d in request.devices]
)
lab = parse_lab(request.lab.model_dump())
constraints = [
Constraint(
type=c.type,
rule_name=c.rule_name,
params=c.params,
weight=c.weight,
)
for c in request.constraints
]
# 1. Resolve seeder
try:
params = resolve_seeder_params(request.seeder, request.seeder_overrides or None)
except ValueError as e:
raise HTTPException(status_code=400, detail=str(e))
# 2. Seed
seed_placements = seed_layout(
devices, lab, params,
request.workflow_edges or None,
)
# 3. Auto-inject orientation soft constraints for DE
if request.run_de and request.seeder != "row_fallback" and seed_placements:
# Resolve orientation mode from seeder preset
orientation_mode = params.orientation_mode if params else "none"
if orientation_mode != "none":
# prefer_orientation_mode: position-aware outward/inward facing penalty
constraints.append(Constraint(
type="soft",
rule_name="prefer_orientation_mode",
params={"mode": orientation_mode},
weight=request.seeder_overrides.get("orientation_weight", 5.0),
))
# prefer_aligned: penalize non-cardinal angles
align_weight = request.seeder_overrides.get("align_weight", 2.0)
if align_weight > 0:
constraints.append(Constraint(
type="soft",
rule_name="prefer_aligned",
weight=align_weight,
))
# 4. Conditional Differential Evolution
de_ran = False
checker = MockCollisionChecker()
if request.run_de:
result_placements = optimize(
devices=devices,
lab=lab,
constraints=constraints,
collision_checker=checker,
seed_placements=seed_placements,
maxiter=request.maxiter,
seed=request.seed,
)
de_ran = True
else:
result_placements = seed_placements
# 5. θ snap post-processing
result_placements = snap_theta(result_placements)
# 6. Evaluate final cost (binary mode for pass/fail reporting)
final_cost = evaluate_default_hard_constraints(
devices, result_placements, lab, checker, graduated=False,
)
return OptimizeResponse(
placements=[
PlacementResult(
device_id=id_to_catalog.get(p.device_id, p.device_id),
uuid=id_to_uuid.get(p.device_id, p.device_id),
position=PositionXYZ(x=round(p.x, 4), y=round(p.y, 4), z=0.0),
rotation=PositionXYZ(x=0.0, y=0.0, z=round(p.theta, 4)),
)
for p in result_placements
],
cost=final_cost,
success=not math.isinf(final_cost),
seeder_used=request.seeder,
de_ran=de_ran,
)
# ---------- 场景状态 API（演示用） ----------
_scene_state: dict = {"version": 0, "placements": []}
_lab_state: dict = {"width": 4.0, "depth": 4.0}
class LabDimensions(BaseModel):
width: float
depth: float
@app.get("/scene/lab")
async def get_lab_dimensions():
"""返回当前实验室尺寸（前端推送，agent 读取）。"""
return _lab_state
@app.post("/scene/lab")
async def set_lab_dimensions(dims: LabDimensions):
"""前端在加载和尺寸变更时推送。"""
_lab_state["width"] = dims.width
_lab_state["depth"] = dims.depth
return _lab_state
class ScenePlacementsRequest(BaseModel):
placements: list[PlacementResult]
@app.post("/scene/placements")
async def set_scene_placements(request: ScenePlacementsRequest):
"""Agent 写入布局结果，前端轮询读取。"""
_scene_state["version"] += 1
_scene_state["placements"] = [p.model_dump() for p in request.placements]
logger.info(
"Scene placements updated: version=%d, count=%d",
_scene_state["version"],
len(request.placements),
)
return {"version": _scene_state["version"], "count": len(request.placements)}
@app.get("/scene/placements")
async def get_scene_placements():
"""前端轮询此端点，检测 version 变化后应用布局。"""
return _scene_state
@app.delete("/scene/placements")
async def clear_scene_placements():
"""重置场景状态（重录时使用）。"""
_scene_state["version"] = 0
_scene_state["placements"] = []
return {"version": 0, "placements": []}

1227
unilabos/layout_optimizer/static/lab3d.html Normal file
View File

File diff suppressed because it is too large Load Diff

0
unilabos/layout_optimizer/tests/__init__.py Normal file
View File

7
unilabos/layout_optimizer/tests/fixtures/sample_devices.json vendored Normal file
View File

@@ -0,0 +1,7 @@
[
{"id": "arm_1", "name": "Elite CS66 Arm", "size": [0.20, 0.20], "device_type": "articulation"},
{"id": "liquid_handler", "name": "Agilent Bravo", "size": [0.80, 0.65], "device_type": "static"},
{"id": "centrifuge", "name": "Centrifuge", "size": [0.50, 0.50], "device_type": "static"},
{"id": "plate_hotel", "name": "Thermo Orbitor RS2", "size": [0.45, 0.55], "device_type": "static"},
{"id": "hplc", "name": "HPLC Station", "size": [0.60, 0.50], "device_type": "static"}
]

7
unilabos/layout_optimizer/tests/fixtures/sample_lab.json vendored Normal file
View File

@@ -0,0 +1,7 @@
{
"width": 5.0,
"depth": 4.0,
"obstacles": [
{"x": 2.5, "y": 0.0, "width": 0.1, "depth": 0.5}
]
}

371
unilabos/layout_optimizer/tests/test_bugfixes_v2.py Normal file
View File

@@ -0,0 +1,371 @@
"""Regression tests for V2 Stage 1 bugfixes.
Covers:
- Duplicate device ID stacking (uuid-based internal IDs)
- DE orientation preservation (prefer_orientation_mode constraint)
- prefer_aligned auto-injection and adjustability
- Preset switch reorientation
- min_spacing with duplicate catalog IDs
"""
import math
import pytest
from ..constraints import evaluate_constraints
from ..mock_checkers import MockCollisionChecker
from ..models import Constraint, Device, Lab, Opening, Placement
from ..obb import obb_corners, obb_overlap
from ..optimizer import (
_placements_to_vector,
_vector_to_placements,
optimize,
snap_theta,
)
from ..seeders import resolve_seeder_params, seed_layout
# ── Helpers ─────────────────────────────────────────────
def _ot(uid: str) -> Device:
return Device(
id=uid, name="Opentrons Liquid Handler",
bbox=(0.6243, 0.5672), openings=[Opening(direction=(0.0, -1.0))],
)
def _tecan(uid: str) -> Device:
return Device(
id=uid, name="Tecan EVO 100",
bbox=(0.8121, 0.8574), openings=[Opening(direction=(0.0, -1.0))],
)
def _facing_dot(p: Placement, device: Device, lab: Lab) -> float:
"""Dot product of rotated front vector with vector from center to device.
Positive = outward, negative = inward."""
cx, cy = lab.width / 2, lab.depth / 2
dx, dy = p.x - cx, p.y - cy
front = device.openings[0].direction if device.openings else (0.0, -1.0)
rf_x = math.cos(p.theta) * front[0] - math.sin(p.theta) * front[1]
rf_y = math.sin(p.theta) * front[0] + math.cos(p.theta) * front[1]
return rf_x * dx + rf_y * dy
def _has_collision(devices, placements):
for i in range(len(devices)):
for j in range(i + 1, len(devices)):
ci = obb_corners(placements[i].x, placements[i].y,
devices[i].bbox[0], devices[i].bbox[1], placements[i].theta)
cj = obb_corners(placements[j].x, placements[j].y,
devices[j].bbox[0], devices[j].bbox[1], placements[j].theta)
if obb_overlap(ci, cj):
return True
return False
# ── Bug 1: Duplicate device ID stacking ────────────────
class TestDuplicateDeviceIDs:
"""When two instances of the same catalog device are placed,
unique uuid-based IDs must prevent dict-key collisions."""
def test_vector_roundtrip_preserves_unique_positions(self):
"""_placements_to_vector → _vector_to_placements with unique IDs."""
devices = [_ot("uuid-a"), _ot("uuid-b")]
placements = [
Placement(device_id="uuid-a", x=0.5, y=0.5, theta=0.0),
Placement(device_id="uuid-b", x=1.5, y=1.5, theta=1.0),
]
vec = _placements_to_vector(placements, devices)
decoded = _vector_to_placements(vec, devices)
assert decoded[0].x == pytest.approx(0.5)
assert decoded[1].x == pytest.approx(1.5)
def test_min_spacing_detects_stacked_unique_ids(self):
"""min_spacing should detect two devices at the same position
when they have unique IDs."""
devices = [_ot("uuid-a"), _ot("uuid-b")]
stacked = [
Placement(device_id="uuid-a", x=1.0, y=1.0, theta=0.0),
Placement(device_id="uuid-b", x=1.0, y=1.0, theta=0.0),
]
lab = Lab(width=5, depth=5)
constraints = [Constraint(type="hard", rule_name="min_spacing",
params={"min_gap": 0.05})]
cost = evaluate_constraints(devices, stacked, lab, constraints,
MockCollisionChecker())
assert math.isinf(cost)
def test_create_devices_uses_uuid(self):
"""create_devices_from_list should use uuid as Device.id."""
from ..device_catalog import create_devices_from_list
specs = [
{"id": "opentrons_liquid_handler", "uuid": "abc-123"},
{"id": "opentrons_liquid_handler", "uuid": "def-456"},
]
devices = create_devices_from_list(specs)
assert devices[0].id == "abc-123"
assert devices[1].id == "def-456"
# Both should have the same bbox from footprints
assert devices[0].bbox == devices[1].bbox
def test_create_devices_fallback_no_uuid(self):
"""Without uuid, Device.id falls back to catalog id."""
from ..device_catalog import create_devices_from_list
specs = [{"id": "opentrons_liquid_handler"}]
devices = create_devices_from_list(specs)
assert devices[0].id == "opentrons_liquid_handler"
# ── Bug 2 & 4: DE orientation preservation ─────────────
class TestOrientationWithDE:
"""DE must preserve seeder orientation direction (outward/inward)
via the prefer_orientation_mode constraint."""
def _run_de_with_orientation(self, mode, seed_val=42):
devices = [_ot("ot1"), _ot("ot2"), _tecan("tecan")]
lab = Lab(width=2.0, depth=2.0)
params = resolve_seeder_params(
"compact_outward" if mode == "outward" else "spread_inward"
)
seed = seed_layout(devices, lab, params)
constraints = [
Constraint(type="hard", rule_name="min_spacing",
params={"min_gap": 0.05}),
Constraint(type="soft", rule_name="prefer_orientation_mode",
params={"mode": mode}, weight=5.0),
Constraint(type="soft", rule_name="prefer_aligned", weight=2.0),
]
result = optimize(devices, lab, constraints, seed_placements=seed,
maxiter=200, seed=seed_val)
result = snap_theta(result)
return devices, lab, result
def test_compact_outward_de_faces_outward(self):
devices, lab, result = self._run_de_with_orientation("outward")
for i, p in enumerate(result):
dot = _facing_dot(p, devices[i], lab)
assert dot > 0, (
f"{p.device_id} faces inward (dot={dot:.3f}) "
f"at ({p.x:.2f},{p.y:.2f}) theta={math.degrees(p.theta):.0f}°"
)
def test_spread_inward_de_faces_inward(self):
devices, lab, result = self._run_de_with_orientation("inward")
for i, p in enumerate(result):
dot = _facing_dot(p, devices[i], lab)
assert dot < 0, (
f"{p.device_id} faces outward (dot={dot:.3f}) "
f"at ({p.x:.2f},{p.y:.2f}) theta={math.degrees(p.theta):.0f}°"
)
def test_switching_preset_changes_orientation(self):
"""Switching from outward to inward should produce opposite facing."""
_, lab, out_result = self._run_de_with_orientation("outward")
devices_in, _, in_result = self._run_de_with_orientation("inward")
# At least one device should have different facing
out_dots = [_facing_dot(p, devices_in[i], lab) for i, p in enumerate(out_result)]
in_dots = [_facing_dot(p, devices_in[i], lab) for i, p in enumerate(in_result)]
# Outward: all positive; inward: all negative
assert all(d > 0 for d in out_dots), f"outward dots: {out_dots}"
assert all(d < 0 for d in in_dots), f"inward dots: {in_dots}"
def test_no_collision_after_de(self):
devices, lab, result = self._run_de_with_orientation("outward")
assert not _has_collision(devices, result)
# ── Bug 3: prefer_aligned & prefer_orientation_mode ────
class TestOrientationConstraints:
"""Test the new constraint rules directly."""
def test_prefer_orientation_mode_outward_zero_at_correct(self):
"""Zero cost when device faces outward from center."""
device = _ot("a")
# Device to the right of center, front pointing right
# front=(0,-1), theta=pi/2 → rotated front = (1, 0) = rightward
lab = Lab(width=4, depth=4)
placements = [Placement("a", 3.0, 2.0, math.pi / 2)]
constraint = Constraint(
type="soft", rule_name="prefer_orientation_mode",
params={"mode": "outward"}, weight=1.0,
)
cost = evaluate_constraints(
[device], placements, lab, [constraint], MockCollisionChecker(),
)
assert cost == pytest.approx(0.0, abs=0.01)
def test_prefer_orientation_mode_outward_penalty_at_inward(self):
"""High cost when device faces inward (opposite of outward)."""
device = _ot("a")
# Device to the right of center, front pointing left (inward)
# front=(0,-1), theta=3*pi/2 → rotated front = (-1, 0) = leftward
lab = Lab(width=4, depth=4)
placements = [Placement("a", 3.0, 2.0, 3 * math.pi / 2)]
constraint = Constraint(
type="soft", rule_name="prefer_orientation_mode",
params={"mode": "outward"}, weight=1.0,
)
cost = evaluate_constraints(
[device], placements, lab, [constraint], MockCollisionChecker(),
)
# 180° off → (1 - cos(pi)) / 2 = 1.0
assert cost == pytest.approx(1.0, abs=0.05)
def test_prefer_orientation_mode_inward(self):
"""Zero cost when device faces inward."""
device = _ot("a")
# Device to the right of center, front pointing left (inward)
lab = Lab(width=4, depth=4)
placements = [Placement("a", 3.0, 2.0, 3 * math.pi / 2)]
constraint = Constraint(
type="soft", rule_name="prefer_orientation_mode",
params={"mode": "inward"}, weight=1.0,
)
cost = evaluate_constraints(
[device], placements, lab, [constraint], MockCollisionChecker(),
)
assert cost == pytest.approx(0.0, abs=0.01)
def test_prefer_seeder_orientation_zero_at_target(self):
"""Zero cost when theta matches target."""
device = Device(id="a", name="A", bbox=(0.5, 0.5))
lab = Lab(width=4, depth=4)
placements = [Placement("a", 2, 2, 1.5)]
constraint = Constraint(
type="soft", rule_name="prefer_seeder_orientation",
params={"target_thetas": {"a": 1.5}}, weight=1.0,
)
cost = evaluate_constraints(
[device], placements, lab, [constraint], MockCollisionChecker(),
)
assert cost == pytest.approx(0.0, abs=1e-9)
def test_prefer_seeder_orientation_penalty_at_deviation(self):
"""Non-zero cost when theta deviates from target."""
device = Device(id="a", name="A", bbox=(0.5, 0.5))
lab = Lab(width=4, depth=4)
placements = [Placement("a", 2, 2, math.pi)] # pi away from 0
constraint = Constraint(
type="soft", rule_name="prefer_seeder_orientation",
params={"target_thetas": {"a": 0.0}}, weight=1.0,
)
cost = evaluate_constraints(
[device], placements, lab, [constraint], MockCollisionChecker(),
)
# (1 - cos(pi)) / 2 = 1.0
assert cost == pytest.approx(1.0)
# ── API endpoint regression ────────────────────────────
class TestEndpointOrientation:
"""Test that /optimize injects orientation constraints."""
def test_endpoint_with_de_injects_orientation(self):
from fastapi.testclient import TestClient
from ..server import app
client = TestClient(app)
resp = client.post("/optimize", json={
"devices": [
{"id": "opentrons_liquid_handler", "uuid": "u1"},
{"id": "opentrons_liquid_handler", "uuid": "u2"},
],
"lab": {"width": 3, "depth": 3},
"seeder": "compact_outward",
"run_de": True,
"maxiter": 50,
"seed": 42,
})
assert resp.status_code == 200
data = resp.json()
# Both devices should have unique uuids in response
uuids = [p["uuid"] for p in data["placements"]]
assert len(set(uuids)) == 2, f"Expected 2 unique uuids, got {uuids}"
def test_endpoint_orientation_weight_override(self):
from fastapi.testclient import TestClient
from ..server import app
client = TestClient(app)
resp = client.post("/optimize", json={
"devices": [{"id": "opentrons_liquid_handler", "uuid": "u1"}],
"lab": {"width": 3, "depth": 3},
"seeder": "compact_outward",
"seeder_overrides": {"orientation_weight": 10, "align_weight": 0},
"run_de": True,
"maxiter": 50,
"seed": 42,
})
assert resp.status_code == 200
def test_endpoint_align_weight_zero_disables(self):
"""Setting align_weight=0 should not inject prefer_aligned."""
from fastapi.testclient import TestClient
from ..server import app
client = TestClient(app)
resp = client.post("/optimize", json={
"devices": [{"id": "opentrons_liquid_handler", "uuid": "u1"}],
"lab": {"width": 3, "depth": 3},
"seeder": "compact_outward",
"seeder_overrides": {"align_weight": 0},
"run_de": True,
"maxiter": 50,
"seed": 42,
})
assert resp.status_code == 200
# ── Broader scenario tests ─────────────────────────────
class TestScenarios:
"""End-to-end scenarios similar to user's real usage."""
def test_user_scenario_2ot_1tecan_compact_outward(self):
"""User's exact scenario: 2 OT + 1 Tecan in 2m×2m, compact outward."""
devices = [_ot("ot1"), _ot("ot2"), _tecan("tecan")]
lab = Lab(width=2.0, depth=2.0)
params = resolve_seeder_params("compact_outward")
seed = seed_layout(devices, lab, params)
constraints = [
Constraint(type="hard", rule_name="min_spacing",
params={"min_gap": 0.05}),
Constraint(type="soft", rule_name="prefer_orientation_mode",
params={"mode": "outward"}, weight=5.0),
Constraint(type="soft", rule_name="prefer_aligned", weight=2.0),
]
result = optimize(devices, lab, constraints, seed_placements=seed,
maxiter=200, seed=42)
result = snap_theta(result)
# No stacking
assert not _has_collision(devices, result)
# All outward
for i, p in enumerate(result):
assert _facing_dot(p, devices[i], lab) > 0
def test_4_medium_devices_mixed_openings(self):
"""4 devices with different opening directions."""
devices = [
Device(id="d0", name="D0", bbox=(0.5, 0.3), openings=[Opening((1, 0))]),
Device(id="d1", name="D1", bbox=(0.5, 0.3), openings=[Opening((-1, 0))]),
Device(id="d2", name="D2", bbox=(0.5, 0.3), openings=[Opening((0, -1))]),
Device(id="d3", name="D3", bbox=(0.5, 0.3), openings=[Opening((0, 1))]),
]
lab = Lab(width=3.0, depth=3.0)
params = resolve_seeder_params("compact_outward")
seed = seed_layout(devices, lab, params)
constraints = [
Constraint(type="hard", rule_name="min_spacing",
params={"min_gap": 0.05}),
Constraint(type="soft", rule_name="prefer_orientation_mode",
params={"mode": "outward"}, weight=5.0),
Constraint(type="soft", rule_name="prefer_aligned", weight=2.0),
]
result = optimize(devices, lab, constraints, seed_placements=seed,
maxiter=200, seed=42)
result = snap_theta(result)
assert not _has_collision(devices, result)
for i, p in enumerate(result):
assert _facing_dot(p, devices[i], lab) > 0

274
unilabos/layout_optimizer/tests/test_constraints.py Normal file
View File

@@ -0,0 +1,274 @@
"""约束体系测试。"""
import math
import pytest
from ..constraints import (
evaluate_constraints,
evaluate_default_hard_constraints,
)
from ..mock_checkers import MockCollisionChecker, MockReachabilityChecker
from ..models import Constraint, Device, Lab, Placement
def _make_devices():
return [
Device(id="a", name="Device A", bbox=(0.5, 0.5)),
Device(id="b", name="Device B", bbox=(0.5, 0.5)),
]
def _make_lab():
return Lab(width=5.0, depth=4.0)
class TestDefaultHardConstraints:
def test_no_collision_passes(self):
"""无碰撞的布局应返回 0。"""
devices = _make_devices()
placements = [
Placement("a", 1.0, 1.0, 0.0),
Placement("b", 3.0, 3.0, 0.0),
]
checker = MockCollisionChecker()
cost = evaluate_default_hard_constraints(devices, placements, _make_lab(), checker)
assert cost == 0.0
def test_collision_returns_graduated_penalty(self):
"""碰撞布局应返回正的graduated penalty（非inf）。"""
devices = _make_devices()
placements = [
Placement("a", 1.0, 1.0, 0.0),
Placement("b", 1.2, 1.0, 0.0),
]
checker = MockCollisionChecker()
cost = evaluate_default_hard_constraints(devices, placements, _make_lab(), checker)
assert cost > 0
assert not math.isinf(cost)
def test_collision_returns_inf_binary_mode(self):
"""Binary mode: 碰撞布局应返回 inf。"""
devices = _make_devices()
placements = [
Placement("a", 1.0, 1.0, 0.0),
Placement("b", 1.2, 1.0, 0.0),
]
checker = MockCollisionChecker()
cost = evaluate_default_hard_constraints(
devices, placements, _make_lab(), checker, graduated=False,
)
assert math.isinf(cost)
def test_out_of_bounds_returns_graduated_penalty(self):
"""越界布局应返回正的graduated penalty（非inf）。"""
devices = _make_devices()
placements = [
Placement("a", 0.1, 0.1, 0.0), # 左下角越界
Placement("b", 3.0, 3.0, 0.0),
]
checker = MockCollisionChecker()
cost = evaluate_default_hard_constraints(devices, placements, _make_lab(), checker)
assert cost > 0
assert not math.isinf(cost)
def test_out_of_bounds_returns_inf_binary_mode(self):
"""Binary mode: 越界布局应返回 inf。"""
devices = _make_devices()
placements = [
Placement("a", 0.1, 0.1, 0.0),
Placement("b", 3.0, 3.0, 0.0),
]
checker = MockCollisionChecker()
cost = evaluate_default_hard_constraints(
devices, placements, _make_lab(), checker, graduated=False,
)
assert math.isinf(cost)
def test_worse_collision_higher_cost(self):
"""Deeper penetration should produce higher cost."""
devices = _make_devices()
checker = MockCollisionChecker()
lab = _make_lab()
# Small overlap
cost_small = evaluate_default_hard_constraints(
devices, [Placement("a", 1.0, 1.0, 0.0), Placement("b", 1.4, 1.0, 0.0)],
lab, checker,
)
# Large overlap
cost_large = evaluate_default_hard_constraints(
devices, [Placement("a", 1.0, 1.0, 0.0), Placement("b", 1.1, 1.0, 0.0)],
lab, checker,
)
assert cost_large > cost_small > 0
class TestUserConstraints:
def test_distance_less_than_satisfied(self):
"""距离约束满足时 cost=0。"""
devices = _make_devices()
placements = [
Placement("a", 1.0, 1.0, 0.0),
Placement("b", 1.5, 1.0, 0.0),
]
constraints = [
Constraint(type="hard", rule_name="distance_less_than",
params={"device_a": "a", "device_b": "b", "distance": 1.0})
]
checker = MockCollisionChecker()
reachability = MockReachabilityChecker()
cost = evaluate_constraints(
devices, placements, _make_lab(), constraints, checker, reachability
)
assert cost == 0.0
def test_distance_less_than_violated_hard(self):
"""硬距离约束违反返回 inf。"""
devices = _make_devices()
placements = [
Placement("a", 1.0, 1.0, 0.0),
Placement("b", 4.0, 3.0, 0.0),
]
constraints = [
Constraint(type="hard", rule_name="distance_less_than",
params={"device_a": "a", "device_b": "b", "distance": 1.0})
]
checker = MockCollisionChecker()
cost = evaluate_constraints(
devices, placements, _make_lab(), constraints, checker
)
assert math.isinf(cost)
def test_minimize_distance_cost(self):
"""minimize_distance 约束应返回正比于距离的 cost。"""
devices = _make_devices()
placements = [
Placement("a", 1.0, 1.0, 0.0),
Placement("b", 3.0, 1.0, 0.0),
]
constraints = [
Constraint(type="soft", rule_name="minimize_distance",
params={"device_a": "a", "device_b": "b"}, weight=2.0)
]
checker = MockCollisionChecker()
cost = evaluate_constraints(
devices, placements, _make_lab(), constraints, checker
)
# edge-to-edge distance = 2.0 - 0.25 - 0.25 = 1.5, weight = 2.0 → cost = 3.0
assert abs(cost - 3.0) < 0.01
def test_reachability_constraint(self):
"""可达性约束：目标在臂展内应通过（不返回 inf）。
Opening-faces-arm penalty may add a small soft cost when the
target's opening doesn't face the arm, but it must not cause
hard failure (inf).
"""
devices = [
Device(id="arm", name="Arm", bbox=(0.2, 0.2), device_type="articulation"),
Device(id="target", name="Target", bbox=(0.5, 0.5)),
]
placements = [
Placement("arm", 1.0, 1.0, 0.0),
Placement("target", 1.5, 1.0, 0.0),
]
constraints = [
Constraint(type="hard", rule_name="reachability",
params={"arm_id": "arm", "target_device_id": "target"})
]
checker = MockCollisionChecker()
reachability = MockReachabilityChecker(arm_reach={"arm": 1.0})
cost = evaluate_constraints(
devices, placements, _make_lab(), constraints, checker, reachability
)
assert not math.isinf(cost) # reachable → no hard failure
def test_reachability_constraint_violated(self):
"""可达性约束：目标超出臂展应返回 inf。"""
devices = [
Device(id="arm", name="Arm", bbox=(0.2, 0.2), device_type="articulation"),
Device(id="target", name="Target", bbox=(0.5, 0.5)),
]
placements = [
Placement("arm", 1.0, 1.0, 0.0),
Placement("target", 4.0, 3.0, 0.0),
]
constraints = [
Constraint(type="hard", rule_name="reachability",
params={"arm_id": "arm", "target_device_id": "target"})
]
checker = MockCollisionChecker()
reachability = MockReachabilityChecker(arm_reach={"arm": 1.0})
cost = evaluate_constraints(
devices, placements, _make_lab(), constraints, checker, reachability
)
assert math.isinf(cost)
def test_distance_less_than_uses_edge_to_edge():
"""distance_less_than should measure edge-to-edge, not center-to-center.
Two devices: centers 3m apart, each 2m wide → edge gap = 1m.
Constraint: distance_less_than 1.5m (edge-to-edge).
Old center-to-center: 3m > 1.5m → violation.
New edge-to-edge: 1m < 1.5m → satisfied.
"""
devices = [
Device(id="a", name="A", bbox=(2.0, 1.0)),
Device(id="b", name="B", bbox=(2.0, 1.0)),
]
placements = [
Placement(device_id="a", x=1.0, y=1.0, theta=0.0),
Placement(device_id="b", x=4.0, y=1.0, theta=0.0),
]
lab = Lab(width=10, depth=10)
constraint = Constraint(
type="soft", rule_name="distance_less_than",
params={"device_a": "a", "device_b": "b", "distance": 1.5},
weight=1.0,
)
checker = MockCollisionChecker()
cost = evaluate_constraints(devices, placements, lab, [constraint], checker)
assert cost == pytest.approx(0.0)
def test_prefer_aligned_zero_at_cardinal():
"""prefer_aligned cost = 0 when all devices at 0/90/180/270°."""
devices = [Device(id="a", name="A", bbox=(1.0, 1.0))]
lab = Lab(width=10, depth=10)
checker = MockCollisionChecker()
for angle in [0, math.pi / 2, math.pi, 3 * math.pi / 2]:
placements = [Placement(device_id="a", x=5, y=5, theta=angle)]
constraint = Constraint(type="soft", rule_name="prefer_aligned", weight=1.0)
cost = evaluate_constraints(devices, placements, lab, [constraint], checker)
assert cost == pytest.approx(0.0, abs=1e-9)
def test_prefer_aligned_max_at_45():
"""prefer_aligned cost is maximum when device at 45°."""
devices = [Device(id="a", name="A", bbox=(1.0, 1.0))]
placements = [Placement(device_id="a", x=5, y=5, theta=math.pi / 4)]
lab = Lab(width=10, depth=10)
constraint = Constraint(type="soft", rule_name="prefer_aligned", weight=1.0)
checker = MockCollisionChecker()
cost = evaluate_constraints(devices, placements, lab, [constraint], checker)
# (1 - cos(4 * pi/4)) / 2 = (1 - cos(pi)) / 2 = (1 - (-1)) / 2 = 1.0
assert cost == pytest.approx(1.0)
def test_prefer_aligned_sums_over_devices():
"""Cost sums across all devices."""
devices = [
Device(id="a", name="A", bbox=(1.0, 1.0)),
Device(id="b", name="B", bbox=(1.0, 1.0)),
]
placements = [
Placement(device_id="a", x=2, y=2, theta=math.pi / 4), # cost = 1.0
Placement(device_id="b", x=7, y=7, theta=math.pi / 4), # cost = 1.0
]
lab = Lab(width=10, depth=10)
constraint = Constraint(type="soft", rule_name="prefer_aligned", weight=2.0)
checker = MockCollisionChecker()
cost = evaluate_constraints(devices, placements, lab, [constraint], checker)
# 2 devices × 1.0 × weight 2.0 = 4.0
assert cost == pytest.approx(4.0)

222
unilabos/layout_optimizer/tests/test_device_catalog.py Normal file
View File

@@ -0,0 +1,222 @@
"""device_catalog 双源加载测试。"""
from __future__ import annotations
from pathlib import Path
import pytest
from ..device_catalog import (
_DEFAULT_FOOTPRINTS,
create_devices_from_list,
load_devices_from_assets,
load_devices_from_registry,
load_footprints,
merge_device_lists,
reset_footprints_cache,
resolve_device,
)
# ---------- fixtures ----------
# LeapLab/layout_optimizer/tests/ → LeapLab/ → DPTech/
_LEAPLAB = Path(__file__).resolve().parent.parent.parent
_DPTECH = _LEAPLAB.parent
DATA_JSON = _DPTECH / "uni-lab-assets" / "data.json"
REGISTRY_DIR = _LEAPLAB / "Uni-Lab-OS" / "unilabos" / "device_mesh" / "devices"
@pytest.fixture(autouse=True)
def _clear_cache():
"""每个测试前清除缓存。"""
reset_footprints_cache()
yield
reset_footprints_cache()
# ---------- footprints ----------
class TestLoadFootprints:
def test_load_footprints_exists(self):
fp = load_footprints(_DEFAULT_FOOTPRINTS)
assert isinstance(fp, dict)
assert len(fp) > 0
def test_footprint_structure(self):
fp = load_footprints()
for dev_id, entry in fp.items():
assert "bbox" in entry, f"{dev_id} missing bbox"
assert len(entry["bbox"]) == 2
assert "height" in entry
assert "origin_offset" in entry
assert "openings" in entry
def test_known_device_in_footprints(self):
fp = load_footprints()
assert "agilent_bravo" in fp
bbox = fp["agilent_bravo"]["bbox"]
assert 0.5 < bbox[0] < 1.0 # width ~0.65m
assert 0.5 < bbox[1] < 1.0 # depth ~0.70m
def test_nonexistent_path_returns_empty(self):
reset_footprints_cache()
fp = load_footprints("/nonexistent/footprints.json")
assert fp == {}
# ---------- assets 加载 ----------
class TestLoadFromAssets:
@pytest.mark.skipif(not DATA_JSON.exists(), reason="data.json not found")
def test_load_returns_devices(self):
devices = load_devices_from_assets(DATA_JSON)
assert len(devices) > 0
@pytest.mark.skipif(not DATA_JSON.exists(), reason="data.json not found")
def test_known_device_has_real_bbox(self):
devices = load_devices_from_assets(DATA_JSON)
bravo = next((d for d in devices if d.id == "agilent_bravo"), None)
assert bravo is not None
assert bravo.bbox != (0.6, 0.4) # 不是默认值
assert bravo.source == "assets"
def test_missing_data_json(self):
devices = load_devices_from_assets("/nonexistent/data.json")
assert devices == []
# ---------- registry 加载 ----------
class TestLoadFromRegistry:
@pytest.mark.skipif(not REGISTRY_DIR.exists(), reason="registry dir not found")
def test_load_returns_devices(self):
devices = load_devices_from_registry(REGISTRY_DIR)
assert len(devices) > 0
@pytest.mark.skipif(not REGISTRY_DIR.exists(), reason="registry dir not found")
def test_elite_robot_present(self):
devices = load_devices_from_registry(REGISTRY_DIR)
elite = next((d for d in devices if d.id == "elite_robot"), None)
assert elite is not None
assert elite.source == "registry"
def test_missing_dir(self):
devices = load_devices_from_registry("/nonexistent/")
assert devices == []
# ---------- 合并与去重 ----------
class TestMergeDedup:
def test_registry_wins_dedup(self):
from ..models import Device
reg = [Device(id="ot2", name="OT-2 Registry", bbox=(0.62, 0.50), source="registry")]
asset = [Device(id="ot2", name="OT-2 Assets", bbox=(0.62, 0.50), source="assets")]
merged = merge_device_lists(reg, asset)
ot2 = next(d for d in merged if d.id == "ot2")
assert ot2.source == "registry"
assert ot2.name == "OT-2 Registry"
def test_merge_preserves_unique(self):
from ..models import Device
reg = [Device(id="elite", name="Elite", source="registry")]
asset = [Device(id="bravo", name="Bravo", source="assets")]
merged = merge_device_lists(reg, asset)
ids = {d.id for d in merged}
assert ids == {"elite", "bravo"}
def test_registry_inherits_asset_model(self):
from ..models import Device
reg = [Device(id="ot2", name="OT-2", source="registry", model_path="")]
asset = [Device(id="ot2", name="OT-2", source="assets", model_path="/models/ot2/mesh.glb")]
merged = merge_device_lists(reg, asset)
ot2 = next(d for d in merged if d.id == "ot2")
assert ot2.model_path == "/models/ot2/mesh.glb"
# ---------- resolve_device ----------
class TestResolveDevice:
def test_known_device(self):
dev = resolve_device("agilent_bravo")
assert dev is not None
assert dev.id == "agilent_bravo"
assert dev.bbox != (0.6, 0.4)
def test_fallback_known_sizes(self):
dev = resolve_device("ot2")
assert dev is not None
assert dev.bbox == (0.62, 0.50)
def test_unknown_device_returns_none(self):
dev = resolve_device("totally_unknown_device_xyz")
assert dev is None
# ---------- create_devices_from_list (向后兼容) ----------
class TestCreateDevicesFromList:
def test_basic(self):
specs = [{"id": "test_dev", "name": "Test"}]
devs = create_devices_from_list(specs)
assert len(devs) == 1
assert devs[0].id == "test_dev"
def test_with_explicit_size(self):
specs = [{"id": "custom", "name": "Custom", "size": [1.0, 0.5]}]
devs = create_devices_from_list(specs)
assert devs[0].bbox == (1.0, 0.5)
def test_footprint_size_used_when_no_explicit(self):
specs = [{"id": "agilent_bravo", "name": "Bravo"}]
devs = create_devices_from_list(specs)
assert devs[0].bbox != (0.6, 0.4) # 使用 footprints 中的真实尺寸
# ---------- server endpoint (需要 httpx) ----------
class TestDevicesEndpoint:
def test_get_devices(self):
try:
from fastapi.testclient import TestClient
except ImportError:
pytest.skip("fastapi testclient not available")
from ..server import app
client = TestClient(app)
resp = client.get("/devices")
assert resp.status_code == 200
data = resp.json()
assert isinstance(data, list)
# 可能为空（取决于 uni-lab-assets 是否在预期路径）
if len(data) > 0:
first = data[0]
assert "id" in first
assert "bbox" in first
assert "source" in first
def test_filter_by_source(self):
try:
from fastapi.testclient import TestClient
except ImportError:
pytest.skip("fastapi testclient not available")
from ..server import app
client = TestClient(app)
resp = client.get("/devices?source=registry")
assert resp.status_code == 200
data = resp.json()
for d in data:
assert d["source"] == "registry"

261
unilabos/layout_optimizer/tests/test_e2e_pcr_pipeline.py Normal file
View File

@@ -0,0 +1,261 @@
"""End-to-end pipeline test: intents → interpret → optimize → verify.
Tests each stage boundary independently so failures are easy to localize.
Uses real PCR workflow devices with footprints from the catalog.
"""
import math
import pytest
from fastapi.testclient import TestClient
from ..server import app
client = TestClient(app)
# -- Scene: 5 PCR devices the user has already placed in the scene --
PCR_DEVICES = [
{"id": "thermo_orbitor_rs2_hotel", "name": "Plate Hotel", "device_type": "static"},
{"id": "arm_slider", "name": "Robot Arm", "device_type": "articulation"},
{"id": "opentrons_liquid_handler", "name": "Liquid Handler", "device_type": "static"},
{"id": "agilent_plateloc", "name": "Plate Sealer", "device_type": "static"},
{"id": "inheco_odtc_96xl", "name": "Thermal Cycler", "device_type": "static"},
]
PCR_LAB = {"width": 6.0, "depth": 4.0}
# -- Stage 1: simulated LLM output (what the LLM would produce from NL) --
# User said: "take plate from hotel, prepare sample in opentrons,
# seal plate then pcr cycle, arm_slider handles transfers"
LLM_INTENTS = [
{
"intent": "reachable_by",
"params": {
"arm": "arm_slider",
"targets": [
"thermo_orbitor_rs2_hotel",
"opentrons_liquid_handler",
"agilent_plateloc",
"inheco_odtc_96xl",
],
},
"description": "arm_slider must reach all workflow devices",
},
{
"intent": "workflow_hint",
"params": {
"workflow": "pcr",
"devices": [
"thermo_orbitor_rs2_hotel",
"opentrons_liquid_handler",
"agilent_plateloc",
"inheco_odtc_96xl",
],
},
"description": "PCR order: hotel → liquid handler → sealer → thermal cycler",
},
{
"intent": "close_together",
"params": {
"devices": ["opentrons_liquid_handler", "agilent_plateloc"],
"priority": "high",
},
"description": "Seal immediately after sample prep",
},
{
"intent": "min_spacing",
"params": {"min_gap": 0.15},
"description": "Minimum 15cm gap for accessibility",
},
]
class TestStage1Interpret:
"""Stage 1: /interpret translates intents → constraints."""
def test_interpret_returns_correct_constraint_count(self):
resp = client.post("/interpret", json={"intents": LLM_INTENTS})
assert resp.status_code == 200
data = resp.json()
# 4 reachability + 3 workflow minimize + 1 close minimize + 1 min_spacing = 9
assert len(data["constraints"]) == 9
assert len(data["errors"]) == 0
def test_interpret_has_translations_for_each_intent(self):
resp = client.post("/interpret", json={"intents": LLM_INTENTS})
data = resp.json()
assert len(data["translations"]) == len(LLM_INTENTS)
# 每个 translation 都有 explanation
for t in data["translations"]:
assert t["explanation"] != ""
def test_interpret_extracts_workflow_edges(self):
resp = client.post("/interpret", json={"intents": LLM_INTENTS})
data = resp.json()
assert len(data["workflow_edges"]) == 3
assert ["thermo_orbitor_rs2_hotel", "opentrons_liquid_handler"] in data["workflow_edges"]
assert ["opentrons_liquid_handler", "agilent_plateloc"] in data["workflow_edges"]
assert ["agilent_plateloc", "inheco_odtc_96xl"] in data["workflow_edges"]
def test_interpret_constraint_types_correct(self):
resp = client.post("/interpret", json={"intents": LLM_INTENTS})
data = resp.json()
constraints = data["constraints"]
by_rule = {}
for c in constraints:
by_rule.setdefault(c["rule_name"], []).append(c)
assert len(by_rule["reachability"]) == 4
assert all(c["type"] == "hard" for c in by_rule["reachability"])
assert len(by_rule["minimize_distance"]) == 4 # 3 workflow + 1 close
assert all(c["type"] == "soft" for c in by_rule["minimize_distance"])
assert len(by_rule["min_spacing"]) == 1
assert by_rule["min_spacing"][0]["type"] == "hard"
class TestStage2Optimize:
"""Stage 2: pipe /interpret output into /optimize → placements."""
@pytest.fixture()
def interpret_result(self):
resp = client.post("/interpret", json={"intents": LLM_INTENTS})
return resp.json()
def test_optimize_accepts_interpret_output(self, interpret_result):
"""Constraints + workflow_edges from /interpret are valid /optimize input."""
resp = client.post("/optimize", json={
"devices": PCR_DEVICES,
"lab": PCR_LAB,
"constraints": interpret_result["constraints"],
"workflow_edges": interpret_result["workflow_edges"],
"run_de": False, # seeder only — fast
})
assert resp.status_code == 200
data = resp.json()
assert len(data["placements"]) == 5
assert data["success"] is True
def test_optimize_with_de(self, interpret_result):
"""Full DE optimization completes without error."""
resp = client.post("/optimize", json={
"devices": PCR_DEVICES,
"lab": PCR_LAB,
"constraints": interpret_result["constraints"],
"workflow_edges": interpret_result["workflow_edges"],
"run_de": True,
"maxiter": 50, # reduced for test speed
"seed": 42,
})
assert resp.status_code == 200
data = resp.json()
assert len(data["placements"]) == 5
assert data["de_ran"] is True
class TestStage3VerifyPlacements:
"""Stage 3: verify optimized placements satisfy constraint intent."""
@pytest.fixture()
def placements(self):
# Full pipeline: interpret → optimize (with DE), all intents including reachability
# MockReachabilityChecker uses large fallback reach for unknown arms like arm_slider
interpret_resp = client.post("/interpret", json={"intents": LLM_INTENTS})
interpret_data = interpret_resp.json()
optimize_resp = client.post("/optimize", json={
"devices": PCR_DEVICES,
"lab": PCR_LAB,
"constraints": interpret_data["constraints"],
"workflow_edges": interpret_data["workflow_edges"],
"run_de": True,
"maxiter": 50,
"seed": 42,
})
return {p["device_id"]: p for p in optimize_resp.json()["placements"]}
def test_all_devices_placed(self, placements):
expected_ids = {d["id"] for d in PCR_DEVICES}
assert set(placements.keys()) == expected_ids
def test_all_within_lab_bounds(self, placements):
for dev_id, p in placements.items():
assert 0 <= p["position"]["x"] <= PCR_LAB["width"], f"{dev_id} x out of bounds"
assert 0 <= p["position"]["y"] <= PCR_LAB["depth"], f"{dev_id} y out of bounds"
def test_no_hard_constraint_violation(self):
"""Full pipeline with all intents including reachability converges cleanly.
MockReachabilityChecker uses large fallback reach for unknown arms,
so arm_slider reachability constraints are satisfied in mock mode.
When real ROS checkers replace mock, this test validates the same pipeline.
"""
interpret_data = client.post("/interpret", json={"intents": LLM_INTENTS}).json()
optimize_resp = client.post("/optimize", json={
"devices": PCR_DEVICES,
"lab": PCR_LAB,
"constraints": interpret_data["constraints"],
"workflow_edges": interpret_data["workflow_edges"],
"run_de": True,
"maxiter": 50,
"seed": 42,
})
data = optimize_resp.json()
assert data["success"] is True
assert not math.isinf(data["cost"])
def test_workflow_neighbors_closer_than_diagonal(self, placements):
"""Workflow-adjacent devices should be closer than lab diagonal (basic sanity)."""
max_diagonal = math.sqrt(PCR_LAB["width"] ** 2 + PCR_LAB["depth"] ** 2)
workflow_pairs = [
("thermo_orbitor_rs2_hotel", "opentrons_liquid_handler"),
("opentrons_liquid_handler", "agilent_plateloc"),
("agilent_plateloc", "inheco_odtc_96xl"),
]
for a_id, b_id in workflow_pairs:
a, b = placements[a_id], placements[b_id]
dist = math.sqrt(
(a["position"]["x"] - b["position"]["x"]) ** 2
+ (a["position"]["y"] - b["position"]["y"]) ** 2
)
# 应该远小于对角线（workflow minimize_distance 约束）
assert dist < max_diagonal * 0.8, (
f"Workflow pair {a_id}↔{b_id} distance {dist:.2f}m "
f"exceeds 80% of diagonal {max_diagonal:.2f}m"
)
class TestPipelineStageIsolation:
"""Verify each stage's output format is valid input for the next stage."""
def test_interpret_output_schema_matches_optimize_input(self):
"""constraints from /interpret have all fields /optimize expects."""
resp = client.post("/interpret", json={"intents": LLM_INTENTS})
data = resp.json()
for c in data["constraints"]:
assert "type" in c
assert "rule_name" in c
assert "params" in c
assert "weight" in c
assert c["type"] in ("hard", "soft")
for edge in data["workflow_edges"]:
assert isinstance(edge, list)
assert len(edge) == 2
def test_round_trip_no_data_loss(self):
"""Interpret → optimize → check that all device IDs survive the pipeline."""
interpret_resp = client.post("/interpret", json={"intents": LLM_INTENTS})
interpret_data = interpret_resp.json()
optimize_resp = client.post("/optimize", json={
"devices": PCR_DEVICES,
"lab": PCR_LAB,
"constraints": interpret_data["constraints"],
"workflow_edges": interpret_data["workflow_edges"],
"run_de": False,
})
result_ids = {p["device_id"] for p in optimize_resp.json()["placements"]}
input_ids = {d["id"] for d in PCR_DEVICES}
assert result_ids == input_ids

197
unilabos/layout_optimizer/tests/test_intent_interpreter.py Normal file
View File

@@ -0,0 +1,197 @@
"""Intent interpreter tests — PCR workflow devices."""
import pytest
from ..intent_interpreter import interpret_intents
from ..models import Intent
# --- reachable_by ---
def test_reachable_by_generates_hard_reachability():
intents = [Intent(
intent="reachable_by",
params={"arm": "arm_slider", "targets": ["opentrons_liquid_handler", "inheco_odtc_96xl"]},
description="Robot arm must reach liquid handler and thermal cycler",
)]
result = interpret_intents(intents)
assert len(result.constraints) == 2
assert all(c.rule_name == "reachability" for c in result.constraints)
assert all(c.type == "hard" for c in result.constraints)
assert result.constraints[0].params == {"arm_id": "arm_slider", "target_device_id": "opentrons_liquid_handler"}
assert result.constraints[1].params == {"arm_id": "arm_slider", "target_device_id": "inheco_odtc_96xl"}
assert len(result.translations) == 1
assert len(result.translations[0]["generated_constraints"]) == 2
def test_reachable_by_missing_arm():
result = interpret_intents([Intent(intent="reachable_by", params={"targets": ["a"]})])
assert len(result.constraints) == 0
assert len(result.errors) == 1
assert "arm" in result.errors[0].lower()
def test_reachable_by_empty_targets():
result = interpret_intents([Intent(intent="reachable_by", params={"arm": "arm_slider", "targets": []})])
assert len(result.constraints) == 0
assert len(result.errors) == 1
assert "targets" in result.errors[0].lower()
# --- close_together ---
def test_close_together_generates_minimize_distance():
intents = [Intent(intent="close_together", params={
"devices": ["opentrons_liquid_handler", "inheco_odtc_96xl", "agilent_plateloc"],
})]
result = interpret_intents(intents)
assert len(result.constraints) == 3 # C(3,2) = 3 pairs
assert all(c.rule_name == "minimize_distance" for c in result.constraints)
assert all(c.type == "soft" for c in result.constraints)
def test_close_together_priority_scales_weight():
low = interpret_intents([Intent(intent="close_together", params={"devices": ["a", "b"], "priority": "low"})])
high = interpret_intents([Intent(intent="close_together", params={"devices": ["a", "b"], "priority": "high"})])
assert high.constraints[0].weight > low.constraints[0].weight
def test_close_together_single_device_error():
result = interpret_intents([Intent(intent="close_together", params={"devices": ["a"]})])
assert len(result.errors) == 1
# --- far_apart ---
def test_far_apart_generates_maximize_distance():
result = interpret_intents([Intent(intent="far_apart", params={
"devices": ["inheco_odtc_96xl", "thermo_orbitor_rs2_hotel"],
})])
assert len(result.constraints) == 1
assert result.constraints[0].rule_name == "maximize_distance"
# --- max_distance / min_distance ---
def test_max_distance_generates_distance_less_than():
result = interpret_intents([Intent(intent="max_distance", params={
"device_a": "opentrons_liquid_handler", "device_b": "inheco_odtc_96xl", "distance": 1.5,
})])
assert len(result.constraints) == 1
c = result.constraints[0]
assert c.rule_name == "distance_less_than"
assert c.type == "hard"
assert c.params["distance"] == 1.5
def test_min_distance_generates_distance_greater_than():
result = interpret_intents([Intent(intent="min_distance", params={
"device_a": "inheco_odtc_96xl", "device_b": "thermo_orbitor_rs2_hotel", "distance": 2.0,
})])
c = result.constraints[0]
assert c.rule_name == "distance_greater_than"
assert c.type == "hard"
assert c.params["distance"] == 2.0
def test_max_distance_zero_is_valid():
"""distance=0 is falsy but valid — must not be rejected."""
result = interpret_intents([Intent(intent="max_distance", params={
"device_a": "a", "device_b": "b", "distance": 0,
})])
assert len(result.constraints) == 1
assert len(result.errors) == 0
def test_max_distance_missing_param():
result = interpret_intents([Intent(intent="max_distance", params={"device_a": "a"})])
assert len(result.errors) == 1
assert len(result.constraints) == 0
# --- orientation ---
def test_face_outward():
result = interpret_intents([Intent(intent="face_outward")])
assert result.constraints[0].rule_name == "prefer_orientation_mode"
assert result.constraints[0].params["mode"] == "outward"
def test_face_inward():
result = interpret_intents([Intent(intent="face_inward")])
assert result.constraints[0].params["mode"] == "inward"
def test_align_cardinal():
result = interpret_intents([Intent(intent="align_cardinal")])
assert result.constraints[0].rule_name == "prefer_aligned"
# --- min_spacing ---
def test_min_spacing():
result = interpret_intents([Intent(intent="min_spacing", params={"min_gap": 0.3})])
c = result.constraints[0]
assert c.rule_name == "min_spacing"
assert c.type == "hard"
assert c.params["min_gap"] == 0.3
# --- workflow_hint (PCR scenario) ---
def test_workflow_hint_pcr():
"""PCR workflow: pipette → thermal cycler → plate sealer → storage."""
intents = [Intent(
intent="workflow_hint",
params={
"workflow": "pcr",
"devices": [
"opentrons_liquid_handler",
"inheco_odtc_96xl",
"agilent_plateloc",
"thermo_orbitor_rs2_hotel",
],
},
)]
result = interpret_intents(intents)
assert len(result.constraints) == 3 # 4 devices → 3 consecutive pairs
assert all(c.rule_name == "minimize_distance" for c in result.constraints)
assert len(result.workflow_edges) == 3
assert ["opentrons_liquid_handler", "inheco_odtc_96xl"] in result.workflow_edges
assert result.translations[0]["confidence"] == "low"
def test_workflow_hint_single_device_error():
result = interpret_intents([Intent(intent="workflow_hint", params={"workflow": "test", "devices": ["a"]})])
assert len(result.errors) == 1
# --- unknown intent ---
def test_unknown_intent():
result = interpret_intents([Intent(intent="nonexistent")])
assert len(result.constraints) == 0
assert len(result.errors) == 1
assert "nonexistent" in result.errors[0]
# --- multi-intent combination ---
def test_full_pcr_scenario():
"""Arm reachability + close together for full PCR setup."""
intents = [
Intent(intent="reachable_by", params={
"arm": "arm_slider",
"targets": [
"opentrons_liquid_handler", "inheco_odtc_96xl",
"agilent_plateloc", "thermo_orbitor_rs2_hotel",
],
}),
Intent(intent="close_together", params={
"devices": ["opentrons_liquid_handler", "inheco_odtc_96xl"],
"priority": "high",
}),
]
result = interpret_intents(intents)
assert len(result.constraints) == 5 # 4 reachability + 1 minimize_distance
assert len(result.translations) == 2
assert len(result.errors) == 0

134
unilabos/layout_optimizer/tests/test_interpret_api.py Normal file
View File

@@ -0,0 +1,134 @@
"""Tests for /interpret and /interpret/schema API endpoints."""
import pytest
from fastapi.testclient import TestClient
from ..server import app
client = TestClient(app)
def test_interpret_reachable_by():
resp = client.post("/interpret", json={
"intents": [
{
"intent": "reachable_by",
"params": {
"arm": "arm_slider",
"targets": ["opentrons_liquid_handler", "inheco_odtc_96xl"],
},
"description": "Arm must reach liquid handler and thermal cycler",
}
]
})
assert resp.status_code == 200
data = resp.json()
assert len(data["constraints"]) == 2
assert all(c["rule_name"] == "reachability" for c in data["constraints"])
assert len(data["translations"]) == 1
assert data["translations"][0]["source_intent"] == "reachable_by"
assert len(data["errors"]) == 0
def test_interpret_pcr_workflow():
"""Full PCR: reachability + workflow_hint + close_together."""
resp = client.post("/interpret", json={
"intents": [
{
"intent": "reachable_by",
"params": {
"arm": "arm_slider",
"targets": [
"opentrons_liquid_handler",
"inheco_odtc_96xl",
"agilent_plateloc",
"thermo_orbitor_rs2_hotel",
],
},
},
{
"intent": "workflow_hint",
"params": {
"workflow": "pcr",
"devices": [
"opentrons_liquid_handler",
"inheco_odtc_96xl",
"agilent_plateloc",
"thermo_orbitor_rs2_hotel",
],
},
},
{
"intent": "close_together",
"params": {
"devices": ["opentrons_liquid_handler", "inheco_odtc_96xl"],
"priority": "high",
},
},
]
})
assert resp.status_code == 200
data = resp.json()
# 4 reachability + 3 workflow + 1 close = 8
assert len(data["constraints"]) == 8
assert len(data["workflow_edges"]) == 3
assert len(data["translations"]) == 3
assert len(data["errors"]) == 0
def test_interpret_returns_errors_for_bad_intents():
resp = client.post("/interpret", json={
"intents": [
{"intent": "reachable_by", "params": {}},
{"intent": "nonexistent_intent"},
]
})
assert resp.status_code == 200
data = resp.json()
assert len(data["errors"]) == 2
assert len(data["constraints"]) == 0
def test_interpret_empty_intents():
resp = client.post("/interpret", json={"intents": []})
assert resp.status_code == 200
data = resp.json()
assert data["constraints"] == []
assert data["translations"] == []
assert data["errors"] == []
def test_interpret_schema_returns_all_intents():
resp = client.get("/interpret/schema")
assert resp.status_code == 200
data = resp.json()
intents = data["intents"]
expected = {
"reachable_by", "close_together", "far_apart",
"max_distance", "min_distance", "min_spacing",
"workflow_hint", "face_outward", "face_inward", "align_cardinal",
}
assert set(intents.keys()) == expected
def test_interpret_constraints_passable_to_optimize():
"""Constraints from /interpret should be directly usable in /optimize."""
# Step 1: interpret
interpret_resp = client.post("/interpret", json={
"intents": [
{"intent": "close_together", "params": {"devices": ["dev_a", "dev_b"]}},
]
})
constraints = interpret_resp.json()["constraints"]
# Step 2: pass to optimize (verify it accepts the format)
optimize_resp = client.post("/optimize", json={
"devices": [
{"id": "dev_a", "name": "Device A", "size": [0.5, 0.4]},
{"id": "dev_b", "name": "Device B", "size": [0.5, 0.4]},
],
"lab": {"width": 4.0, "depth": 3.0},
"constraints": constraints,
"run_de": False,
})
assert optimize_resp.status_code == 200
assert len(optimize_resp.json()["placements"]) == 2

277
unilabos/layout_optimizer/tests/test_llm_skill.py Normal file
View File

@@ -0,0 +1,277 @@
"""LLM 技能文档测试：用真实 LLM 验证模糊用户输入 → 结构化意图的翻译质量。
需要 ANTHROPIC_API_KEY 环境变量。无 key 时自动跳过。
测试覆盖：设备名模糊匹配、工作流顺序推理、约束类型选择、JSON 格式正确性。
"""
import json
import os
import pytest
HAS_API_KEY = bool(os.environ.get("ANTHROPIC_API_KEY"))
pytestmark = pytest.mark.skipif(not HAS_API_KEY, reason="ANTHROPIC_API_KEY not set")
# 读取技能文档
_SKILL_DOC_PATH = os.path.join(
os.path.dirname(__file__), "..", "llm_skill", "layout_intent_translator.md"
)
# PCR 场景设备列表（模拟用户场景中已有的设备）
SCENE_DEVICE_LIST = """\
Devices in scene:
- thermo_orbitor_rs2_hotel: Thermo Orbitor RS2 Hotel (type: static, bbox: 0.68×0.52m)
- arm_slider: Arm Slider (type: articulation, bbox: 1.20×0.30m)
- opentrons_liquid_handler: Opentrons Liquid Handler (type: static, bbox: 0.65×0.60m)
- agilent_plateloc: Agilent PlateLoc (type: static, bbox: 0.35×0.40m)
- inheco_odtc_96xl: Inheco ODTC 96XL (type: static, bbox: 0.30×0.35m)
"""
VALID_DEVICE_IDS = {
"thermo_orbitor_rs2_hotel",
"arm_slider",
"opentrons_liquid_handler",
"agilent_plateloc",
"inheco_odtc_96xl",
}
VALID_INTENT_TYPES = {
"reachable_by", "close_together", "far_apart", "max_distance",
"min_distance", "min_spacing", "workflow_hint",
"face_outward", "face_inward", "align_cardinal",
}
def _call_llm(user_message: str) -> dict:
"""调用 LLM，使用技能文档作为 system prompt，返回解析后的 JSON。"""
import anthropic
with open(_SKILL_DOC_PATH) as f:
skill_doc = f.read()
client = anthropic.Anthropic()
response = client.messages.create(
model="claude-sonnet-4-20250514",
max_tokens=2000,
system=skill_doc,
messages=[
{"role": "user", "content": f"{SCENE_DEVICE_LIST}\n\n{user_message}"},
],
)
# 从 response 中提取 JSON
text = response.content[0].text
# LLM 可能返回 ```json ... ``` 包裹的 JSON
if "```json" in text:
text = text.split("```json")[1].split("```")[0]
elif "```" in text:
text = text.split("```")[1].split("```")[0]
return json.loads(text.strip())
def _extract_all_device_ids(intents: list[dict]) -> set[str]:
"""从意图列表中提取所有引用的设备 ID。"""
ids = set()
for intent in intents:
params = intent.get("params", {})
if "arm" in params:
ids.add(params["arm"])
for key in ("targets", "devices"):
if key in params:
ids.update(params[key])
for key in ("device_a", "device_b"):
if key in params:
ids.add(params[key])
return ids
class TestLLMFuzzyDeviceResolution:
"""测试 LLM 能否将模糊设备名映射到精确 ID。"""
def test_pcr_machine_resolves_to_inheco(self):
"""'PCR machine' 应解析为 inheco_odtc_96xl。"""
result = _call_llm(
"Keep the PCR machine close to the plate sealer"
)
intents = result["intents"]
all_ids = _extract_all_device_ids(intents)
assert "inheco_odtc_96xl" in all_ids, f"Expected inheco_odtc_96xl in {all_ids}"
assert "agilent_plateloc" in all_ids, f"Expected agilent_plateloc in {all_ids}"
def test_robot_resolves_to_articulation_type(self):
"""'the robot' / 'robot arm' 应解析为 arm_slider（唯一 articulation 类型）。"""
result = _call_llm(
"The robot should be able to reach the liquid handler and the storage hotel"
)
intents = result["intents"]
all_ids = _extract_all_device_ids(intents)
assert "arm_slider" in all_ids, f"Expected arm_slider in {all_ids}"
assert "opentrons_liquid_handler" in all_ids
assert "thermo_orbitor_rs2_hotel" in all_ids
def test_all_resolved_ids_are_valid(self):
"""LLM 输出的所有设备 ID 必须来自场景设备列表。"""
result = _call_llm(
"Take plate from hotel, prepare sample in the pipetting robot, "
"seal it, then run thermal cycling. The arm handles all transfers."
)
intents = result["intents"]
all_ids = _extract_all_device_ids(intents)
invalid = all_ids - VALID_DEVICE_IDS
assert not invalid, f"LLM produced invalid device IDs: {invalid}"
class TestLLMWorkflowInterpretation:
"""测试 LLM 对工作流描述的理解和翻译。"""
def test_pcr_workflow_full(self):
"""完整 PCR 工作流描述应生成 reachable_by + workflow_hint + close_together。"""
result = _call_llm(
"I need to set up a PCR workflow: take plate from the hotel, "
"prepare the sample in the liquid handler, seal the plate, "
"then run the thermal cycler. The robot arm handles all plate transfers. "
"Keep the liquid handler and sealer close together."
)
intents = result["intents"]
intent_types = {i["intent"] for i in intents}
# 应包含核心意图类型
assert "reachable_by" in intent_types, f"Missing reachable_by in {intent_types}"
assert "workflow_hint" in intent_types, f"Missing workflow_hint in {intent_types}"
# reachable_by 应包含所有工作流设备作为 targets
reach_intents = [i for i in intents if i["intent"] == "reachable_by"]
assert len(reach_intents) >= 1
reach_targets = set()
for ri in reach_intents:
reach_targets.update(ri["params"].get("targets", []))
# 至少液体处理器和热循环仪应在可达范围内
assert "opentrons_liquid_handler" in reach_targets
assert "inheco_odtc_96xl" in reach_targets
def test_workflow_device_order(self):
"""workflow_hint 的设备顺序应反映工作流步骤。"""
result = _call_llm(
"PCR process: first the hotel dispenses a plate, then the opentrons "
"prepares the sample, next the plateloc seals it, finally the thermal "
"cycler runs PCR. Generate a workflow hint."
)
intents = result["intents"]
wf_intents = [i for i in intents if i["intent"] == "workflow_hint"]
assert len(wf_intents) >= 1, f"No workflow_hint found in {[i['intent'] for i in intents]}"
devices = wf_intents[0]["params"]["devices"]
# 验证顺序：hotel → liquid_handler → plateloc → thermal_cycler
hotel_idx = devices.index("thermo_orbitor_rs2_hotel")
lh_idx = devices.index("opentrons_liquid_handler")
seal_idx = devices.index("agilent_plateloc")
tc_idx = devices.index("inheco_odtc_96xl")
assert hotel_idx < lh_idx < seal_idx < tc_idx, (
f"Wrong workflow order: {devices}"
)
class TestLLMOutputFormat:
"""测试 LLM 输出格式的正确性。"""
def test_output_has_intents_array(self):
"""输出必须有 intents 数组。"""
result = _call_llm("Keep all devices at least 30cm apart")
assert "intents" in result
assert isinstance(result["intents"], list)
assert len(result["intents"]) > 0
def test_each_intent_has_required_fields(self):
"""每个意图必须有 intent、params、description。"""
result = _call_llm(
"The robot arm should reach the liquid handler. "
"Keep the thermal cycler away from the plate hotel."
)
for intent in result["intents"]:
assert "intent" in intent, f"Missing 'intent' field: {intent}"
assert "params" in intent, f"Missing 'params' field: {intent}"
assert "description" in intent, f"Missing 'description' field: {intent}"
def test_intent_types_are_valid(self):
"""所有意图类型必须是已知类型。"""
result = _call_llm(
"Set up a compact PCR line: hotel → liquid handler → sealer → thermal cycler. "
"Robot arm handles transfers. Align everything neatly."
)
for intent in result["intents"]:
assert intent["intent"] in VALID_INTENT_TYPES, (
f"Unknown intent type: {intent['intent']}"
)
class TestLLMInterpretThenOptimize:
"""端到端：LLM 翻译 → /interpret → /optimize → 验证布局。"""
def test_llm_output_accepted_by_interpret_endpoint(self):
"""LLM 输出应能直接被 /interpret 端点接受。"""
from fastapi.testclient import TestClient
from ..server import app
test_client = TestClient(app)
llm_result = _call_llm(
"Take plate from hotel, prepare sample in opentrons, "
"seal plate then pcr cycle, arm_slider handles all transfers. "
"Keep liquid handler and sealer close."
)
# /interpret 应接受 LLM 输出
resp = test_client.post("/interpret", json=llm_result)
assert resp.status_code == 200, f"Interpret failed: {resp.text}"
data = resp.json()
assert len(data["constraints"]) > 0, "No constraints generated"
assert len(data["errors"]) == 0, f"Interpretation errors: {data['errors']}"
def test_full_pipeline_llm_to_placement(self):
"""LLM 翻译 → interpret → optimize → 所有设备有 placement。"""
from fastapi.testclient import TestClient
from ..server import app
test_client = TestClient(app)
# Stage 1: LLM 翻译
llm_result = _call_llm(
"I want a PCR workflow lab. Take plate from the hotel, pipette in the "
"liquid handler, seal with the plateloc, then thermal cycle. "
"The robot arm does all transfers between devices. "
"Minimum 15cm gap between everything."
)
# Stage 2: interpret
interpret_resp = test_client.post("/interpret", json=llm_result)
assert interpret_resp.status_code == 200
interpret_data = interpret_resp.json()
assert len(interpret_data["errors"]) == 0
# Stage 3: optimize
pcr_devices = [
{"id": "thermo_orbitor_rs2_hotel", "name": "Plate Hotel", "device_type": "static"},
{"id": "arm_slider", "name": "Robot Arm", "device_type": "articulation"},
{"id": "opentrons_liquid_handler", "name": "Liquid Handler", "device_type": "static"},
{"id": "agilent_plateloc", "name": "Plate Sealer", "device_type": "static"},
{"id": "inheco_odtc_96xl", "name": "Thermal Cycler", "device_type": "static"},
]
optimize_resp = test_client.post("/optimize", json={
"devices": pcr_devices,
"lab": {"width": 6.0, "depth": 4.0},
"constraints": interpret_data["constraints"],
"workflow_edges": interpret_data.get("workflow_edges", []),
"run_de": True,
"maxiter": 50,
"seed": 42,
})
assert optimize_resp.status_code == 200
data = optimize_resp.json()
# Stage 4: 验证所有设备都有 placement
placed_ids = {p["device_id"] for p in data["placements"]}
expected_ids = {d["id"] for d in pcr_devices}
assert placed_ids == expected_ids
assert data["success"] is True

138
unilabos/layout_optimizer/tests/test_mock_checkers.py Normal file
View File

@@ -0,0 +1,138 @@
"""MockCollisionChecker 和 MockReachabilityChecker 测试。"""
import math
from ..mock_checkers import MockCollisionChecker, MockReachabilityChecker
class TestMockCollisionChecker:
def setup_method(self):
self.checker = MockCollisionChecker()
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_touching_edges(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_collision_with_rotation(self):
"""旋转后的设备 OBB 可能导致碰撞。"""
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)}, # closer: OBB overlap
]
collisions = self.checker.check(placements)
assert ("a", "b") in collisions
def test_no_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": (2.0, 1.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_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
def test_obb_collision_rotated_no_false_positive():
"""A rotated narrow device should NOT collide with a nearby device
that the old AABB method would have flagged as colliding.
Old AABB expands footprint; OBB is precise.
"""
checker = MockCollisionChecker()
# Narrow device (2.0 x 0.5) rotated 45°:
# AABB would be ~1.77 x 1.77, OBB is the actual narrow rectangle
placements = [
{"id": "narrow", "bbox": (2.0, 0.5), "pos": (3.0, 3.0, math.pi / 4)},
{"id": "nearby", "bbox": (0.5, 0.5), "pos": (4.5, 3.0, 0.0)},
]
collisions = checker.check(placements)
# With OBB: no collision (the narrow rotated box doesn't reach)
assert ("narrow", "nearby") not in collisions and ("nearby", "narrow") not in collisions
class TestMockReachabilityChecker:
def setup_method(self):
self.checker = MockReachabilityChecker()
def test_reachable_within_radius(self):
"""目标在臂展半径内。"""
arm_pose = {"x": 0.0, "y": 0.0, "theta": 0.0}
target = {"x": 0.5, "y": 0.5, "z": 0.0}
assert self.checker.is_reachable("elite_cs66", arm_pose, target)
def test_not_reachable_outside_radius(self):
"""目标超出臂展半径。"""
arm_pose = {"x": 0.0, "y": 0.0, "theta": 0.0}
target = {"x": 2.0, "y": 2.0, "z": 0.0}
assert not self.checker.is_reachable("elite_cs66", arm_pose, target)
def test_reachable_at_boundary(self):
"""目标恰好在臂展边界上（应可达）。"""
arm_pose = {"x": 0.0, "y": 0.0, "theta": 0.0}
target = {"x": 0.914, "y": 0.0, "z": 0.0}
assert self.checker.is_reachable("elite_cs66", arm_pose, target)
def test_unknown_arm_uses_default(self):
"""未知型号使用 1.0m 回退臂展（realistic lab-scale default）。"""
arm_pose = {"x": 0.0, "y": 0.0, "theta": 0.0}
# Within 1.0m fallback reach
target_near = {"x": 0.8, "y": 0.0, "z": 0.0}
assert self.checker.is_reachable("unknown_arm", arm_pose, target_near)
# Beyond 1.0m fallback reach
target_far = {"x": 1.5, "y": 0.0, "z": 0.0}
assert not self.checker.is_reachable("unknown_arm", arm_pose, target_far)
def test_custom_arm_reach(self):
"""自定义臂展参数。"""
checker = MockReachabilityChecker(arm_reach={"custom_arm": 1.5})
arm_pose = {"x": 0.0, "y": 0.0, "theta": 0.0}
target = {"x": 1.4, "y": 0.0, "z": 0.0}
assert checker.is_reachable("custom_arm", arm_pose, target)

117
unilabos/layout_optimizer/tests/test_obb.py Normal file
View File

@@ -0,0 +1,117 @@
"""Tests for OBB (Oriented Bounding Box) geometry utilities."""
import math
import pytest
from ..obb import obb_corners, obb_overlap, obb_min_distance
class TestObbCorners:
"""obb_corners(cx, cy, w, h, theta) → 4 corner points of the rotated rectangle."""
def test_no_rotation(self):
"""Axis-aligned box at origin: corners at ±half extents."""
corners = obb_corners(0, 0, 2.0, 1.0, 0.0)
assert len(corners) == 4
xs = sorted(c[0] for c in corners)
ys = sorted(c[1] for c in corners)
assert xs == pytest.approx([-1.0, -1.0, 1.0, 1.0])
assert ys == pytest.approx([-0.5, -0.5, 0.5, 0.5])
def test_90_degree_rotation(self):
"""90° rotation swaps width and height extents."""
corners = obb_corners(0, 0, 2.0, 1.0, math.pi / 2)
xs = sorted(c[0] for c in corners)
ys = sorted(c[1] for c in corners)
assert xs == pytest.approx([-0.5, -0.5, 0.5, 0.5])
assert ys == pytest.approx([-1.0, -1.0, 1.0, 1.0])
def test_offset_center(self):
"""Corners shift by (cx, cy)."""
corners = obb_corners(3.0, 2.0, 2.0, 1.0, 0.0)
xs = sorted(c[0] for c in corners)
ys = sorted(c[1] for c in corners)
assert xs == pytest.approx([2.0, 2.0, 4.0, 4.0])
assert ys == pytest.approx([1.5, 1.5, 2.5, 2.5])
def test_45_degree_rotation(self):
"""45° rotation: corners on diagonals."""
corners = obb_corners(0, 0, 2.0, 2.0, math.pi / 4)
for cx, cy in corners:
dist = math.sqrt(cx**2 + cy**2)
assert dist == pytest.approx(math.sqrt(2), abs=1e-9)
class TestObbOverlap:
"""obb_overlap(corners_a, corners_b) → True if the two OBBs overlap."""
def test_separated_boxes(self):
"""Two boxes far apart: no overlap."""
a = obb_corners(0, 0, 1.0, 1.0, 0.0)
b = obb_corners(5, 0, 1.0, 1.0, 0.0)
assert obb_overlap(a, b) is False
def test_overlapping_boxes(self):
"""Two boxes sharing space: overlap."""
a = obb_corners(0, 0, 2.0, 2.0, 0.0)
b = obb_corners(1, 0, 2.0, 2.0, 0.0)
assert obb_overlap(a, b) is True
def test_touching_edges_no_overlap(self):
"""Boxes touching at edge: no overlap (strict <, not <=)."""
a = obb_corners(0, 0, 2.0, 2.0, 0.0)
b = obb_corners(2.0, 0, 2.0, 2.0, 0.0)
assert obb_overlap(a, b) is False
def test_rotated_overlap(self):
"""One box rotated 45°, overlapping the other."""
a = obb_corners(0, 0, 2.0, 2.0, 0.0)
b = obb_corners(1.0, 1.0, 2.0, 2.0, math.pi / 4)
assert obb_overlap(a, b) is True
def test_rotated_no_overlap(self):
"""One box rotated 45°, separated from the other."""
a = obb_corners(0, 0, 1.0, 1.0, 0.0)
b = obb_corners(3, 0, 1.0, 1.0, math.pi / 4)
assert obb_overlap(a, b) is False
def test_identical_boxes(self):
"""Same position and size: overlap."""
a = obb_corners(1, 1, 1.0, 1.0, 0.0)
b = obb_corners(1, 1, 1.0, 1.0, 0.0)
assert obb_overlap(a, b) is True
class TestObbMinDistance:
"""obb_min_distance(corners_a, corners_b) → minimum edge-to-edge distance."""
def test_overlapping_returns_zero(self):
"""Overlapping boxes: distance = 0."""
a = obb_corners(0, 0, 2.0, 2.0, 0.0)
b = obb_corners(1, 0, 2.0, 2.0, 0.0)
assert obb_min_distance(a, b) == pytest.approx(0.0)
def test_separated_axis_aligned(self):
"""Two axis-aligned boxes with 2m gap."""
a = obb_corners(0, 0, 2.0, 2.0, 0.0) # edges at x=±1
b = obb_corners(4, 0, 2.0, 2.0, 0.0) # edges at x=3,5
# Gap = 3 - 1 = 2.0
assert obb_min_distance(a, b) == pytest.approx(2.0)
def test_diagonal_separation(self):
"""Boxes separated diagonally: distance to nearest corner."""
a = obb_corners(0, 0, 2.0, 2.0, 0.0) # corners at (±1, ±1)
b = obb_corners(4, 4, 2.0, 2.0, 0.0) # corners at (3..5, 3..5)
# Nearest corners: (1,1) to (3,3) → sqrt(8) ≈ 2.828
assert obb_min_distance(a, b) == pytest.approx(math.sqrt(8), abs=0.01)
def test_rotated_separation(self):
"""One rotated box separated from axis-aligned box."""
a = obb_corners(0, 0, 1.0, 1.0, 0.0)
b = obb_corners(3, 0, 1.0, 1.0, math.pi / 4)
dist = obb_min_distance(a, b)
assert dist > 0
def test_touching_returns_zero(self):
"""Touching edges: distance = 0."""
a = obb_corners(0, 0, 2.0, 2.0, 0.0)
b = obb_corners(2.0, 0, 2.0, 2.0, 0.0)
assert obb_min_distance(a, b) == pytest.approx(0.0)

219
unilabos/layout_optimizer/tests/test_optimizer.py Normal file
View File

@@ -0,0 +1,219 @@
"""差分进化优化器端到端测试。"""
import math
from ..mock_checkers import MockCollisionChecker
from ..models import Device, Lab, Placement
import pytest
from ..optimizer import optimize, snap_theta
def test_optimize_three_devices_no_collision():
"""3 台设备在 5m×5m 实验室中优化，结果应无碰撞且在边界内。"""
devices = [
Device(id="a", name="A", bbox=(0.8, 0.6)),
Device(id="b", name="B", bbox=(0.6, 0.5)),
Device(id="c", name="C", bbox=(0.5, 0.5)),
]
lab = Lab(width=5.0, depth=5.0)
placements = optimize(devices, lab, seed=42, maxiter=100, popsize=10)
assert len(placements) == 3
# 验证无碰撞
checker = MockCollisionChecker()
checker_placements = [
{"id": p.device_id, "bbox": next(d.bbox for d in devices if d.id == p.device_id),
"pos": (p.x, p.y, p.theta)}
for p in placements
]
collisions = checker.check(checker_placements)
assert collisions == [], f"Unexpected collisions: {collisions}"
# 验证在边界内
oob = checker.check_bounds(checker_placements, lab.width, lab.depth)
assert oob == [], f"Devices out of bounds: {oob}"
def test_optimize_single_device():
"""单个设备应直接放置成功。"""
devices = [Device(id="solo", name="Solo", bbox=(0.5, 0.5))]
lab = Lab(width=3.0, depth=3.0)
placements = optimize(devices, lab, seed=42, maxiter=50)
assert len(placements) == 1
p = placements[0]
assert 0.25 <= p.x <= 2.75
assert 0.25 <= p.y <= 2.75
def test_optimize_tight_space():
"""紧凑空间：2 台设备在刚好够大的实验室中。"""
devices = [
Device(id="x", name="X", bbox=(1.0, 1.0)),
Device(id="y", name="Y", bbox=(1.0, 1.0)),
]
# 2.5m 宽足够放 2 个 1m 宽设备（加间距）
lab = Lab(width=2.5, depth=2.0)
placements = optimize(devices, lab, seed=42, maxiter=100)
assert len(placements) == 2
checker = MockCollisionChecker()
checker_placements = [
{"id": p.device_id, "bbox": next(d.bbox for d in devices if d.id == p.device_id),
"pos": (p.x, p.y, p.theta)}
for p in placements
]
collisions = checker.check(checker_placements)
assert collisions == []
def test_optimize_returns_valid_placement_ids():
"""验证返回的 placement device_id 与输入设备一致。"""
devices = [
Device(id="dev_1", name="D1", bbox=(0.5, 0.5)),
Device(id="dev_2", name="D2", bbox=(0.5, 0.5)),
]
lab = Lab(width=5.0, depth=5.0)
placements = optimize(devices, lab, seed=42, maxiter=50)
result_ids = {p.device_id for p in placements}
expected_ids = {d.id for d in devices}
assert result_ids == expected_ids
def test_snap_theta_near_cardinal():
"""Theta within 15° of 90° snaps to 90°."""
placements = [Placement(device_id="a", x=1, y=1, theta=math.radians(85))]
result = snap_theta(placements, threshold_deg=15)
assert result[0].theta == pytest.approx(math.pi / 2)
def test_snap_theta_far_from_cardinal():
"""Theta 30° away from nearest cardinal: no snap."""
placements = [Placement(device_id="a", x=1, y=1, theta=math.radians(60))]
result = snap_theta(placements, threshold_deg=15)
assert result[0].theta == pytest.approx(math.radians(60))
def test_snap_theta_at_cardinal():
"""Already at cardinal: unchanged."""
placements = [Placement(device_id="a", x=1, y=1, theta=math.pi)]
result = snap_theta(placements, threshold_deg=15)
assert result[0].theta == pytest.approx(math.pi)
def test_snap_theta_near_360():
"""Theta near 360° (=0°) snaps correctly."""
placements = [Placement(device_id="a", x=1, y=1, theta=math.radians(355))]
result = snap_theta(placements, threshold_deg=15)
snapped = result[0].theta % (2 * math.pi)
assert snapped == pytest.approx(0.0, abs=0.01) or snapped == pytest.approx(2 * math.pi, abs=0.01)
def test_optimize_endpoint_accepts_seeder_field():
"""POST /optimize should accept seeder and run_de fields."""
from fastapi.testclient import TestClient
from ..server import app
client = TestClient(app)
resp = client.post("/optimize", json={
"devices": [{"id": "test_device", "name": "Test"}],
"lab": {"width": 5, "depth": 4},
"seeder": "compact_outward",
"run_de": False,
})
assert resp.status_code == 200
data = resp.json()
assert data["seeder_used"] == "compact_outward"
assert data["de_ran"] is False
assert len(data["placements"]) == 1
def test_optimize_endpoint_unknown_seeder_returns_400():
"""Unknown seeder preset should return 400."""
from fastapi.testclient import TestClient
from ..server import app
client = TestClient(app)
resp = client.post("/optimize", json={
"devices": [{"id": "test_device", "name": "Test"}],
"lab": {"width": 5, "depth": 4},
"seeder": "nonexistent_preset",
"run_de": False,
})
assert resp.status_code == 400
def test_optimize_endpoint_backward_compatible():
"""Existing calls without seeder/run_de fields still work."""
from fastapi.testclient import TestClient
from ..server import app
client = TestClient(app)
resp = client.post("/optimize", json={
"devices": [{"id": "test_device", "name": "Test"}],
"lab": {"width": 5, "depth": 4},
})
assert resp.status_code == 200
data = resp.json()
assert "seeder_used" in data
assert "de_ran" in data
def test_full_pipeline_seed_only():
"""Full pipeline: seeder → snap_theta → correct count and bounds.
compact_outward is tested for collision-free (devices clustered, not at walls).
spread_inward pushes to walls where rotated AABB bounds may flag — tested separately.
"""
from ..seeders import seed_layout, PRESETS
from ..constraints import evaluate_default_hard_constraints
devices = [
Device(id=f"dev{i}", name=f"Device {i}", bbox=(0.6, 0.4))
for i in range(6)
]
lab = Lab(width=6.0, depth=5.0)
# compact_outward: devices cluster toward center, should be collision-free
placements = seed_layout(devices, lab, PRESETS["compact_outward"])
placements = snap_theta(placements)
assert len(placements) == len(devices)
checker = MockCollisionChecker()
cost = evaluate_default_hard_constraints(devices, placements, lab, checker)
assert cost < 1e17, f"compact_outward: hard constraint violation (cost={cost})"
# spread_inward: verify correct count + all positions within lab canvas
placements = seed_layout(devices, lab, PRESETS["spread_inward"])
placements = snap_theta(placements)
assert len(placements) == len(devices)
for p in placements:
assert 0 <= p.x <= lab.width, f"spread_inward: x={p.x} out of bounds"
assert 0 <= p.y <= lab.depth, f"spread_inward: y={p.y} out of bounds"
def test_full_pipeline_with_de():
"""Full pipeline: seeder → DE → snap_theta."""
from ..seeders import seed_layout, PRESETS
devices = [
Device(id=f"dev{i}", name=f"Device {i}", bbox=(0.6, 0.4))
for i in range(4)
]
lab = Lab(width=5.0, depth=4.0)
checker = MockCollisionChecker()
seed = seed_layout(devices, lab, PRESETS["compact_outward"])
result = optimize(devices, lab, seed_placements=seed, collision_checker=checker,
maxiter=50, seed=42)
result = snap_theta(result)
assert len(result) == len(devices)
for p in result:
assert 0 <= p.x <= lab.width
assert 0 <= p.y <= lab.depth

430
unilabos/layout_optimizer/tests/test_ros_checkers.py Normal file
View File

@@ -0,0 +1,430 @@
"""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)

113
unilabos/layout_optimizer/tests/test_seeders.py Normal file
View File

@@ -0,0 +1,113 @@
"""Tests for the force-directed seeder engine."""
import math
import pytest
from ..seeders import SeederParams, PRESETS, seed_layout
from ..models import Device, Lab, Placement
class TestSeederParams:
def test_presets_exist(self):
assert "compact_outward" in PRESETS
assert "spread_inward" in PRESETS
assert "row_fallback" in PRESETS
def test_compact_has_negative_boundary(self):
assert PRESETS["compact_outward"].boundary_attraction < 0
def test_spread_has_positive_boundary(self):
assert PRESETS["spread_inward"].boundary_attraction > 0
class TestSeedLayout:
"""seed_layout must return valid placements: within bounds, one per device."""
def _make_devices(self, n: int) -> list[Device]:
return [Device(id=f"d{i}", name=f"Device {i}", bbox=(0.6, 0.4)) for i in range(n)]
def test_returns_one_placement_per_device(self):
devices = self._make_devices(5)
lab = Lab(width=5.0, depth=4.0)
result = seed_layout(devices, lab, PRESETS["compact_outward"])
assert len(result) == 5
ids = {p.device_id for p in result}
assert ids == {f"d{i}" for i in range(5)}
def test_placements_within_bounds(self):
devices = self._make_devices(5)
lab = Lab(width=5.0, depth=4.0)
for preset_name in ["compact_outward", "spread_inward"]:
result = seed_layout(devices, lab, PRESETS[preset_name])
for p in result:
assert 0 <= p.x <= lab.width, f"{preset_name}: x={p.x} out of bounds"
assert 0 <= p.y <= lab.depth, f"{preset_name}: y={p.y} out of bounds"
def test_empty_devices(self):
result = seed_layout([], Lab(width=5, depth=4), PRESETS["compact_outward"])
assert result == []
def test_single_device(self):
devices = self._make_devices(1)
lab = Lab(width=5.0, depth=4.0)
result = seed_layout(devices, lab, PRESETS["compact_outward"])
assert len(result) == 1
assert 0 <= result[0].x <= lab.width
assert 0 <= result[0].y <= lab.depth
def test_row_fallback_delegates(self):
"""row_fallback preset uses generate_fallback, not force engine."""
devices = self._make_devices(3)
lab = Lab(width=5.0, depth=4.0)
# row_fallback is None in PRESETS; seed_layout detects and delegates
result = seed_layout(devices, lab, None) # None = row_fallback
assert len(result) == 3
def test_lab_too_small_returns_results_not_crash(self):
"""When space is insufficient, seeder still returns placements (may have collisions)."""
devices = [Device(id=f"d{i}", name=f"D{i}", bbox=(1.0, 1.0)) for i in range(20)]
lab = Lab(width=2.0, depth=2.0) # Way too small for 20 1m×1m devices
result = seed_layout(devices, lab, PRESETS["compact_outward"])
assert len(result) == 20 # All placed, even if overlapping
for p in result:
assert 0 <= p.x <= lab.width
assert 0 <= p.y <= lab.depth
def test_compact_clusters_toward_center(self):
"""compact_outward should place devices closer to center than spread_inward."""
devices = self._make_devices(4)
lab = Lab(width=8.0, depth=8.0)
center_x, center_y = lab.width / 2, lab.depth / 2
compact = seed_layout(devices, lab, PRESETS["compact_outward"])
spread = seed_layout(devices, lab, PRESETS["spread_inward"])
avg_dist_compact = sum(
math.sqrt((p.x - center_x)**2 + (p.y - center_y)**2) for p in compact
) / len(compact)
avg_dist_spread = sum(
math.sqrt((p.x - center_x)**2 + (p.y - center_y)**2) for p in spread
) / len(spread)
assert avg_dist_compact < avg_dist_spread
class TestOrientation:
"""Orientation modes should set theta based on position relative to center."""
def test_outward_orientation_sets_theta(self):
"""compact_outward: devices should have non-zero theta."""
devices = [
Device(id="a", name="A", bbox=(0.6, 0.4)),
Device(id="b", name="B", bbox=(0.6, 0.4)),
]
lab = Lab(width=5.0, depth=4.0)
result = seed_layout(devices, lab, PRESETS["compact_outward"])
thetas = [p.theta for p in result]
assert any(t != 0.0 for t in thetas) or len(devices) == 1
def test_none_orientation_keeps_zero(self):
"""orientation_mode='none': all thetas stay 0."""
devices = [Device(id="a", name="A", bbox=(0.6, 0.4))]
lab = Lab(width=5.0, depth=4.0)
params = SeederParams(boundary_attraction=0.0, orientation_mode="none")
result = seed_layout(devices, lab, params)
assert result[0].theta == 0.0