Files
a7l20-hug-sim/scripts/wrist_flange_transform.py

92 lines
3.7 KiB
Python
Raw Permalink Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
"""真机预抓取:手基座位姿 → 法兰位姿的转换
抓取流程中,HUG 重定向输出的 `wrist_pos` 是 hand_base_link 的位姿。
真机执行时需要转换为法兰(R8_Link/arm_right_R8_Link)的位姿。
关系:
T_world_flange = T_world_wrist · T_wrist_flange
其中 T_wrist_flange 来自:
1. 仿真 URDF:hand_base_link 相对法兰的固连变换(从 scene.json 的 mount_quat 可知)
2. 真机:需要标定或从机械图纸获取
方案 A:用仿真的固连关系(快速但可能有误差)
从 a7l20_scene.json 可知:
右手 mount_quat_wxyz = [0.0, 0.7071, 0.7071, 0.0]
即绕 Y 轴旋转 90°,hand_base_link 的 +z 沿法兰 -z
位置偏移:从 URDF 解析或从仿真 FK 读取(零位下 hand_base_link 相对法兰的位移)
方案 B:真机实测标定(精确)
1. 装好手,零位下使能
2. 用 RealArm.read() 读法兰位姿(FK 服务)
3. 手工测量或从 URDF FK 得到 hand_base_link 相对法兰的位移/姿态
4. 存为标定参数
推荐:先用方案 A(仿真几何),如果抓取偏差大再标定(方案 B)
"""
import numpy as np
from scipy.spatial.transform import Rotation as Rot
def wrist_to_flange(wrist_pos, wrist_R, mount_quat_wxyz=[0.0, 0.7071, 0.7071, 0.0],
offset_flange_to_wrist=[0.0, 0.0, 0.0]):
"""手基座位姿 → 法兰位姿
参数:
wrist_pos: hand_base_link 的世界坐标位置 (3,)
wrist_R: hand_base_link 的世界坐标姿态矩阵 (3, 3)
mount_quat_wxyz: 安装四元数 [w, x, y, z],hand_base_link 相对法兰的旋转
offset_flange_to_wrist: 法兰坐标系中,法兰→手基座的位移 (3,)
返回:
flange_pos, flange_R: 法兰的世界坐标位姿
"""
# 手基座相对法兰的固连变换
R_mount = Rot.from_quat([mount_quat_wxyz[1], mount_quat_wxyz[2],
mount_quat_wxyz[3], mount_quat_wxyz[0]]).as_matrix()
# T_world_flange = T_world_wrist · inv(T_wrist_flange)
# 即:T_flange_wrist · T_world_wrist → T_world_flange
flange_R = wrist_R @ R_mount.T
flange_pos = wrist_pos - flange_R @ offset_flange_to_wrist
return flange_pos, flange_R
def flange_to_wrist(flange_pos, flange_R, mount_quat_wxyz=[0.0, 0.7071, 0.7071, 0.0],
offset_flange_to_wrist=[0.0, 0.0, 0.0]):
"""法兰位姿 → 手基座位姿(反向转换,用于验证)"""
R_mount = Rot.from_quat([mount_quat_wxyz[1], mount_quat_wxyz[2],
mount_quat_wxyz[3], mount_quat_wxyz[0]]).as_matrix()
wrist_R = flange_R @ R_mount
wrist_pos = flange_pos + flange_R @ offset_flange_to_wrist
return wrist_pos, wrist_R
if __name__ == "__main__":
# 测试:手基座 → 法兰 → 手基座(往返一致性)
wrist_pos_orig = np.array([0.26, -0.45, 0.93])
wrist_R_orig = np.eye(3)
# 使用 scene.json 的右手安装四元数
mount_quat = [0.0, 0.7071067811865475, 0.7071067811865476, 0.0]
# 正向转换
flange_pos, flange_R = wrist_to_flange(wrist_pos_orig, wrist_R_orig, mount_quat)
print("手基座 → 法兰:")
print(f" wrist_pos: {wrist_pos_orig}")
print(f" flange_pos: {flange_pos.round(4).tolist()}")
print(f" flange_R:\n{flange_R.round(4)}")
# 反向转换
wrist_pos_back, wrist_R_back = flange_to_wrist(flange_pos, flange_R, mount_quat)
print("\n法兰 → 手基座:")
print(f" wrist_pos_back: {wrist_pos_back.round(4).tolist()}")
print(f" 误差: {np.linalg.norm(wrist_pos_back - wrist_pos_orig):.6f} m")
print(f" R 误差: {np.linalg.norm(wrist_R_back - wrist_R_orig, 'fro'):.6f}")
print("\n✓ 转换一致性验证通过" if np.allclose(wrist_pos_back, wrist_pos_orig) else "✗ 转换有误")