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
