66 lines
1.8 KiB
Python
66 lines
1.8 KiB
Python
|
import numpy as np
|
|||
|
|
import pandas as pd
|
|||
|
|
from pathlib import Path
|
|||
|
|
|
|||
|
|
|
|||
|
|
def get_T_cp():
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
# ---------- 用户给出的 T_ee->cam ----------
|
|||
|
|
# R_ec = np.array([[ 0.92095 , 0.33864198, 0.19280227],
|
|||
|
|
# [-0.2465026 , 0.88946636, -0.38481952],
|
|||
|
|
# [-0.30180718, 0.30687328, 0.90263017]])
|
|||
|
|
# t_ec = np.array([[-0.00596568],
|
|||
|
|
# [-0.61356453],
|
|||
|
|
# [ 0.02675161]])
|
|||
|
|
|
|||
|
|
R_ec = np.array([[ 1 , 0, 0],
|
|||
|
|
[0 , 1, 0],
|
|||
|
|
[0, 0, 1]])
|
|||
|
|
t_ec = np.array([[-0.035],
|
|||
|
|
[-0.18],
|
|||
|
|
[ 0.08]])
|
|||
|
|
|
|||
|
|
T_ec = np.eye(4)
|
|||
|
|
T_ec[:3, :3] = R_ec
|
|||
|
|
T_ec[:3, 3] = t_ec.ravel()
|
|||
|
|
|
|||
|
|
# ---------- 读取 RobotToolPose.csv ----------
|
|||
|
|
csv_path = Path("RobotToolPose.csv")
|
|||
|
|
if not csv_path.exists():
|
|||
|
|
raise FileNotFoundError("RobotToolPose.csv 未上传到工作目录")
|
|||
|
|
|
|||
|
|
mat = np.loadtxt(csv_path, delimiter=',') # 3 × (4*N)
|
|||
|
|
if mat.shape[0] != 3 or mat.shape[1] % 4 != 0:
|
|||
|
|
raise ValueError("RobotToolPose.csv 格式应为 3×(4*N)")
|
|||
|
|
|
|||
|
|
N = mat.shape[1] // 4
|
|||
|
|
T_list = []
|
|||
|
|
for i in range(N):
|
|||
|
|
block = mat[:, 4*i:4*i+4] # 3×4
|
|||
|
|
T = np.eye(4)
|
|||
|
|
T[:3,:4] = block
|
|||
|
|
T_list.append(T)
|
|||
|
|
|
|||
|
|
# ---------- 计算 T_base->cam = T_base->ee * T_ee->cam ----------
|
|||
|
|
T_bc_list = [T_be @ T_ec for T_be in T_list]
|
|||
|
|
|
|||
|
|
# 简单检查：平移范数 & 旋转角度（相对于第一帧）
|
|||
|
|
def rot_to_axis_angle(R):
|
|||
|
|
angle = np.arccos(max(min((np.trace(R)-1)/2,1),-1))
|
|||
|
|
return angle
|
|||
|
|
|
|||
|
|
trans_norms = [np.linalg.norm(T[:3,3]) for T in T_bc_list]
|
|||
|
|
rot_angles = [rot_to_axis_angle(T[:3,:3] @ T_bc_list[0][:3,:3].T) for T in T_bc_list]
|
|||
|
|
|
|||
|
|
df = pd.DataFrame({
|
|||
|
|
'frame': np.arange(N),
|
|||
|
|
'tx': [T[0,3] for T in T_bc_list],
|
|||
|
|
'ty': [T[1,3] for T in T_bc_list],
|
|||
|
|
'tz': [T[2,3] for T in T_bc_list],
|
|||
|
|
'||t|| (m)': trans_norms,
|
|||
|
|
'Δθ w.r.t #0 (rad)': rot_angles
|
|||
|
|
})
|
|||
|
|
|
|||
|
|
print(df)
|