def f_min(p):
global_basis = ogp_util.make_parametrized_basis(p[0], p[1], p[2], p[3],
p[4], p[5])
#print p
def residual(mod_basis, uch_basis, y, z, dz):
edge = ogp_util.transform_pt(
ball_basis, uch_basis,
ogp_util.transform_pt(null_basis, mod_basis, axial_edge))
#print edge
#nominal = [math.sin(0.0305)*(z+dz-700), y, math.cos(0.0305)*(z+dz-700)+700]
nominal = [
math.sin(0.0305) * (z + dz - 700) - 8.789, y,
math.cos(0.0305) * (z + dz)
]
vec = ogp_util.transform_vec(
ball_basis, uch_basis,
ogp_util.transform_vec(null_basis, mod_basis, axial_vec))
#print vec
angle = math.atan2(vec[1], vec[0])
if (abs(angle) > math.pi / 2):
angle = angle - math.copysign(math.pi, angle)
#print edge-nominal
#print angle
return np.append(edge - nominal, angle * 1000)
return np.concatenate(
(residual(l1_basis, global_basis, is_top * 6.5, 500,
dz), residual(l2_basis, global_basis, is_top * 9.5, 700, dz),
residual(l3_basis, global_basis, is_top * 12.5, 900, dz)))
def f_min(p): global_basis =ogp_util.make_parametrized_basis(p[0],p[1],p[2],p[3],p[4],p[5]) #print p def residual(mod_basis,uch_basis,y,z,dz): edge = ogp_util.transform_pt(ball_basis,uch_basis,ogp_util.transform_pt(null_basis,mod_basis,axial_edge)) #print edge #nominal = [math.sin(0.0305)*(z+dz-700), y, math.cos(0.0305)*(z+dz-700)+700] nominal = [math.sin(0.0305)*(z+dz-700)-8.789, y, math.cos(0.0305)*(z+dz)] vec = ogp_util.transform_vec(ball_basis,uch_basis,ogp_util.transform_vec(null_basis,mod_basis,axial_vec)) #print vec angle = math.atan2(vec[1],vec[0]) if (abs(angle)>math.pi/2): angle = angle-math.copysign(math.pi,angle) #print edge-nominal #print angle return np.append(edge-nominal,angle*1000) return np.concatenate((residual(l1_basis,global_basis,is_top*6.5,500,dz), residual(l2_basis,global_basis,is_top*9.5,700,dz), residual(l3_basis,global_basis,is_top*12.5,900,dz)))
#print l1a_uch #print ogp_util.transform_pt(l1a,l1_basis_trans,np.array([10,0,0])) #print l2s #print l2a #l2s_uch = ogp_util.transform_pt(l2s,l2_basis_trans,np.array([0,0,0])) #print l2s_uch #l2a_uch = ogp_util.transform_pt(l2a,l2_basis_trans,np.array([0,0,0])) #print l2a_uch #l3s_uch = ogp_util.transform_pt(l3s,l3_basis_trans,np.array([0,0,0])) #print l3s_uch #l3a_uch = ogp_util.transform_pt(l3a,l3_basis_trans,np.array([0,0,0])) #print l3a_uch null_basis = ogp_util.make_parametrized_basis(0.0, 0.0, 0.0, 0.0, 0.0,0.0) #axial_origin = np.array([124.083, 9.525, 39.1898]) #stereo_origin = np.array([124.067, 13.776, 39.1898]) #axial_edge = np.array([124.083, 9.525, 39.1898+20.17]) axial_edge = np.array([123.66, 0.375*25.4, 2.337*25.4]) axial_vec = np.array([1.0, 0.0, 0.0]) #dz = -0.3125*25.4 #z = 100 def f_min(p): global_basis =ogp_util.make_parametrized_basis(p[0],p[1],p[2],p[3],p[4],p[5]) #print p def residual(mod_basis,uch_basis,y,z,dz): edge = ogp_util.transform_pt(ball_basis,uch_basis,ogp_util.transform_pt(null_basis,mod_basis,axial_edge)) #print edge
l3slotpin[1]=noballsdict['L6 slot pin intersection']['Y Location'][1] l3slotpin[2]=noballsdict['L6 slot pin intersection']['Z Location'][1] l3az = math.radians(noballsdict['L6 plane, touch']['XY Angle'][1]) l3el = math.radians(noballsdict['L6 plane, touch']['Elevation'][1]) if (l3el<0): l3az = l3az+math.pi l3el = -1.0*l3el l3normal = np.array([math.cos(l3az)*math.cos(l3el), math.sin(l3az)*math.cos(l3el), math.sin(l3el)]) #print l3normal l3_basis = ogp_util.make_pin_basis(l3holepin, l3slotpin, l3normal) #print l3_basis null_basis = ogp_util.make_parametrized_basis(0.0, 0.0, 0.0, 0.0, 0.0,0.0) #axial_origin = np.array([138.125, 9.525, 35.1004]) #stereo_origin = np.array([124.067, 13.776, 39.1898]) axial_edge = np.array([149.225, 0.375*25.4, 35.1004+20.17]) #axial_edge = np.array([138.125, 9.525, 35.1004+20.17]) axial_vec = np.array([1.0, 0.0, 0.0]) #dz = -0.3125*25.4 #z = 100 def f_min(p): global_basis =ogp_util.make_parametrized_basis(p[0],p[1],p[2],p[3],p[4],p[5]) #print p def residual(mod_basis,uch_basis,y,z,dz): edge = ogp_util.transform_pt(ball_basis,uch_basis,ogp_util.transform_pt(null_basis,mod_basis,axial_edge)) #print edge #nominal = [math.sin(0.0305)*(z+dz-700), y, math.cos(0.0305)*(z+dz-700)+700]