# Calculate Roll Pitch Yaw from Modified Denavit Hartenberg

I'm using modified denavit hartenberg to represent my robot end-effector. But i couldnt find equation to calculate rpy if im using mdh. Below my MDH.

I try use rpy equation for dh paramater(from Getting pitch, yaw and roll from Rotation Matrix in DH Parameter), but when i apply it doesnt seem correct. Does anyone know the correct equations? I'd be very thankful for any help.

## 1 Answer

The orientation of the end-effector is obtained by multiplying together the MDH homogeneous transformation matrices for each of the joints. There are well known algorithms and code implementations to extract roll, pitch, yaw angles from such a homogeneous transformation matrix. You need to be careful about how you define roll, pitch and yaw angles, there are multiple conventions which is frustrating and confusing to those entering the field.

It doesn't seem particularly useful to go directly from MDH parameters to RPY angles, since in general a robot has more than one joint.

However, if you did want to go this path, then just looking at the expression we see that the orientation is a rotation about the x-axis of $$\alpha$$ followed by a rotation about the z-axis of $$\theta$$. The XYZ RPY convention, often used for problems with robot manipulator arms, is defined as an SO(3) rotation $$R = R_x(y) R_y( p) R_z( r)$$ so it is clear that $$\alpha$$ is the yaw angle, $$\theta$$ is the roll angle, and pitch angle is always 0.

• ok sir, i multiply every mdh and get htm. Then for 6 joint case what is ur recommendation for algorithm that i can use? – Albert H M Jun 12 '19 at 4:00
• See any textbook about robotics, if you’re familiar with MATLAB checkout github.com/petercorke/spatial-math/blob/master/tr2rpy.m – Peter Corke Jun 13 '19 at 1:56
• Sorry if i keep asking the same question, can i use directly delta of orientation using ur algorithm for input inverse jacobian(SerialLink.jacob0 then inversed)? – Albert H M Jun 13 '19 at 8:00
• thank you sir, i will use ur algorithm for xyz arm – Albert H M Jun 13 '19 at 8:12