内容
# Panda-TAMP — 封闭环任务与运动规划
**PDDL 符号规划，与实时几何相结合，在模拟的 Franka Panda 上执行，具有规划器，能识别错误并重新规划。**
[](https://github.com/Manas-arumalla/panda-tamp/actions/workflows/ci.yml)
[](LICENSE)
我构建了这个项目，以回答大多数机器人规划演示回避的问题：**当符号计划遇到与其不符的几何形状时会发生什么？** 经典任务规划器可以轻松生成 `pick(cup); place(cup, tray)`，但不知道杯子是否可达、是否可抓取，或者托盘是否距离 2.5 米。大多数演示都隐藏了这一差距。这个项目使这一差距成为重点：规划器和几何引擎在封闭环中相互争论，直到它们同意一个既逻辑正确又物理可执行的计划 —— 或者系统诚实地报告没有这样的计划存在。
```mermaid
flowchart LR
subgraph 感知
CAM[RGB-D 相机] --> YOLO[YOLOv8 检测器] --> BP[深度反投影] --> WS[/世界状态/]
end
subgraph 规划["封闭 TAMP 环"]
WS --> PG[PDDL 问题生成器]
PG --> FD[快速下降<br/>符号规划]
FD --> GR[几何精炼器<br/>IK · 抓取采样 · 扫路径碰撞]
GR -- "不可行：伪造桥接谓词" --> PG
GR -- 可行 --> PLAN[/任务计划/]
end
subgraph 执行
PLAN --> EXE[MoveIt 2 / OMPL 执行器]
EXE --> GZ[Gazebo Harmonic<br/>物理抓取]
GZ -- "执行失败 -> 伪造 + 重新规划" --> PG
end
subgraph 语言["语言层（可选）"]
NL["自然语言命令"] --> GG[目标基础<br/>LLM 或语法] --> PG
MCP[MCP 服务器] --> PG
end
```
## 核心思想：桥接谓词
PDDL 计划纯粹是符号性的（`pick / place / stack / unstack`）。几何 —— 运动学、抓取、碰撞 —— 存在于一个单独的引擎中（pykin FK + 我编写的阻尼最小二乘 IK + trimesh/FCL 碰撞检查）。两层通过四个 **几何桥接谓词** 相遇：
`reachable(o)` · `graspable(o)` · `placeable(o, s)` · `stackable(o, o')`
符号规划器将它们视为普通事实，但它永远无法决定它们 —— 几何层决定。环路：
1. **乐观种子。** 每个桥接谓词首先为 `true`。
2. **符号规划。** Fast Downward 生成候选动作序列。
3. **几何精炼。** 对于每个动作：采样抓取、求解 IK、扫过执行器沿实际关节空间路径，并检查臂链接与场景 —— 而不仅仅是端点姿势。
4. **失败时，伪造和循环。** 相关的谓词在 PDDL 初始状态下设置为 `false`，规划器再次运行 —— 现在使用该事实。
5. **执行失败馈送相同的环路。** 在运行时滑脱或放置失败会伪造相同的谓词并触发从 *观察* 的世界状态（而不是假设的世界状态）重新规划。
我最喜欢的部分：符号规划器不需要几何知识，几何引擎不需要搜索。每个都做它擅长的事情，伪造协议是它们之间的整个接口。
<p align="center">
<img src="docs/images/plan_stack3.png" width="46%" alt="3 块堆叠场景的精炼 TAMP 计划">
<img src="docs/images/plan_pnp.png" width="46%" alt="杯子/托盘拾取和放置场景的精炼 TAMP 计划">
<br>
<em>直接从求解器渲染的精炼计划（<code>tamp-offline --viz</code>）：带有感知场景的编号末端执行器航点 —— 每个姿势都经过 IK、抓取采样和扫路径碰撞检查。</em>
</p>
### 诚实是一种设计约束
规划演示的一个常见的失败模式是悄悄报告成功。我在每一层都构建了相反的偏差：
- 重新规划基准测试断言重要的属性（块只能落在 **可达** 的托盘上），而不是脚本化的重新规划计数 —— 并且它包含一个反事实测试，证明求解器 **报告失败**，当没有可行的表面时，而不是发出一个无法到达的计划。
- Gazebo 执行器通过 TF 验证 **到达** （计划不是执行），在 **测量手指闭合** （带有打开手指的焊接不是抓取）上阻塞每个抓取，并针对模拟器地面实况运行 **掉落检测器**。
- 可靠性套件（`scripts/reliability_pnp.sh`）仅当计划完成 **且** Gazebo 的地面实况姿势显示每个物体物理上都在目标表面时，才将运行计为通过。仅凭日志解析不计。
## 内部
| 层 | 我构建的内容 |
|---|---|
| **符号** | 一个桌面 PDDL 领域（规范 `.pddl` + 运行时 `unified-planning` 模型保持同步）、一个问题生成器，从感知姿势通过支持启发式导出 `on/stacked-on/clear`，以及一个 Fast Downward 包装器。 |
| **几何** | 抓取采样（顶部/侧面方法）、我自己的具有关节限制钳位的 DLS IK、随机重启和近工作空间包络目标的重试梯度；扫路径抓取器碰撞和每链接臂检查；一个模拟计划效果的阴影场景，以便后续动作可以针对早期动作创建的世界进行精炼。 |
| **环路** | `TampSolver`：计划 ⇄ 精炼 ⇄ 伪造 ⇄ 重新规划，执行时间伪造（`pre_falsified`）在计划 → 执行 → 观察 → 重新规划之间闭合环路。 |
| **执行** | ROS 2 Jazzy + MoveIt 2（MoveItPy/OMPL）：带有 TF 验证到达的动作接口轨迹执行、填充的规划场景（表面、感知物体、附加有效载荷）、抓取的允许碰撞处理，以及 Gazebo Harmonic 中的基于物理的抓取（DetachableJoint + 手指闭合门控 + 接触带验证）。 |
| **感知** | YOLOv8（COCO）模拟 RGB-D 相机，具有深度反投影到度量物体姿势（~4 cm），发布与模拟路径相同的 `/world_state` —— 规划器无法区分它们。 |
| **语言** | 神经符号目标基础：LLM（Anthropic API / NVIDIA NIM / 本地 Ollama）*或* 确定性语法提出一个**目标，而非动作**；验证 DSL 将其与感知物体和规划器可行性相匹配。关闭问题、否定和幻觉 ID。以及一个 MCP 服务器，通过工具公开整个规划栈。 |
<p align="center">
<img src="docs/images/yolo_detections.png" width="46%" alt="模拟相机上的 YOLOv8 检测结果">
<img src="docs/images/camera_frame.png" width="46%" alt="模拟 RGB-D 相机帧">
<br>
<em>感知前端：模拟上方相机的 YOLOv8 检测结果，通过深度反投影到度量物体姿势。</em>
</p>
## 快速入门
核心部分 **不依赖 ROS** —— 完整的计划⇄精炼⇄重新规划环路在几秒钟内在纯 Python 中运行：
```bash
python3 -m virtualenv -p python3.12 .venv && . .venv/bin/activate
pip install -e ".[dev]"
tamp-offline --scene stack3 # 3 块堆叠的符号和几何计划
tamp-offline --scene replan # 观察环路否决一个不可达的托盘
tamp-command "put the cups on the tray" --scene pnp # 语言 -> 目标 -> 计划
python -m pytest tests/unit -q # 70+ 测试，无 ROS 要求
```
示例输出（`replan` 场景 —— 目标提供两个托盘，一个在物理上不可达）：
```
=== TAMP [replan] ===
success=True replans=0 reason=ok
1. PICK(block, ...) -> EEF [0.45 0. 0.435]
2. PLACE(block, s=tray_near, ...) -> EEF [0.55 0.2 0.453]
[replan] 反事实（执行后证伪 tray_near 也失败）：
[replan] success=False ... falsified=[('placeable','block','tray_near'), ('placeable','block','tray_far')]
[replan] -> 诚实的失败：没有可行的托盘。
```
### 完整栈（ROS 2 Jazzy + MoveIt 2 + Gazebo Harmonic）
```bash
cd ros2_ws && colcon build --symlink-install && cd ..
bash scripts/run_ros_full.sh stack3 # 计划 + MoveIt 执行
bash scripts/run_ros_full.sh replan # ROS 主题上的闭环恢复
bash run.sh pnp # Gazebo 物理拾取和放置（GUI）
bash scripts/reliability_pnp.sh 6 # 地面实况验证的可靠性批次
```
ROS 层运行两个 Python 解释器（规划器节点需要固定的几何栈；MoveItPy 需要系统 ROS Python）—— `scripts/ros_env.sh` 和启动文件处理分割。设置详细信息和非显而易见的 MoveItPy / Gazebo 集成说明在 [docs/RUNNING.md](docs/RUNNING.md) 和 [docs/ros_integration.md](docs/ros_integration.md) 中。
## 语言基础的目标和 MCP 服务器
我最喜欢的部分：你可以对话式地驱动规划器，它仍然不能被说服做不可行的事情。
```
$ tamp-command --scene pnp "put all the cups on the tray"
goal [rule]: {'type': 'sort_by_class', 'assignment': {'cup_1': 'tray', 'cup_2': 'tray'}}
✓ 计划（4 步，0 次重新规划）：
1. PICK cup_1
2. PLACE cup_1
3. PICK cup_2
4. PLACE cup_2
$ tamp-command --scene pnp "do not put cup_1 on the tray"
✗ 拒绝：命令是禁止，而不是目标
```
基础链是可选的 LLM：Anthropic API → OpenAI 兼容（NVIDIA NIM）→ 本地 Ollama → 确定性语法始终离线工作。每个后端的输出 —— 无论是 LLM 还是非 LLM —— 都通过相同的验证 DSL 和相同的规划器。模型提出；规划器验证。还有一个 [MCP 服务器](docs/nl_grounding.md) 以工具形式公开 `get_world_state` / `plan_goal` / `plan_command`，因此任何具有 MCP 能力的客户端或代理都可以读取机器人的世界并在相同的验证合约下提交目标。
详情：[docs/nl_grounding.md](docs/nl_grounding.md)。
## 仓库布局
```
tamp/ ROS-free 核心
├── pddl/ 领域模型 + 问题生成器（桥接谓词种子）
├── planning/ Fast Downward 包装器
├── geometry/ 机器人模型（DLS IK）、抓取采样、碰撞、精炼器
├── reasoning/ 姿势 -> 符号事实（on / stacked-on / clear）
├── execution/ 执行失败 -> 伪造谓词映射
├── nl/ 目标 DSL、语法 + LLM 基础
└── mcp_server.py 规划栈作为 MCP 工具
pddl/domains/ 规范的 tabletop.pddl
benchmarks/ 场景装置（stack3、replan、pnp）—— 也是测试装置
goals/ 示例目标规范（YAML）
ros2_ws/src/ ROS 2 叠加层：规划器节点、MoveIt 执行器、抓取管理器、
YOLOv8 + 模拟感知、启动、世界、自定义消息
scripts/ 运行程序：离线演示、完整 ROS 启动、可靠性套件
tests/ 单元 + 集成（核心在任何 Python 上运行）
docs/ 架构、运行指南、ROS 注释、语言层
```
## 测试
```bash
python -m pytest tests/unit tests/integration -q
```
70+ 测试涵盖领域编码、推理器、精炼器（包括扫路径和每链接碰撞回归）、闭环（恢复 *和* 诚实失败）、NL 语法的安全拒绝、DSL 验证和 MCP 工具表面。CI 在每次推送时运行 ROS-free 套件 —— 故意仅运行：我不在无法运行它的容器中伪造 Gazebo 覆盖；物理依赖层由真实机器上的地面实况可靠性套件进行测试。
## 工程笔记和诚实的局限性
我通过艰苦努力学习到的东西，以及我尚未磨平的边缘：
- **模拟抓取是一个独立的研究问题。** 抓取器使用 Gazebo 的 DetachableJoint，具有手指闭合门控和接触带验证 —— 一个有文档记录的妥协方案，而不是力闭合物理。在 Gazebo Harmonic 的 bullet-featherstone 后端下，固定关节约束是 **可违反的** （上游修复将在后续 Gazebo 版本中提供），这限制了多物体拾取和放置的可靠性；可靠性套件正是为了诚实地衡量这一点而存在的，而不是挑选绿色运行。
- **物理后端存在分歧。** DART 静默地丢弃 URDF 模拟关节；子弹强制执行动力学，忠实地暴露出不良惯性和接触裕度效应，而 DART 掩盖了这些。抓取器栈（独立手指关节、力矩限制、碰撞几何、关节限制）是针对测量值调谐的，测量脚本位于 `scripts/` 中。
- **精炼器的臂模型故意是粗糙的** —— 一个扫过抓取器代理加上每链接球体。它可以廉价地捕捉到粗糙的路径穿越；MoveIt 的完整网格规划场景在执行时仍然是权威的。
- **仅模拟。** 暂无物理机器人：执行层讲标准的 MoveIt 2 / ros2_control 接口，因此真正的 Panda 是下一个自然步骤，但我对硬件没有任何声明，直到它在一台硬件上运行。
## 路线图
- 力感知抓取（力矩接口抓取控制）以完全淘汰可拆卸关节的妥协方案
- 上游固定约束强制执行一旦二进制文件发布（或本地反移植）以提高多物体可靠性
- 物理 Panda 机器人启动
- 更丰富的领域：障碍、重新抓取、多步骤操作谜题
## 许可证
MIT — 见 [LICENSE](LICENSE)。
