(q - des) is expressed in deg, while I am not 100% sure but I am afraid speed[j] is expressed in rad/s .