import math

# Link3: rpy="0 0 -1.5708" (纯z轴旋转-90°)
# rc_urdf = (0.0016734, -0.086963, -0.059109)
# Rz(90°) 逆旋转
c, s = math.cos(math.pi/2), math.sin(math.pi/2)
x, y, z = 0.0016734, -0.086963, -0.059109
rc_dh_3 = (x*c + y*s, -x*s + y*c, z)
print(f"Link3: rc_dh = ({rc_dh_3[0]:.8f}, {rc_dh_3[1]:.8f}, {rc_dh_3[2]:.8f})")

# Link4: rpy="-1.5708 0 -1.5708" (roll-90°, yaw-90°)
# rc_urdf = (4.9724e-5, -0.00207071, 0.143600)
# 先Rx(-90°), 再Rz(90°)的逆
# R = Rz(-90°) * Rx(-90°), R^T = Rx(90°) * Rz(90°)

# Rx(90°): [1,0,0; 0,0,-1; 0,1,0]
# Rz(90°): [0,-1,0; 1,0,0; 0,0,1]
# R_inv = Rx(90°) * Rz(90°) = [0,-1,0; 0,0,-1; 1,0,0]
x, y, z = 4.9724e-5, -0.00207071, 0.143600
rc_dh_4 = (-y, -z, x)
print(f"Link4: rc_dh = ({rc_dh_4[0]:.8f}, {rc_dh_4[1]:.8f}, {rc_dh_4[2]:.8f})")

# Link5: rpy="1.5708 0 3.1416" (roll+90°, yaw+180°)
# rc_urdf = (-1.00614e-5, 0.05816669, -0.02402677)
# R = Rz(180°) * Rx(90°)
# R_inv = Rx(-90°) * Rz(-180°)
# Rx(-90°): [1,0,0; 0,0,1; 0,-1,0]
# Rz(-180°): [-1,0,0; 0,-1,0; 0,0,1]  
# R_inv = [[-1,0,0],[0,0,1],[0,1,0]]
x, y, z = -1.00614e-5, 0.05816669, -0.02402677
rc_dh_5 = (-x, z, y)
print(f"Link5: rc_dh = ({rc_dh_5[0]:.8f}, {rc_dh_5[1]:.8f}, {rc_dh_5[2]:.8f})")