Six-axis joint angle calculation fix