KUKA has a built in function (At least I thought it was built in):
GLOBAL DEF mat_to_rot (t[,]:OUT, a:OUT, b:OUT, c:OUT) ;Conversion of a transformer matrix T into the ROT angles A, B, C ;T = Rot_z(A) * Rot_y(B) * Rot_x(C) REAL t[,], a, b, c REAL sin_a, cos_a, sin_b, abs_cos_b, sin_c, cos_c a = ATAN2 (t[2,1], t[1,1]) sin_a = SIN(a) cos_a = COS(a) sin_b = -t[3,1] abs_cos_b = cos_a*t[1,1] + sin_a*t[2,1] b = atan2 (sin_b, abs_cos_b); Hier: -90 <= B <= +90 !! sin_c = sin_a*t[1,3] - cos_a*t[2,3] cos_c = -sin_a*t[1,2] + cos_a*t[2,2] c = ATAN2 (sin_c, cos_c) END
Even if you aren't writing this code for KRL, its pretty easy to read this code and convert it to whatever language you are working with. It says that T[,] is an output, but thats only because kuka forces more complicated variables to be declared as outputs. Its actually being used as an input in this scenario. Last note, a lot of other programming languages use radians by default, but not KRL, so keep that in mind if porting this over into a different language.