mirror of
https://github.com/deepmodeling/Uni-Lab-OS
synced 2026-05-23 07:29:59 +00:00
Compare commits
1 Commits
feat/3d_la
...
dependabot
| Author | SHA1 | Date | |
|---|---|---|---|
|
8019f83d6a |
@@ -1,6 +1,6 @@
|
||||
---
|
||||
name: batch-submit-experiment
|
||||
description: Batch submit experiments (notebooks) to Uni-Lab platform — list workflows, generate node_params from registry schemas, submit multiple rounds, check notebook status. Use when the user wants to submit experiments, create notebooks, batch run workflows, check experiment status, or mentions 提交实验/批量实验/notebook/实验轮次/实验状态.
|
||||
description: Batch submit experiments (notebooks) to Uni-Lab platform — list workflows, generate node_params from registry schemas, submit multiple rounds. Use when the user wants to submit experiments, create notebooks, batch run workflows, or mentions 提交实验/批量实验/notebook/实验轮次.
|
||||
---
|
||||
|
||||
# 批量提交实验指南
|
||||
@@ -59,7 +59,7 @@ AUTH="Authorization: Lab <上面命令输出的 token>"
|
||||
|
||||
### 4. workflow_uuid（目标工作流）
|
||||
|
||||
用户需要提供要提交的 workflow UUID。如果用户不确定，通过 API #3 列出可用 workflow 供选择。
|
||||
用户需要提供要提交的 workflow UUID。如果用户不确定，通过 API #2 列出可用 workflow 供选择。
|
||||
|
||||
**四项全部就绪后才可开始。**
|
||||
|
||||
@@ -68,9 +68,8 @@ AUTH="Authorization: Lab <上面命令输出的 token>"
|
||||
在整个对话过程中，agent 需要记住以下状态，避免重复询问用户：
|
||||
|
||||
- `lab_uuid` — 实验室 UUID（首次通过 API #1 自动获取，**不需要问用户**）
|
||||
- `project_uuid` — 项目 UUID（通过 API #2 列出项目列表，**让用户选择**）
|
||||
- `workflow_uuid` — 工作流 UUID（用户提供或从列表选择）
|
||||
- `workflow_nodes` — workflow 中各 action 节点的 uuid、设备 ID、动作名（从 API #4 获取）
|
||||
- `workflow_nodes` — workflow 中各 action 节点的 uuid、设备 ID、动作名（从 API #3 获取）
|
||||
|
||||
## 请求约定
|
||||
|
||||
@@ -98,17 +97,7 @@ curl -s -X GET "$BASE/api/v1/edge/lab/info" -H "$AUTH"
|
||||
|
||||
记住 `data.uuid` 为 `lab_uuid`。
|
||||
|
||||
### 2. 列出实验室项目（让用户选择项目）
|
||||
|
||||
```bash
|
||||
curl -s -X GET "$BASE/api/v1/lab/project/list?lab_uuid=$lab_uuid" -H "$AUTH"
|
||||
```
|
||||
|
||||
返回项目列表，展示给用户选择。列出每个项目的 `uuid` 和 `name`。
|
||||
|
||||
用户**必须**选择一个项目，记住 `project_uuid`，后续创建 notebook 时需要提供。
|
||||
|
||||
### 3. 列出可用 workflow
|
||||
### 2. 列出可用 workflow
|
||||
|
||||
```bash
|
||||
curl -s -X GET "$BASE/api/v1/lab/workflow/workflows?page=1&page_size=20&lab_uuid=$lab_uuid" -H "$AUTH"
|
||||
@@ -116,7 +105,7 @@ curl -s -X GET "$BASE/api/v1/lab/workflow/workflows?page=1&page_size=20&lab_uuid
|
||||
|
||||
返回 workflow 列表，展示给用户选择。列出每个 workflow 的 `uuid` 和 `name`。
|
||||
|
||||
### 4. 获取 workflow 模板详情
|
||||
### 3. 获取 workflow 模板详情
|
||||
|
||||
```bash
|
||||
curl -s -X GET "$BASE/api/v1/lab/workflow/template/detail/$workflow_uuid" -H "$AUTH"
|
||||
@@ -130,7 +119,7 @@ curl -s -X GET "$BASE/api/v1/lab/workflow/template/detail/$workflow_uuid" -H "$A
|
||||
|
||||
> **注意**：此 API 返回格式可能因版本不同而有差异。首次调用时，先打印完整响应分析结构，再提取节点信息。常见的节点字段路径为 `data.nodes[]` 或 `data.workflow_nodes[]`。
|
||||
|
||||
### 5. 提交实验（创建 notebook）
|
||||
### 4. 提交实验（创建 notebook）
|
||||
|
||||
```bash
|
||||
curl -s -X POST "$BASE/api/v1/lab/notebook" \
|
||||
@@ -143,7 +132,6 @@ curl -s -X POST "$BASE/api/v1/lab/notebook" \
|
||||
```json
|
||||
{
|
||||
"lab_uuid": "<lab_uuid>",
|
||||
"project_uuid": "<project_uuid>",
|
||||
"workflow_uuid": "<workflow_uuid>",
|
||||
"name": "<实验名称>",
|
||||
"node_params": [
|
||||
@@ -171,16 +159,6 @@ curl -s -X POST "$BASE/api/v1/lab/notebook" \
|
||||
|
||||
> **注意**：`sample_uuids` 必须是 **UUID 数组**（`[]uuid.UUID`），不是字符串。无样品时传空数组 `[]`。
|
||||
|
||||
### 6. 查询 notebook 状态
|
||||
|
||||
提交成功后，使用返回的 notebook UUID 查询执行状态：
|
||||
|
||||
```bash
|
||||
curl -s -X GET "$BASE/api/v1/lab/notebook/status?uuid=$notebook_uuid" -H "$AUTH"
|
||||
```
|
||||
|
||||
提交后应**立即查询一次**状态，确认 notebook 已被正确接收并开始调度。
|
||||
|
||||
---
|
||||
|
||||
## Notebook 请求体详解
|
||||
@@ -203,7 +181,7 @@ curl -s -X GET "$BASE/api/v1/lab/notebook/status?uuid=$notebook_uuid" -H "$AUTH"
|
||||
|
||||
| 字段 | 类型 | 说明 |
|
||||
|------|------|------|
|
||||
| `node_uuid` | string | workflow 模板中的节点 UUID（从 API #4 获取） |
|
||||
| `node_uuid` | string | workflow 模板中的节点 UUID（从 API #3 获取） |
|
||||
| `param` | object | 动作参数（根据本地注册表 schema 填写） |
|
||||
| `sample_params` | array | 样品相关参数（液体名、体积等） |
|
||||
|
||||
@@ -244,7 +222,7 @@ python scripts/gen_notebook_params.py \
|
||||
|
||||
如果脚本不可用或注册表不存在：
|
||||
|
||||
1. 调用 API #4 获取 workflow 详情
|
||||
1. 调用 API #3 获取 workflow 详情
|
||||
2. 找到每个 action 节点的 `node_uuid`
|
||||
3. 在本地注册表中查找对应设备的 `action_value_mappings`：
|
||||
```
|
||||
@@ -297,15 +275,13 @@ Task Progress:
|
||||
- [ ] Step 1: 确认 ak/sk → 生成 AUTH token
|
||||
- [ ] Step 2: 确认 --addr → 设置 BASE URL
|
||||
- [ ] Step 3: GET /edge/lab/info → 获取 lab_uuid
|
||||
- [ ] Step 4: GET /lab/project/list → 列出项目，让用户选择 → 获取 project_uuid
|
||||
- [ ] Step 5: 确认 workflow_uuid（用户提供或从 GET #3 列表选择）
|
||||
- [ ] Step 6: GET workflow detail (#4) → 提取各节点 uuid、设备ID、动作名
|
||||
- [ ] Step 7: 定位本地注册表 req_device_registry_upload.json
|
||||
- [ ] Step 8: 运行 gen_notebook_params.py 或手动匹配 → 生成 node_params 模板
|
||||
- [ ] Step 9: 引导用户填写每轮的参数（sample_uuids、param、sample_params）
|
||||
- [ ] Step 10: 构建完整请求体（含 project_uuid）→ POST /lab/notebook 提交
|
||||
- [ ] Step 11: 检查返回结果，记录 notebook UUID
|
||||
- [ ] Step 12: GET /lab/notebook/status → 查询 notebook 状态，确认已调度
|
||||
- [ ] Step 4: 确认 workflow_uuid（用户提供或从 GET #2 列表选择）
|
||||
- [ ] Step 5: GET workflow detail (#3) → 提取各节点 uuid、设备ID、动作名
|
||||
- [ ] Step 6: 定位本地注册表 req_device_registry_upload.json
|
||||
- [ ] Step 7: 运行 gen_notebook_params.py 或手动匹配 → 生成 node_params 模板
|
||||
- [ ] Step 8: 引导用户填写每轮的参数（sample_uuids、param、sample_params）
|
||||
- [ ] Step 9: 构建完整请求体 → POST /lab/notebook 提交
|
||||
- [ ] Step 10: 检查返回结果，确认提交成功
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
@@ -265,7 +265,6 @@ def generate_template(nodes, registry_index, rounds):
|
||||
|
||||
return {
|
||||
"lab_uuid": "$TODO_LAB_UUID",
|
||||
"project_uuid": "$TODO_PROJECT_UUID",
|
||||
"workflow_uuid": "$TODO_WORKFLOW_UUID",
|
||||
"name": "$TODO_EXPERIMENT_NAME",
|
||||
"node_params": node_params,
|
||||
|
||||
@@ -158,7 +158,6 @@ python ./scripts/extract_device_actions.py [--registry <path>] <device_id> ./ski
|
||||
- `unilabos_devices` → **DeviceSlot**，填入路径字符串如 `"/host_node"`（从资源树筛选 type=device）
|
||||
- `unilabos_nodes` → **NodeSlot**，填入路径字符串如 `"/PRCXI/PRCXI_Deck"`（资源树中任意节点）
|
||||
- `unilabos_class` → **ClassSlot**，填入类名字符串如 `"container"`（从注册表查找）
|
||||
- `unilabos_formulation` → **FormulationSlot**，填入配方数组 `[{well_name, liquids: [{name, volume}]}]`（well_name 为目标物料的 name）
|
||||
- array 类型字段 → `[{id, name, uuid}, ...]`
|
||||
- 特殊：`create_resource` 的 `res_id`（ResourceSlot）可填不存在的路径
|
||||
|
||||
@@ -189,30 +188,17 @@ API 模板结构：
|
||||
- lab_uuid（通过 GET /edge/lab/info 直接获取，不要问用户）, device_name
|
||||
|
||||
## API Endpoints
|
||||
# - #1 GET /edge/lab/info → 直接拿到 lab_uuid
|
||||
# - #2 创建工作流 POST /lab/workflow/owner → 拼 URL 告知用户
|
||||
# - #3 创建节点 POST /edge/workflow/node
|
||||
# body: {workflow_uuid, resource_template_name: "<device_id>", node_template_name: "<action_name>"}
|
||||
# - #4 删除节点 DELETE /lab/workflow/nodes
|
||||
# - #5 更新节点参数 PATCH /lab/workflow/node
|
||||
# - #6 查询节点 handles POST /lab/workflow/node-handles
|
||||
# body: {node_uuids: ["uuid1","uuid2"]} → 返回各节点的 handle_uuid
|
||||
# - #7 批量创建边 POST /lab/workflow/edges
|
||||
# body: {edges: [{source_node_uuid, target_node_uuid, source_handle_uuid, target_handle_uuid}]}
|
||||
# - #8 启动工作流 POST /lab/workflow/{uuid}/run
|
||||
# - #9 运行设备单动作 POST /lab/mcp/run/action
|
||||
# - #10 查询任务状态 GET /lab/mcp/task/{task_uuid}
|
||||
# - #11 运行工作流单节点 POST /lab/mcp/run/workflow/action
|
||||
# - #12 获取资源树 GET /lab/material/download/{lab_uuid}
|
||||
# - #13 获取工作流模板详情 GET /lab/workflow/template/detail/{workflow_uuid}
|
||||
# 返回 workflow 完整结构：data.nodes[] 含每个节点的 uuid、name、param、device_name、handles
|
||||
# - #1 GET /edge/lab/info → 直接拿到 lab_uuid
|
||||
# - #2 创建工作流 POST /lab/workflow/owner → 拼 URL 告知用户
|
||||
# - #3 创建节点 POST /edge/workflow/node
|
||||
# body: {workflow_uuid, resource_template_name: "<device_id>", node_template_name: "<action_name>"}
|
||||
# - #10 获取资源树 GET /lab/material/download/{lab_uuid}
|
||||
|
||||
## Placeholder Slot 填写规则
|
||||
- unilabos_resources → ResourceSlot → {"id":"/path/name","name":"name","uuid":"xxx"}
|
||||
- unilabos_devices → DeviceSlot → "/parent/device" 路径字符串
|
||||
- unilabos_nodes → NodeSlot → "/parent/node" 路径字符串
|
||||
- unilabos_class → ClassSlot → "class_name" 字符串
|
||||
- unilabos_formulation → FormulationSlot → [{well_name, liquids: [{name, volume}]}] 配方数组
|
||||
- 特例：create_resource 的 res_id 允许填不存在的路径
|
||||
- 列出本设备所有 Slot 字段、类型及含义
|
||||
|
||||
@@ -223,8 +209,8 @@ API 模板结构：
|
||||
### Step 5 — 验证
|
||||
|
||||
检查文件完整性：
|
||||
- [ ] `SKILL.md` 包含 API endpoint（#1 获取 lab_uuid、#2-#7 工作流/节点/边、#8-#11 运行/查询、#12 资源树、#13 工作流模板详情）
|
||||
- [ ] `SKILL.md` 包含 Placeholder Slot 填写规则（ResourceSlot / DeviceSlot / NodeSlot / ClassSlot / FormulationSlot + create_resource 特例）和本设备的 Slot 字段表
|
||||
- [ ] `SKILL.md` 包含 API endpoint（#1 获取 lab_uuid、#2-#9 工作流/动作、#10 资源树）
|
||||
- [ ] `SKILL.md` 包含 Placeholder Slot 填写规则（ResourceSlot / DeviceSlot / NodeSlot / ClassSlot + create_resource 特例）和本设备的 Slot 字段表
|
||||
- [ ] `action-index.md` 列出所有 action 并有描述
|
||||
- [ ] `actions/` 目录中每个 action 有对应 JSON 文件
|
||||
- [ ] JSON 文件包含 `type`, `schema`（已提升为 goal 内容）, `goal`, `goal_default`, `placeholder_keys` 字段
|
||||
@@ -270,7 +256,7 @@ API 模板结构：
|
||||
|
||||
## Placeholder Slot 类型体系
|
||||
|
||||
`placeholder_keys` / `_unilabos_placeholder_info` 中有 5 种值，对应不同的填写方式：
|
||||
`placeholder_keys` / `_unilabos_placeholder_info` 中有 4 种值，对应不同的填写方式：
|
||||
|
||||
| placeholder 值 | Slot 类型 | 填写格式 | 选取范围 |
|
||||
|---------------|-----------|---------|---------|
|
||||
@@ -278,7 +264,6 @@ API 模板结构：
|
||||
| `unilabos_devices` | DeviceSlot | `"/parent/device_name"` | 仅**设备**节点（type=device），路径字符串 |
|
||||
| `unilabos_nodes` | NodeSlot | `"/parent/node_name"` | **设备 + 物料**，即所有节点，路径字符串 |
|
||||
| `unilabos_class` | ClassSlot | `"class_name"` | 注册表中已上报的资源类 name |
|
||||
| `unilabos_formulation` | FormulationSlot | `[{well_name, liquids: [{name, volume}]}]` | 资源树中物料节点的 **name**，配合液体配方 |
|
||||
|
||||
### ResourceSlot（`unilabos_resources`）
|
||||
|
||||
@@ -325,41 +310,7 @@ API 模板结构：
|
||||
"container"
|
||||
```
|
||||
|
||||
### FormulationSlot（`unilabos_formulation`）
|
||||
|
||||
描述**液体配方**：向哪些物料容器中加入哪些液体及体积。填写为**对象数组**：
|
||||
|
||||
```json
|
||||
[
|
||||
{
|
||||
"sample_uuid": "",
|
||||
"well_name": "YB_PrepBottle_15mL_Carrier_bottle_A1",
|
||||
"liquids": [
|
||||
{ "name": "LiPF6", "volume": 0.6 },
|
||||
{ "name": "DMC", "volume": 1.2 }
|
||||
]
|
||||
}
|
||||
]
|
||||
```
|
||||
|
||||
#### 字段说明
|
||||
|
||||
| 字段 | 类型 | 说明 |
|
||||
|------|------|------|
|
||||
| `sample_uuid` | string | 样品 UUID，无样品时传空字符串 `""` |
|
||||
| `well_name` | string | 目标物料容器的 **name**（从资源树中取物料节点的 `name` 字段，如瓶子、孔位名称） |
|
||||
| `liquids` | array | 要加入的液体列表 |
|
||||
| `liquids[].name` | string | 液体名称（如试剂名、溶剂名） |
|
||||
| `liquids[].volume` | number | 液体体积（单位由设备决定，通常为 mL） |
|
||||
|
||||
#### 填写规则
|
||||
|
||||
- `well_name` 必须是资源树中已存在的物料节点 `name`（不是 `id` 路径），通过 API #12 获取资源树后筛选
|
||||
- 每个数组元素代表一个目标容器的配方
|
||||
- 一个容器可以加入多种液体（`liquids` 数组多条记录）
|
||||
- 与 ResourceSlot 的区别：ResourceSlot 填 `{id, name, uuid}` 指向物料本身；FormulationSlot 用 `well_name` 引用物料，并附带液体配方信息
|
||||
|
||||
### 通过 API #12 获取资源树
|
||||
### 通过 API #10 获取资源树
|
||||
|
||||
```bash
|
||||
curl -s -X GET "$BASE/api/v1/lab/material/download/$lab_uuid" -H "$AUTH"
|
||||
|
||||
2
.github/workflows/deploy-docs.yml
vendored
2
.github/workflows/deploy-docs.yml
vendored
@@ -84,7 +84,7 @@ jobs:
|
||||
|
||||
- name: Setup Pages
|
||||
id: pages
|
||||
uses: actions/configure-pages@v5
|
||||
uses: actions/configure-pages@v6
|
||||
if: |
|
||||
github.event.workflow_run.head_branch == 'main' ||
|
||||
(github.event_name == 'workflow_dispatch' && github.event.inputs.deploy_to_pages == 'true')
|
||||
|
||||
@@ -80,20 +80,19 @@ class HTTPClient:
|
||||
f.write(json.dumps(payload, indent=4))
|
||||
# 从序列化数据中提取所有节点的UUID（保存旧UUID）
|
||||
old_uuids = {n.res_content.uuid: n for n in resources.all_nodes}
|
||||
nodes_info = [x for xs in resources.dump() for x in xs]
|
||||
if not self.initialized or first_add:
|
||||
self.initialized = True
|
||||
info(f"首次添加资源，当前远程地址: {self.remote_addr}")
|
||||
response = requests.post(
|
||||
f"{self.remote_addr}/edge/material",
|
||||
json={"nodes": nodes_info, "mount_uuid": mount_uuid},
|
||||
json={"nodes": [x for xs in resources.dump() for x in xs], "mount_uuid": mount_uuid},
|
||||
headers={"Authorization": f"Lab {self.auth}"},
|
||||
timeout=60,
|
||||
)
|
||||
else:
|
||||
response = requests.put(
|
||||
f"{self.remote_addr}/edge/material",
|
||||
json={"nodes": nodes_info, "mount_uuid": mount_uuid},
|
||||
json={"nodes": [x for xs in resources.dump() for x in xs], "mount_uuid": mount_uuid},
|
||||
headers={"Authorization": f"Lab {self.auth}"},
|
||||
timeout=10,
|
||||
)
|
||||
@@ -112,7 +111,6 @@ class HTTPClient:
|
||||
uuid_mapping[i["uuid"]] = i["cloud_uuid"]
|
||||
else:
|
||||
logger.error(f"添加物料失败: {response.text}")
|
||||
logger.trace(f"添加物料失败: {nodes_info}")
|
||||
for u, n in old_uuids.items():
|
||||
if u in uuid_mapping:
|
||||
n.res_content.uuid = uuid_mapping[u]
|
||||
|
||||
@@ -1113,7 +1113,7 @@ class MessageProcessor:
|
||||
"task_id": task_id,
|
||||
"job_id": job_id,
|
||||
"free": free,
|
||||
"need_more": need_more + 1,
|
||||
"need_more": need_more,
|
||||
},
|
||||
}
|
||||
|
||||
@@ -1253,7 +1253,7 @@ class QueueProcessor:
|
||||
"task_id": job_info.task_id,
|
||||
"job_id": job_info.job_id,
|
||||
"free": False,
|
||||
"need_more": 10 + 1,
|
||||
"need_more": 10,
|
||||
},
|
||||
}
|
||||
self.message_processor.send_message(message)
|
||||
@@ -1286,7 +1286,7 @@ class QueueProcessor:
|
||||
"task_id": job_info.task_id,
|
||||
"job_id": job_info.job_id,
|
||||
"free": False,
|
||||
"need_more": 10 + 1,
|
||||
"need_more": 10,
|
||||
},
|
||||
}
|
||||
success = self.message_processor.send_message(message)
|
||||
@@ -1369,10 +1369,6 @@ class WebSocketClient(BaseCommunicationClient):
|
||||
self.message_processor = MessageProcessor(self.websocket_url, self.send_queue, self.device_manager)
|
||||
self.queue_processor = QueueProcessor(self.device_manager, self.message_processor)
|
||||
|
||||
# running状态debounce缓存: {job_id: (last_send_timestamp, last_feedback_data)}
|
||||
self._job_running_last_sent: Dict[str, tuple] = {}
|
||||
self._job_running_debounce_interval: float = 10.0 # 秒
|
||||
|
||||
# 设置相互引用
|
||||
self.message_processor.set_queue_processor(self.queue_processor)
|
||||
self.message_processor.set_websocket_client(self)
|
||||
@@ -1472,32 +1468,22 @@ class WebSocketClient(BaseCommunicationClient):
|
||||
logger.debug(f"[WebSocketClient] Not connected, cannot publish job status for job_id: {item.job_id}")
|
||||
return
|
||||
|
||||
job_log = format_job_log(item.job_id, item.task_id, item.device_id, item.action_name)
|
||||
|
||||
# 拦截最终结果状态，与原版本逻辑一致
|
||||
if status in ["success", "failed"]:
|
||||
self._job_running_last_sent.pop(item.job_id, None)
|
||||
|
||||
host_node = HostNode.get_instance(0)
|
||||
if host_node:
|
||||
# 从HostNode的device_action_status中移除job_id
|
||||
try:
|
||||
host_node._device_action_status[item.device_action_key].job_ids.pop(item.job_id, None)
|
||||
except (KeyError, AttributeError):
|
||||
logger.warning(f"[WebSocketClient] Failed to remove job {item.job_id} from HostNode status")
|
||||
|
||||
# logger.debug(f"[WebSocketClient] Intercepting final status for job_id: {item.job_id} - {status}")
|
||||
|
||||
# 通知队列处理器job完成（包括timeout的job）
|
||||
self.queue_processor.handle_job_completed(item.job_id, status)
|
||||
|
||||
# running状态按job_id做debounce，内容变化时仍然上报
|
||||
if status == "running":
|
||||
now = time.time()
|
||||
cached = self._job_running_last_sent.get(item.job_id)
|
||||
if cached is not None:
|
||||
last_ts, last_data = cached
|
||||
if now - last_ts < self._job_running_debounce_interval and last_data == feedback_data:
|
||||
logger.trace(f"[WebSocketClient] Job status debounced (skip): {job_log} - {status}")
|
||||
return
|
||||
self._job_running_last_sent[item.job_id] = (now, feedback_data)
|
||||
|
||||
# 发送job状态消息
|
||||
message = {
|
||||
"action": "job_status",
|
||||
"data": {
|
||||
@@ -1513,6 +1499,7 @@ class WebSocketClient(BaseCommunicationClient):
|
||||
}
|
||||
self.message_processor.send_message(message)
|
||||
|
||||
job_log = format_job_log(item.job_id, item.task_id, item.device_id, item.action_name)
|
||||
logger.trace(f"[WebSocketClient] Job status published: {job_log} - {status}")
|
||||
|
||||
def send_ping(self, ping_id: str, timestamp: float) -> None:
|
||||
|
||||
@@ -1,634 +0,0 @@
|
||||
# Layout Optimizer Handover
|
||||
|
||||
**Date**: 2026-04-10 | **Branch**: `feat/3d_layout_and_visualize` | **Commit**: `99dc821a` | **Tests**: 270 (260 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) │
|
||||
│ Custom DE loop (best1bin/currenttobest1bin│
|
||||
│ /rand1bin strategies) │
|
||||
│ 3N-dim: [x0, y0, θ0, x1, y1, θ1, ...] │
|
||||
│ Broad-phase AABB sweep (broad_phase.py) │
|
||||
│ θ lattice snap in joint discrete mode │
|
||||
│ Cost = hard_penalties + soft_penalties │
|
||||
│ Graduated collision penalties (not binary) │
|
||||
│ │
|
||||
│ 4. θ snap (optimizer.snap_theta) │
|
||||
│ Snap near-cardinal angles to 0/90/180/270 │
|
||||
│ (opt-in via snap_cardinal=True) │
|
||||
│ │
|
||||
│ 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 11 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,
|
||||
"angle_granularity": 4,
|
||||
"snap_cardinal": false,
|
||||
"strategy": "currenttobest1bin",
|
||||
"mutation": [0.5, 1.0],
|
||||
"theta_mutation": null,
|
||||
"recombination": 0.7,
|
||||
"crossover_mode": "device"
|
||||
}
|
||||
```
|
||||
|
||||
**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.
|
||||
|
||||
**DE hyperparameters:**
|
||||
| Param | Default | Description |
|
||||
|-------|---------|-------------|
|
||||
| `strategy` | `"currenttobest1bin"` | DE mutation strategy (`best1bin`, `currenttobest1bin`, `rand1bin`) |
|
||||
| `mutation` | `[0.5, 1.0]` | Dithered F range for position dimensions |
|
||||
| `theta_mutation` | `null` (same as `mutation`) | Separate F range for θ dimensions (decoupled mutation) |
|
||||
| `recombination` | `0.7` | Crossover probability |
|
||||
| `crossover_mode` | `"device"` | `"device"` = per-device CR, `"dimension"` = per-dimension CR |
|
||||
| `angle_granularity` | `null` | `4`/`8`/`12`/`24` — snaps θ to a discrete lattice during DE (joint mode). `4` = axis-aligned (0/90/180/270). `null` = continuous θ |
|
||||
| `snap_cardinal` | `false` | Post-DE snap to nearest cardinal angle with collision rollback |
|
||||
|
||||
### Scene State API
|
||||
|
||||
Shared scene state between the LLM agent and the frontend. The agent pushes layout results here; the frontend polls for updates.
|
||||
|
||||
#### `GET /scene/lab` / `POST /scene/lab` — Lab dimensions
|
||||
|
||||
**GET** returns current lab dimensions. **POST** sets them (frontend sends this when user changes lab size).
|
||||
|
||||
```json
|
||||
{"width": 6.0, "depth": 4.0}
|
||||
```
|
||||
|
||||
#### `GET /scene/placements` / `POST /scene/placements` / `DELETE /scene/placements`
|
||||
|
||||
**GET** returns current placements + a version counter. Frontend polls this every 1s and re-renders when version changes.
|
||||
|
||||
```json
|
||||
{"version": 3, "placements": [...]}
|
||||
```
|
||||
|
||||
**POST** pushes new placements (from `/optimize` result or agent). Bumps version.
|
||||
|
||||
**DELETE** clears all placements (resets scene).
|
||||
|
||||
### `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 (11 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 |
|
||||
| `keep_adjacent` | `devices` (list[str]), `priority` | `minimize_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 |
|
||||
|
||||
Intent priorities are baked into the final emitted constraint `weight` during interpretation. The caller only sees the resulting weight, not a separate constraint-level priority field.
|
||||
|
||||
---
|
||||
|
||||
## 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 θ |
|
||||
| `crossing_penalty` | (auto, part of `reachability` eval) | Segment-OBB intersection length of opening-to-arm path blocked by other devices (Cyrus-Beck clipping via `obb.segment_obb_intersection_length`) |
|
||||
|
||||
### Weight Normalization
|
||||
|
||||
| Constant | Value | Meaning |
|
||||
|----------|-------|---------|
|
||||
| `DEFAULT_WEIGHT_DISTANCE` | 100.0 | 1 cm → penalty 1.0 |
|
||||
| `DEFAULT_WEIGHT_ANGLE` | 60.0 | 5° → penalty ~1.0 |
|
||||
| `HARD_MULTIPLIER` | 5.0 | Hard constraint penalty multiplier during graduated DE |
|
||||
|
||||
Constraints support a `priority` field (`critical` / `high` / `normal` / `low`) with multipliers 5× / 2× / 1× / 0.5×.
|
||||
|
||||
### 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` | 97 | Dataclasses: Device, Lab, Placement, Constraint, Intent, Opening |
|
||||
| `device_catalog.py` | 303 | Loads devices from footprints.json + uni-lab-assets + registry |
|
||||
| `footprints.json` | 183KB | 499 device bounding boxes, heights, openings (offline extracted) |
|
||||
| `seeders.py` | 331 | Force-directed initial layout with presets |
|
||||
| `optimizer.py` | 1056 | Custom DE loop: per-device crossover, θ wrapping, discrete angle lattice, multi-strategy |
|
||||
| `broad_phase.py` | 66 | 2-axis sweep-and-prune AABB broad phase for collision pair pruning |
|
||||
| `constraints.py` | 627 | Unified constraint evaluation (hard + soft + graduated + crossing penalty) |
|
||||
| `obb.py` | 257 | OBB geometry: corners, overlap SAT, min_distance, penetration_depth, segment intersection |
|
||||
| `intent_interpreter.py` | 366 | 11 intent handlers, pure translation, no side effects |
|
||||
| `server.py` | 743 | FastAPI: /interpret, /optimize, /devices, /scene/* endpoints |
|
||||
| `lab_parser.py` | 50 | Parse lab floor plan JSON to Lab dataclass |
|
||||
|
||||
### Reference / Utilities
|
||||
| File | Purpose |
|
||||
|------|---------|
|
||||
| `extract_footprints.py` | How footprints.json was generated (offline STL/GLB → 2D bbox extraction via trimesh) |
|
||||
| `generate_asset_registry.py` | Generate YAML registry entries for uni-lab-assets devices not already registered |
|
||||
|
||||
### 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 |
|
||||
| `llm_skill/demo_agent.md` | LLM agent orchestration instructions for demo (GET /devices → intents → /interpret → /optimize → /scene/placements) |
|
||||
|
||||
### Demo / Frontend
|
||||
| File | Purpose |
|
||||
|------|---------|
|
||||
| `static/lab3d.html` | Three.js 3D visualization frontend (1227 lines): device library, drag-to-add, auto layout, scene polling |
|
||||
|
||||
### Configuration
|
||||
| File | Purpose |
|
||||
|------|---------|
|
||||
| `pyproject.toml` | Package deps: scipy, numpy, fastapi, uvicorn, pydantic |
|
||||
|
||||
### Tests (270 total: 260 pass + 10 skip without API key)
|
||||
| File | Tests | Coverage |
|
||||
|------|-------|----------|
|
||||
| `test_intent_interpreter.py` | 19 | All 11 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` | 30 | Constraint evaluation, hard/soft, graduated penalties, crossing penalty |
|
||||
| `test_optimizer.py` | 50 | DE optimizer, vector encoding, bounds, discrete angles, strategies |
|
||||
| `test_mock_checkers.py` | 15 | MockCollisionChecker, MockReachabilityChecker |
|
||||
| `test_ros_checkers.py` | 40 | MoveIt2/IKFast adapter tests |
|
||||
| `test_seeders.py` | 12 | Force-directed seeder presets |
|
||||
| `test_device_catalog.py` | 25 | Device loading, footprint merging |
|
||||
| `test_obb.py` | 18 | OBB geometry functions, segment intersection |
|
||||
| `test_bugfixes_v2.py` | 28 | Regression: duplicate IDs, orientation, min_spacing, cardinal snap defaults |
|
||||
| `test_broad_phase.py` | 5 | Sweep-and-prune AABB broad phase |
|
||||
|
||||
---
|
||||
|
||||
## 8. How to Run
|
||||
|
||||
### Quick Start
|
||||
```bash
|
||||
# Install
|
||||
pip install -e ".[dev]"
|
||||
|
||||
# Run server
|
||||
uvicorn unilabos.layout_optimizer.server:app --host 0.0.0.0 --port 8000 --reload
|
||||
|
||||
# Run server with debug logging (shows DE cost breakdown per generation)
|
||||
LAYOUT_DEBUG=1 uvicorn unilabos.layout_optimizer.server:app --host 0.0.0.0 --port 8000 --reload
|
||||
|
||||
# Run tests
|
||||
pytest unilabos/layout_optimizer/tests/ -v
|
||||
|
||||
# Run LLM skill tests (needs API key)
|
||||
ANTHROPIC_API_KEY=sk-... pytest unilabos/layout_optimizer/tests/test_llm_skill.py -v
|
||||
```
|
||||
|
||||
**Log files**: All requests are logged to `unilabos/layout_optimizer/logs/{YYYYMMDD_HHMMSS}.log` at DEBUG level (frontend polling GET /scene/placements excluded).
|
||||
|
||||
### 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 |
|
||||
| `LAYOUT_DEBUG` | (unset) | Set to `1` for DEBUG-level console logging (DE cost breakdown per generation) |
|
||||
| `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.
|
||||
|
||||
6. **`align_weight` and `snap_cardinal` default to off**: `prefer_aligned` weight defaults to 0 (was `DEFAULT_WEIGHT_ANGLE=60`) and `snap_theta_safe` is opt-in via `snap_cardinal=True`. Both remain available when explicitly requested via `align_cardinal` intent or API param.
|
||||
|
||||
7. **Hybrid angle mode deprecated**: The angle-first hybrid mode (separate angle sweep + position-only DE) has been replaced by joint discrete mode as the default when `angle_granularity` is set. Joint mode snaps θ to the discrete lattice within the normal 3N DE loop.
|
||||
|
||||
---
|
||||
|
||||
## 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,
|
||||
"angle_granularity": 4,
|
||||
"maxiter": 100,
|
||||
"seed": 42
|
||||
}' | python3 -m json.tool
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 11. Demo Setup
|
||||
|
||||
This section documents the device processing pipeline, test frontend, and LLM agent demo for the layout optimizer.
|
||||
|
||||
### 11.1 Device Processing Pipeline
|
||||
|
||||
How devices go from 3D meshes to collision footprints:
|
||||
|
||||
1. **Source data**:
|
||||
- `uni-lab-assets/` repository: GLB/STL 3D models + XACRO robot descriptions
|
||||
- `Uni-Lab-OS/device_mesh/devices/` registry: device metadata directories
|
||||
|
||||
2. **Extraction** (`extract_footprints.py`):
|
||||
- Load meshes via `trimesh` (STL for geometry, GLB for display)
|
||||
- Compute oriented bounding box (OBB): width, depth, height
|
||||
- Apply GLB root node rotation to align with world frame
|
||||
- Detect openings from XACRO `<joint type="fixed">` elements containing "socket" in name
|
||||
- Compute opening direction: centroid of socket origins → cardinal direction mapping
|
||||
- Manual overrides for devices with non-standard opening patterns (`MANUAL_OPENINGS` dict)
|
||||
- Write results to `footprints.json` (499 devices, 183KB)
|
||||
|
||||
3. **Catalog merging** (`device_catalog.py`):
|
||||
- Load `footprints.json` (OBB + openings)
|
||||
- Load `uni-lab-assets/data.json` (asset tree structure)
|
||||
- Load `Uni-Lab-OS/device_mesh/devices/` (registry devices)
|
||||
- Merge: registry devices get priority for metadata, but assets' 3D model paths preferred
|
||||
- Fallback sizes: `KNOWN_SIZES` dict provides manual dimensions when trimesh extraction fails
|
||||
|
||||
4. **Standalone filtering** (`server.py:_is_standalone_device`):
|
||||
- Bbox >30cm = device (standalone equipment)
|
||||
- Bbox <5cm = consumable (plates, tubes, tips)
|
||||
- 5-30cm = keyword heuristic (check name for "plate", "tube", "tip", "rack")
|
||||
|
||||
### 11.2 Test Frontend (`static/lab3d.html`)
|
||||
|
||||
Interactive 3D lab layout visualization and design tool (1227 lines).
|
||||
|
||||
**Technology stack**:
|
||||
- Three.js v0.169.0 (ES modules from esm.sh CDN)
|
||||
- WebGL renderer with PCF soft shadow maps, ACES filmic tone mapping
|
||||
- OrbitControls for camera interaction
|
||||
|
||||
**Features**:
|
||||
- **Device library**: Left sidebar with search/filter, toggle between devices and consumables
|
||||
- **Drag-to-add**: Click device in library → adds to scene with random position
|
||||
- **Selected devices panel**: Right panel lists all placed devices, click to remove
|
||||
- **Lab dimensions**: Width × Depth inputs (meters), collision margin slider
|
||||
- **View modes**: 3D perspective (default) and top-down orthographic
|
||||
- **Grid system**: 0.5m grid with lab boundary highlighting
|
||||
- **Device visualization**: Box geometry with emissive materials, edge highlights, CSS2D labels
|
||||
- **Opening markers**: Orange arrows and semi-transparent strips showing device access directions
|
||||
- **Auto Layout button**: Calls `POST /optimize` with current devices + constraints
|
||||
- **Scene polling**: 1-second polling of `GET /scene/placements` for agent-pushed updates (version-based change detection)
|
||||
- **Smooth animation**: Lerp interpolation for device placement changes
|
||||
|
||||
**Backend integration**:
|
||||
- `GET /devices` — Load device catalog on startup
|
||||
- `POST /optimize` — Send devices + constraints, receive placements
|
||||
- `POST /scene/lab` — Push lab dimensions when changed
|
||||
- `GET /scene/placements` — Poll every 1s for agent-pushed updates
|
||||
|
||||
**Key JavaScript functions**:
|
||||
- `loadDeviceCatalog()` — Fetch device list, build catalog with color pool
|
||||
- `createDeviceMesh(deviceId, uuid)` — Create Three.js Group with body, edges, opening markers
|
||||
- `addDevice(deviceId)` / `removeDevice(uuid)` — Manage selected devices
|
||||
- `runLayout()` — Call backend `/optimize` or local bin packing fallback
|
||||
- `animatePlacement(uuid, tx, tz, theta)` — Smooth lerp to target position
|
||||
- `setView('3d' | 'top')` — Switch camera perspective
|
||||
|
||||
### 11.3 LLM Agent Demo (`llm_skill/demo_agent.md`)
|
||||
|
||||
LLM agent orchestration instructions for natural language lab layout design.
|
||||
|
||||
**Agent workflow**:
|
||||
1. `GET /devices` — Fetch device catalog for context
|
||||
2. Parse user natural language request
|
||||
3. Build structured intents JSON (using `layout_intent_translator.md` skill)
|
||||
4. `POST /interpret` — Translate intents to constraints
|
||||
5. `POST /optimize` — Run layout optimization
|
||||
6. `POST /scene/placements` — Push results to shared scene state
|
||||
7. Frontend auto-updates via polling (no manual refresh needed)
|
||||
|
||||
**Example user requests**:
|
||||
- "Design a PCR lab with robot arm automation, keep it compact"
|
||||
- "Place liquid handler, thermal cycler, and plate sealer. Arm must reach all devices."
|
||||
- "Add a plate hotel, make sure it's close to the liquid handler"
|
||||
|
||||
### 11.4 Running the Demo
|
||||
|
||||
```bash
|
||||
# Start the server
|
||||
uvicorn unilabos.layout_optimizer.server:app --host 0.0.0.0 --port 8000 --reload
|
||||
|
||||
# Open in browser
|
||||
# http://localhost:8000/
|
||||
|
||||
# Use Claude Code with demo_agent.md skill to orchestrate via natural language
|
||||
# The agent will call the API endpoints and push results to /scene/placements
|
||||
# The frontend will automatically update via polling
|
||||
```
|
||||
|
||||
**Demo flow**:
|
||||
1. Open `http://localhost:8000/` in browser
|
||||
2. Frontend loads device catalog and displays 3D scene
|
||||
3. Use Claude Code with `demo_agent.md` skill to send natural language requests
|
||||
4. Agent translates request → intents → constraints → optimization → scene update
|
||||
5. Frontend polls `/scene/placements` every 1s and animates changes
|
||||
6. User can manually add/remove devices or adjust lab size in the UI
|
||||
7. Click "Auto Layout" to re-optimize with current devices
|
||||
|
||||
@@ -1,9 +0,0 @@
|
||||
"""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"]
|
||||
@@ -1,66 +0,0 @@
|
||||
"""2 轴 sweep-and-prune 宽相碰撞检测。
|
||||
|
||||
对每个设备计算旋转后的 AABB，先沿 x 轴排序并剪枝，
|
||||
再用 y 轴交叠过滤。返回候选碰撞对（索引对列表），
|
||||
供后续 OBB SAT 精确检测使用。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from .models import Device, Placement
|
||||
|
||||
|
||||
def sweep_and_prune_pairs(
|
||||
devices: list[Device],
|
||||
placements: list[Placement],
|
||||
) -> list[tuple[int, int]]:
|
||||
"""2 轴 sweep-and-prune，返回 AABB 交叠的索引对。
|
||||
|
||||
Args:
|
||||
devices: 设备列表，与 placements 一一对应。
|
||||
placements: 布局位姿列表。
|
||||
|
||||
Returns:
|
||||
候选碰撞对列表，每个元素为 (i, j)，
|
||||
i < j，索引对应 placements 原始顺序。
|
||||
"""
|
||||
n = len(devices)
|
||||
if n < 2:
|
||||
return []
|
||||
|
||||
# --- 计算每个设备旋转后的 AABB ---
|
||||
aabbs: list[tuple[float, float, float, float]] = []
|
||||
for dev, pl in zip(devices, placements):
|
||||
hw, hd = pl.rotated_bbox(dev)
|
||||
aabbs.append((pl.x - hw, pl.x + hw, pl.y - hd, pl.y + hd))
|
||||
|
||||
# --- 按 xmin 排序，保留原始索引映射 ---
|
||||
sorted_indices = sorted(range(n), key=lambda k: aabbs[k][0])
|
||||
|
||||
# --- 扫描 x 轴，y 轴过滤 ---
|
||||
candidates: list[tuple[int, int]] = []
|
||||
for si in range(len(sorted_indices)):
|
||||
i = sorted_indices[si]
|
||||
x_min_i, x_max_i, y_min_i, y_max_i = aabbs[i]
|
||||
for sj in range(si + 1, len(sorted_indices)):
|
||||
j = sorted_indices[sj]
|
||||
x_min_j, _x_max_j, y_min_j, y_max_j = aabbs[j]
|
||||
# 由于按 xmin 排序，x_min_j >= x_min_i
|
||||
if x_min_j > x_max_i:
|
||||
break # 后续设备 xmin 更大，不可能与 i 在 x 轴交叠
|
||||
# x 轴交叠确认，检查 y 轴
|
||||
if y_min_i <= y_max_j and y_min_j <= y_max_i:
|
||||
# 保证输出 (min_idx, max_idx) 方便去重和测试
|
||||
pair = (min(i, j), max(i, j))
|
||||
candidates.append(pair)
|
||||
|
||||
return candidates
|
||||
|
||||
|
||||
def broad_phase_device_pairs(
|
||||
devices: list[Device],
|
||||
placements: list[Placement],
|
||||
) -> list[tuple[str, str]]:
|
||||
"""返回候选碰撞对的 device_id 字符串元组列表。"""
|
||||
index_pairs = sweep_and_prune_pairs(devices, placements)
|
||||
return [(placements[i].device_id, placements[j].device_id) for i, j in index_pairs]
|
||||
@@ -1,626 +0,0 @@
|
||||
"""约束体系：硬约束 / 软约束定义与统一评估。
|
||||
|
||||
硬约束违反 → cost = inf（方案直接淘汰）
|
||||
软约束违反 → 加权 penalty 累加到 cost
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import math
|
||||
from typing import TYPE_CHECKING
|
||||
|
||||
from .broad_phase import sweep_and_prune_pairs
|
||||
from .models import Constraint, Device, Lab, Placement
|
||||
from .obb import (
|
||||
nearest_point_on_obb,
|
||||
obb_corners,
|
||||
obb_min_distance,
|
||||
obb_penetration_depth,
|
||||
segment_obb_intersection_length,
|
||||
)
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from typing import Any
|
||||
|
||||
from .interfaces import CollisionChecker, ReachabilityChecker
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
# 归一化默认权重 — 1cm距离违规 ≈ 5°角度违规 的惩罚量级
|
||||
DEFAULT_WEIGHT_DISTANCE: float = 100.0 # 1cm → penalty 1.0
|
||||
DEFAULT_WEIGHT_ANGLE: float = 60.0 # 5° → penalty ~1.0
|
||||
|
||||
# 硬约束graduated模式下的惩罚倍数
|
||||
HARD_MULTIPLIER: float = 5.0
|
||||
|
||||
# 优先级等级对应的权重乘数
|
||||
PRIORITY_MULTIPLIERS: dict[str, float] = {
|
||||
"critical": 5.0,
|
||||
"high": 2.0,
|
||||
"normal": 1.0,
|
||||
"low": 0.5,
|
||||
}
|
||||
|
||||
|
||||
def evaluate_constraints(
|
||||
devices: list[Device],
|
||||
placements: list[Placement],
|
||||
lab: Lab,
|
||||
constraints: list[Constraint],
|
||||
collision_checker: CollisionChecker,
|
||||
reachability_checker: ReachabilityChecker | None = None,
|
||||
*,
|
||||
graduated: bool = True,
|
||||
) -> float:
|
||||
"""统一评估所有约束，返回总 cost。
|
||||
|
||||
Args:
|
||||
devices: 设备列表（与 placements 一一对应）
|
||||
placements: 当前布局方案
|
||||
lab: 实验室平面图
|
||||
constraints: 约束规则列表
|
||||
collision_checker: 碰撞检测实例
|
||||
reachability_checker: 可达性检测实例（可选）
|
||||
graduated: True=比例惩罚（DE优化用），False=二值inf（最终pass/fail用）
|
||||
|
||||
Returns:
|
||||
总 cost。硬约束违反在非graduated模式返回 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,
|
||||
graduated=graduated,
|
||||
)
|
||||
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 = DEFAULT_WEIGHT_DISTANCE * HARD_MULTIPLIER, # 500
|
||||
boundary_weight: float = DEFAULT_WEIGHT_DISTANCE * HARD_MULTIPLIER, # 500
|
||||
) -> 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: 2 轴 sweep-and-prune 宽相 + OBB SAT 精确检测
|
||||
candidate_pairs = sweep_and_prune_pairs(devices, placements)
|
||||
for i, j in candidate_pairs:
|
||||
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,
|
||||
*,
|
||||
graduated: bool = True,
|
||||
) -> float:
|
||||
"""评估单条约束规则。
|
||||
|
||||
graduated=True 时硬约束返回比例惩罚（DE用），
|
||||
graduated=False 时硬约束返回 inf（最终 pass/fail）。
|
||||
"""
|
||||
rule = constraint.rule_name
|
||||
params = constraint.params
|
||||
is_hard = constraint.type == "hard"
|
||||
|
||||
effective_weight = constraint.weight
|
||||
|
||||
if rule == "no_collision":
|
||||
checker_placements = _to_checker_format_from_maps(device_map, placement_map)
|
||||
collisions = collision_checker.check(checker_placements)
|
||||
if collisions:
|
||||
if is_hard and not graduated:
|
||||
return math.inf
|
||||
w = effective_weight * (HARD_MULTIPLIER if is_hard else 1.0)
|
||||
return w * 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:
|
||||
if is_hard and not graduated:
|
||||
return math.inf
|
||||
w = effective_weight * (HARD_MULTIPLIER if is_hard else 1.0)
|
||||
return w * 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)
|
||||
missing_cost = _missing_reference_cost(constraint, placement_map, a_id, b_id)
|
||||
if missing_cost is not None:
|
||||
return missing_cost
|
||||
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:
|
||||
if is_hard and not graduated:
|
||||
return math.inf
|
||||
w = effective_weight * (HARD_MULTIPLIER if is_hard else 1.0)
|
||||
return w * (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)
|
||||
missing_cost = _missing_reference_cost(constraint, placement_map, a_id, b_id)
|
||||
if missing_cost is not None:
|
||||
return missing_cost
|
||||
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:
|
||||
if is_hard and not graduated:
|
||||
return math.inf
|
||||
w = effective_weight * (HARD_MULTIPLIER if is_hard else 1.0)
|
||||
return w * (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)
|
||||
missing_cost = _missing_reference_cost(constraint, placement_map, a_id, b_id)
|
||||
if missing_cost is not None:
|
||||
return missing_cost
|
||||
if da and db:
|
||||
dist = _device_distance_obb(da, pa, db, pb)
|
||||
else:
|
||||
dist = _device_distance_center(pa, pb) or 0.0
|
||||
return effective_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)
|
||||
missing_cost = _missing_reference_cost(constraint, placement_map, a_id, b_id)
|
||||
if missing_cost is not None:
|
||||
return missing_cost
|
||||
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 effective_weight * (max_possible - dist)
|
||||
|
||||
if rule == "min_spacing":
|
||||
min_gap = params.get("min_gap", 0.0)
|
||||
all_placements = list(placement_map.values())
|
||||
total_penalty = 0.0
|
||||
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:
|
||||
total_penalty += (min_gap - dist)
|
||||
if total_penalty > 0:
|
||||
if is_hard and not graduated:
|
||||
return math.inf
|
||||
w = effective_weight * (HARD_MULTIPLIER if is_hard else 1.0)
|
||||
return w * total_penalty
|
||||
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)
|
||||
missing_cost = _missing_reference_cost(
|
||||
constraint, placement_map, arm_id, target_device_id,
|
||||
)
|
||||
if missing_cost is not None:
|
||||
return missing_cost
|
||||
arm_dev = device_map.get(arm_id)
|
||||
target_dev = device_map.get(target_device_id)
|
||||
|
||||
# opening surface center → nearest point on arm OBB
|
||||
if arm_dev and target_dev:
|
||||
opening_pt = _opening_surface_center(target_dev, target_p)
|
||||
arm_corners = obb_corners(
|
||||
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:
|
||||
opening_pt = (target_p.x, target_p.y)
|
||||
nearest = (arm_p.x, arm_p.y)
|
||||
dist = _device_distance_center(arm_p, target_p) or 0.0
|
||||
|
||||
# 交叉惩罚始终计算（soft, 不依赖可达性结果）
|
||||
crossing_cost = _crossing_penalty(
|
||||
opening_pt, nearest,
|
||||
arm_id, target_device_id,
|
||||
device_map, placement_map,
|
||||
)
|
||||
|
||||
arm_pose = {"x": arm_p.x, "y": arm_p.y, "theta": arm_p.theta}
|
||||
target_point = {"x": target_p.x, "y": target_p.y, "z": 0.0}
|
||||
target_point["_obb_dist"] = dist
|
||||
if not reachability_checker.is_reachable(arm_id, arm_pose, target_point):
|
||||
if is_hard and not graduated:
|
||||
return math.inf
|
||||
# Graduated: overshoot penalty + crossing cost
|
||||
max_reach = reachability_checker.arm_reach.get(arm_id, 2.0)
|
||||
overshoot = max(0.0, dist - max_reach)
|
||||
w = effective_weight * (HARD_MULTIPLIER if is_hard else 1.0)
|
||||
return w * overshoot * 10.0 + crossing_cost
|
||||
|
||||
return crossing_cost
|
||||
|
||||
if rule == "prefer_aligned":
|
||||
alignment_cost = sum(
|
||||
(1 - math.cos(4 * p.theta)) / 2 for p in placement_map.values()
|
||||
)
|
||||
if is_hard:
|
||||
if not graduated:
|
||||
return math.inf if alignment_cost > 1e-6 else 0.0
|
||||
return HARD_MULTIPLIER * effective_weight * alignment_cost
|
||||
return effective_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 effective_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 effective_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 evaluate_default_hard_constraints_breakdown(
|
||||
devices: list[Device],
|
||||
placements: list[Placement],
|
||||
lab: Lab,
|
||||
collision_checker: CollisionChecker,
|
||||
*,
|
||||
collision_weight: float = DEFAULT_WEIGHT_DISTANCE * HARD_MULTIPLIER,
|
||||
boundary_weight: float = DEFAULT_WEIGHT_DISTANCE * HARD_MULTIPLIER,
|
||||
) -> dict[str, float]:
|
||||
"""与 evaluate_default_hard_constraints 逻辑相同，但返回分项明细。"""
|
||||
device_map = {d.id: d for d in devices}
|
||||
collision_cost = 0.0
|
||||
boundary_cost = 0.0
|
||||
|
||||
candidate_pairs = sweep_and_prune_pairs(devices, placements)
|
||||
for i, j in candidate_pairs:
|
||||
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:
|
||||
collision_cost += collision_weight * depth
|
||||
|
||||
for p in placements:
|
||||
dev = device_map[p.device_id]
|
||||
hw, hd = p.rotated_bbox(dev)
|
||||
overshoot = 0.0
|
||||
overshoot += max(0.0, hw - p.x)
|
||||
overshoot += max(0.0, (p.x + hw) - lab.width)
|
||||
overshoot += max(0.0, hd - p.y)
|
||||
overshoot += max(0.0, (p.y + hd) - lab.depth)
|
||||
boundary_cost += boundary_weight * overshoot
|
||||
|
||||
return {
|
||||
"collision": collision_cost,
|
||||
"boundary": boundary_cost,
|
||||
"total": collision_cost + boundary_cost,
|
||||
"collision_weight": collision_weight,
|
||||
"boundary_weight": boundary_weight,
|
||||
}
|
||||
|
||||
|
||||
def evaluate_constraints_breakdown(
|
||||
devices: list[Device],
|
||||
placements: list[Placement],
|
||||
lab: Lab,
|
||||
constraints: list[Constraint],
|
||||
collision_checker: CollisionChecker,
|
||||
reachability_checker: ReachabilityChecker | None = None,
|
||||
) -> list[dict[str, Any]]:
|
||||
"""与 evaluate_constraints 逻辑相同，但返回每条约束的分项明细。"""
|
||||
device_map = {d.id: d for d in devices}
|
||||
placement_map = {p.device_id: p for p in placements}
|
||||
|
||||
results = []
|
||||
for c in constraints:
|
||||
cost = _evaluate_single(
|
||||
c, device_map, placement_map, lab, collision_checker, reachability_checker,
|
||||
graduated=True,
|
||||
)
|
||||
results.append({
|
||||
"name": _constraint_display_name(c),
|
||||
"rule": c.rule_name,
|
||||
"type": c.type,
|
||||
"cost": cost,
|
||||
"weight": c.weight,
|
||||
})
|
||||
return results
|
||||
|
||||
|
||||
def _missing_reference_cost(
|
||||
constraint: Constraint,
|
||||
placement_map: dict[str, Placement],
|
||||
*device_ids: str,
|
||||
) -> float | None:
|
||||
"""当约束引用不存在的设备时返回对应 cost。"""
|
||||
missing = sorted({device_id for device_id in device_ids if device_id not in placement_map})
|
||||
if not missing:
|
||||
return None
|
||||
|
||||
logger.warning(
|
||||
"Constraint %s references missing device IDs: %s",
|
||||
constraint.rule_name,
|
||||
", ".join(missing),
|
||||
)
|
||||
if constraint.type == "hard":
|
||||
return math.inf
|
||||
return 0.0
|
||||
|
||||
|
||||
def _constraint_display_name(c: Constraint) -> str:
|
||||
"""为约束生成可读的显示名称。"""
|
||||
params = c.params
|
||||
if c.rule_name in (
|
||||
"distance_less_than", "distance_greater_than",
|
||||
"minimize_distance", "maximize_distance",
|
||||
):
|
||||
return f"{c.rule_name}({params.get('device_a', '?')}, {params.get('device_b', '?')})"
|
||||
if c.rule_name == "reachability":
|
||||
return f"reachability({params.get('arm_id', '?')}, {params.get('target_device_id', '?')})"
|
||||
if c.rule_name == "min_spacing":
|
||||
return f"min_spacing(gap={params.get('min_gap', '?')})"
|
||||
if c.rule_name == "prefer_orientation_mode":
|
||||
return f"prefer_orientation_mode({params.get('mode', '?')})"
|
||||
return c.rule_name
|
||||
|
||||
|
||||
def _crossing_penalty(
|
||||
opening_pt: tuple[float, float],
|
||||
arm_nearest_pt: tuple[float, float],
|
||||
arm_id: str,
|
||||
target_id: str,
|
||||
device_map: dict[str, Device],
|
||||
placement_map: dict[str, Placement],
|
||||
) -> float:
|
||||
"""交叉惩罚：其他设备 OBB 遮挡 opening→arm 路径的长度加权 penalty。
|
||||
|
||||
Soft penalty，权重 = DEFAULT_WEIGHT_DISTANCE * 穿过各遮挡设备 OBB 的线段长度之和。
|
||||
始终生效（不论可达性是否通过），为 DE 提供清晰的梯度信号。
|
||||
"""
|
||||
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)
|
||||
crossing_len = segment_obb_intersection_length(opening_pt, arm_nearest_pt, corners)
|
||||
cost += DEFAULT_WEIGHT_DISTANCE * crossing_len
|
||||
return cost
|
||||
@@ -1,302 +0,0 @@
|
||||
"""双源设备目录：从 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
|
||||
|
||||
from collections import Counter
|
||||
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 = []
|
||||
catalog_counts = Counter(spec["id"] for spec in device_specs)
|
||||
catalog_seen: Counter[str] = Counter()
|
||||
|
||||
for spec in device_specs:
|
||||
catalog_id = spec["id"]
|
||||
catalog_seen[catalog_id] += 1
|
||||
instance_idx = catalog_seen[catalog_id]
|
||||
if catalog_counts[catalog_id] > 1 and instance_idx > 1:
|
||||
dev_id = f"{catalog_id}#{instance_idx}"
|
||||
else:
|
||||
dev_id = 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,
|
||||
uuid=spec.get("uuid", ""),
|
||||
)
|
||||
)
|
||||
return devices
|
||||
@@ -1,559 +0,0 @@
|
||||
"""从 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()
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,187 +0,0 @@
|
||||
"""
|
||||
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()
|
||||
@@ -1,373 +0,0 @@
|
||||
"""意图解释器：将语义化意图翻译为 Constraint 列表。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import itertools
|
||||
from collections.abc import Callable
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from .constraints import PRIORITY_MULTIPLIERS
|
||||
from .models import Constraint, Intent
|
||||
|
||||
# 优先级权重映射
|
||||
_PRIORITY_WEIGHTS: dict[str, float] = {"low": 1.0, "medium": 3.0, "high": 8.0}
|
||||
_DEFAULT_WEIGHT = _PRIORITY_WEIGHTS["medium"]
|
||||
|
||||
|
||||
def _priority_key(priority: str) -> str:
|
||||
"""将 intent priority 映射到 constraint 权重等级。"""
|
||||
return "normal" if priority == "medium" else priority
|
||||
|
||||
|
||||
def _final_weight(base_weight: float, priority: str) -> float:
|
||||
"""在解释阶段直接烘焙优先级乘数。"""
|
||||
return base_weight * PRIORITY_MULTIPLIERS.get(priority, 1.0)
|
||||
|
||||
|
||||
@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},
|
||||
weight=_final_weight(1.0, "critical"),
|
||||
)
|
||||
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 = _final_weight(
|
||||
_PRIORITY_WEIGHTS.get(priority, _DEFAULT_WEIGHT),
|
||||
_priority_key(priority),
|
||||
)
|
||||
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 = _final_weight(
|
||||
_PRIORITY_WEIGHTS.get(priority, _DEFAULT_WEIGHT),
|
||||
_priority_key(priority),
|
||||
)
|
||||
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},
|
||||
weight=_final_weight(1.0, "normal"),
|
||||
)
|
||||
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},
|
||||
weight=_final_weight(1.0, "normal"),
|
||||
)
|
||||
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},
|
||||
weight=_final_weight(1.0, "high"),
|
||||
)
|
||||
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"},
|
||||
weight=_final_weight(1.0, "low"),
|
||||
)
|
||||
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"},
|
||||
weight=_final_weight(1.0, "low"),
|
||||
)
|
||||
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={},
|
||||
weight=_final_weight(1.0, "low"),
|
||||
)
|
||||
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_keep_adjacent(intent: Intent, result: InterpretResult) -> None:
|
||||
"""keep_adjacent：两个设备保持相邻（同 close_together 逻辑，支持 priority 映射）。"""
|
||||
devices: list[str] = intent.params.get("devices", [])
|
||||
priority: str = intent.params.get("priority", "medium")
|
||||
|
||||
if len(devices) < 2:
|
||||
result.errors.append(f"keep_adjacent: 参数 'devices' 至少需要 2 个设备，当前 {len(devices)} 个")
|
||||
return
|
||||
|
||||
weight = _final_weight(
|
||||
_PRIORITY_WEIGHTS.get(priority, _DEFAULT_WEIGHT),
|
||||
_priority_key(priority),
|
||||
)
|
||||
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_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},
|
||||
weight=_final_weight(1.0, "normal"),
|
||||
)
|
||||
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,
|
||||
"keep_adjacent": _handle_keep_adjacent,
|
||||
"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
|
||||
@@ -1,41 +0,0 @@
|
||||
"""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 如果可达
|
||||
"""
|
||||
...
|
||||
@@ -1,49 +0,0 @@
|
||||
"""解析实验室平面图 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)
|
||||
@@ -1,187 +0,0 @@
|
||||
# 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`: generally not needed. Cardinal alignment is handled by the `align_cardinal` intent (generates `prefer_aligned` constraint). Do NOT use `align_weight` in seeder_overrides — it is deprecated.
|
||||
- `snap_cardinal`: `false` (default). Set `true` only if user explicitly requests snapping to 0/90/180/270.
|
||||
- `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
|
||||
```
|
||||
@@ -1,312 +0,0 @@
|
||||
# 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)
|
||||
Priority is only part of the intent input. The interpreter automatically bakes it into the emitted constraint `weight`; there is no separate constraint-level `priority` field in `/interpret` output or `/optimize` input.
|
||||
|
||||
### `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.
|
||||
|
||||
### `keep_adjacent` — Devices should stay adjacent
|
||||
```json
|
||||
{
|
||||
"intent": "keep_adjacent",
|
||||
"params": {
|
||||
"devices": ["device_a", "device_b"],
|
||||
"priority": "high"
|
||||
}
|
||||
}
|
||||
```
|
||||
**When to use:** User explicitly asks for a pair or group to stay side-by-side / adjacent. This currently maps to the same optimizer behavior as `close_together`, but is semantically more precise.
|
||||
|
||||
### `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.
|
||||
|
||||
### Duplicate Device Convention
|
||||
|
||||
When the same catalog device appears multiple times in the scene:
|
||||
|
||||
- first instance keeps the bare catalog ID, e.g. `plate_reader`
|
||||
- second and later instances use `#N`, e.g. `plate_reader#2`, `plate_reader#3`
|
||||
- a bare ID in an intent fans out to all instances
|
||||
- a suffixed ID applies only to that specific instance
|
||||
|
||||
Examples:
|
||||
|
||||
- `{"devices": ["plate_reader", "storage_hotel"]}` applies to every `plate_reader` instance
|
||||
- `{"devices": ["plate_reader#2", "storage_hotel"]}` applies only to the second instance
|
||||
|
||||
### 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 user explicitly says "keep these adjacent", "side by side", or "next to each other" → `keep_adjacent`
|
||||
- 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.
|
||||
@@ -1,110 +0,0 @@
|
||||
"""Mock 检测器：无 ROS 依赖的简化碰撞与可达性检测。
|
||||
|
||||
碰撞检测基于 OBB SAT（O(n²) 两两比较）。
|
||||
可达性检测基于最大臂展半径的欧几里得距离判断。
|
||||
|
||||
集成阶段由 ros_checkers.py 中的 MoveItCollisionChecker / IKFastReachabilityChecker 替代。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
from .obb import obb_corners, obb_overlap
|
||||
|
||||
|
||||
class MockCollisionChecker:
|
||||
"""基于 OBB SAT 的碰撞检测。
|
||||
|
||||
输入格式与 CollisionChecker Protocol 一致：
|
||||
placements: [{"id": str, "bbox": (w, d), "pos": (x, y, θ)}, ...]
|
||||
"""
|
||||
|
||||
def check(self, placements: list[dict]) -> list[tuple[str, str]]:
|
||||
"""返回所有碰撞的设备对。"""
|
||||
collisions: list[tuple[str, str]] = []
|
||||
n = len(placements)
|
||||
for i in range(n):
|
||||
for j in range(i + 1, n):
|
||||
a, b = placements[i], placements[j]
|
||||
corners_a = obb_corners(
|
||||
a["pos"][0], a["pos"][1],
|
||||
a["bbox"][0], a["bbox"][1],
|
||||
a["pos"][2] if len(a["pos"]) > 2 else 0.0,
|
||||
)
|
||||
corners_b = obb_corners(
|
||||
b["pos"][0], b["pos"][1],
|
||||
b["bbox"][0], b["bbox"][1],
|
||||
b["pos"][2] if len(b["pos"]) > 2 else 0.0,
|
||||
)
|
||||
if obb_overlap(corners_a, corners_b):
|
||||
collisions.append((a["id"], b["id"]))
|
||||
return collisions
|
||||
|
||||
def check_bounds(
|
||||
self, placements: list[dict], lab_width: float, lab_depth: float
|
||||
) -> list[str]:
|
||||
"""返回超出实验室边界的设备 ID 列表。"""
|
||||
out_of_bounds: list[str] = []
|
||||
for p in placements:
|
||||
hw, hd = self._rotated_half_extents(p)
|
||||
x, y = p["pos"][:2]
|
||||
if x - hw < 0 or x + hw > lab_width or y - hd < 0 or y + hd > lab_depth:
|
||||
out_of_bounds.append(p["id"])
|
||||
return out_of_bounds
|
||||
|
||||
@staticmethod
|
||||
def _rotated_half_extents(p: dict) -> tuple[float, float]:
|
||||
"""计算旋转后 AABB 的半宽和半深。"""
|
||||
w, d = p["bbox"]
|
||||
theta = p["pos"][2] if len(p["pos"]) > 2 else 0.0
|
||||
cos_t = abs(math.cos(theta))
|
||||
sin_t = abs(math.sin(theta))
|
||||
half_w = (w * cos_t + d * sin_t) / 2
|
||||
half_d = (w * sin_t + d * cos_t) / 2
|
||||
return half_w, half_d
|
||||
|
||||
|
||||
class MockReachabilityChecker:
|
||||
"""基于最大臂展半径的简化可达性判断。
|
||||
|
||||
内置常见 Elite CS 系列机械臂的臂展参数。
|
||||
自定义臂展可通过构造参数传入。
|
||||
"""
|
||||
|
||||
# 默认臂展参数（单位：米）
|
||||
DEFAULT_ARM_REACH: dict[str, float] = {
|
||||
"elite_cs63": 0.624,
|
||||
"elite_cs66": 0.914,
|
||||
"elite_cs612": 1.304,
|
||||
"elite_cs620": 1.800,
|
||||
"arm_slider": 0.3, # 线性导轨臂：1.07 body 2.14m × 0.35m，reach ≈ half length
|
||||
}
|
||||
|
||||
# 未知型号回退臂展：realistic default for lab-scale arms
|
||||
DEFAULT_FALLBACK_REACH: float = 0.4
|
||||
|
||||
def __init__(self, arm_reach: dict[str, float] | None = None):
|
||||
self.arm_reach = {**self.DEFAULT_ARM_REACH, **(arm_reach or {})}
|
||||
|
||||
def is_reachable(self, arm_id: str, arm_pose: dict, target: dict) -> bool:
|
||||
"""判断目标点是否在机械臂最大臂展半径内。
|
||||
|
||||
Uses OBB edge-to-edge distance when available (passed as _obb_dist),
|
||||
otherwise falls back to center-to-center Euclidean distance.
|
||||
|
||||
Args:
|
||||
arm_id: 机械臂型号 ID（用于查臂展）
|
||||
arm_pose: {"x": float, "y": float, "theta": float}
|
||||
target: {"x": float, "y": float, "z": float, "_obb_dist": float (optional)}
|
||||
|
||||
Returns:
|
||||
True 如果目标在臂展半径内
|
||||
"""
|
||||
max_reach = self.arm_reach.get(arm_id, self.DEFAULT_FALLBACK_REACH)
|
||||
if "_obb_dist" in target:
|
||||
return target["_obb_dist"] <= max_reach
|
||||
dx = target["x"] - arm_pose["x"]
|
||||
dy = target["y"] - arm_pose["y"]
|
||||
dist_sq = dx**2 + dy**2
|
||||
return dist_sq <= max_reach**2
|
||||
@@ -1,96 +0,0 @@
|
||||
"""数据模型定义：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 = ""
|
||||
uuid: 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 = "" # 可选的自然语言描述（用于审计/调试）
|
||||
@@ -1,257 +0,0 @@
|
||||
"""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 segment_obb_intersection_length(
|
||||
p1: tuple[float, float],
|
||||
p2: tuple[float, float],
|
||||
corners: list[tuple[float, float]],
|
||||
) -> float:
|
||||
"""线段 p1-p2 与 OBB（凸多边形）的交集长度。
|
||||
|
||||
Cyrus-Beck 线段裁剪算法。corners 假定为 CCW 顺序（obb_corners 生成）。
|
||||
无交集返回 0.0。
|
||||
"""
|
||||
dx = p2[0] - p1[0]
|
||||
dy = p2[1] - p1[1]
|
||||
seg_len_sq = dx * dx + dy * dy
|
||||
if seg_len_sq < 1e-24:
|
||||
return 0.0
|
||||
|
||||
t_enter = 0.0
|
||||
t_exit = 1.0
|
||||
n = len(corners)
|
||||
|
||||
for i in range(n):
|
||||
ax, ay = corners[i]
|
||||
bx, by = corners[(i + 1) % n]
|
||||
# CCW 多边形边的外法线: (ey, -ex), e = b - a
|
||||
ex, ey = bx - ax, by - ay
|
||||
nx, ny = ey, -ex
|
||||
|
||||
denom = nx * dx + ny * dy
|
||||
numer = nx * (p1[0] - ax) + ny * (p1[1] - ay)
|
||||
|
||||
if abs(denom) < 1e-12:
|
||||
if numer > 0:
|
||||
return 0.0 # 在此边外侧且平行
|
||||
continue
|
||||
|
||||
t = -numer / denom
|
||||
if denom < 0:
|
||||
t_enter = max(t_enter, t) # 进入
|
||||
else:
|
||||
t_exit = min(t_exit, t) # 退出
|
||||
|
||||
if t_enter > t_exit:
|
||||
return 0.0
|
||||
|
||||
if t_enter >= t_exit:
|
||||
return 0.0
|
||||
|
||||
return (t_exit - t_enter) * math.sqrt(seg_len_sq)
|
||||
|
||||
|
||||
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)
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,144 +0,0 @@
|
||||
"""初始布局生成：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
|
||||
@@ -1,29 +0,0 @@
|
||||
[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"]
|
||||
@@ -1,433 +0,0 @@
|
||||
"""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()
|
||||
@@ -1,331 +0,0 @@
|
||||
"""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)
|
||||
]
|
||||
@@ -1,742 +0,0 @@
|
||||
"""FastAPI 开发服务器。
|
||||
|
||||
开发阶段独立运行于 localhost:8000，前端通过 CORS 调用。
|
||||
集成阶段合并到 Uni-Lab-OS 的 FastAPI 服务中。
|
||||
|
||||
运行方式：
|
||||
uvicorn unilabos.layout_optimizer.server:app --host 0.0.0.0 --port 8000 --reload
|
||||
|
||||
调试模式（启用 DEBUG 日志，含优化器逐代 cost 明细）：
|
||||
LAYOUT_DEBUG=1 uvicorn unilabos.layout_optimizer.server:app --host 0.0.0.0 --port 8000 --reload
|
||||
|
||||
日志文件：
|
||||
自动写入 layout_optimizer/logs/{YYYYMMDD_HHMMSS}.log（始终 DEBUG 级别）。
|
||||
前端 1s 轮询的 GET /scene/placements 200 行不写入日志文件。
|
||||
|
||||
前端访问：
|
||||
http://localhost:8000/
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from collections import defaultdict
|
||||
import itertools
|
||||
import logging
|
||||
import logging.handlers
|
||||
import math
|
||||
import os
|
||||
from datetime import datetime
|
||||
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 .constraints import DEFAULT_WEIGHT_ANGLE # noqa: F401 — kept for external use
|
||||
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
|
||||
|
||||
_console_level = logging.DEBUG if os.getenv("LAYOUT_DEBUG") else logging.INFO
|
||||
# root logger must be DEBUG so the file handler receives all records;
|
||||
# console output level is controlled separately via its handler.
|
||||
logging.basicConfig(level=logging.DEBUG)
|
||||
# basicConfig creates a default StreamHandler — set its level to the console level
|
||||
for _h in logging.getLogger().handlers:
|
||||
if isinstance(_h, logging.StreamHandler):
|
||||
_h.setLevel(_console_level)
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
# --- 文件日志：实时写入 logs/ 目录，按启动时间命名 ---
|
||||
_LOG_DIR = Path(__file__).parent / "logs"
|
||||
_LOG_DIR.mkdir(exist_ok=True)
|
||||
_log_file = _LOG_DIR / f"{datetime.now():%Y%m%d_%H%M%S}.log"
|
||||
|
||||
|
||||
class _PollingFilter(logging.Filter):
|
||||
"""过滤掉前端 1s 轮询产生的 GET /scene/placements 日志行。"""
|
||||
|
||||
def filter(self, record: logging.LogRecord) -> bool:
|
||||
msg = record.getMessage()
|
||||
if "GET /scene/placements" in msg and "200" in msg:
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
_file_handler = logging.FileHandler(_log_file, encoding="utf-8")
|
||||
_file_handler.setLevel(logging.DEBUG)
|
||||
_file_handler.setFormatter(
|
||||
logging.Formatter("%(asctime)s %(levelname)-5s [%(name)s] %(message)s")
|
||||
)
|
||||
_file_handler.addFilter(_PollingFilter())
|
||||
logging.getLogger().addHandler(_file_handler)
|
||||
|
||||
STATIC_DIR = Path(__file__).parent / "static"
|
||||
|
||||
# 可配置路径
|
||||
# __file__ -> Uni-Lab-OS/unilabos/layout_optimizer/server.py
|
||||
_UNILABOS_DIR = Path(__file__).resolve().parent.parent # .../Uni-Lab-OS/unilabos/
|
||||
|
||||
UNI_LAB_ASSETS_DIR = Path(
|
||||
os.getenv("UNI_LAB_ASSETS_DIR", str(_UNILABOS_DIR.parent.parent.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(_UNILABOS_DIR / "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
|
||||
_DEVICE_PARAM_KEYS = {"device_a", "device_b", "arm_id", "target_device_id", "device"}
|
||||
|
||||
|
||||
# 消耗品/配件关键词（不独立放置于实验台）
|
||||
_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
|
||||
|
||||
|
||||
def _catalog_id_from_internal(device_id: str) -> str:
|
||||
"""内部实例 ID → catalog ID。"""
|
||||
return device_id.split("#", 1)[0]
|
||||
|
||||
|
||||
def _expand_constraints_for_duplicates(
|
||||
constraints: list[Constraint], devices: list,
|
||||
) -> list[Constraint]:
|
||||
"""将引用 bare catalog ID 的约束扩展到所有重复实例。"""
|
||||
catalog_instances: dict[str, list[str]] = defaultdict(list)
|
||||
for dev in devices:
|
||||
catalog_instances[_catalog_id_from_internal(dev.id)].append(dev.id)
|
||||
|
||||
expanded_constraints: list[Constraint] = []
|
||||
for constraint in constraints:
|
||||
fan_out_keys: list[str] = []
|
||||
fan_out_values: list[list[str]] = []
|
||||
|
||||
for key in _DEVICE_PARAM_KEYS:
|
||||
if key not in constraint.params:
|
||||
continue
|
||||
ref_id = constraint.params[key]
|
||||
if "#" in ref_id:
|
||||
continue
|
||||
instances = catalog_instances.get(ref_id, [])
|
||||
if len(instances) > 1:
|
||||
fan_out_keys.append(key)
|
||||
fan_out_values.append(instances)
|
||||
logger.info(
|
||||
"Fan-out: %s %s=%s -> %d instances",
|
||||
constraint.rule_name, key, ref_id, len(instances),
|
||||
)
|
||||
|
||||
if not fan_out_keys:
|
||||
expanded_constraints.append(constraint)
|
||||
continue
|
||||
|
||||
for combo in itertools.product(*fan_out_values):
|
||||
new_params = dict(constraint.params)
|
||||
for key, internal_id in zip(fan_out_keys, combo):
|
||||
new_params[key] = internal_id
|
||||
expanded_constraints.append(
|
||||
Constraint(
|
||||
type=constraint.type,
|
||||
rule_name=constraint.rule_name,
|
||||
params=new_params,
|
||||
weight=constraint.weight,
|
||||
)
|
||||
)
|
||||
|
||||
return expanded_constraints
|
||||
|
||||
|
||||
def _maybe_add_prefer_aligned_constraint(
|
||||
constraints: list[Constraint], align_weight: float,
|
||||
) -> list[Constraint]:
|
||||
"""仅在用户未显式提供 prefer_aligned 时注入对齐约束。"""
|
||||
if align_weight <= 0:
|
||||
return constraints
|
||||
|
||||
if any(c.rule_name == "prefer_aligned" for c in constraints):
|
||||
logger.info("Skipping auto-injected prefer_aligned because one already exists")
|
||||
return constraints
|
||||
|
||||
constraints.append(
|
||||
Constraint(
|
||||
type="soft",
|
||||
rule_name="prefer_aligned",
|
||||
weight=align_weight,
|
||||
)
|
||||
)
|
||||
return constraints
|
||||
|
||||
|
||||
# ---------- 路由 ----------
|
||||
|
||||
|
||||
@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",
|
||||
},
|
||||
"keep_adjacent": {
|
||||
"description": "Devices should stay adjacent, similar to close_together",
|
||||
"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",
|
||||
},
|
||||
"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
|
||||
snap_cardinal: bool = False
|
||||
angle_granularity: int | None = None
|
||||
arm_reach: dict[str, float] = {}
|
||||
# DE 超参数
|
||||
strategy: str = "currenttobest1bin"
|
||||
angle_mode: str = "joint"
|
||||
mutation: list[float] = [0.5, 1.0]
|
||||
theta_mutation: list[float] | None = None
|
||||
recombination: float = 0.7
|
||||
crossover_mode: str = "device"
|
||||
|
||||
|
||||
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, evaluate_constraints
|
||||
from .mock_checkers import MockCollisionChecker, MockReachabilityChecker
|
||||
from .optimizer import optimize, snap_theta, snap_theta_safe
|
||||
from .seeders import resolve_seeder_params, seed_layout
|
||||
|
||||
logger.info(
|
||||
"Optimize request: %d devices, lab %.1f×%.1f, %d constraints, seeder=%s, run_de=%s, angle_granularity=%s",
|
||||
len(request.devices),
|
||||
request.lab.width,
|
||||
request.lab.depth,
|
||||
len(request.constraints),
|
||||
request.seeder,
|
||||
request.run_de,
|
||||
request.angle_granularity,
|
||||
)
|
||||
|
||||
if request.angle_granularity not in (None, 4, 8, 12, 24):
|
||||
raise HTTPException(
|
||||
status_code=400,
|
||||
detail="angle_granularity must be one of: 4, 8, 12, 24",
|
||||
)
|
||||
|
||||
# 转换输入
|
||||
devices = create_devices_from_list(
|
||||
[d.model_dump() for d in request.devices]
|
||||
)
|
||||
id_to_catalog = {dev.id: _catalog_id_from_internal(dev.id) for dev in devices}
|
||||
id_to_uuid = {dev.id: (dev.uuid or dev.id) for dev in 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
|
||||
]
|
||||
constraints = _expand_constraints_for_duplicates(constraints, devices)
|
||||
|
||||
# 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 alignment soft constraint (opt-in via seeder_overrides)
|
||||
if request.run_de and seed_placements:
|
||||
# prefer_aligned: penalize non-cardinal angles（默认关闭，用户可通过 align_cardinal intent 或 seeder_overrides 开启）
|
||||
constraints = _maybe_add_prefer_aligned_constraint(
|
||||
constraints,
|
||||
request.seeder_overrides.get("align_weight", 0),
|
||||
)
|
||||
|
||||
# 4. Validate DE hyperparameters
|
||||
if request.strategy not in {"currenttobest1bin", "best1bin", "rand1bin"}:
|
||||
raise HTTPException(
|
||||
status_code=400,
|
||||
detail=f"strategy must be one of: currenttobest1bin, best1bin, rand1bin (got {request.strategy!r})",
|
||||
)
|
||||
if request.angle_mode not in {"joint", "hybrid"}:
|
||||
raise HTTPException(
|
||||
status_code=400,
|
||||
detail=f"angle_mode must be one of: joint, hybrid (got {request.angle_mode!r})",
|
||||
)
|
||||
if request.crossover_mode not in {"device", "dimension"}:
|
||||
raise HTTPException(
|
||||
status_code=400,
|
||||
detail=f"crossover_mode must be one of: device, dimension (got {request.crossover_mode!r})",
|
||||
)
|
||||
if len(request.mutation) != 2 or request.mutation[0] > request.mutation[1]:
|
||||
raise HTTPException(status_code=400, detail="mutation must be [F_min, F_max] with F_min <= F_max")
|
||||
if request.mutation[0] < 0 or request.mutation[1] > 2.0:
|
||||
raise HTTPException(status_code=400, detail="mutation values must be in [0, 2.0]")
|
||||
if request.theta_mutation is not None:
|
||||
if len(request.theta_mutation) != 2 or request.theta_mutation[0] > request.theta_mutation[1]:
|
||||
raise HTTPException(status_code=400, detail="theta_mutation must be [F_min, F_max] with F_min <= F_max")
|
||||
if request.theta_mutation[0] < 0 or request.theta_mutation[1] > 2.0:
|
||||
raise HTTPException(status_code=400, detail="theta_mutation values must be in [0, 2.0]")
|
||||
if not (0 <= request.recombination <= 1.0):
|
||||
raise HTTPException(status_code=400, detail="recombination must be in [0, 1.0]")
|
||||
|
||||
# 5. Conditional Differential Evolution
|
||||
de_ran = False
|
||||
checker = MockCollisionChecker()
|
||||
reachability_checker = MockReachabilityChecker(request.arm_reach or None)
|
||||
if request.run_de:
|
||||
result_placements = optimize(
|
||||
devices=devices,
|
||||
lab=lab,
|
||||
constraints=constraints,
|
||||
collision_checker=checker,
|
||||
reachability_checker=reachability_checker,
|
||||
seed_placements=seed_placements,
|
||||
maxiter=request.maxiter,
|
||||
seed=request.seed,
|
||||
strategy=request.strategy,
|
||||
workflow_edges=request.workflow_edges or None,
|
||||
angle_granularity=request.angle_granularity,
|
||||
angle_mode=request.angle_mode,
|
||||
mutation=tuple(request.mutation),
|
||||
theta_mutation=tuple(request.theta_mutation) if request.theta_mutation else None,
|
||||
recombination=request.recombination,
|
||||
crossover_mode=request.crossover_mode,
|
||||
)
|
||||
de_ran = True
|
||||
else:
|
||||
result_placements = seed_placements
|
||||
|
||||
# 5. θ snap post-processing（opt-in，默认关闭）
|
||||
if request.snap_cardinal and request.angle_granularity is None:
|
||||
result_placements = snap_theta_safe(result_placements, devices, lab, checker)
|
||||
elif request.snap_cardinal and request.angle_granularity is not None:
|
||||
logger.info(
|
||||
"snap_cardinal ignored because angle_granularity=%s already constrains theta",
|
||||
request.angle_granularity,
|
||||
)
|
||||
|
||||
# 6. Evaluate final cost (binary mode for pass/fail reporting)
|
||||
final_cost = evaluate_default_hard_constraints(
|
||||
devices, result_placements, lab, checker, graduated=False,
|
||||
)
|
||||
# 也检查用户硬约束（binary 模式）
|
||||
if constraints and not math.isinf(final_cost):
|
||||
user_hard_cost = evaluate_constraints(
|
||||
devices, result_placements, lab, constraints, checker, reachability_checker,
|
||||
graduated=False,
|
||||
)
|
||||
if math.isinf(user_hard_cost):
|
||||
final_cost = math.inf
|
||||
|
||||
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": []}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,7 +0,0 @@
|
||||
[
|
||||
{"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"}
|
||||
]
|
||||
@@ -1,7 +0,0 @@
|
||||
{
|
||||
"width": 5.0,
|
||||
"depth": 4.0,
|
||||
"obstacles": [
|
||||
{"x": 2.5, "y": 0.0, "width": 0.1, "depth": 0.5}
|
||||
]
|
||||
}
|
||||
@@ -1,241 +0,0 @@
|
||||
"""Tests for broad_phase.py — 2 轴 sweep-and-prune 宽相碰撞检测。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import random
|
||||
|
||||
import pytest
|
||||
|
||||
from ..broad_phase import broad_phase_device_pairs, sweep_and_prune_pairs
|
||||
from ..models import Device, Placement
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 测试用辅助函数
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def _make_device(device_id: str, w: float = 0.6, d: float = 0.4) -> Device:
|
||||
"""创建简单测试设备。"""
|
||||
return Device(id=device_id, name=device_id, bbox=(w, d))
|
||||
|
||||
|
||||
def _make_placement(
|
||||
device_id: str, x: float, y: float, theta: float = 0.0
|
||||
) -> Placement:
|
||||
"""创建简单测试放置。"""
|
||||
return Placement(device_id=device_id, x=x, y=y, theta=theta)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 测试类
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
class TestNoOverlap:
|
||||
"""两台设备距离足够远，宽相不返回候选对。"""
|
||||
|
||||
def test_no_overlap_returns_empty(self):
|
||||
"""水平方向间距远大于 AABB 尺寸 → 0 候选对。"""
|
||||
devices = [_make_device("A", 1.0, 1.0), _make_device("B", 1.0, 1.0)]
|
||||
placements = [
|
||||
_make_placement("A", 0.0, 0.0),
|
||||
_make_placement("B", 10.0, 0.0),
|
||||
]
|
||||
pairs = sweep_and_prune_pairs(devices, placements)
|
||||
assert pairs == []
|
||||
|
||||
|
||||
class TestOverlapping:
|
||||
"""两台设备 AABB 明显重叠。"""
|
||||
|
||||
def test_overlapping_devices_returned(self):
|
||||
"""两台 1×1 设备中心距 0.5m → 1 候选对。"""
|
||||
devices = [_make_device("A", 1.0, 1.0), _make_device("B", 1.0, 1.0)]
|
||||
placements = [
|
||||
_make_placement("A", 0.0, 0.0),
|
||||
_make_placement("B", 0.5, 0.0),
|
||||
]
|
||||
pairs = sweep_and_prune_pairs(devices, placements)
|
||||
assert len(pairs) == 1
|
||||
assert pairs[0] == (0, 1)
|
||||
|
||||
|
||||
class TestXOverlapYNoOverlap:
|
||||
"""x 轴投影交叠但 y 轴不交叠，应被 y 轴检查剪枝。"""
|
||||
|
||||
def test_x_overlap_y_no_overlap(self):
|
||||
"""水平接近但垂直方向偏移足够大 → 0 候选对。"""
|
||||
devices = [_make_device("A", 2.0, 1.0), _make_device("B", 2.0, 1.0)]
|
||||
placements = [
|
||||
_make_placement("A", 0.0, 0.0),
|
||||
_make_placement("B", 0.5, 5.0), # x 轴交叠但 y 轴相距很远
|
||||
]
|
||||
pairs = sweep_and_prune_pairs(devices, placements)
|
||||
assert pairs == []
|
||||
|
||||
|
||||
class TestTouchingDevices:
|
||||
"""AABB 恰好接触（边缘距离 = 0）应作为候选对返回。"""
|
||||
|
||||
def test_touching_devices_included(self):
|
||||
"""两个 1×1 设备中心距恰好为 1.0（半宽 0.5 + 0.5 = 1.0），
|
||||
AABB 边界接触 → 应包含在候选对中（<= 判定）。"""
|
||||
devices = [_make_device("A", 1.0, 1.0), _make_device("B", 1.0, 1.0)]
|
||||
placements = [
|
||||
_make_placement("A", 0.0, 0.0),
|
||||
_make_placement("B", 1.0, 0.0), # xmax_A = 0.5, xmin_B = 0.5 → 接触
|
||||
]
|
||||
pairs = sweep_and_prune_pairs(devices, placements)
|
||||
# 接触算作潜在碰撞，安全起见需保留
|
||||
assert len(pairs) == 1
|
||||
assert pairs[0] == (0, 1)
|
||||
|
||||
|
||||
class TestMultipleDevices:
|
||||
"""4 台设备验证精确的候选对列表。"""
|
||||
|
||||
def test_multiple_devices_correct_pairs(self):
|
||||
"""排列 4 台设备，只有特定配对 AABB 交叠。
|
||||
|
||||
布局（1×1 设备）：
|
||||
A(0,0) B(0.8,0) — A-B 交叠（中心距 0.8 < 1.0）
|
||||
C(0,5) — 远离 A、B
|
||||
D(0.9,5) — C-D 交叠（中心距 0.9 < 1.0）
|
||||
|
||||
期望候选对: (A,B) 和 (C,D)。
|
||||
"""
|
||||
devices = [
|
||||
_make_device("A", 1.0, 1.0),
|
||||
_make_device("B", 1.0, 1.0),
|
||||
_make_device("C", 1.0, 1.0),
|
||||
_make_device("D", 1.0, 1.0),
|
||||
]
|
||||
placements = [
|
||||
_make_placement("A", 0.0, 0.0),
|
||||
_make_placement("B", 0.8, 0.0),
|
||||
_make_placement("C", 0.0, 5.0),
|
||||
_make_placement("D", 0.9, 5.0),
|
||||
]
|
||||
pairs = sweep_and_prune_pairs(devices, placements)
|
||||
pair_set = set(pairs)
|
||||
assert (0, 1) in pair_set # A-B
|
||||
assert (2, 3) in pair_set # C-D
|
||||
assert len(pair_set) == 2
|
||||
|
||||
|
||||
class TestRotatedDeviceAabb:
|
||||
"""旋转设备导致 AABB 变大，命中候选对。"""
|
||||
|
||||
def test_rotated_device_aabb(self):
|
||||
"""一台窄长设备 (2.0×0.2)：
|
||||
- 未旋转时 AABB 半宽 = 1.0，两设备中心距 2.5 → 不交叠
|
||||
- 旋转 90° 后 AABB 半宽 = 0.1，半深 = 1.0 → 仍不交叠
|
||||
- 旋转 45° 后 AABB 半宽 ≈ (2*cos45 + 0.2*sin45)/2 ≈ 0.778
|
||||
但另一台放在 x=1.6，半宽 = 1.0
|
||||
所以 xmax_A = 0 + 0.778 = 0.778 < 1.6 - 1.0 = 0.6 → 不够
|
||||
|
||||
更好的方案：用中心距 1.5，未旋转时不交叠，旋转后交叠。
|
||||
未旋转: half_w_A = 0.3 (bbox 0.6x2.0), half_w_B = 0.3
|
||||
A: xmax = 0 + 0.3 = 0.3, B: xmin = 1.5 - 0.3 = 1.2 → 不交叠
|
||||
旋转 90°: A 的 bbox (0.6, 2.0) → half_w = (0.6*0 + 2.0*1)/2 = 1.0
|
||||
A: xmax = 0 + 1.0 = 1.0, B: xmin = 1.5 - 0.3 = 1.2 → 仍不交叠
|
||||
|
||||
用 bbox (0.4, 2.0)，间距 1.2：
|
||||
未旋转: half_w = 0.2, xmax_A = 0.2, xmin_B = 1.2 - 0.2 = 1.0 → 不交叠
|
||||
旋转 45°: half_w = (0.4*cos45 + 2.0*sin45)/2 = (0.283+1.414)/2 = 0.849
|
||||
xmax_A = 0.849, xmin_B = 1.2 - 0.2 = 1.0 → 不交叠
|
||||
|
||||
间距 0.8：
|
||||
未旋转: xmax_A = 0.2, xmin_B = 0.8 - 0.2 = 0.6 → 不交叠 ✓
|
||||
旋转 45°: xmax_A = 0.849, xmin_B = 0.6 → 交叠 ✓ (0.849 > 0.6)
|
||||
y 轴: half_d_A_rot = (0.4*sin45 + 2.0*cos45)/2 = 0.849, half_d_B = 1.0
|
||||
ymax_A = 0.849, ymin_B = -1.0 → 交叠 ✓
|
||||
"""
|
||||
dev_narrow = _make_device("narrow", 0.4, 2.0)
|
||||
dev_normal = _make_device("normal", 0.4, 2.0)
|
||||
# 未旋转：不交叠
|
||||
placements_no_rot = [
|
||||
_make_placement("narrow", 0.0, 0.0, theta=0.0),
|
||||
_make_placement("normal", 0.8, 0.0, theta=0.0),
|
||||
]
|
||||
assert sweep_and_prune_pairs([dev_narrow, dev_normal], placements_no_rot) == []
|
||||
|
||||
# narrow 旋转 45° → AABB 变大 → 交叠
|
||||
placements_rot = [
|
||||
_make_placement("narrow", 0.0, 0.0, theta=math.pi / 4),
|
||||
_make_placement("normal", 0.8, 0.0, theta=0.0),
|
||||
]
|
||||
pairs = sweep_and_prune_pairs([dev_narrow, dev_normal], placements_rot)
|
||||
assert len(pairs) == 1
|
||||
|
||||
|
||||
class TestOriginalIndices:
|
||||
"""验证返回的索引对应 placements 原始顺序而非排序后顺序。"""
|
||||
|
||||
def test_sorted_output_preserves_original_indices(self):
|
||||
"""故意让 placements 按 x 坐标逆序排列，
|
||||
验证返回的索引仍是原始顺序。"""
|
||||
devices = [
|
||||
_make_device("A", 1.0, 1.0),
|
||||
_make_device("B", 1.0, 1.0),
|
||||
_make_device("C", 1.0, 1.0),
|
||||
]
|
||||
# 逆序排列：C 在最左，A 在最右
|
||||
placements = [
|
||||
_make_placement("A", 5.0, 0.0), # idx 0, 最右
|
||||
_make_placement("B", 4.5, 0.0), # idx 1, 中间（与 A 交叠）
|
||||
_make_placement("C", 0.0, 0.0), # idx 2, 最左（独立）
|
||||
]
|
||||
pairs = sweep_and_prune_pairs(devices, placements)
|
||||
# A(idx=0) 和 B(idx=1) AABB 交叠，索引应为 (0, 1)
|
||||
assert len(pairs) == 1
|
||||
assert pairs[0] == (0, 1)
|
||||
|
||||
# 同时验证 broad_phase_device_pairs 返回正确 device_id
|
||||
id_pairs = broad_phase_device_pairs(devices, placements)
|
||||
assert id_pairs == [("A", "B")]
|
||||
|
||||
|
||||
class TestPairCountReduction:
|
||||
"""大规模随机测试：宽相候选对数应远小于 N*(N-1)/2。"""
|
||||
|
||||
def test_pair_count_reduction(self):
|
||||
"""N=15 台设备随机放置在 10×10 实验室 → 候选对数显著少于全量。"""
|
||||
random.seed(42)
|
||||
n = 15
|
||||
devices = [_make_device(f"D{i}", 0.5, 0.5) for i in range(n)]
|
||||
placements = [
|
||||
_make_placement(f"D{i}", random.uniform(0, 10), random.uniform(0, 10))
|
||||
for i in range(n)
|
||||
]
|
||||
pairs = sweep_and_prune_pairs(devices, placements)
|
||||
full_pairs = n * (n - 1) // 2 # = 105
|
||||
# 在 10×10 区域放 15 台 0.5×0.5 设备，交叠率应很低
|
||||
assert len(pairs) < full_pairs
|
||||
# 额外断言：候选对数不超过全量的一半（保守判定）
|
||||
assert len(pairs) < full_pairs * 0.5
|
||||
|
||||
|
||||
class TestEdgeCases:
|
||||
"""边界情况。"""
|
||||
|
||||
def test_empty_input(self):
|
||||
"""空列表 → 空结果。"""
|
||||
assert sweep_and_prune_pairs([], []) == []
|
||||
|
||||
def test_single_device(self):
|
||||
"""单台设备 → 无候选对。"""
|
||||
devices = [_make_device("A")]
|
||||
placements = [_make_placement("A", 0.0, 0.0)]
|
||||
assert sweep_and_prune_pairs(devices, placements) == []
|
||||
|
||||
def test_identical_positions(self):
|
||||
"""两台设备完全重叠 → 1 候选对。"""
|
||||
devices = [_make_device("A", 1.0, 1.0), _make_device("B", 1.0, 1.0)]
|
||||
placements = [
|
||||
_make_placement("A", 0.0, 0.0),
|
||||
_make_placement("B", 0.0, 0.0),
|
||||
]
|
||||
pairs = sweep_and_prune_pairs(devices, placements)
|
||||
assert len(pairs) == 1
|
||||
@@ -1,435 +0,0 @@
|
||||
"""Regression tests for V2 Stage 1 bugfixes.
|
||||
|
||||
Covers:
|
||||
- Duplicate device ID stacking (catalog ID + #N 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})]
|
||||
# graduated=True (default): 返回有限惩罚
|
||||
cost = evaluate_constraints(devices, stacked, lab, constraints,
|
||||
MockCollisionChecker())
|
||||
assert cost > 0
|
||||
assert not math.isinf(cost)
|
||||
# graduated=False: binary inf
|
||||
cost_binary = evaluate_constraints(devices, stacked, lab, constraints,
|
||||
MockCollisionChecker(),
|
||||
graduated=False)
|
||||
assert math.isinf(cost_binary)
|
||||
|
||||
def test_create_devices_uses_catalog_id_with_suffixes(self):
|
||||
"""create_devices_from_list should keep catalog IDs and suffix duplicates."""
|
||||
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 == "opentrons_liquid_handler"
|
||||
assert devices[1].id == "opentrons_liquid_handler#2"
|
||||
assert devices[0].uuid == "abc-123"
|
||||
assert devices[1].uuid == "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
|
||||
|
||||
|
||||
# ── V2 Stage 1: 默认关闭 cardinal snap/alignment ────────
|
||||
|
||||
class TestV2Stage1Bugfixes:
|
||||
"""align_weight 默认为 0，snap_cardinal 默认关闭。"""
|
||||
|
||||
def test_default_align_weight_is_zero(self):
|
||||
"""Default request (no seeder_overrides) 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",
|
||||
"run_de": True,
|
||||
"maxiter": 50,
|
||||
"seed": 42,
|
||||
})
|
||||
assert resp.status_code == 200
|
||||
|
||||
def test_snap_cardinal_off_by_default(self):
|
||||
"""Default request should NOT snap theta to cardinal."""
|
||||
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",
|
||||
"run_de": True,
|
||||
"maxiter": 10,
|
||||
"seed": 42,
|
||||
})
|
||||
assert resp.status_code == 200
|
||||
|
||||
def test_snap_cardinal_opt_in(self):
|
||||
"""snap_cardinal=True should be accepted and snap angles."""
|
||||
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",
|
||||
"snap_cardinal": True,
|
||||
"run_de": True,
|
||||
"maxiter": 10,
|
||||
"seed": 42,
|
||||
})
|
||||
assert resp.status_code == 200
|
||||
@@ -1,505 +0,0 @@
|
||||
"""约束体系测试。"""
|
||||
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from ..constraints import (
|
||||
_crossing_penalty,
|
||||
_opening_surface_center,
|
||||
DEFAULT_WEIGHT_DISTANCE,
|
||||
evaluate_constraints,
|
||||
evaluate_default_hard_constraints,
|
||||
)
|
||||
from ..mock_checkers import MockCollisionChecker, MockReachabilityChecker
|
||||
from ..models import Constraint, Device, Opening, Placement, Lab
|
||||
from ..obb import nearest_point_on_obb, obb_corners
|
||||
|
||||
|
||||
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):
|
||||
"""硬距离约束违反：graduated模式返回有限惩罚，binary模式返回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()
|
||||
# graduated=True (default): 有限惩罚
|
||||
cost = evaluate_constraints(
|
||||
devices, placements, _make_lab(), constraints, checker
|
||||
)
|
||||
assert cost > 0
|
||||
assert not math.isinf(cost)
|
||||
# graduated=False: binary inf
|
||||
cost_binary = evaluate_constraints(
|
||||
devices, placements, _make_lab(), constraints, checker,
|
||||
graduated=False,
|
||||
)
|
||||
assert math.isinf(cost_binary)
|
||||
|
||||
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):
|
||||
"""可达性约束：目标超出臂展 — graduated返回有限惩罚，binary返回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})
|
||||
# graduated=True (default): 有限惩罚
|
||||
cost = evaluate_constraints(
|
||||
devices, placements, _make_lab(), constraints, checker, reachability
|
||||
)
|
||||
assert cost > 0
|
||||
assert not math.isinf(cost)
|
||||
# graduated=False: binary inf
|
||||
cost_binary = evaluate_constraints(
|
||||
devices, placements, _make_lab(), constraints, checker, reachability,
|
||||
graduated=False,
|
||||
)
|
||||
assert math.isinf(cost_binary)
|
||||
|
||||
|
||||
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)
|
||||
|
||||
|
||||
class TestGraduatedHardConstraints:
|
||||
"""graduated 模式下硬约束返回比例惩罚而非 inf。"""
|
||||
|
||||
def test_hard_reachability_graduated_finite(self):
|
||||
"""graduated=True: 硬可达性返回有限惩罚。"""
|
||||
devices = [
|
||||
Device(id="arm", name="Arm", bbox=(0.2, 0.2), device_type="articulation"),
|
||||
Device(id="t", name="Target", bbox=(0.5, 0.5)),
|
||||
]
|
||||
placements = [
|
||||
Placement("arm", 1.0, 1.0, 0.0),
|
||||
Placement("t", 4.0, 3.0, 0.0),
|
||||
]
|
||||
constraints = [
|
||||
Constraint(type="hard", rule_name="reachability",
|
||||
params={"arm_id": "arm", "target_device_id": "t"}, weight=1.0)
|
||||
]
|
||||
checker = MockCollisionChecker()
|
||||
reach = MockReachabilityChecker(arm_reach={"arm": 1.0})
|
||||
cost = evaluate_constraints(
|
||||
devices, placements, _make_lab(), constraints, checker, reach,
|
||||
graduated=True,
|
||||
)
|
||||
assert cost > 0
|
||||
assert not math.isinf(cost)
|
||||
|
||||
def test_hard_reachability_binary_inf(self):
|
||||
"""graduated=False: 硬可达性返回 inf。"""
|
||||
devices = [
|
||||
Device(id="arm", name="Arm", bbox=(0.2, 0.2), device_type="articulation"),
|
||||
Device(id="t", name="Target", bbox=(0.5, 0.5)),
|
||||
]
|
||||
placements = [
|
||||
Placement("arm", 1.0, 1.0, 0.0),
|
||||
Placement("t", 4.0, 3.0, 0.0),
|
||||
]
|
||||
constraints = [
|
||||
Constraint(type="hard", rule_name="reachability",
|
||||
params={"arm_id": "arm", "target_device_id": "t"}, weight=1.0)
|
||||
]
|
||||
checker = MockCollisionChecker()
|
||||
reach = MockReachabilityChecker(arm_reach={"arm": 1.0})
|
||||
cost = evaluate_constraints(
|
||||
devices, placements, _make_lab(), constraints, checker, reach,
|
||||
graduated=False,
|
||||
)
|
||||
assert math.isinf(cost)
|
||||
|
||||
def test_hard_min_spacing_graduated_sums_all_pairs(self):
|
||||
"""graduated模式：min_spacing 对所有违规对求和（不只第一对）。"""
|
||||
devices = [
|
||||
Device(id="a", name="A", bbox=(0.5, 0.5)),
|
||||
Device(id="b", name="B", bbox=(0.5, 0.5)),
|
||||
Device(id="c", name="C", bbox=(0.5, 0.5)),
|
||||
]
|
||||
# 三个设备间距都小于 min_gap=1.0
|
||||
placements = [
|
||||
Placement("a", 1.0, 2.0, 0.0),
|
||||
Placement("b", 1.3, 2.0, 0.0), # OBB 边缘距 a 约 0.3
|
||||
Placement("c", 1.6, 2.0, 0.0), # OBB 边缘距 b 约 0.3, 距 a 约 0.6
|
||||
]
|
||||
constraints = [
|
||||
Constraint(type="hard", rule_name="min_spacing",
|
||||
params={"min_gap": 1.0}, weight=1.0)
|
||||
]
|
||||
checker = MockCollisionChecker()
|
||||
cost = evaluate_constraints(
|
||||
devices, placements, _make_lab(), constraints, checker,
|
||||
graduated=True,
|
||||
)
|
||||
# 应大于 0 且有限（累加多对违规）
|
||||
assert cost > 0
|
||||
assert not math.isinf(cost)
|
||||
|
||||
def test_hard_min_spacing_binary_inf(self):
|
||||
"""graduated=False: min_spacing 违规返回 inf。"""
|
||||
devices = _make_devices()
|
||||
placements = [
|
||||
Placement("a", 1.0, 2.0, 0.0),
|
||||
Placement("b", 1.3, 2.0, 0.0),
|
||||
]
|
||||
constraints = [
|
||||
Constraint(type="hard", rule_name="min_spacing",
|
||||
params={"min_gap": 1.0}, weight=1.0)
|
||||
]
|
||||
checker = MockCollisionChecker()
|
||||
cost = evaluate_constraints(
|
||||
devices, placements, _make_lab(), constraints, checker,
|
||||
graduated=False,
|
||||
)
|
||||
assert math.isinf(cost)
|
||||
|
||||
def test_hard_distance_less_than_graduated(self):
|
||||
"""graduated模式：distance_less_than 硬约束返回比例惩罚。"""
|
||||
devices = _make_devices()
|
||||
placements = [
|
||||
Placement("a", 1.0, 2.0, 0.0),
|
||||
Placement("b", 4.0, 2.0, 0.0),
|
||||
]
|
||||
constraints = [
|
||||
Constraint(type="hard", rule_name="distance_less_than",
|
||||
params={"device_a": "a", "device_b": "b", "distance": 0.5},
|
||||
weight=2.0)
|
||||
]
|
||||
checker = MockCollisionChecker()
|
||||
cost = evaluate_constraints(
|
||||
devices, placements, _make_lab(), constraints, checker,
|
||||
graduated=True,
|
||||
)
|
||||
# HARD_MULTIPLIER(5) × weight(2) × overshoot > 0
|
||||
assert cost > 0
|
||||
assert not math.isinf(cost)
|
||||
|
||||
def test_graduated_default_is_true(self):
|
||||
"""不传 graduated 参数时默认使用 graduated 模式。"""
|
||||
devices = _make_devices()
|
||||
placements = [
|
||||
Placement("a", 1.0, 2.0, 0.0),
|
||||
Placement("b", 4.0, 2.0, 0.0),
|
||||
]
|
||||
constraints = [
|
||||
Constraint(type="hard", rule_name="distance_less_than",
|
||||
params={"device_a": "a", "device_b": "b", "distance": 0.5},
|
||||
weight=1.0)
|
||||
]
|
||||
checker = MockCollisionChecker()
|
||||
# 不指定 graduated — 默认应为 True → 有限惩罚
|
||||
cost = evaluate_constraints(
|
||||
devices, placements, _make_lab(), constraints, checker,
|
||||
)
|
||||
assert not math.isinf(cost)
|
||||
|
||||
|
||||
class TestCrossingPenalty:
|
||||
"""_crossing_penalty: 交叉长度加权的 soft penalty。"""
|
||||
|
||||
def _make_device(self, dev_id, bbox=(0.5, 0.5), direction=(0.0, -1.0)):
|
||||
return Device(
|
||||
id=dev_id, name=dev_id, device_type="static",
|
||||
bbox=bbox, height=0.3,
|
||||
openings=[Opening(direction=direction, label="front")],
|
||||
)
|
||||
|
||||
def test_no_blockers_returns_zero(self):
|
||||
"""arm 与 target 之间无遮挡设备 → 交叉代价为 0。"""
|
||||
arm = self._make_device("arm", bbox=(2.14, 0.35))
|
||||
target = self._make_device("target")
|
||||
arm_p = Placement(device_id="arm", x=2.0, y=1.0, theta=0.0)
|
||||
target_p = Placement(device_id="target", x=0.5, y=1.0, theta=3.14159)
|
||||
device_map = {"arm": arm, "target": target}
|
||||
placement_map = {"arm": arm_p, "target": target_p}
|
||||
|
||||
opening_pt = _opening_surface_center(target, target_p)
|
||||
arm_corners = obb_corners(arm_p.x, arm_p.y, arm.bbox[0], arm.bbox[1], arm_p.theta)
|
||||
nearest = nearest_point_on_obb(opening_pt[0], opening_pt[1], arm_corners)
|
||||
|
||||
cost = _crossing_penalty(
|
||||
opening_pt, nearest,
|
||||
"arm", "target",
|
||||
device_map, placement_map,
|
||||
)
|
||||
assert cost == 0.0
|
||||
|
||||
def test_one_blocker_proportional_to_length(self):
|
||||
"""一个遮挡设备 → cost = DEFAULT_WEIGHT_DISTANCE * 穿过长度。"""
|
||||
arm = self._make_device("arm", bbox=(2.14, 0.35))
|
||||
target = self._make_device("target")
|
||||
blocker = self._make_device("blocker", bbox=(0.5, 0.5))
|
||||
arm_p = Placement(device_id="arm", x=3.0, y=1.0, theta=0.0)
|
||||
target_p = Placement(device_id="target", x=0.0, y=1.0, theta=0.0)
|
||||
blocker_p = Placement(device_id="blocker", x=1.5, y=1.0, theta=0.0)
|
||||
device_map = {"arm": arm, "target": target, "blocker": blocker}
|
||||
placement_map = {"arm": arm_p, "target": target_p, "blocker": blocker_p}
|
||||
|
||||
opening_pt = _opening_surface_center(target, target_p)
|
||||
arm_corners = obb_corners(arm_p.x, arm_p.y, arm.bbox[0], arm.bbox[1], arm_p.theta)
|
||||
nearest = nearest_point_on_obb(opening_pt[0], opening_pt[1], arm_corners)
|
||||
|
||||
cost = _crossing_penalty(
|
||||
opening_pt, nearest,
|
||||
"arm", "target",
|
||||
device_map, placement_map,
|
||||
)
|
||||
# blocker 宽 0.5m，theta=0，路径水平 → 穿过长度 ≈ 0.5m
|
||||
# cost = DEFAULT_WEIGHT_DISTANCE * 0.5 = 100 * 0.5 = 50
|
||||
assert cost > 0
|
||||
assert abs(cost - DEFAULT_WEIGHT_DISTANCE * 0.5) < DEFAULT_WEIGHT_DISTANCE * 0.1
|
||||
|
||||
def test_blocker_off_path_returns_zero(self):
|
||||
"""不在路径上的设备 → 交叉代价为 0。"""
|
||||
arm = self._make_device("arm", bbox=(2.14, 0.35))
|
||||
target = self._make_device("target")
|
||||
bystander = self._make_device("bystander", bbox=(0.5, 0.5))
|
||||
arm_p = Placement(device_id="arm", x=3.0, y=1.0, theta=0.0)
|
||||
target_p = Placement(device_id="target", x=0.0, y=1.0, theta=0.0)
|
||||
bystander_p = Placement(device_id="bystander", x=1.5, y=3.0, theta=0.0)
|
||||
device_map = {"arm": arm, "target": target, "bystander": bystander}
|
||||
placement_map = {"arm": arm_p, "target": target_p, "bystander": bystander_p}
|
||||
|
||||
opening_pt = _opening_surface_center(target, target_p)
|
||||
arm_corners = obb_corners(arm_p.x, arm_p.y, arm.bbox[0], arm.bbox[1], arm_p.theta)
|
||||
nearest = nearest_point_on_obb(opening_pt[0], opening_pt[1], arm_corners)
|
||||
|
||||
cost = _crossing_penalty(
|
||||
opening_pt, nearest,
|
||||
"arm", "target",
|
||||
device_map, placement_map,
|
||||
)
|
||||
assert cost == 0.0
|
||||
@@ -1,234 +0,0 @@
|
||||
"""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 中的真实尺寸
|
||||
|
||||
def test_duplicate_catalog_ids_use_suffixes_and_store_uuid(self):
|
||||
specs = [
|
||||
{"id": "opentrons_liquid_handler", "uuid": "u1"},
|
||||
{"id": "opentrons_liquid_handler", "uuid": "u2"},
|
||||
]
|
||||
devs = create_devices_from_list(specs)
|
||||
assert [dev.id for dev in devs] == [
|
||||
"opentrons_liquid_handler",
|
||||
"opentrons_liquid_handler#2",
|
||||
]
|
||||
assert [dev.uuid for dev in devs] == ["u1", "u2"]
|
||||
|
||||
|
||||
# ---------- 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"
|
||||
@@ -1,263 +0,0 @@
|
||||
"""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 now includes arm_slider in the default reach table
|
||||
(1.07m). Binary final evaluation checks all hard constraints including
|
||||
user-defined reachability.
|
||||
"""
|
||||
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": 100,
|
||||
"seed": 42,
|
||||
"snap_cardinal": True,
|
||||
"seeder_overrides": {"align_weight": 60},
|
||||
})
|
||||
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
|
||||
@@ -1,210 +0,0 @@
|
||||
"""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
|
||||
assert high.constraints[0].weight == pytest.approx(16.0)
|
||||
assert low.constraints[0].weight == pytest.approx(0.5)
|
||||
|
||||
|
||||
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"
|
||||
assert result.constraints[0].weight == pytest.approx(0.5)
|
||||
|
||||
|
||||
def test_keep_adjacent_generates_minimize_distance():
|
||||
result = interpret_intents([Intent(intent="keep_adjacent", params={
|
||||
"devices": ["opentrons_liquid_handler", "agilent_plateloc"],
|
||||
"priority": "high",
|
||||
})])
|
||||
assert len(result.constraints) == 1
|
||||
assert result.constraints[0].rule_name == "minimize_distance"
|
||||
assert result.constraints[0].weight == pytest.approx(16.0)
|
||||
|
||||
|
||||
# --- 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
|
||||
@@ -1,134 +0,0 @@
|
||||
"""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", "keep_adjacent",
|
||||
"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
|
||||
@@ -1,277 +0,0 @@
|
||||
"""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
|
||||
@@ -1,138 +0,0 @@
|
||||
"""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)
|
||||
@@ -1,156 +0,0 @@
|
||||
"""Tests for OBB (Oriented Bounding Box) geometry utilities."""
|
||||
import math
|
||||
import pytest
|
||||
from ..obb import obb_corners, obb_overlap, obb_min_distance, segment_obb_intersection_length
|
||||
|
||||
|
||||
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)
|
||||
|
||||
|
||||
class TestSegmentOBBIntersectionLength:
|
||||
"""segment_obb_intersection_length: Cyrus-Beck clipping."""
|
||||
|
||||
def test_segment_fully_outside(self):
|
||||
corners = obb_corners(0, 0, 2, 2, 0)
|
||||
length = segment_obb_intersection_length((-5, 3), (5, 3), corners)
|
||||
assert length == 0.0
|
||||
|
||||
def test_segment_fully_inside(self):
|
||||
corners = obb_corners(0, 0, 4, 4, 0)
|
||||
length = segment_obb_intersection_length((-0.5, 0), (0.5, 0), corners)
|
||||
assert abs(length - 1.0) < 1e-6
|
||||
|
||||
def test_segment_crosses_through(self):
|
||||
corners = obb_corners(0, 0, 2, 2, 0)
|
||||
length = segment_obb_intersection_length((-5, 0), (5, 0), corners)
|
||||
assert abs(length - 2.0) < 1e-6
|
||||
|
||||
def test_segment_partial_overlap(self):
|
||||
corners = obb_corners(0, 0, 2, 2, 0)
|
||||
length = segment_obb_intersection_length((0, 0), (5, 0), corners)
|
||||
assert abs(length - 1.0) < 1e-6
|
||||
|
||||
def test_rotated_obb(self):
|
||||
corners = obb_corners(0, 0, 2, 2, math.pi / 4)
|
||||
length = segment_obb_intersection_length((-3, 0), (3, 0), corners)
|
||||
expected = 2 * math.sqrt(2)
|
||||
assert abs(length - expected) < 1e-4
|
||||
|
||||
def test_zero_length_segment(self):
|
||||
corners = obb_corners(0, 0, 2, 2, 0)
|
||||
assert segment_obb_intersection_length((0, 0), (0, 0), corners) == 0.0
|
||||
|
||||
def test_parallel_outside(self):
|
||||
corners = obb_corners(0, 0, 2, 2, 0)
|
||||
length = segment_obb_intersection_length((-5, 2), (5, 2), corners)
|
||||
assert length == 0.0
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,430 +0,0 @@
|
||||
"""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)
|
||||
@@ -1,113 +0,0 @@
|
||||
"""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
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,5 +1,4 @@
|
||||
import json
|
||||
import os
|
||||
|
||||
# from nt import device_encoding
|
||||
import threading
|
||||
@@ -62,7 +61,7 @@ def main(
|
||||
rclpy.init(args=rclpy_init_args)
|
||||
else:
|
||||
logger.info("[ROS] rclpy already initialized, reusing context")
|
||||
executor = rclpy.__executor = MultiThreadedExecutor(num_threads=max(os.cpu_count() * 4, 48))
|
||||
executor = rclpy.__executor = MultiThreadedExecutor()
|
||||
# 创建主机节点
|
||||
host_node = HostNode(
|
||||
"host_node",
|
||||
@@ -123,7 +122,7 @@ def slave(
|
||||
rclpy.init(args=rclpy_init_args)
|
||||
executor = rclpy.__executor
|
||||
if not executor:
|
||||
executor = rclpy.__executor = MultiThreadedExecutor(num_threads=max(os.cpu_count() * 4, 48))
|
||||
executor = rclpy.__executor = MultiThreadedExecutor()
|
||||
|
||||
# 1.5 启动 executor 线程
|
||||
thread = threading.Thread(target=executor.spin, daemon=True, name="slave_executor_thread")
|
||||
|
||||
@@ -486,12 +486,18 @@ class BaseROS2DeviceNode(Node, Generic[T]):
|
||||
if len(rts.root_nodes) == 1 and parent_resource is not None:
|
||||
plr_instance = plr_instances[0]
|
||||
if isinstance(plr_instance, Plate):
|
||||
empty_liquid_info_in: List[Tuple[Optional[str], float]] = [(None, 0)] * plr_instance.num_items
|
||||
if len(ADD_LIQUID_TYPE) == 1 and len(LIQUID_VOLUME) == 1 and len(LIQUID_INPUT_SLOT) > 1:
|
||||
ADD_LIQUID_TYPE = ADD_LIQUID_TYPE * len(LIQUID_INPUT_SLOT)
|
||||
LIQUID_VOLUME = LIQUID_VOLUME * len(LIQUID_INPUT_SLOT)
|
||||
self.lab_logger().warning(
|
||||
f"增加液体资源时，数量为1，自动补全为 {len(LIQUID_INPUT_SLOT)} 个"
|
||||
)
|
||||
for liquid_type, liquid_volume, liquid_input_slot in zip(
|
||||
ADD_LIQUID_TYPE, LIQUID_VOLUME, LIQUID_INPUT_SLOT
|
||||
):
|
||||
empty_liquid_info_in[liquid_input_slot] = (liquid_type, liquid_volume)
|
||||
plr_instance.set_well_liquids(empty_liquid_info_in)
|
||||
try:
|
||||
# noinspection PyProtectedMember
|
||||
keys = list(plr_instance._ordering.keys())
|
||||
@@ -505,10 +511,6 @@ class BaseROS2DeviceNode(Node, Generic[T]):
|
||||
input_wells = []
|
||||
for r in LIQUID_INPUT_SLOT:
|
||||
input_wells.append(plr_instance.children[r])
|
||||
for input_well, liquid_type, liquid_volume, liquid_input_slot in zip(
|
||||
input_wells, ADD_LIQUID_TYPE, LIQUID_VOLUME, LIQUID_INPUT_SLOT
|
||||
):
|
||||
input_well.set_liquids([(liquid_type, liquid_volume, "uL")])
|
||||
final_response["liquid_input_resource_tree"] = ResourceTreeSet.from_plr_resources(
|
||||
input_wells
|
||||
).dump()
|
||||
@@ -1254,8 +1256,9 @@ class BaseROS2DeviceNode(Node, Generic[T]):
|
||||
return self._lab_logger
|
||||
|
||||
def create_ros_publisher(self, attr_name, msg_type, initial_period=5.0):
|
||||
"""创建ROS发布者。已在 status_types 中声明的属性直接创建；@topic_config 用于覆盖默认参数。"""
|
||||
topic_cfg = {}
|
||||
"""创建ROS发布者，仅当方法/属性有 @topic_config 装饰器时才创建。"""
|
||||
# 检测 @topic_config 装饰器配置
|
||||
topic_config = {}
|
||||
driver_class = type(self.driver_instance)
|
||||
|
||||
# 区分 @property 和普通方法两种情况
|
||||
@@ -1264,17 +1267,23 @@ class BaseROS2DeviceNode(Node, Generic[T]):
|
||||
)
|
||||
|
||||
if is_prop:
|
||||
# @property: 检测 fget 上的 @topic_config
|
||||
class_attr = getattr(driver_class, attr_name)
|
||||
if class_attr.fget is not None:
|
||||
topic_cfg = get_topic_config(class_attr.fget)
|
||||
topic_config = get_topic_config(class_attr.fget)
|
||||
else:
|
||||
# 普通方法: 直接检测 attr_name 方法上的 @topic_config
|
||||
if hasattr(self.driver_instance, attr_name):
|
||||
method = getattr(self.driver_instance, attr_name)
|
||||
if callable(method):
|
||||
topic_cfg = get_topic_config(method)
|
||||
topic_config = get_topic_config(method)
|
||||
|
||||
# 没有 @topic_config 装饰器则跳过发布
|
||||
if not topic_config:
|
||||
return
|
||||
|
||||
# 发布名称优先级: @topic_config(name=...) > get_ 前缀去除 > attr_name
|
||||
cfg_name = topic_cfg.get("name")
|
||||
cfg_name = topic_config.get("name")
|
||||
if cfg_name:
|
||||
publish_name = cfg_name
|
||||
elif attr_name.startswith("get_"):
|
||||
@@ -1282,10 +1291,10 @@ class BaseROS2DeviceNode(Node, Generic[T]):
|
||||
else:
|
||||
publish_name = attr_name
|
||||
|
||||
# @topic_config 参数覆盖默认值
|
||||
cfg_period = topic_cfg.get("period")
|
||||
cfg_print = topic_cfg.get("print_publish")
|
||||
cfg_qos = topic_cfg.get("qos")
|
||||
# 使用装饰器配置或默认值
|
||||
cfg_period = topic_config.get("period")
|
||||
cfg_print = topic_config.get("print_publish")
|
||||
cfg_qos = topic_config.get("qos")
|
||||
period: float = cfg_period if cfg_period is not None else initial_period
|
||||
print_publish: bool = cfg_print if cfg_print is not None else self._print_publish
|
||||
qos: int = cfg_qos if cfg_qos is not None else 10
|
||||
|
||||
@@ -1632,7 +1632,6 @@ class HostNode(BaseROS2DeviceNode):
|
||||
def manual_confirm(self, timeout_seconds: int, assignee_user_ids: list[str], **kwargs) -> dict:
|
||||
"""
|
||||
timeout_seconds: 超时时间（秒），默认3600秒
|
||||
修改的结果无效，是只读的
|
||||
"""
|
||||
return kwargs
|
||||
|
||||
|
||||
@@ -346,7 +346,7 @@ def refactor_data(
|
||||
"template_name": template_name,
|
||||
"resource_name": resource_name,
|
||||
"description": step.get("description", step.get("purpose", f"{operation} operation")),
|
||||
"lab_node_type": "ILab",
|
||||
"lab_node_type": "Device",
|
||||
"param": step.get("parameters", step.get("action_args", {})),
|
||||
"footer": f"{template_name}-{resource_name}",
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user