# Incorrect RPY angles

**URL:** <https://forum.universal-robots.com/t/incorrect-rpy-angles/34277>\
**Category:** Technical Questions\
**Created:** [May 17, 2024, 5:15pm UTC](https://forum.universal-robots.com/t/incorrect-rpy-angles/34277 "2024-05-17T17:15:19Z")\
**Posts on this page:** 1\
**Page:** 1

<div class="post-metadata">

**Author:** ![ADS\_Facy](https://sea2.discourse-cdn.com/flex020/user_avatar/forum.universal-robots.com/ads_facy/32/5519_2.png) [@ADS\_Facy](https://forum.universal-robots.com/u/ADS_Facy)\
**Post date:** [May 17, 2024, 5:15pm UTC](https://forum.universal-robots.com/t/incorrect-rpy-angles/34277/1 "2024-05-17T17:15:19Z")

</div>

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](https://www.universal-robots.com/articles/ur/application-installation/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:

 ![teach_pendant_RPY](https://us1.discourse-cdn.com/flex020/uploads/universal_robots/original/2X/c/c290075b9b72298420250b973487adcef708a204.png)