92 lines
3.7 KiB
Python
92 lines
3.7 KiB
Python
"""真机预抓取:手基座位姿 → 法兰位姿的转换
|
||
|
||
抓取流程中,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 "✗ 转换有误")
|