def quat_to_angle(quat): rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w) return rot.GetRPY()[2]