From 1b73641e1895dfe6d016dbd50dad21c02b66bb94 Mon Sep 17 00:00:00 2001
From: Andrea Feletto <andrea@andreafeletto.com>
Date: Tue, 17 Jan 2023 01:55:11 +0100
Subject: [PATCH] cloud and spline generation

---
 cloud.py            |  40 ++++++++
 lasers.py           | 234 ++++++++++++++++++++++++++++++++++++++++++++
 lasers_mat_to_csv.m |  25 +++++
 3 files changed, 299 insertions(+)
 create mode 100644 cloud.py
 create mode 100644 lasers.py
 create mode 100644 lasers_mat_to_csv.m

diff --git a/cloud.py b/cloud.py
new file mode 100644
index 0000000..31bcc96
--- /dev/null
+++ b/cloud.py
@@ -0,0 +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=',')
diff --git a/lasers.py b/lasers.py
new file mode 100644
index 0000000..a3baba2
--- /dev/null
+++ b/lasers.py
@@ -0,0 +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')
diff --git a/lasers_mat_to_csv.m b/lasers_mat_to_csv.m
new file mode 100644
index 0000000..a9509c2
--- /dev/null
+++ b/lasers_mat_to_csv.m
@@ -0,0 +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');
-- 
2.45.2