Files
a7l20-hug-sim/docs/pregrasp_wrist_flange.md

3.0 KiB
Raw Permalink Blame History

"""预抓取位置处理说明:手基座 vs 法兰

问题

装上 L20 手后,抓取点不再是法兰末端,而是手指尖。 预抓取位置的计算需要考虑手的几何偏移。

当前方案(已正确实现)

hug_to_l20.py 输出:

  • wrist_pos: hand_base_link 的世界坐标位置
  • wrist_quat: hand_base_link 的世界坐标姿态
  • hand_qpos: 16 个手关节角

锚点策略(--anchor closure,默认): 让机器手的"闭合中心"(五指尖闭合 80% 时的质心)与人手五指尖质心对齐 → 确保指尖在正确的抓取位置

2. 预抓取位置计算

pos = wrist_pos                    # 抓取位姿(手基座)
app = approach                     # 进给方向(通常 = -掌心法线)
pre = pos - pre_dist * app         # 预抓取 = 沿 approach 后退 10cm

3. 仿真执行(已验证)

Sim.move_to(pre, R, ...) 使用 mink IK,目标是 hand_base_link 位姿 → 自动计算法兰(R8_Link)需要的关节角 → 手的几何偏移由 URDF 固连关系自动处理

4. 真机执行(需要转换)

真机控制器的 IK 服务输入是法兰位姿,需要转换:

# 手基座 → 法兰
from wrist_flange_transform import wrist_to_flange

flange_pos, flange_R = wrist_to_flange(
    wrist_pos, wrist_R,
    mount_quat_wxyz=[0.0, 0.7071, 0.7071, 0.0],  # 从 scene.json
    offset_flange_to_wrist=[0.0, 0.0, 0.0]       # 待标定或从 URDF 读取
)

# 调用真机 IK
arm.ik_sweep(flange_pos, euler_from_R(flange_R))

5. 安装变换参数(从 scene.json)

右手:

"mount_quat_wxyz": [0.0, 0.7071067811865475, 0.7071067811865476, 0.0]

→ 绕 Y 轴旋转 90°,hand_base_link 的 +z 沿法兰 -z

位置偏移:

  • 初始值:[0.0, 0.0, 0.0](假设 hand_base_link 与法兰重合)
  • 如果抓取偏差大,需要实测标定:
    1. 装好手,零位下使能
    2. 读法兰位姿(FK 服务)
    3. 手工测量 hand_base_link 位置
    4. 计算偏移并更新

进给过程的补偿(已实现)

由于人手和机器手尺寸差异,到达抓取位可能"悬空"。 grasp_exec.py 的下沉补偿:

# line 304-306: 沿 approach 继续下沉至首次触物(最多 sink=3cm)
sim.approach_until_touch(app, sink, speed=0.5)

真机部署 TODO

  1. ✅ 手基座↔法兹坐标转换(wrist_flange_transform.py)
  2. ⏸ 在 real_arm.py 中集成转换(movej/movel 前自动转换)
  3. ⏸ 标定 offset_flange_to_wrist(如果初始零偏移精度不够)
  4. ⏸ 端到端测试:感知→HUG→重定向→真机预抓取→进给→合指

关键点

  • 仿真已正确处理:wrist_pos 是手基座,IK 自动处理几何偏移
  • 真机需要显式转换:控制器 IK 输入法兰位姿,需调用 wrist_to_flange()
  • 安装参数来自 scene.json:mount_quat + offset(初始零)
  • 下沉补偿:解决人手/机器手尺寸差导致的悬空问题