KUKA has a built in function (At least I thought it was built in):
Code
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
Display More
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.