Exemplo n.º 1
0

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
Exemplo n.º 2
0
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)

Exemplo n.º 3
0
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:"
Exemplo n.º 4
0
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:"