Files
a7l20-hug-sim/docs/table_contact_escape.md
T

3.6 KiB
Raw Blame History

起始接触桌面的离线脱离方案

适用装配:控制器下载的左臂 + 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 的起点阶段已接入:先检查脱离,再独立验证正常净空下到预抓取的关节线性路径。 这不替代整条抓取流程的其他验证。