l1_basis_ball = ogp_util.transform_basis(ball_basis,null_basis,l1_basis) #pin basis in ball frame: pin +X is ball +X, pin +Y is ball +/-Y print "L1 pin basis in U-channel fiducial frame:" print l1_basis_ball l2_basis_ball = ogp_util.transform_basis(ball_basis,null_basis,l2_basis) #pin basis in ball frame: pin +X is ball +X, pin +Y is ball +/-Y print "L2 pin basis in U-channel fiducial frame:" print l2_basis_ball l3_basis_ball = ogp_util.transform_basis(ball_basis,null_basis,l3_basis) #pin basis in ball frame: pin +X is ball +X, pin +Y is ball +/-Y print "L3 pin basis in U-channel fiducial frame:" print l3_basis_ball print "L1S sensor basis in pin frame:" print ogp_util.reverse_basis(l1s) print "L1A sensor basis in pin frame:" print ogp_util.reverse_basis(l1a) print "L2S sensor basis in pin frame:" print ogp_util.reverse_basis(l2s) print "L2A sensor basis in pin frame:" print ogp_util.reverse_basis(l2a) print "L3S sensor basis in pin frame:" print ogp_util.reverse_basis(l3s) print "L3A sensor basis in pin frame:" print ogp_util.reverse_basis(l3a) sys.exit() print l2s #stuff in frame 1 print l2_basis #stuff in frame 2 print ogp_util.transform_basis(l2_basis,l2s,l2_basis) #transform stuff in frame 2 to frame 1
def fitfunc(p,positions,nominal): axis_nom = transform_axis(null_basis,nominal,p) residuals = np.empty(0) for position in positions: #print position axis = transform_axis(null_basis,position,p) residuals = np.append(residuals,(axis-axis_nom)[:,1:3]) #print axis-axis_nom #print residuals return residuals #print fitfunc(p0,b13_positions,b13_nom) sol = scipy.optimize.leastsq(fitfunc,p0,args=(b13_positions,b13_nom))[0] #print sol print "B13 ball basis in pivot frame:" print ogp_util.reverse_basis(get_basis(sol)) #print transform_axis(null_basis,b13_nom,sol) print "B13 pivot basis in box frame:" print ogp_util.transform_basis(null_basis,b13_nom,get_basis(sol)) #print fitfunc(sol,b13_positions,b13_nom).reshape(-1,4) sol = scipy.optimize.leastsq(fitfunc,p0,args=(t13_positions,t13_nom))[0] #print sol print "T13 ball basis in pivot frame:" print ogp_util.reverse_basis(get_basis(sol)) #print transform_axis(null_basis,t13_nom,sol) print "T13 pivot basis in box frame:" print ogp_util.transform_basis(null_basis,t13_nom,get_basis(sol)) #print fitfunc(sol,t13_positions,t13_nom).reshape(-1,4)
l4_basis_ball = ogp_util.transform_basis(ball_basis,null_basis,l4_basis) #pin basis in ball frame: pin +X is ball +X, pin +Y is ball +/-Y print "L4 pin basis in U-channel fiducial frame:" print l4_basis_ball l5_basis_ball = ogp_util.transform_basis(ball_basis,null_basis,l5_basis) #pin basis in ball frame: pin +X is ball +X, pin +Y is ball +/-Y print "L5 pin basis in U-channel fiducial frame:" print l5_basis_ball l6_basis_ball = ogp_util.transform_basis(ball_basis,null_basis,l6_basis) #pin basis in ball frame: pin +X is ball +X, pin +Y is ball +/-Y print "L6 pin basis in U-channel fiducial frame:" print l6_basis_ball print "L4SE sensor basis in pin frame:" print ogp_util.reverse_basis(l4se) print "L4SP sensor basis in pin frame:" print ogp_util.reverse_basis(l4sp) print "L4AE sensor basis in pin frame:" print ogp_util.reverse_basis(l4ae) print "L4AP sensor basis in pin frame:" print ogp_util.reverse_basis(l4ap) print "L5SE sensor basis in pin frame:" print ogp_util.reverse_basis(l5se) print "L5SP sensor basis in pin frame:" print ogp_util.reverse_basis(l5sp) print "L5AE sensor basis in pin frame:" print ogp_util.reverse_basis(l5ae) print "L5AP sensor basis in pin frame:" print ogp_util.reverse_basis(l5ap) print "L6SE sensor basis in pin frame:"
print l4_basis_ball l5_basis_ball = ogp_util.transform_basis( ball_basis, null_basis, l5_basis) #pin basis in ball frame: pin +X is ball +X, pin +Y is ball +/-Y print "L5 pin basis in U-channel fiducial frame:" print l5_basis_ball l6_basis_ball = ogp_util.transform_basis( ball_basis, null_basis, l6_basis) #pin basis in ball frame: pin +X is ball +X, pin +Y is ball +/-Y print "L6 pin basis in U-channel fiducial frame:" print l6_basis_ball print "L4SE sensor basis in pin frame:" print ogp_util.reverse_basis(l4se) print "L4SP sensor basis in pin frame:" print ogp_util.reverse_basis(l4sp) print "L4AE sensor basis in pin frame:" print ogp_util.reverse_basis(l4ae) print "L4AP sensor basis in pin frame:" print ogp_util.reverse_basis(l4ap) print "L5SE sensor basis in pin frame:" print ogp_util.reverse_basis(l5se) print "L5SP sensor basis in pin frame:" print ogp_util.reverse_basis(l5sp) print "L5AE sensor basis in pin frame:" print ogp_util.reverse_basis(l5ae) print "L5AP sensor basis in pin frame:" print ogp_util.reverse_basis(l5ap) print "L6SE sensor basis in pin frame:"