R2ypr.py

def R2ypr(R: np.ndarray, unit="rad"):
    n = R[:, 0]
    o = R[:, 1]
    a = R[:, 2]

    y = np.arctan2(n[1], n[0])
    p = np.arctan2(-n[2], n[0] * np.cos(y) + n[1] * np.sin(y))
    r = np.arctan2(a[0] * np.sin(y) - a[1] * np.cos(y),
                   -o[0] * np.sin(y) + o[1] * np.cos(y))
    
    if unit == "rad":
        return np.array([y, p, r])
    elif unit == "deg":
        unit_conversion = 180.0 / 3.14159265358
        return np.array([y * unit_conversion, 
                         p * unit_conversion,
                         r * unit_conversion])
    else:
        raise RuntimeError("Wrong unit has given")