def plotMagnets(magnets): # calculate B-field on a grid xs = np.linspace(-40, 40, 33) ys = np.linspace(-40, 40, 44) zs = np.linspace(-40, 40, 44) POS0 = np.array([(x, 0, z) for z in zs for x in xs]) POS1 = np.array([(x, y, 0) for y in ys for x in xs]) fig = plt.figure(figsize=(18, 7)) ax1 = fig.add_subplot(131, projection='3d') # 3D-axis ax2 = fig.add_subplot(133) # 2D-axis ax3 = fig.add_subplot(132) # 2D-axis ax2.set_xlabel('x') ax2.set_ylabel('z') Bs = magnets.getB(POS0).reshape(44, 33, 3) #<--VECTORIZED X, Y = np.meshgrid(xs, zs) U, V = Bs[:, :, 0], Bs[:, :, 2] ax2.streamplot(X, Y, U, V, color=np.log(U**2 + V**2)) ax3.set_xlabel('x') ax3.set_ylabel('y') Bs = magnets.getB(POS1).reshape(44, 33, 3) #<--VECTORIZED X, Z = np.meshgrid(xs, ys) U, V = Bs[:, :, 0], Bs[:, :, 1] ax3.streamplot(X, Z, U, V, color=np.log(U**2 + V**2)) displaySystem(magnets, subplotAx=ax1, suppress=True, sensors=sensors, direc=True) plt.show()
def visualizeSetup(self): field_strength = self.balljoint_config['field_strength'] magnet_pos = self.balljoint_config['magnet_pos'] magnet_angle = self.balljoint_config['magnet_angle'] sensor_pos = self.balljoint_config['sensor_pos'] sensor_angle = self.balljoint_config['sensor_angle'] field_strength, magnet_pos_offsets, magnet_angle_offsets, sensor_pos_offsets, sensor_angle_offsets = self.decode( self.initial) for pos in self.sensor_log['position']: magnets = Collection(self.gen_magnets(field_strength,magnet_pos,magnet_pos_offsets, \ magnet_angle,magnet_angle_offsets)) magnets.rotate(angle=pos[0] * 180.0 / math.pi, axis=(1, 0, 0), anchor=(0, 0, 0)) magnets.rotate(angle=pos[1] * 180.0 / math.pi, axis=(0, 1, 0), anchor=(0, 0, 0)) magnets.rotate(angle=pos[2] * 180.0 / math.pi, axis=(0, 0, 1), anchor=(0, 0, 0)) sensors = self.gen_sensors(sensor_pos, sensor_pos_offsets, sensor_angle, sensor_angle_offsets) # calculate B-field on a grid xs = np.linspace(-40, 40, 33) ys = np.linspace(-40, 40, 44) zs = np.linspace(-40, 40, 44) POS0 = np.array([(x, 0, z) for z in zs for x in xs]) POS1 = np.array([(x, y, 0) for y in ys for x in xs]) fig = plt.figure(figsize=(18, 7)) ax1 = fig.add_subplot(131, projection='3d') # 3D-axis ax2 = fig.add_subplot(132) # 2D-axis ax3 = fig.add_subplot(133) # 2D-axis Bs = magnets.getB(POS0).reshape(44, 33, 3) #<--VECTORIZED X, Y = np.meshgrid(xs, ys) U, V = Bs[:, :, 0], Bs[:, :, 2] ax2.streamplot(X, Y, U, V, color=np.log(U**2 + V**2)) Bs = magnets.getB(POS1).reshape(44, 33, 3) #<--VECTORIZED X, Z = np.meshgrid(xs, zs) U, V = Bs[:, :, 0], Bs[:, :, 2] ax3.streamplot(X, Z, U, V, color=np.log(U**2 + V**2)) displaySystem(magnets, subplotAx=ax1, suppress=True, sensors=sensors, direc=True) for sens in sensors: print(sens.getB(magnets)) plt.show()
def Plot(self): if (self.show is True and self.save is not True and self.animate is not True): self.fig = plt.figure() self.ax = self.fig.add_subplot(111, projection='3d') self.ax.plot(self.all_points[0][:, 0], self.all_points[0][:, 1], self.all_points[0][:, 2], color='black') self.ax.set_xlabel("X (mm)") self.ax.set_ylabel("Y (mm)") self.ax.set_zlabel("Z (mm)") if (self.collection is not None): if (self.sensors is not None): magpy.displaySystem(self.collection, subplotAx=self.ax, sensors=self.sensors, suppress=True) else: magpy.displaySystem(self.collection, subplotAx=self.ax, suppress=True) plt.show() else: self.fig = plt.figure() self.ax = p3.Axes3D(self.fig) self.line, = self.ax.plot([], [], [], color='black') self.ax.set_xlabel("X (mm)") self.ax.set_ylabel("Y (mm)") self.ax.set_zlabel("Z (mm)") self.ax.set_xlim3d(self.lim[0]) self.ax.set_ylim3d(self.lim[1]) self.ax.set_zlim3d(self.lim[2]) self.ax.set_title("Proprioceptive Chain") blitting = True if self.show is True and self.rotate is True: blitting = False anim = animation.FuncAnimation(self.fig, self._PlotAnimate, init_func=self._PlotInit, frames=len(self.all_points), interval=500, blit=blitting) if self.show is True: plt.show() if self.save is True: anim.save('videos/' + str(self.filename) + '.mp4', fps=40, extra_args=['-vcodec', 'libx264'])
def compareMagnets(self, magnet_A, magnet_B): # calculate B-field on a grid xs = np.linspace(-25, 25, 50) ys = np.linspace(-25, 25, 50) zs = np.linspace(-25, 25, 50) POS0 = np.array([(x, 0, z) for z in zs for x in xs]) POS1 = np.array([(x, y, 0) for y in ys for x in xs]) fig = plt.figure(figsize=(18, 14)) ax1 = fig.add_subplot(231, projection='3d') # 3D-axis ax2 = fig.add_subplot(233) # 2D-axis ax3 = fig.add_subplot(232) # 2D-axis ax4 = fig.add_subplot(234, projection='3d') # 3D-axis ax5 = fig.add_subplot(236) # 2D-axis ax6 = fig.add_subplot(235) # 2D-axis ax2.set_xlabel('x') ax2.set_ylabel('z') Bs = magnet_A.getB(POS0).reshape(50, 50, 3) #<--VECTORIZED XA, YA = np.meshgrid(xs, zs) UA, VA = Bs[:, :, 0], Bs[:, :, 2] ax2.streamplot(XA, YA, UA, VA, color=np.log(UA**2 + VA**2)) ax3.set_xlabel('x') ax3.set_ylabel('y') Bs = magnet_A.getB(POS1).reshape(50, 50, 3) #<--VECTORIZED XA, ZA = np.meshgrid(xs, ys) UA, VA = Bs[:, :, 0], Bs[:, :, 1] ax3.streamplot(XA, ZA, UA, VA, color=np.log(UA**2 + VA**2)) ax5.set_xlabel('x') ax5.set_ylabel('z') Bs = magnet_B.getB(POS0).reshape(50, 50, 3) #<--VECTORIZED XB, YB = np.meshgrid(xs, zs) UB, VB = Bs[:, :, 0], Bs[:, :, 2] ax5.streamplot(XB, YB, UB, VB, color=np.log(UB**2 + VB**2)) ax6.set_xlabel('x') ax6.set_ylabel('y') Bs = magnet_B.getB(POS1).reshape(50, 50, 3) #<--VECTORIZED XB, ZB = np.meshgrid(xs, ys) UB, VB = Bs[:, :, 0], Bs[:, :, 1] ax6.streamplot(XB, ZB, UB, VB, color=np.log(UB**2 + VB**2)) displaySystem(magnet_A, subplotAx=ax1, suppress=True, sensors=self.sensors, direc=True) displaySystem(magnet_B, subplotAx=ax4, suppress=True, sensors=self.sensors, direc=True) plt.show()
def fitness(self): # calculate new particle scores i = 0 for p in self.particles: sensor_values = np.zeros((self.number_of_sensors,3),dtype=np.float32,order='C') magnets = self.ball_joint.gen_magnets_angle(p['magnet_angles']) for sens,i in zip(self.sensors,range(0,self.number_of_sensors)): value = sens.getB(magnets) if self.normalize_magnetic_field: sensor_values[i]=value/np.linalg.norm(value) else: sensor_values[i]=value p1_gpu = gpuarray.to_gpu(sensor_values) out_gpu = gpuarray.empty(self.number_of_sensors**2, np.float32) number_of_samples = np.int32(self.number_of_sensors) bdim = (16, 16, 1) dx, mx = divmod(number_of_samples, bdim[0]) dy, my = divmod(number_of_samples, bdim[1]) gdim = ( int((dx + (mx>0))), int((dy + (my>0)))) # print(bdim) # print(gdim) self.distance(number_of_samples, p1_gpu, out_gpu, block=bdim, grid=gdim) out = np.reshape(out_gpu.get(),(number_of_samples,number_of_samples)) # sum = 0 # for val in out_gpu.get(): # sum += val # print(out) # print(sum) # print(gpuarray.sum(out_gpu)) score = gpuarray.sum(out_gpu).get() if score > p['personal_best_score']: # print('score of particle %d improved from %d to %d'%(i,p['personal_best_score'],score)) p['personal_best_score'] = score p['personal_best'] = p['magnet_angles'] i+=1 fig = plt.figure(figsize=(9,9)) ax1 = fig.add_subplot(111, projection='3d') displaySystem(magnets, subplotAx=ax1, suppress=True, direc=True) fig.savefig('pics/'+self.target_folder+'/'+p['name']+'/'+'%03d.png'%self.iteration) plt.close(fig) self.status_bar.update(1) # calculate global best score i = 0 for p in self.particles: if p['personal_best_score']>self.global_best_score: self.global_best_score = p['personal_best_score'] self.global_best_particle = i print('new global best score %d of %s'%(self.global_best_score,p['name'])) i+=1
def generate_view(self, show_geom=False): if self.data.size == 1: m_coll = magnet_collection() for m in self.data[0].u_mag_positions: m.set_magnetisation(self.data[0].magnetisation) m_coll += m if show_geom == True: fig = plt.figure(figsize=(9, 5)) ax1 = fig.add_subplot(121, projection='3d') magpy.displaySystem(m_coll.coll, subplotAx=ax1, suppress=True) return plot_view(m_coll, view=None) else: raise ValueError('no support for multidimensional views.')
def _PlotAnimate(self, i): if self.rotate is True: self.ax.view_init(azim=130 + (i * .6)) self.line.set_data(self.all_points[i][:, 0], self.all_points[i][:, 1]) self.line.set_3d_properties(self.all_points[i][:, 2]) if (self.collection is not None): if (self.sensors is not None): magpy.displaySystem(self.collection, subplotAx=self.ax, sensors=self.sensors, suppress=True) else: magpy.displaySystem(self.collection, subplotAx=self.ax, suppress=True) return self.line,
# create figure fig = plt.figure(figsize=(18,7)) ax1 = fig.add_subplot(131, projection='3d') # 3D-axis ax2 = fig.add_subplot(132) # 2D-axis ax3 = fig.add_subplot(133) # 2D-axis Bs = c.getB(POS0).reshape(44,33,3) #<--VECTORIZED X,Y = np.meshgrid(xs,ys) U,V = Bs[:,:,0], Bs[:,:,2] ax2.streamplot(X, Y, U, V, color=np.log(U**2+V**2)) Bs = c.getB(POS1).reshape(44,33,3) #<--VECTORIZED X,Z = np.meshgrid(xs,zs) U,V = Bs[:,:,0], Bs[:,:,2] ax3.streamplot(X, Z, U, V, color=np.log(U**2+V**2)) displaySystem(c, subplotAx=ax1, sensors=sensors, suppress=True, direc=True) plt.show() # result = func(mag_pos) # print(result) print("starting poseestimator") args = [] for i in range(0,num_random_samples): mag_pos = np.zeros(dimx*dimy*dimz) for j in range(0,dimx*dimy*dimz): mag_pos[j] = random.randint(0, 6) args.append(mag_pos) print(args) with Pool(processes=num_processes) as pool:
def func2(iter): c = Collection(gen_magnets(mag_pos)) if axis == 0: rot = [iter - 90, 0, 0] if axis == 1: rot = [0, iter - 90, 0] if axis == 2: rot = [0, 0, iter - 90] c.rotate(rot[0], (1, 0, 0), anchor=(0, 0, 0)) c.rotate(rot[1], (0, 1, 0), anchor=(0, 0, 0)) c.rotate(rot[2], (0, 0, 1), anchor=(0, 0, 0)) b_target = [] for sens in sensors: b_target.append(sens.getB(c)) # calculate B-field on a grid xs = np.linspace(-30, 30, 33) ys = np.linspace(-30, 30, 44) POS = np.array([(x, y, 0) for y in ys for x in xs]) fig = plt.figure(figsize=(24, 15)) ax1 = fig.add_subplot(121, projection='3d') # 3D-axis ax2 = fig.add_subplot(122) # 2D-axis Bs = c.getB(POS).reshape(44, 33, 3) #<--VECTORIZED X, Y = np.meshgrid(xs, ys) U, V = Bs[:, :, 0], Bs[:, :, 1] plt.xlabel("x") plt.ylabel("y") ax2.streamplot(X, Y, U, V, color=np.log(U**2 + V**2)) def func(x): c = Collection(gen_magnets(mag_pos)) c.rotate(x[0], (1, 0, 0), anchor=(0, 0, 0)) c.rotate(x[1], (0, 1, 0), anchor=(0, 0, 0)) c.rotate(x[2], (0, 0, 1), anchor=(0, 0, 0)) b_error = 0 i = 0 for sens in sensors: b_error = b_error + np.linalg.norm(sens.getB(c) - b_target[i]) i = i + 1 # print(b_error) return [b_error, b_error, b_error] if printouts: print("starting pose estimator") res = least_squares(func, [0, 0, 0], bounds=((-360, -360, -360), (360, 360, 360))) angle_error[iter] = ((rot[0] - res.x[0])**2 + (rot[1] - res.x[1])**2 + (rot[2] - res.x[2])**2)**0.5 b_field_error[iter] = res.cost if printouts: print( "target %.3f %.3f %.3f result %.3f %.3f %.3f b-field error %.3f, angle_error %.3f" % (rot[0], rot[1], rot[2], res.x[0], res.x[1], res.x[2], res.cost, angle_error[iter])) c = Collection(gen_magnets(mag_pos)) c.rotate(rot[0], (1, 0, 0), anchor=(0, 0, 0)) c.rotate(rot[1], (0, 1, 0), anchor=(0, 0, 0)) c.rotate(rot[2], (0, 0, 1), anchor=(0, 0, 0)) result = Collection(gen_magnets(mag_pos)) result.rotate(res.x[0], (1, 0, 0), anchor=(0, 0, 0)) result.rotate(res.x[1], (0, 1, 0), anchor=(0, 0, 0)) result.rotate(res.x[2], (0, 0, 1), anchor=(0, 0, 0)) d = Collection(c, result) displaySystem(d, subplotAx=ax1, suppress=True, sensors=sensors, direc=True) if axis == 0: fig.savefig(movie_path + '/x-axis/movie003/' + 'anim%05d.png' % (iter)) if axis == 1: fig.savefig(movie_path + '/y-axis/movie003/' + 'anim%05d.png' % (iter)) if axis == 2: fig.savefig(movie_path + '/z-axis/movie003/' + 'anim%05d.png' % (iter)) return (angle_error[iter], b_field_error[iter])
# Millimeter for lengths # Degree for angles # Millitesla for magnetization/remanence, magnetic moment and magnetic field, # Ampere for currents. #create sources #magnetization is in x dir with magnitude 1 in CHECK UNITS LATER #dimesions are [diameter, height]=[4,5] (mm) #make cylinder with center 5 mm above origin in z direction s1 = magpy.source.magnet.Cylinder(mag=[1, 0, 0], dim=[10, 5], pos=[0, 0, 5]) #makes loop in x-y plane s3 = magpy.source.current.Circular( curr=1, dim=10) #current is one amp clockwise (in z direction), dimension is 10 mm #create collection c = magpy.Collection(s1, s3) #now the system can be moved and manipulated as one #display system #specify markers to show places of interest: markerPos = [(0, 0, 0, 'origin'), (10, 10, 10), (-10, -10, -10)] #Evaluate field at orign print(c.getB([0, 0, 0])) #show the collection magpy.displaySystem(c, markers=markerPos, direc=True)
# taken from https://magpylib.readthedocs.io/en/latest/_pages/2_guideExamples/ import magpylib as magpy from magpylib.source.magnet import Box # fixed magnet parameters M = [0, 0, 1] #magnetization D = [3, 3, 3] #dimension # rotation axis rax = [-1, 1, -1] # magnets with different orientations s1 = Box(mag=M, dim=D, pos=[-6, 0, 4], angle=0, axis=rax) s2 = Box(mag=M, dim=D, pos=[0, 0, 4], angle=45, axis=rax) s3 = Box(mag=M, dim=D, pos=[6, 0, 4], angle=90, axis=rax) # magnets that are rotated differently s4 = Box(mag=M, dim=D, pos=[-6, 0, -4]) s5 = Box(mag=M, dim=D, pos=[0, 0, -4]) s5.rotate(45, rax) s6 = Box(mag=M, dim=D, pos=[6, 0, -4]) s6.rotate(90, rax) # collect all sources c = magpy.Collection(s1, s2, s3, s4, s5, s6) # display collection magpy.displaySystem(c, figsize=(6, 6))
xyz0=0.000001, figureTittle='Main coil ("+z")\ny = 0', compareToCenter=True) if ZCOILSCONNECTORPLOT: plotBxyz(collectionToPlot=connectorsCollection, plotBounds=[-10, 10, -10, 10], orientation='y', orderMagnitude='uT', fieldDif=False, figureSize=[16, 8], nPlotPoints=10, xyz0=0, figureTittle='Connectors field ("+z")\ny = 0', compareToCenter=True) magpy.displaySystem(zCoil) if YCOILPLOT: plotBxyz(collectionToPlot=yCoil, plotBounds=[-10, 10, -10, 10], orientation='x', orderMagnitude='uT', fieldDif=False, figureSize=[16, 8], nPlotPoints=10, xyz0=0.000001, figureTittle='Second coil ("+y")\nx = 0', compareToCenter=True) if YCOILSCONNECTORPLOT: plotBxyz(collectionToPlot=connectorsCollection, plotBounds=[-10, 10, -10, 10],
import magpylib as magpy # create sources s1 = magpy.source.magnet.Cylinder(mag=[1, 1, 0], dim=[4, 5], pos=[0, 0, 5]) s2 = magpy.source.magnet.Box(mag=[0, 0, -1], dim=[1, 2, 3], pos=[0, 0, -5]) s3 = magpy.source.current.Circular(curr=1, dim=10) #create collection c = magpy.Collection(s1, s2, s3) # create sensors se1 = magpy.Sensor(pos=[10, 0, 0]) se2 = magpy.Sensor(pos=[10, 0, 0]) se3 = magpy.Sensor(pos=[10, 0, 0]) se2.rotate(70, [0, 0, 1], anchor=[0, 0, 0]) se3.rotate(140, [0, 0, 1], anchor=[0, 0, 0]) #display system markerPos = [(0, 0, 0, 'origin'), (10, 10, 10), (-10, -10, -10)] magpy.displaySystem(c, sensors=[se1, se2, se3], markers=markerPos)
math.pi / 180 * x_step): for j in np.arange(-math.pi, math.pi, math.pi / 180 * y_step): positions.append([ 25 * math.sin(i) * math.cos(j), 25 * math.sin(i) * math.sin(j), 25 * math.cos(i) ]) pos_offsets.append([0, 0, 0]) angles.append([0, 0, 90]) angle_offsets.append([0, 0, 0]) number_of_sensors = len(positions) print('number_of_sensors %d' % number_of_sensors) start = time.time() sensors = ball.gen_sensors_all(positions, pos_offsets, angles, angle_offsets) if visualize_only: matplotlib.use('TkAgg') displaySystem(magnets, suppress=False, sensors=sensors, direc=True) sys.exit() particle_swarm = ParticleSwarm(50, sensors, ball) status_bar = tqdm(total=100, desc='particle_status', position=1) while particle_swarm.iteration < 100: particle_swarm.step() status_bar.update(1) if particle_swarm.global_best_score == 0: print('%s reached best possible score 0' % particle_swarm.particles[ particle_swarm.global_best_particle]['name']) break magnets = ball.gen_magnets_angle(particle_swarm.particles[ particle_swarm.global_best_particle]['magnet_angles']) ball.plotMagnets(magnets) ball.config['magnet_angle'] = particle_swarm.particles[
x.resample() x.update() pos.text(x.best_pos) if graph_sensor == True: #Save best filter data best_data.append(x.sensor_data) #Update filter data plot if i % 10 == 0 and i > 100: data_plot.line_chart(best_data[-100:-1]) if mode == "Training Mode": #Plot real sensor data real_data_plot.line_chart(sensor_data[0:i]) #Calculate and plot error data_difference.append(np.subtract(sensor_data[i],x.best_data[0])) error_data_plot.line_chart(data_difference) if graph_pose == True: magnet = Box(mag=[0,0,1],dim=[6.35,6.35,6.35],pos=x.best_pos[0],angle=x.best_angle[0],axis=x.best_axis[0]) magpy.displaySystem(magnet,suppress=True) pos_plot.pyplot() plt.close() if mode == "Training Mode": if graph_pose_error == True: pos_difference.append(np.linalg.norm(np.subtract(x.best_pos[0],magnet_pos[i]))) error_plot.line_chart(pos_difference) i = i+1
fig = plt.figure() ax = fig.add_subplot(111,projection='3d') pi = np.pi seg = Segment() seg.bend_angle = pi/2 seg.bend_direction = 3*pi rotvec = [0,0,1] seg.apply_rotvec(rotvec,[1,0,0]) x = seg.bend_line(rotvec) ax.plot(x[0],x[1],x[2],color='black') c = magpy.Collection(seg.magnet) magpy.displaySystem(c,subplotAx=ax,suppress=True) ax.set_xlabel('X') ax.set_ylabel('Y') ax.set_zlabel('Z') ''' ax.set_xlim(-200,200) ax.set_ylim(-200,200) ax.set_zlim(-200,200) ''' plt.show()
def optimizeMagnetArrangement(self, type): positions = [] angles = [] for i in np.arange(0, 360, 30): for j in np.arange(0, 360, 30): positions.append([\ 25*math.sin(i/180.0*math.pi)*math.cos(j/180.0*math.pi),\ 25*math.sin(i/180.0*math.pi)*math.sin(j/180.0*math.pi),\ 25*math.cos(i/180.0*math.pi)\ ]) angles.append([0, 0, 90]) self.sensors = self.gen_sensors_custom(positions, angles) # magnets = self.gen_magnets() # print('number of sensors %d'%len(positions)) # displaySystem(magnets, suppress=False, sensors=self.sensors, direc=True) positions = [] if type == 'posangle': x_init = [0] * self.number_of_magnets * 3 * 2 x_lower_bound = [0] * self.number_of_magnets * 3 * 2 x_upper_bound = [0] * self.number_of_magnets * 3 * 2 for i in range(0, self.number_of_magnets * 3): x_init[i] = random.uniform(-12, 12) x_lower_bound[i] = -12 x_upper_bound[i] = 12 for i in range(self.number_of_magnets * 3, self.number_of_magnets * 3 * 2): x_init[i] = random.uniform(-90, 90) x_lower_bound[i] = -90 x_upper_bound[i] = 90 elif type == 'angle': self.config['field_strength'] = [] self.config['magnet_dimension'] = [] for i in range(0, 360, 100): for j in range(60, 300, 100): positions.append([\ 10*math.sin(i/180.0*math.pi)*math.cos(j/180.0*math.pi),\ 10*math.sin(i/180.0*math.pi)*math.sin(j/180.0*math.pi),\ 10*math.cos(i/180.0*math.pi)\ ]) self.config['field_strength'].append(1300) self.config['magnet_dimension'].append([7, 7, 7]) self.number_of_magnets = len(positions) x_init = [0] * self.number_of_magnets * 3 x_lower_bound = [0] * self.number_of_magnets * 3 x_upper_bound = [0] * self.number_of_magnets * 3 for i in range(0, self.number_of_magnets * 3): x_init[i] = 0 #random.uniform(-90,90) x_lower_bound[i] = -90 x_upper_bound[i] = 90 self.positions = positions print("number_of_magnets: %d" % self.number_of_magnets) positions, angles = self.decodeX(x_init, type) magnets = self.gen_magnets_custom(positions, angles) displaySystem(magnets, suppress=False, direc=True) self.type = type def optimizeFun(x): positions, angles = self.decodeX(x, self.type) magnets = self.gen_magnets_custom(positions, angles) sensor_values = [] for sens in self.sensors: sensor_values.append(sens.getB(magnets)) b_error = 0 for i in range(0, len(sensor_values)): for j in range(i + 1, len(sensor_values)): norm = np.linalg.norm(sensor_values[i] - sensor_values[j]) if (norm > 0.001): b_error += -math.log(norm) else: b_error += 1000 return [b_error] res = least_squares(optimizeFun, x_init,\ bounds = (x_lower_bound, x_upper_bound),\ ftol=1e-8, \ xtol=1e-8,verbose=2,\ max_nfev=self.config['calibration']['max_nfev']) print(res) positions, angles = self.decodeX(res.x, type) magnets = self.gen_magnets_custom(positions, angles) displaySystem(magnets, suppress=False, direc=True)
def calibrateSensor(self): rospy.init_node('BallJoint', anonymous=True) print('calibrating sensor') print('calibration magnet positions') print(self.config['calibration']['magnet_pos']) print('calibration magnet angles') print(self.config['calibration']['magnet_angle']) calibration_status = tqdm(total=len( self.config['calibration']['magnet_pos']), desc='calibration_status', position=0) self.sensor_values = [] sensor_log = { 'magnet_pos': [], 'magnet_angle': [], 'sensor_values': [] } for pos, angle in zip(self.config['calibration']['magnet_pos'], self.config['calibration']['magnet_angle']): print(pos) print(angle) sensor_log['magnet_pos'].append(pos) sensor_log['magnet_angle'].append(angle) magnets = self.gen_magnets_all(self.config['field_strength'], [pos], self.config['magnet_pos_offsets'], [angle], self.config['magnet_angle_offsets']) print('target:') for sens in self.sensors: print(sens.getB(magnets)) displaySystem(magnets, suppress=False, sensors=self.sensors, direc=True) values = [] for i in range(0, self.number_of_sensors): values.append([0, 0, 0]) sample = 0 sensor_record_status = tqdm(total=100, desc='sensor_record_status', position=1) while sample < 100: msg = rospy.wait_for_message( "/roboy/middleware/MagneticSensor", MagneticSensor, timeout=None) if (msg.id == self.config['id']): j = 0 for x, y, z in zip(msg.x, msg.y, msg.z): values[j][0] += x values[j][1] += y values[j][2] += z j += 1 sample += 1 sensor_record_status.update(1) for j in range(0, self.number_of_sensors): values[j][0] /= sample values[j][1] /= sample values[j][2] /= sample self.sensor_values.append(values) sensor_log['sensor_values'].append(values) calibration_status.update(1) print("optimizing: ") initial = [] upper_bound = [] lower_bound = [] for c in self.config['calibration']['optimize']: if c == 'field_strength': self.number_of_parameters = self.number_of_parameters + self.number_of_magnets for i in range(0, self.number_of_magnets): initial.append(1300) upper_bound.append(1600) lower_bound.append(1000) self.calib.append(0) print('\tfield_strength') if c == 'magnet_pos': self.number_of_parameters = self.number_of_parameters + self.number_of_magnets * 3 for i in range(0, self.number_of_magnets * 3): initial.append(0) upper_bound.append(5) lower_bound.append(-5) self.calib.append(1) print('\tmagnet_pos') if c == 'magnet_angle': self.number_of_parameters = self.number_of_parameters + self.number_of_magnets * 3 for i in range(0, self.number_of_magnets * 3): initial.append(0) upper_bound.append(10) lower_bound.append(-10) self.calib.append(2) print('\tmagnet_angle') if c == 'sensor_pos': self.number_of_parameters = self.number_of_parameters + self.number_of_sensors * 3 for i in range(0, self.number_of_sensors * 3): initial.append(0) upper_bound.append(5) lower_bound.append(-5) self.calib.append(3) print('\tsensor_pos') if c == 'sensor_angle': self.number_of_parameters = self.number_of_parameters + self.number_of_sensors * 3 for i in range(0, self.number_of_sensors * 3): initial.append(0) upper_bound.append(10) lower_bound.append(-10) self.calib.append(4) print('\tsensor_angle') print('number_of_magnets: %d\nnumber_of_sensors: %d\nnumber_of_parameters: %d'\ %(self.number_of_magnets,self.number_of_sensors,self.number_of_parameters)) res = least_squares(self.calibrationFunc, initial, bounds = (lower_bound, upper_bound), \ ftol=1e-8, \ xtol=1e-8,verbose=2, \ max_nfev=self.config['calibration']['max_nfev']) field_strength = self.config['field_strength'] magnet_pos = self.config['magnet_pos'] magnet_angle = self.config['magnet_angle'] sensor_pos = self.config['sensor_pos'] sensor_angle = self.config['sensor_angle'] field_strength, magnet_pos_offsets, magnet_angle_offsets, sensor_pos_offsets, sensor_angle_offsets = self.decodeCalibrationX( res.x) sensors = self.gen_sensors_all(sensor_pos, sensor_pos_offsets, sensor_angle, sensor_angle_offsets) print("b_field_error with calibration: %f\n" % self.calibrationFunc(res.x)[0]) j = 0 for target, pos, angle in zip( self.sensor_values, self.config['calibration']['magnet_pos'], self.config['calibration']['magnet_angle']): print( "target b_field for magnet pos %f %f %f magnet angle %f %f %f" % (pos[0], pos[1], pos[2], angle[0], angle[1], angle[2])) i = 0 for sens in sensors: print('%.4f %.4f %.4f' % (target[i][0], target[i][1], target[i][2])) i = i + 1 print("b_field with calibration:") magnets = self.gen_magnets_all(field_strength, [pos], magnet_pos_offsets, [angle], magnet_angle_offsets) for sens in sensors: mag = sens.getB(magnets) print('%.4f %.4f %.4f' % (mag[0], mag[1], mag[2])) print('----------------------------------') j = j + 1 print("\noptimization results:\n") for c in self.calib: if c == 0: print('field_strength') print(field_strength) self.config['field_strength'] = field_strength if c == 1: print('magnet_pos_offsets') print(magnet_pos_offsets) self.config['magnet_pos_offsets'] = magnet_pos_offsets if c == 2: print('magnet_angle_offsets') print(magnet_angle_offsets) self.config['magnet_angle_offsets'] = magnet_angle_offsets if c == 3: print('sensor_pos_offsets') print(sensor_pos_offsets) self.config['sensor_pos_offsets'] = sensor_pos_offsets if c == 4: print('sensor_angle_offsets') print(sensor_angle_offsets) self.config['sensor_angle_offsets'] = sensor_angle_offsets timestamp = time.strftime("%Y%m%d-%H%M%S") sensor_log_file = timestamp + '.log' with open(sensor_log_file, 'w') as file: documents = dump(sensor_log, file) print('sensor log written to ' + sensor_log_file) input("Enter to write optimization to config file %s..." % (self.config_file)) with open(self.config_file, 'w') as file: documents = dump(self.config, file)
#X= determined by Y and Z a=5 # radius parameter of coils (cm) a=a*10.# change to mm # create collection of two magnets s1 = magpy.source.current.Circular( curr = 1, dim =2.*a, pos=[0,0,a/2.]) s2 = magpy.source.current.Circular( curr = 1, dim =2.*a, pos=[0,0,-a/2.]) #ensuring d=a for homogenous magnetic field from hemoltz coils #s1.move([0,0,a/2.]) #s2.move([0,0,-a/2.]) c = magpy.Collection(s1,s2) magpy.displaySystem(c,direc=True) r=[[0,0,z] for z in np.linspace(-a*2.,a*2.,100)] r=np.asarray(r) Bfield=c.getB(r)*10./(c.getB([0,0,0])*10) #convert from mT to guass plt.plot(r[:,2]/a,np.abs(1.-Bfield[:,2])) #plot z comp of field along z axis plt.show() # # create positions # xs = np.linspace(-8,8,100) ; dxs=xs[1]-xs[0] # zs = np.linspace(-6,6,100) ; dzs=zs[1]-zs[0] # posis = [[x,0,z] for z in zs for x in xs] #increments x first #
dim = [6.35, 6.35, 6.35] sen = [] loops = 150 for i in range(5, loops): b = Box(mag=mag, dim=dim, pos=data[i][0], angle=data[i][1], axis=data[i][2]) sen.append(magpy.Sensor(pos=data[i][0], angle=data[i][1], axis=data[i][2])) col = Collection(b) if (i == (loops - 1)): h = 1 magpy.displaySystem(col, sensors=sen) fig = plt.figure() ax = fig.add_subplot(111, projection='3d') length = 3 for i in range(5, loops): x = [1, 0, 0] y = [0, 1, 0] z = [0, 0, 1] pos = data[i][0] axis = data[i][2] axis = axis / np.linalg.norm(axis) angle = np.radians(data[i][1]) r = R.from_rotvec(axis * angle)
# taken from https://magpylib.readthedocs.io/en/latest/_pages/2_guideExamples/ from magpylib.source.magnet import Box import magpylib as magpy # fixed magnet parameters M = [0, 0, 1] #magnetization D = [4, 2, 2] #dimension # magnets with Euler angle orientations s1 = Box(mag=M, dim=D, pos=[-4, 0, 4]) s2 = Box(mag=M, dim=D, pos=[4, 0, 4], angle=45, axis=[0, 0, 1]) s3 = Box(mag=M, dim=D, pos=[-4, 0, -4], angle=45, axis=[0, 1, 0]) s4 = Box(mag=M, dim=D, pos=[4, 0, -4], angle=45, axis=[1, 0, 0]) # collection c = magpy.Collection(s1, s2, s3, s4) # display collection magpy.displaySystem(c, direc=True, figsize=(6, 6))
# magnet collection definition m1 = Cylinder(mag=[0, 0, 1300], # 1300mT of magnetization along z axis dim=[10, 20], # 10mm diameter, 20mm height pos=[0, 0, -20]) # center is at z = -20mm m2 = Cylinder(mag=[0, 0, 1300], # 1300mT of magnetization along z axis dim=[10, 20], # 10mm diameter, 20mm height pos=[0, 0, 20]) # center is at z = 20mm both = Collection(m1, m2) # arrangement with both magnets # magnet collection visualisation displaySystem(both, suppress=True) # 3D plot of B and F in a 3D space plot_3D(xs=linspace(-10, 10, 9), ys=linspace(-10, 10, 9), zs=linspace(-20, 20, 9), collections={'z-20': m1, 'both': both}, sample=sample, BF='BF', saveCSV=False, showim=True)
sL_1.move((-mag_size_x,0,0)) sR_1.move((mag_size_x,0,0)) sL_2.move((-2*mag_size_x,0,0)) sR_2.move((2*mag_size_x,0,0)) # create collection c = Collection(sL,sL_1,sL_2,sR,sR_1,sR_2) # calculate B-field on a grid xs = np.linspace(-10,10,33) zs = np.linspace(-10,10,44) POS = np.array([(x,0,z) for z in zs for x in xs]) Bs = c.getB(POS).reshape(44,33,3) #<--VECTORIZED # create figure fig = plt.figure(figsize=(9,5)) ax1 = fig.add_subplot(121, projection='3d') # 3D-axis ax2 = fig.add_subplot(122) # 2D-axis # display system geometry on ax1 displaySystem(c, subplotAx=ax1, suppress=True) # display field in xz-plane using matplotlib X,Z = np.meshgrid(xs,zs) U,V = Bs[:,:,0], Bs[:,:,2] ax2.streamplot(X, Z, U, V, color=np.log(U**2+V**2)) print(c.getB([0,0,2])) plt.show()
posis = np.array([(x, 0, z) for z in ts for x in ts]) X, Y = np.meshgrid(ts, ts) # create the source objects s1 = magpy.source.magnet.Box(mag=[500, 0, 500], dim=[4, 4, 4]) #Box s2 = magpy.source.magnet.Cylinder(mag=[0, 0, 500], dim=[3, 5]) #Cylinder s3 = magpy.source.magnet.Sphere(mag=[-200, 0, 500], dim=5) #Sphere s4 = magpy.source.current.Line(curr=10, vertices=[(0, -5, 0), (0, 5, 0)]) #Line s5 = magpy.source.current.Circular(curr=10, dim=5) #Circular s6 = magpy.source.moment.Dipole(moment=[0, 0, 100]) #Dipole for i, s in enumerate([s1, s2, s3, s4, s5, s6]): # display system on respective axes, use marker to zoom out magpy.displaySystem(s, subplotAx=axsA[i], markers=[(6, 0, 6)], suppress=True) axsA[i].plot([-6, 6, 6, -6, -6], [0, 0, 0, 0, 0], [-6, -6, 6, 6, -6]) # plot field on respective axes B = np.array([s.getB(p) for p in posis]).reshape(50, 50, 3) axsB[i].pcolor(X, Y, np.linalg.norm(B, axis=2), cmap=plt.cm.get_cmap('coolwarm')) # amplitude axsB[i].streamplot(X, Y, B[:, :, 0], B[:, :, 2], color='k', linewidth=1) # field lines plt.show()
piv4 = [-7,0,-5] anch4 = [-7,0,-2] s4 = Box(mag=M, dim=D, pos = [-7,-3,-5]) piv5 = [0,0,-5] anch5 = [0,0,-2] s5 = Box(mag=M, dim=D, pos = [0,-3,-5]) s5.rotate(-45,[0,0,1],anchor=anch5) piv6 = [7,0,-5] anch6 = [7,0,-8] s6 = Box(mag=M, dim=D, pos = [7,-3,-5]) s6.rotate(-45,[0,0,1],anchor=anch6) # collect all sources c = magpy.Collection(s1,s2,s3,s4,s5,s6) # draw rotation axes for x in [-7,0,7]: for z in [-5,5]: ax.plot([x,x],[0,0],[z-3,z+4],color='.3') # define markers Ms = [piv1+['piv1'], piv2+['piv2'], piv3+['piv3'], piv4+['piv4'], piv5+['piv5'], piv6+['piv6'], anch4+['anch4'],anch5+['anch5'],anch6+['anch6']] # display system magpy.displaySystem(c,subplotAx=ax,markers=Ms,suppress=True) plt.show()
m1 = Cylinder( mag=[0, 0, 1300], # 1300mT of magnetization along z axis dim=[10, 20], # 10mm diameter, 20mm height pos=[0, 0, -20]) # center is at z = -20mm m2 = Cylinder( mag=[0, 0, 1300], # 1300mT of magnetization along z axis dim=[10, 20], # 10mm diameter, 20mm height pos=[0, 0, 20]) # center is at z = 20mm both = Collection(m1, m2) # arrangement with both magnets # magnet collection visualisation displaySystem(both, suppress=False) # calculate magnetization imposed in the sample if its center was on a given point point = (0, 0, 0) M_000_m1 = getM(point, m1, sample) print(f'M_000_m1 = {M_000_m1}') M_000_m2 = getM(point, m2, sample) print(f'M_000_m2 = {M_000_m2}') M_000_both = getM(point, both, sample) print(f'M_000_both = {M_000_both}') # calculate force imposed in the sample if its center was on a given point