Added calculation of nearest equivelent joint angle