#NEWLINE# def _compare_positions( a , b , max_dist = 0.003 , max_angle = 5.0 ) : #NEWLINE# #INDENT# 'Compare estimated cHPI positions' #NEWLINE# from scipy.interpolate import interp1d #NEWLINE# ( trans , rot , t ) = a #NEWLINE# ( trans_est , rot_est , t_est ) = b #NEWLINE# quats_est = rot_to_quat ( rot_est ) #NEWLINE# use_mask = ( ( t >= t_est [ 0 ] ) & ( t <= t_est [ ( - 1 ) ] ) ) #NEWLINE# t = t [ use_mask ] #NEWLINE# trans = trans [ use_mask ] #NEWLINE# quats = rot_to_quat ( rot ) #NEWLINE# quats = quats [ use_mask ] #NEWLINE# for q in ( quats , quats_est ) : #NEWLINE# #INDENT# angles = _angle_between_quats ( q , q ) #NEWLINE# assert_allclose ( trans_est_interp , 0.0 , atol = 1e-05 ) #NEWLINE# #UNINDENT# trans_est_interp = interp1d ( t_est , trans_est , axis = 0 ) ( t ) #NEWLINE# worst = np . sqrt ( np . sum ( ( ( trans - trans_est_interp ) ** 2 ) , axis = 1 ) ) . max ( ) #NEWLINE# assert_true ( ( worst <= max_dist ) , ( '%0.1f > %0.1f mm' % ( ( 1000 * worst ) , ( 1000 * max_dist ) ) ) ) #NEWLINE# quats_est_interp = interp1d ( t_est , quats_est , axis = 0 ) ( t ) #NEWLINE# worst = ( ( 180 * _angle_between_quats ( quats_est_interp , quats ) . max ( ) ) / np . pi ) #NEWLINE# assert_true ( ( worst <= max_angle ) , ( '%0.1f > %0.1f deg' % ( worst , max_angle ) ) )