Six-axis joint angle calculation bugfix