Example
For the angle calculation the same contents of the rotation matrice is used
(calculated from one angle set using the corresponding rotation matrice)
post #14 shows how to calculate the euler angles Z0,Y, Z1
KUEWEG.SRC (check MAT_TO_RPY) shows how to calculate the T-B angles A,B,C
(for Fanuc and Motoman in reversed order)
This way of doing the angle calculation also handels the gimbal lock
With 4 routines:
2 calculations of the rotation matrice
2 calculations of the angle values
ABB, KUKA, Nachi, Fanuc, Motoman, Adept, Comau and Kawasaki are covered