Final fixes six-axis joint angle calculations