3.6 KiB
起始接触桌面的离线脱离方案
适用装配:控制器下载的左臂 + 36 mm 连接法兰 + 用户已确认的左手45°安装。
本模块不连接机器人、不使能、不发命令,也不修改 config/table_safety.json。
行为
程序启动、感知/读取保存数据和规划不要求机器人一开始已有20 mm净空。 运动候选按以下三种起点区分:
| 起点的保守几何下界 | 处理 |
|---|---|
| 所有部件至少20 mm | 进入正常路径验证,无需脱离 |
| 最低下界在0到20 mm之间(包括恰好接触) | 搜索小步安全脱离,完整验证后恢复正常20 mm约束 |
| 任一部件下界小于0 | 保存诊断,不输出可用脱离计划;可能是几何/不确定性未解决,不直接断言实物穿透 |
不会通过删掉起点、临时关闭碰撞检测或允许起点穿透来解决无法启动。 若误差带覆盖了桌内,即使实物看起来接触,也不能据此证明能自动脱离。
连续检查范围
- 每一段均检查左臂L1~L7、实际法兰凸包和固定手指姿态的整手凸包;不用大球替代当前手形。
- 每个部件锁定一个整体分离平面:桌上方,或已观测近桌沿外侧。 不是给各顶点分别选择不同平面,避免漏掉凸包穿桌角。
- 原本不足20 mm的顶点必须沿选定平面单调离开;其他顶点必须保持至少20 mm。 同一小段内即使某个顶点刚越过20 mm,仍保守地要求它继续向外。
- 间隔内用点Jacobian及二阶位移上界检查,而不是仅看端点。
纯转动串联臂、手指固定、关节线性插值下,
|d''| ≤ reach × ||Δq||₁²。 - 采用桌沿外侧分离的部件,整个运动区间都必须留在已观测桌沿跨度内。
- 终点所有部件重新获得20 mm,脱离才算完成。之后到预抓取的路径单独验证;不能继承接触阶段的豁免。
- LP只生成候选,所有候选另作连续验证;完整路径最后从原起点再验证一次。 搜索失败、超预算或仅部分路径通过时,不释放任何运动计划。
当前离线提案上限:每关节每小段0.01 rad、速度0.03 rad/s、加速度0.05 rad/s²。 速度/加速度只是后续执行器必须遵守的上限,本模块没有实际下发或实现控制器时序。
已知边界
这是桌面避碰的离线方案,不等于真实抓取已通过。尚不认证自碰撞、另一臂、杯体、
桌腿/未知桌底、接触力与摩擦、手臂受力变形、真实控制器插补和在线停止。
手指必须保持输入的固定姿态;换手形应重新构建几何并验证,不能沿用此计划。
现有保存的 hand_pre 是规划姿态,不是实时手指反馈。
全局几何界是保守的。初始一阶导数为零、但二阶可以离桌的姿态仍可能被局部搜索拒绝; 这只表示“未找到可证明安全的脱离路径”,不表示物理上无法离桌。
使用
保存快照+已采集桌面的条件诊断(不访问硬件):
.venv/bin/python scripts/plan_table_escape.py
真实装配几何+合成接触桌面演示(不是实际桌面验证,不改变标定):
.venv/bin/python scripts/plan_table_escape.py --demo
回归测试:scripts/test_table_contact_escape.py。
报告分别保存为 data/left_cup_current/table_escape_start_report.json 和
table_escape_synthetic_demo.json,一律 executable=false。
replan_pinky_low_4cm.py 的起点阶段已接入:先检查脱离,再独立验证正常净空下到预抓取的关节线性路径。
这不替代整条抓取流程的其他验证。