@@ 1,40 @@
+
+import numpy as np
+import scipy as sp
+import scipy.linalg
+import scipy.spatial
+
+rng = np.random.default_rng(23051998)
+
+n_samples = 80000
+
+displacements = np.column_stack([
+ rng.uniform(-40e-6, 40e-6, n_samples), # x
+ rng.uniform(-40e-6, 40e-6, n_samples), # y
+ rng.uniform(-40e-6, 20e-6, n_samples), # z
+])
+
+angles = np.column_stack([
+ rng.uniform(-1e-3, 1e-3, n_samples), # x
+ rng.uniform(-1e-3, 1e-3, n_samples), # y
+ rng.uniform(-5e-3, 5e-3, n_samples), # z
+])
+
+contact_base = np.array([
+ [4e-3, 0, 21.9e-3],
+ [-4e-3, 0, 21.9e-3],
+ [0, 4e-3, 21.9e-3],
+ [0, -4e-3, 21.9e-3],
+])
+offsets = rng.choice(contact_base, n_samples)
+
+rotations = sp.spatial.transform.Rotation.from_euler('xyz', angles)
+contacts = rotations.apply(displacements + offsets)
+
+error = sp.linalg.norm(contact_base.reshape(4, 1, 3) - contacts, axis=-1)
+mask = np.any(error < 30e-6, axis=0)
+
+output = np.zeros((mask.sum(), 6))
+output[:, :3] = displacements[mask]
+output[:, 3:] = rotations[mask].as_euler('zxz')
+np.savetxt('cloud.csv', output, delimiter=',')
@@ 1,234 @@
+
+import os
+
+import matplotlib.pyplot as plt
+import numpy as np
+import pandas as pd
+import scipy as sp
+import scipy.linalg
+import scipy.optimize
+import skspatial as sks
+import skspatial.objects
+
+
+# plunger dimensions in [um]
+plunger_length = 69950
+plunger_radius = 5000
+plunger_head = 5200
+plunger_base = 9500
+
+
+def signal_to_keypoints(data):
+ '''
+ Convert distances to contact points between laser beams and plunger.
+ '''
+
+ points = np.zeros((data.shape[0], 4, 3))
+
+ points[:, 0, 0] = plunger_radius - data.x_base
+ points[:, 0, 2] = plunger_base
+
+ points[:, 1, 0] = plunger_radius - data.x_head
+ points[:, 1, 2] = plunger_head
+
+ points[:, 2, 1] = plunger_radius - data.y_base
+ points[:, 2, 2] = plunger_base
+
+ points[:, 3, 1] = plunger_radius - data.y_head
+ points[:, 3, 2] = plunger_head
+
+ return points
+
+
+def cylindricity_err_mss(axis_points, keypoints):
+ P0, P1 = np.zeros((2, 3))
+
+ # point on the plunger axis at z=0
+ P0[:2] = axis_points[:2]
+
+ # point on the plunger axis at z=L
+ P1[:2] = axis_points[2:]
+ P1[2] = plunger_length
+
+ # direction unit vector of the plunger axis
+ axis = (P1 - P0) / sp.linalg.norm(P1 - P0)
+
+ # distance between keypoints and plunger axis
+ dists = sp.linalg.norm(np.cross(axis, keypoints - P0), axis=-1)
+
+ return np.mean((dists - plunger_radius) ** 2)
+
+
+def keypoints_to_axis_points(keypoints):
+ '''
+ Find the axis of the plunger such that the distance to every keypoint is
+ equal to the radius.
+ The two points are at z=0 and z=L.
+ '''
+
+ # the optimization parameters are x and y for each point
+ axis_points = np.zeros((keypoints.shape[0] + 1, 4))
+
+ for i, keypoint in enumerate(keypoints):
+ result = sp.optimize.minimize(
+ cylindricity_err_mss,
+ x0=axis_points[i],
+ args=(keypoint,),
+ )
+ axis_points[i + 1] = result.x
+
+ # discard the first row and add the z coordinate
+ result = np.zeros((2, keypoints.shape[0], 3))
+ result[0, :, :2] = axis_points[1:, :2]
+ result[1, :, :2] = axis_points[1:, 2:]
+ result[1, :, 2] = plunger_length
+
+ return result
+
+
+def axis_points_to_dofs(data, axis_points):
+ '''
+ Use the z measurement to position the plunger along the axis defined by
+ axis_point.
+ Return the (x, y, z, rx, ry) coordinates of the plunger.
+ '''
+
+ P0, P1 = axis_points
+
+ # normal of the back plane
+ n = (P1 - P0) / sp.linalg.norm(P1 - P0, axis=1).reshape(-1, 1)
+
+ # point on back plane at (x, y) = (0, 0)
+ P_laser = np.zeros((data.shape[0], 3))
+ P_laser[:, 2] = plunger_length - data.z_back
+
+ # intersection between the normal and the back plane
+ # see: https://en.wikipedia.org/wiki/Line-plane_intersection
+ t = np.einsum('ij,ij->i', P_laser - P0, n)
+ back_center = P0 + n * t.reshape(-1, 1)
+
+ # convert to (x, y, z, rx, ry) coordinates
+ tip_center = back_center - n * plunger_length
+ rot_y = np.arcsin(n[:, 0])
+ rot_x = np.arcsin(n[:, 1])
+
+ return pd.DataFrame({
+ 'x': tip_center[:, 0],
+ 'y': tip_center[:, 1],
+ 'z': tip_center[:, 2],
+ 'rx': rot_x,
+ 'ry': rot_y,
+ })
+
+
+def signal_to_dofs(data):
+ '''
+ Process laser measurements into (x, y, z, rx, ry) coordinates.
+ '''
+
+ keypoints = signal_to_keypoints(data)
+ axis_points = keypoints_to_axis_points(keypoints)
+ return axis_points_to_dofs(data, axis_points)
+
+
+def dofs_to_signal(dofs):
+ '''
+ Generate laser measurements from the coordinates.
+ '''
+
+ tip_center = dofs[['x', 'y', 'z']].values
+ rot_x = dofs.rx.values
+ rot_y = dofs.ry.values
+
+ n = np.zeros((dofs.shape[0], 3))
+ n[:, 0] = np.sin(rot_y)
+ n[:, 1] = np.sin(rot_x)
+ n[:, 2] = 1 - n[:, 0] ** 2 - n[:, 1] ** 2
+
+ line_x_base = sks.objects.Line([0, 0, plunger_base], [1, 0, 0])
+ line_x_head = sks.objects.Line([0, 0, plunger_head], [1, 0, 0])
+ line_y_base = sks.objects.Line([0, 0, plunger_base], [0, 1, 0])
+ line_y_head = sks.objects.Line([0, 0, plunger_head], [0, 1, 0])
+ line_z_back = sks.objects.Line([0, 0, 0], [0, 0, 1])
+
+ R = plunger_radius
+ L = plunger_length
+
+ lasers = np.zeros((dofs.shape[0], 5))
+ for i in range(dofs.shape[0]):
+ cyl = sks.objects.Cylinder(tip_center[i], n[i] * plunger_length, plunger_radius)
+ lasers[i, 0] = R - cyl.intersect_line(line_x_base)[1][0]
+ lasers[i, 1] = R - cyl.intersect_line(line_x_head)[1][0]
+ lasers[i, 2] = R - cyl.intersect_line(line_y_base)[1][1]
+ lasers[i, 3] = R - cyl.intersect_line(line_y_head)[1][1]
+ lasers[i, 4] = L - cyl.intersect_line(line_z_back, infinite=False)[1][2]
+
+ return lasers
+
+
+def dofs_to_splines(dofs):
+ '''
+ Make a dataframe for each dof.
+ Add a time column with sampling period 4us.
+ Convert dof from [um] to [m].
+ '''
+
+ data = dofs.copy()
+ data[['x', 'y', 'z']] *= 1e-6
+ time = np.arange(dofs.shape[0]) * 4e-6
+ return [
+ pd.DataFrame({'time': time, column: data[column]})
+ for column in data
+ ]
+
+
+def save_spline(spline, kind):
+ '''
+ Serialize to csv an plot the curve.
+ '''
+ name = spline.columns[1]
+ spline.to_csv(f'splines/{kind}/{name}.csv', index=False)
+
+ unit = 'rad' if name.startswith('r') else 'm'
+ plt.plot(spline.time * 1e6, spline[name] * 1e6)
+ plt.xlabel('time ($\mu s$)')
+ plt.ylabel(f'{name} ($\mu {unit}$)')
+ plt.savefig(f'splines/{kind}/{name}.png')
+ plt.close()
+
+
+cone = pd.read_csv('lasers/cone.csv')
+pyramid = pd.read_csv('lasers/pyramid.csv')
+
+# downsample and only keep high amplitude section
+cone = cone.iloc[1000:2300:4]
+pyramid = pyramid.iloc[1000:2300:4]
+
+cone_dofs = signal_to_dofs(cone)
+pyramid_dofs = signal_to_dofs(pyramid)
+
+cone_lasers = dofs_to_signal(cone_dofs)
+pyramid_lasers = dofs_to_signal(pyramid_dofs)
+
+cone_check = np.isclose(cone, cone_lasers, rtol=1e-3, atol=1e-3).all()
+pyramid_check = np.isclose(pyramid, pyramid_lasers, rtol=1e-3, atol=1e-3).all()
+
+if not cone_check:
+ print('test failed: cone')
+ quit()
+
+if not pyramid_check:
+ print('test failed: pyramid')
+ quit()
+
+cone_splines = dofs_to_splines(cone_dofs)
+pyramid_splines = dofs_to_splines(pyramid_dofs)
+
+os.makedirs('splines/cone', exist_ok=True)
+os.makedirs('splines/pyramid', exist_ok=True)
+
+for spline in cone_splines:
+ save_spline(spline, 'cone')
+
+for spline in pyramid_splines:
+ save_spline(spline, 'pyramid')
@@ 1,25 @@
+
+load('lasers/measurements.mat');
+
+columns = {'x_base' 'x_head' 'y_base' 'y_head' 'z_back'};
+
+cone = table( ...
+ data_new.xz.con.base.mean(:, 2), ...
+ data_new.xz.con.head.mean(:, 2), ...
+ data_new.yz.con.base.mean(:, 2), ...
+ data_new.yz.con.head.mean(:, 2), ...
+ data_new.xz.con.base.mean(:, 1), ...
+ 'VariableNames', columns ...
+);
+
+pyramid = table( ...
+ data_new.xz.pyr.base.mean(:, 2), ...
+ data_new.xz.pyr.head.mean(:, 2), ...
+ data_new.yz.pyr.base.mean(:, 2), ...
+ data_new.yz.pyr.head.mean(:, 2), ...
+ data_new.xz.pyr.base.mean(:, 1), ...
+ 'VariableNames', columns ...
+);
+
+writetable(cone, 'lasers/cone.csv');
+writetable(pyramid, 'lasers/pyramid.csv');