3.0 KiB
3.0 KiB
"""预抓取位置处理说明:手基座 vs 法兰
问题
装上 L20 手后,抓取点不再是法兰末端,而是手指尖。 预抓取位置的计算需要考虑手的几何偏移。
当前方案(已正确实现)
1. HUG 重定向输出的是 hand_base_link 位姿
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 与法兰重合)
- 如果抓取偏差大,需要实测标定:
- 装好手,零位下使能
- 读法兰位姿(FK 服务)
- 手工测量 hand_base_link 位置
- 计算偏移并更新
进给过程的补偿(已实现)
由于人手和机器手尺寸差异,到达抓取位可能"悬空"。
grasp_exec.py 的下沉补偿:
# line 304-306: 沿 approach 继续下沉至首次触物(最多 sink=3cm)
sim.approach_until_touch(app, sink, speed=0.5)
真机部署 TODO
- ✅ 手基座↔法兹坐标转换(wrist_flange_transform.py)
- ⏸ 在 real_arm.py 中集成转换(movej/movel 前自动转换)
- ⏸ 标定 offset_flange_to_wrist(如果初始零偏移精度不够)
- ⏸ 端到端测试:感知→HUG→重定向→真机预抓取→进给→合指
关键点
- 仿真已正确处理:wrist_pos 是手基座,IK 自动处理几何偏移
- 真机需要显式转换:控制器 IK 输入法兰位姿,需调用 wrist_to_flange()
- 安装参数来自 scene.json:mount_quat + offset(初始零)
- 下沉补偿:解决人手/机器手尺寸差导致的悬空问题