Commit 46c94bfe by Torbjørn Ludvigsen 👷

### Exploring data set number 3, some plotting

parent b9d5aa5b
plotting.py 0 → 100644
 import matplotlib.pyplot as plt # See where_to_move_during_calibration.py to find definition of diffs plt.close("all") plt.plot(diffs[:,2]) # Throw up (blue line) plot with C measurements data plt.title('Expected C length minus measured C length') plt.plot(diffs[:,2],'*') # Also plot some orange stars there plt.tick_params( # Remove the x axis tics and numbering axis='x', # changes apply to the x-axis which='both', # both major and minor ticks are affected bottom='off', # ticks along the bottom edge are off top='off', # ticks along the top edge are off labelbottom='off') # labels along the bottom edge are off ax = plt.gca() # Grab an axis object ax.set_ylabel('C/mm') plt.plot(x, m*x + c, 'r', label='Fitted line') plt.legend()
 ... ... @@ -128,17 +128,21 @@ def cost(anchors, pos, samp): return np.sum(np.abs(samples(anchors, pos, fuzz = 0) - samp)) def cost_sq(anchors, pos, samp): pos[0] = [0.0, 0.0, 0.0] return np.sum(pow((samples(anchors, pos, fuzz = 0) - samp), 2)) # Sum of squares def anchorsvec2matrix(anchorsvec): def anchorsvec2matrix(anchorsvec, z = 0): """ Create a 4x3 anchors matrix from 6 element anchors vector. """ anchors = np.array(np.zeros((4, 3))) anchors[A,Y] = anchorsvec[0]; anchors[A,Z] = z; anchors[B,X] = anchorsvec[1]; anchors[B,Y] = anchorsvec[2]; anchors[B,Z] = z; anchors[C,X] = anchorsvec[3]; anchors[C,Y] = anchorsvec[4]; anchors[C,Z] = z; anchors[D,Z] = anchorsvec[5]; return anchors ... ... @@ -151,7 +155,7 @@ def posvec2matrix(v, u): def posmatrix2vec(m): return np.reshape(m, np.shape(m)[0]*3) def solve(samp, cb, _cost = cost_sq): def solve(samp, cb, _cost = cost_sq, z = 0): """Find reasonable positions and anchors given a set of samples. """ def costx(posvec, anchvec): ... ... @@ -162,7 +166,7 @@ def solve(samp, cb, _cost = cost_sq): x : [A_ay A_bx A_by A_cx A_cy A_dz x1 y1 z1 x2 y2 z2 ... xu yu zu """ anchors = anchorsvec2matrix(anchvec) anchors = anchorsvec2matrix(anchvec, z) pos = np.reshape(posvec, (u,3)) return _cost(anchors, pos, samp) ... ... @@ -175,13 +179,12 @@ def solve(samp, cb, _cost = cost_sq): sin30 = np.sin(30*np.pi/180) number_of_params_pos = 3*u number_of_params_anch = 6 anchors_est = symmetric_anchors(l_anch) # Define bounds lb = [ -l_long, # A_ay > -5000.0 0, # A_bx > 0 -l_long*cos30, # A_bx > -5000*cos(30) 0, # A_by > 0 -l_long*cos30, # A_cx > -5000*cos(30) 0, # A_cx > 0 0, # A_cy > 0 0, # A_dz > 0 -50.0, # x0 > -50.0 ... ... @@ -189,11 +192,11 @@ def solve(samp, cb, _cost = cost_sq): -10, # z0 > -10 ] + [-l_short, -l_short, -10]*(u-1) ub = [ 0, # A_ay < 0 l_long*cos30, # A_bx < 5000.0*cos(30) 0, # A_bx < 0 l_long*sin30, # A_by < 5000.0*sin(30) 0, # A_cx < 0 l_long*cos30, # A_cx < 5000.0*cos(30) l_long*sin30, # A_cy < 5000.0*sin(30) 2*l_long, # A_dz < 10000.0 l_long, # A_dz < 10000.0 50.0, # x0 < 50.0 50.0, # y0 < 50.0 l_short, # z0 < l_short ... ... @@ -202,7 +205,19 @@ def solve(samp, cb, _cost = cost_sq): from mystic.termination import ChangeOverGeneration, NormalizedChangeOverGeneration, VTR from mystic.solvers import DifferentialEvolutionSolver2, PowellDirectionalSolver pos_est0 = np.random.rand(u,3)*l_short - [l_short/2, l_short/2, 0] #pos_est0 = np.random.rand(u,3)*l_short - [l_short/2, l_short/2, 0] #pos_est0 = positions(5*5*5, 0, fuzz = 0) pos_est0 = np.zeros((u,3)) #anchors_est = symmetric_anchors(l_anch) # anchors_est = np.array([[0.0, -2163.0, -75.5], # [-1841.0, 741.0, -75.5], # [1639.0, 1404.0, -75.5], # [0.0, 0.0, 3250.5]]) anchors_est = np.array([[0.0, 0.0, z], [0.0, 0.0, z], [0.0, 0.0, z], [0.0, 0.0, 0]]) x_guess0 = list(anchorsmatrix2vec(anchors_est)) + list(posmatrix2vec(pos_est0)) print("Solver 0") ... ... @@ -216,21 +231,142 @@ def solve(samp, cb, _cost = cost_sq): print("Solver %d" % i) solver0 = PowellDirectionalSolver(number_of_params_pos+number_of_params_anch) solver0.SetInitialPoints(x_guess0) solver0.SetStrictRanges(lb, ub) solver0.Solve(lambda x: costx(x[6:], x[0:6]), callback = cb) x_guess0 = solver0.bestSolution anch_0 = anchorsvec2matrix(solver0.bestSolution[0:6]) #anch_0 = anchorsvec2matrix(solver0.bestSolution[0:6], z) return anch_0 #return anch_0 return solver0.bestSolution if __name__ == "__main__": l_long = 1500 l_short = 400 n = 7 pos_fuzz = 20 anchors = irregular_anchors(l_long, 0.5) pos = positions(n, l_short, fuzz = pos_fuzz) u = np.shape(pos)[0] z = -75.5 # Gotten from manual measuring anchors = np.array([[0.0, -2163.0, -75.5], [-1841.0, 741.0, -75.5], [1639.0, 1404.0, -75.5], [0.0, 0.0, 3250.5]]) # Calculated, not measured # Gives exact solution up to e-8 precision #samp = np.array([[ 0. , 0. , 0. , 0. ], # [-160.05450141, -7.41578381, 508.81184094, -566.25890613], # [-240.31902526, -88.76779973, 449.05946137, -269.65276306], # [-275.807475 , -124.76718348, 422.99492029, 27.57111729], # [ 121.48001868, -147.47176573, 327.9627796 , -583.0761023 ], # [-182.63383271, 234.22786546, 299.45677071, -583.0761023 ], # [ 51.43751 , -235.32008868, 263.75505139, -284.78756451], # [ 20.69285473, -274.3991353 , 235.67481709, 13.81466774], # [-263.85173603, 162.04723258, 234.48460575, -284.78756451], # [-299.78804098, 130.31868502, 206.05693659, 13.81466774], # [ 407.1938119 , -246.58073095, 171.82074196, -566.25890613], # [-160.05450141, 488.67753676, 110.5331805 , -566.25890613], # [ 345.13869583, -339.709284 , 103.18644535, -269.65276306], # [ 101.70771773, 110.37864749, 101.92650368, -600. ], # [ 318.03428538, -381.3285948 , 73.08753928, 27.57111729], # [-240.31902526, 424.12957408, 39.987215 , -269.65276306], # [ 31.03446609, 33.77678835, 31.10338123, -300. ], # [-275.807475 , 395.89476307, 9.01126285, 27.57111729], # [ 389.63442186, 24.02428293, -71.04756549, -583.0761023 ], # [ 121.48001868, 378.19573436, -105.08271722, -583.0761023 ], # [ 327.14199005, -56.00201336, -147.95175501, -284.78756451], # [ 299.8396334 , -91.37870646, -181.86930811, 13.81466774], # [ 51.43751 , 310.5445596 , -183.31098627, -284.78756451], # [ 20.69285473, 280.89543594, -217.84612468, 13.81466774], # [ 407.1938119 , 301.97210143, -297.18630107, -566.25890613], # [ 345.13869583, 231.99721017, -383.85809756, -269.65276306], # [ 318.03428538, 201.28360206, -422.37578312, 27.57111729]]) # Measured3 samp = np.array([[ 0.0 , 0.0 , 0.0 , 0.0], [ -159.88 , -7.42 , 498.96 , -566.56], [ -240.01 , -88.81 , 449.46 , -269.41], [ -275.44 , -124.74 , 427.20 , 27.56], [ 121.50 , -147.40 , 316.38 , -583.50], [ -182.43 , 234.08 , 303.25 , -583.50], [ 51.45 , -235.10 , 261.35 , -284.52], [ 20.70 , -274.11 , 234.92 , 13.81], [ -263.50 , 162.03 , 238.27 , -284.52], [ -299.37 , 130.35 , 212.02 , 13.81], [ 406.84 , -246.34 , 163.49 , -566.57], [ -159.88 , 488.26 , 124.25 , -566.56], [ 344.89 , -339.30 , 104.47 , -269.41], [ 101.72 , 110.40 , 101.92 , -600.54], [ 317.83 , -380.85 , 72.50 , 27.56], [ -240.01 , 423.67 , 58.79 , -269.41], [ 31.05 , 33.78 , 35.93 , -299.71], [ -275.44 , 395.48 , 26.22 , 27.56], [ 389.31 , 24.03 , -75.26 , -583.50]]) # Measured 3 and compensated with constant -2 #samp = np.array([[ 0. , 0. , 0. , 0. ], # [-159.88, -7.42, 496.96, -566.56], # [-240.01, -88.81, 447.46, -269.41], # [-275.44, -124.74, 425.2 , 27.56], # [ 121.5 , -147.4 , 314.38, -583.5 ], # [-182.43, 234.08, 301.25, -583.5 ], # [ 51.45, -235.1 , 259.35, -284.52], # [ 20.7 , -274.11, 232.92, 13.81], # [-263.5 , 162.03, 236.27, -284.52], # [-299.37, 130.35, 210.02, 13.81], # [ 406.84, -246.34, 161.49, -566.57], # [-159.88, 488.26, 122.25, -566.56], # [ 344.89, -339.3 , 102.47, -269.41], # [ 101.72, 110.4 , 99.92, -600.54], # [ 317.83, -380.85, 70.5 , 27.56], # [-240.01, 423.67, 56.79, -269.41], # [ 31.05, 33.78, 33.93, -299.71], # [-275.44, 395.48, 24.22, 27.56], # [ 389.31, 24.03, -77.26, -583.5 ]]) #Compensating -2 mm out of every C measurement reduced errors from #[[ 0. -170.74775148 0. ] # [-229.31816957 150.55807494 0. ] # [-260.75172296 -196.38194866 0. ] # [ 0. 0. -373.52002034]] # to #[[ 0. -139.74915668 0. ] # [-211.47544526 122.72584696 0. ] # [-204.44639263 -140.1629383 0. ] # [ 0. 0. -425.38964298]] # All coordinates except D_z got better # Measured 3 and compensated with -0.64x + 3.85 #samp = np.array([[ 0. , 0. , 0. , 0. ], # [-159.88 , -7.42 , 502.1720535 , -566.56 ], # [-240.01 , -88.81 , 452.03203292, -269.41 ], # [-275.44 , -124.74 , 429.13201234, 27.56 ], # [ 121.5 , -147.4 , 317.67199176, -583.5 ], # [-182.43 , 234.08 , 303.90197117, -583.5 ], # [ 51.45 , -235.1 , 261.36195059, -284.52 ], # [ 20.7 , -274.11 , 234.29193001, 13.81 ], # [-263.5 , 162.03 , 237.00190943, -284.52 ], # [-299.37 , 130.35 , 210.11188885, 13.81 ], # [ 406.84 , -246.34 , 160.94186826, -566.57 ], # [-159.88 , 488.26 , 121.06184768, -566.56 ], # [ 344.89 , -339.3 , 100.6418271 , -269.41 ], # [ 101.72 , 110.4 , 97.45180652, -600.54 ], # [ 317.83 , -380.85 , 67.39178594, 27.56 ], # [-240.01 , 423.67 , 53.04176536, -269.41 ], # [ 31.05 , 33.78 , 29.54174477, -299.71 ], # [-275.44 , 395.48 , 19.19172419, 27.56 ], # [ 389.31 , 24.03 , -82.92829639, -583.5 ]]) #Compensating the linear measurment error reduced anchor errors from #[[ 0. -170.74775148 0. ] # [-229.31816957 150.55807494 0. ] # [-260.75172296 -196.38194866 0. ] # [ 0. 0. -373.52002034]] # to #[[ 0. -81.27827747 0. ] # [-197.95233601 127.92453157 0. ] # [-120.13780415 -76.13893988 0. ] # [ 0. 0. -600.36145758]] # All coordinates except D_z got better. D_z got 22.5 cm worse. u = np.shape(samp)[0] pos = np.zeros((u, 3)) # Plot out real position and anchor import matplotlib.pyplot as plt ... ... @@ -244,7 +380,7 @@ if __name__ == "__main__": ax_anch = fig_anch.add_subplot(111, projection='3d') ax_pos = fig_pos.add_subplot(111, projection='3d') scat_anch0 = ax_anch.scatter(anchors[:,0], anchors[:,1], anchors[:,2], 'ro') scat_pos0 = ax_pos.scatter(pos[:,0], pos[:,1], pos[:,2], 'k+') #scat_pos0 = ax_pos.scatter(pos[:,0], pos[:,1], pos[:,2], 'k+') plt.pause(1) scat_anch = ax_anch.scatter(anchors[:,0], anchors[:,1], anchors[:,2], 'yx') scat_pos = ax_pos.scatter(pos[:,0], pos[:,1], pos[:,2], 'b+') ... ... @@ -258,38 +394,31 @@ if __name__ == "__main__": if len(x) == 6 + 3*u: ps = posvec2matrix(x[6:], u) scat_pos._offsets3d = (ps[:,0], ps[:,1], ps[:,2]) anch = anchorsvec2matrix(x[0:6]) anch = anchorsvec2matrix(x[0:6], z) scat_anch._offsets3d = (anch[:,0], anch[:,1], anch[:,2]) print("Anchor errors: ") print(anchorsvec2matrix(x[0:6]) - anchors) print(anchorsvec2matrix(x[0:6], z) - anchors) print("cost: %f" % \ cost(anchorsvec2matrix(x[0:6]), np.reshape(x[6:], (u,3)), samp)) cost(anchorsvec2matrix(x[0:6], z), np.reshape(x[6:], (u,3)), samp)) elif len(x) == 6: anch = anchorsvec2matrix(x[0:6]) anch = anchorsvec2matrix(x[0:6], z) scat_anch._offsets3d = (anch[:,0], anch[:,1], anch[:,2]) else: ps = posvec2matrix(x, u) scat_pos._offsets3d = (ps[:,0], ps[:,1], ps[:,2]) #scat_pos._offsets3d = (ps[:,0], ps[:,1], ps[:,2]) plt.draw() plt.pause(0.001) iter += 1 sample_fuzz = 2 samp = samples(anchors, pos, fuzz = sample_fuzz) solution = solve(samp, replot) print("Used number of calibration points (mm)") print(n*n*n) print("Movement length along each axis while sampling were ca (mm)") print(l_short) print("With and uncertainty of (mm)") print(pos_fuzz) print("Samples were fuzzed by (mm)") print(sample_fuzz) print("Exact Anchors were: ") print(anchors) solution = solve(samp, replot, z = -75.5) sol_anch = anchorsvec2matrix(solution[0:6], z = -75.5) print("Output Anchors were: ") print(solution) print(sol_anch) print("Anchor errors were: ") print(solution - anchors) print("Real cost were: %f" % cost(solution, pos, samp)) print(sol_anch - anchors) print("cost: %f" % \ cost(anchorsvec2matrix(solution[0:6], z), np.reshape(solution[6:], (u,3)), samp)) #print("Real cost were: %f" % cost(solution, pos, samp)) #plt.pause(2000)
 from simulation import * a = symmetric_anchors(1500) pos = positions(3, 400, 0) samp = samples(a, pos, 0) samp = samp[samp[:,2].argsort()] samp = samp[samp[:,3].argsort(kind='mergesort')] samp_rel = np.zeros(np.shape(samp)) #a = symmetric_anchors(1500) for i in range(1, 27): samp_rel[i] = samp[i] - samp[i-1] # From measurment a = np.array([[0.0, -2163.0, -75.5], [-1841.0, 741.0, -75.5], [1639.0, 1404.0, -75.5], [0.0, 0.0, 3250.5]]) last_movement = -samp[26] # Go back pos = positions(3, 300, 0) samp_exp = samples(a, pos, 0) # Keep the zero measurement as the first row samp_exp_view = samp_exp[1:] # Make index 2 = C motor the sensor motor samp_exp_view = samp_exp_view[samp_exp_view[:,1].argsort()] samp_exp_view = samp_exp_view[samp_exp_view[:,2].argsort(kind='mergesort')] samp_exp[1:] = samp_exp_view # Array samp is gotten from measurements u = np.shape(samp)[0] diffs = samp_exp[0:u] - samp x = np.arange(u) A = np.vstack([x, np.ones(len(x))]).T # A = [[x 1]] m, c = np.linalg.lstsq(A, diffs[:,2])[0] # Linear regression. diffs[:,2] ~= mx + c # Results: # np.mean(diffs[:,2]) = -1.9 # np.std(diffs[:,2]) = 7.95 # m = -0.64 # c = 3.85 # After removing linear error: # np.mean(diffs[:,2]) = 0.2 # np.std(diffs[:,2]) = 7.1
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!