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)
|