Hi,
I am trying to calculate forward kinematics of UR5. I am using DH parameters from Universal Robots - DH Parameters for calculations of kinematics and dynamics,
But I am receiving different roll, pitch, and yaw angles compared with the teach pendant. I am attaching figure for reference. I wanted to know is there any error in my code (python script) or am I missing something?
Code:
####Number of links
n = 6
Define DH parameters
alp = [np.pi/2, 0, 0, np.pi/2, -np.pi/2, 0]
a = [0, -0.425, -0.39225, 0, 0, 0]
d = [0.089159, 0, 0, 0.10915, 0.09465, 0.0823]
#d = [0.0892, 0, 0, 0.109, 0.095, 0.0823]
th = np.deg2rad([180.00, -180.00, 90.0, -180.0, -90.0, 180.0])
Value of d_E_n
d_E_n = np.array([0, 0, 0.218])
Initialization
T_i = np.eye(4)
R = np.zeros((3, 3, n))
O = np.zeros((3, n))
Forward kinematics
for i in range(n):
T_i_i_m = np.array([
[np.cos(th[i]), -np.sin(th[i]) * np.cos(alp[i]), np.sin(th[i]) * np.sin(alp[i]), a[i] * np.cos(th[i])],
[np.sin(th[i]), np.cos(th[i]) * np.cos(alp[i]), -np.cos(th[i]) * np.sin(alp[i]), a[i] * np.sin(th[i])],
[0, np.sin(alp[i]), np.cos(alp[i]), d[i]],
[0, 0, 0, 1]
])
T_i_0 = T_i @ T_i_i_m
R_i_0 = T_i_0[:3, :3]
o_i_0 = T_i_0[:3, 3]
R[:, :, i] = R_i_0
O[:, i] = o_i_0
T_i = T_i_0
T_n_0 = T_i_0
P_E_0 = T_n_0 @ np.append(d_E_n, 1)
p_E_0 = P_E_0[:3]
Extract rotation matrix from T_n_0
R_n_0 = T_n_0[:3, :3]
Compute roll, pitch, and yaw from the rotation matrix
sy = np.sqrt(R_n_0[2, 2] ** 2 + R_n_0[2, 1] ** 2)
singular = sy < 1e-6
if not singular:
roll = np.arctan2(R_n_0[2, 1], R_n_0[2, 2])
pitch = np.arctan2(-R_n_0[2, 0], sy)
yaw = np.arctan2(R_n_0[1, 0], R_n_0[0, 0])
else:
roll = np.arctan2(-R_n_0[1, 2], R_n_0[1, 1])
pitch = np.arctan2(-R_n_0[2, 0], sy)
yaw = 0
if not singular:
roll = np.arctan2(R_n_0[2, 1], R_n_0[2, 2])
pitch = np.arctan2(-R_n_0[2, 0], sy)
yaw = np.arctan2(R_n_0[1, 0], R_n_0[0, 0])
else:
roll = np.arctan2(-R_n_0[1, 2], R_n_0[1, 1])
pitch = np.arctan2(-R_n_0[2, 0], sy)
yaw = 0
Display results
print(‘T_6_0 is:’)
print(T_n_0)
print(‘Position at end effector at (0,0,0.218):’)
print(p_E_0)
print(‘Roll, Pitch, Yaw:’)
print(‘Roll:’, roll)
print(‘Pitch:’, pitch)
print(‘Yaw:’, yaw)
Output:
T_6_0 is:
[[ 3.06161700e-16 -1.00000000e+00 1.22464680e-16 -5.20000000e-01]
[ 1.00000000e+00 3.06161700e-16 -1.07775327e-33 1.09000000e-01]
[-2.73235495e-32 1.22464680e-16 1.00000000e+00 5.63750000e-01]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
Position at end effector at (0,0,0.218):
[-0.52 0.109 0.78175]
Roll, Pitch, Yaw:
Roll: 1.224646799147353e-16
Pitch: 2.7323549470696886e-32
Yaw: 1.5707963267948963
End-effector values shown in teach pendant of UR5:
