This page was generated by nbsphinx from docs/notebooks/transform/lapd_xy_transform.ipynb.
Demo of LaPDXYTransformο
[1]:
%matplotlib inline
[2]:
import numpy as np
import matplotlib.pyplot as plt
import sys
plt.rcParams["figure.figsize"] = [10.5, 0.56 * 10.5]
[3]:
try:
from bapsf_motion.transform import LaPDXYTransform
except ModuleNotFoundError:
from pathlib import Path
HERE = Path().cwd()
BAPSF_MOTION = (HERE / ".." / ".." / ".." ).resolve()
sys.path.append(str(BAPSF_MOTION))
from bapsf_motion.transform import LaPDXYTransform
[4]:
tr = LaPDXYTransform(
("x", "y"),
pivot_to_center=57.288,
pivot_to_drive=134.0,
pivot_to_feedthru=21.6,
# probe_axis_offset=10.00125,
probe_axis_offset=20.16125,
droop_correct=False,
)
[5]:
figwidth, figheight = plt.rcParams["figure.figsize"]
figwidth = 1.4 * figwidth
figheight = 2.0 * figheight
fig, axs = plt.subplots(2, 3, figsize=[figwidth, figheight])
axs[0,0].set_xlabel("MSpace X")
axs[0,0].set_ylabel("MSpace Y")
axs[0,1].set_xlabel("Drive X")
axs[0,1].set_ylabel("Drive Y")
axs[0,2].set_xlabel("MSpace X")
axs[0,2].set_ylabel("MSpace Y")
points = np.zeros((40, 2))
points[0:10, 0] = np.linspace(-5, 5, num=10, endpoint=False)
points[0:10, 1] = 5 * np.ones(10)
points[10:20, 0] = 5 * np.ones(10)
points[10:20, 1] = np.linspace(5, -5, num=10, endpoint=False)
points[20:30, 0] = np.linspace(5, -5, num=10, endpoint=False)
points[20:30, 1] = -5 * np.ones(10)
points[30:40, 0] = -5 * np.ones(10)
points[30:40, 1] = np.linspace(-5, 5, num=10, endpoint=False)
dpoints = tr(points, to_coords="drive")
mpoints = tr(dpoints, to_coords="motion_space")
axs[0,0].fill(points[...,0], points[...,1])
axs[0,1].fill(dpoints[...,0], dpoints[...,1])
axs[0,2].fill(mpoints[...,0], mpoints[...,1])
for pt, color in zip(
[
[-5, 5],
[-5, -5],
[5, -5],
[5, 5],
[0, 0]
],
["red", "orange", "green", "purple", "black"]
):
dpt = tr(pt, to_coords="drive")
mpt = tr(dpt, to_coords="motion_space")
print(pt, dpt, mpt)
axs[0,0].plot(pt[0], pt[1], 'o', color=color)
axs[0,1].plot(dpt[..., 0], dpt[..., 1], 'o', color=color)
axs[0,2].plot(mpt[..., 0], mpt[..., 1], 'o', color=color)
##
axs[1,0].set_xlabel("Drive X")
axs[1,0].set_ylabel("Drive Y")
axs[1,1].set_xlabel("MSpace X")
axs[1,1].set_ylabel("MSpace Y")
axs[1,2].set_xlabel("Drive X")
axs[1,2].set_ylabel("Drive Y")
points = np.zeros((40, 2))
points[0:10, 0] = np.linspace(-5, 5, num=10, endpoint=False)
points[0:10, 1] = 5 * np.ones(10)
points[10:20, 0] = 5 * np.ones(10)
points[10:20, 1] = np.linspace(5, -5, num=10, endpoint=False)
points[20:30, 0] = np.linspace(5, -5, num=10, endpoint=False)
points[20:30, 1] = -5 * np.ones(10)
points[30:40, 0] = -5 * np.ones(10)
points[30:40, 1] = np.linspace(-5, 5, num=10, endpoint=False)
mpoints = tr(points, to_coords="motion_space")
dpoints = tr(mpoints, to_coords="drive")
axs[1,0].fill(points[...,0], points[...,1])
axs[1,1].fill(mpoints[...,0], mpoints[...,1])
axs[1,2].fill(dpoints[...,0], dpoints[...,1])
for pt, color in zip(
[
[-5, 5],
[-5, -5],
[5, -5],
[5, 5],
[0, 0]
],
["red", "orange", "green", "purple", "black"]
):
mpt = tr(pt, to_coords="motion_space")
dpt = tr(mpt, to_coords="drive")
axs[1,0].plot(pt[0], pt[1], 'o', color=color)
axs[1,1].plot(mpt[..., 0], mpt[..., 1], 'o', color=color)
axs[1,2].plot(dpt[..., 0], dpt[..., 1], 'o', color=color)
print(f"X = {pt[0]} Ξ = {dpt[...,0] - pt[0]} || Y = {pt[1]} Ξ = {dpt[...,1] - pt[1]}")
[-5, 5] [[ 5.20035847 -10.82133761]] [[-5. 5.]]
[-5, -5] [[ 5.20035847 10.69163439]] [[-5. -5.]]
[5, -5] [[-4.76148342 12.72168007]] [[ 5. -5.]]
[5, 5] [[ -4.76148342 -12.90561491]] [[5. 5.]]
[0, 0] [[0. 0.]] [[ 0.00000000e+00 -1.59006142e-15]]
X = -5 Ξ = [0.] || Y = 5 Ξ = [-6.21724894e-15]
X = -5 Ξ = [0.] || Y = -5 Ξ = [8.8817842e-16]
X = 5 Ξ = [0.] || Y = -5 Ξ = [8.8817842e-16]
X = 5 Ξ = [0.] || Y = 5 Ξ = [-6.21724894e-15]
X = 0 Ξ = [0.] || Y = 0 Ξ = [3.71924713e-15]
Test Transforming drive -> motion space -> driveο
[6]:
mpoints = tr(points, to_coords="motion_space")
dpoints = tr(mpoints, to_coords="drive")
(
np.allclose(dpoints, points),
np.allclose(dpoints[...,0], points[...,0]),
np.allclose(dpoints[...,1], points[...,1]),
np.min(dpoints - points),
np.max(dpoints - points),
)
[6]:
(True,
True,
True,
np.float64(-7.105427357601002e-15),
np.float64(7.105427357601002e-15))
[7]:
points = np.array([[5, 5], [5, 5]])
mpoints = tr(points, to_coords="motion_space")
dpoints = tr(mpoints, to_coords="drive")
(
np.isclose(dpoints, points),
np.allclose(dpoints, points),
np.allclose(dpoints[...,0], points[...,0]),
np.allclose(dpoints[...,1], points[...,1]),
np.min(dpoints - points),
np.max(dpoints - points),
)
[7]:
(array([[ True, True],
[ True, True]]),
True,
True,
True,
np.float64(-6.217248937900877e-15),
np.float64(0.0))
Test Transforming motion space -> drive -> motion spaceο
[8]:
dpoints = tr(points, to_coords="drive")
mpoints = tr(dpoints, to_coords="motion_space")
(
np.allclose(mpoints, points),
np.allclose(mpoints[...,0], points[...,0]),
np.allclose(mpoints[...,1], points[...,1]),
np.min(mpoints - points),
np.max(mpoints - points),
)
[8]:
(True,
True,
True,
np.float64(-1.7763568394002505e-15),
np.float64(-8.881784197001252e-16))
Prototypingο
[9]:
pts = [
[-5, 5],
[-5, -5],
[5, -5],
[5, 5],
[0, 0]
]
# pts = [[-5, 5]]
pts = tr._condition_points(pts)
matrix = tr.matrix(pts, to_coords="mspace")
pts = np.concatenate(
(pts, np.ones((pts.shape[0], 1))),
axis=1,
)
results = np.einsum("kmn,kn->km", matrix, pts)[:-1,...]
ii = 1
# pts[ii, ...]
(pts[ii,...], results[ii,...])
[9]:
(array([-5., -5., 1.]), array([5.03615969, 1.94425505, 1. ]))
[10]:
matrix[ii, ...]
[10]:
array([[-0.99930845, 0. , 0.03961743],
[ 0.03718358, 0. , 2.13017296],
[ 0. , 0. , 1. ]])
[11]:
(
pts[ii, :-1],
tr(pts[ii, :-1], to_coords="mspace"),
)
[11]:
(array([-5., -5.]), array([[5.03615969, 1.94425505]]))
[12]:
tr(pts[ii, :-1], to_coords="mspace")
[12]:
array([[5.03615969, 1.94425505]])
Testing Matrix Mathο
[13]:
pivot_to_center = 57.288
pivot_to_drive = 134.0
drive_polarity = np.array([1.0, 1.0])
mspace_polarity = np.array([-1.0, 1.0])
[14]:
def matrix_to_mspace(
points,
pivot_to_center,
pivot_to_drive,
drive_polarity,
mspace_polarity,
):
points = drive_polarity * points # type: np.ndarray
theta = np.arctan(points[..., 1] / pivot_to_drive)
alpha = np.pi - theta
npoints = 1 if points.ndim == 1 else points.shape[0]
T1 = np.zeros((npoints, 3, 3)).squeeze()
T1[..., 0, 0] = np.cos(theta)
T1[..., 0, 2] = -pivot_to_drive * np.cos(theta)
T1[..., 1, 0] = -np.sin(theta)
T1[..., 1, 2] = pivot_to_drive * np.sin(theta)
T1[..., 2, 2] = 1.0
T2 = np.zeros((npoints, 3, 3)).squeeze()
T2[..., 0, 0] = 1.0
T2[..., 0, 2] = -(pivot_to_drive + pivot_to_center) * np.cos(alpha)
T2[..., 1, 1] = 1.0
T2[..., 1, 2] = -(pivot_to_drive + pivot_to_center) * np.sin(alpha)
T2[..., 2, 2] = 1.0
T3 = np.zeros((npoints, 3, 3)).squeeze()
T3[..., 0, 0] = 1.0
T3[..., 0, 2] = -pivot_to_center
T3[..., 1, 1] = 1.0
T3[..., 2, 2] = 1.0
# return T1, T2, T3
T_dpolarity = np.diag(drive_polarity.tolist() + [1.0])
T_mpolarity = np.diag(mspace_polarity.tolist() + [1.0])
return np.matmul(
T_mpolarity,
np.matmul(
T3,
np.matmul(
T2,
np.matmul(T1, T_dpolarity),
),
),
)
[15]:
def matrix_to_drive(
points,
pivot_to_center,
pivot_to_drive,
drive_polarity,
mspace_polarity,
):
points = mspace_polarity * points # type: np.ndarray
# need to handle when x_L = pivot_to_center
# since alpha can never be 90deg we done need to worry about that case
alpha = np.arctan(points[..., 1] / (pivot_to_center + points[...,0]))
npoints = 1 if points.ndim == 1 else points.shape[0]
T1 = np.zeros((npoints, 3, 3)).squeeze()
T1[..., 0, 0] = 1.0
T1[..., 0, 2] = pivot_to_center
T1[..., 1, 1] = 1.0
T1[..., 2, 2] = 1.0
T2 = np.zeros((npoints, 3, 3)).squeeze()
T2[..., 0, 0] = 1.0
T2[..., 0, 2] = -(pivot_to_drive + pivot_to_center) * np.cos(alpha)
T2[..., 1, 1] = 1.0
T2[..., 1, 2] = -(pivot_to_drive + pivot_to_center) * np.sin(alpha)
T2[..., 2, 2] = 1.0
T3 = np.zeros((npoints, 3, 3)).squeeze()
T3[..., 0, 0] = 1 / np.cos(alpha)
T3[..., 0, 2] = pivot_to_drive
T3[..., 1, 2] = -pivot_to_drive * np.tan(alpha)
T3[..., 2, 2] = 1.0
# return T1, T2, T3
T_dpolarity = np.diag(drive_polarity.tolist() + [1.0])
T_mpolarity = np.diag(mspace_polarity.tolist() + [1.0])
return np.matmul(
T_dpolarity,
np.matmul(
T3,
np.matmul(
T2,
np.matmul(T1, T_mpolarity),
),
),
)
[16]:
def convert(
points,
pivot_to_center,
pivot_to_drive,
drive_polarity,
mspace_polarity,
to_coord="drive",
):
if not isinstance(points, np.ndarray):
points = np.array(points)
if to_coord == "drive":
matrix = matrix_to_drive(
points,
pivot_to_center=pivot_to_center,
pivot_to_drive=pivot_to_drive,
drive_polarity=drive_polarity,
mspace_polarity=mspace_polarity,
)
elif to_coord == "motion_space":
matrix = matrix_to_mspace(
points,
pivot_to_center=pivot_to_center,
pivot_to_drive=pivot_to_drive,
drive_polarity=drive_polarity,
mspace_polarity=mspace_polarity,
)
else:
raise ValueError
if points.ndim == 1:
points = np.concatenate((points, [1]))
return np.matmul(matrix, points)[:2]
points = np.concatenate(
(points, np.ones((points.shape[0], 1))),
axis=1,
)
return np.einsum("kmn,kn->km", matrix, points)[..., :2]
[17]:
point = np.array([[0, 0], [1,2], [3,4], [-1, -1]])
dpoints = convert(
points=point,
to_coord="drive",
pivot_to_drive=pivot_to_drive,
pivot_to_center=pivot_to_center,
drive_polarity=drive_polarity,
mspace_polarity=mspace_polarity,
)
dpoints
[17]:
array([[ 0. , 0. ],
[-0.96447966, -4.76122797],
[-2.85283725, -9.87326849],
[ 1.00857746, 2.29892945]])
[18]:
mpoints = convert(
points=dpoints,
to_coord="motion_space",
pivot_to_drive=pivot_to_drive,
pivot_to_center=pivot_to_center,
drive_polarity=drive_polarity,
mspace_polarity=mspace_polarity,
)
mpoints
[18]:
array([[-1.42108547e-14, -2.34260237e-14],
[ 1.00000000e+00, 2.00000000e+00],
[ 3.00000000e+00, 4.00000000e+00],
[-1.00000000e+00, -1.00000000e+00]])
[19]:
np.isclose(mpoints, point)
[19]:
array([[ True, True],
[ True, True],
[ True, True],
[ True, True]])
[20]:
(mpoints - point) / point
/tmp/ipykernel_1888/1382693254.py:1: RuntimeWarning: divide by zero encountered in divide
(mpoints - point) / point
[20]:
array([[ -inf, -inf],
[ 2.88657986e-14, 5.99520433e-15],
[-2.36847579e-15, 2.66453526e-15],
[ 3.55271368e-15, -5.55111512e-15]])
[21]:
point = np.array([[0, 0], [1,2], [3,4], [-1, -1]])
# T1, T2, T3 = matrix_to_mspace(
# points=point,
# pivot_to_center=pivot_to_center,
# pivot_to_drive=pivot_to_drive,
# drive_polarity=drive_polarity,
# mspace_polarity=mspace_polarity,
# )
T = matrix_to_mspace(
points=point,
pivot_to_center=pivot_to_center,
pivot_to_drive=pivot_to_drive,
drive_polarity=drive_polarity,
mspace_polarity=mspace_polarity,
)
TT.shape
---------------------------------------------------------------------------
NameError Traceback (most recent call last)
Cell In[21], line 16
12 pivot_to_drive=pivot_to_drive,
13 drive_polarity=drive_polarity,
14 mspace_polarity=mspace_polarity,
15 )
---> 16 TT.shape
NameError: name 'TT' is not defined
[22]:
# (
# T1[1,...],
# T2[1,...],
# T3[1,...],
# )
[23]:
npt = np.concatenate(
(
point,
np.ones((point.shape[0], 1)),
),
axis=1,
)
npt
[23]:
array([[ 0., 0., 1.],
[ 1., 2., 1.],
[ 3., 4., 1.],
[-1., -1., 1.]])
[24]:
# np.matmul(TT, npt, axes="(k,m,n),(k,m)->(k,n)")
np.einsum("kmn,kn->km", TT, npt)[..., :2]
---------------------------------------------------------------------------
NameError Traceback (most recent call last)
Cell In[24], line 2
1 # np.matmul(TT, npt, axes="(k,m,n),(k,m)->(k,n)")
----> 2 np.einsum("kmn,kn->km", TT, npt)[..., :2]
NameError: name 'TT' is not defined
[25]:
point
[25]:
array([[ 0, 0],
[ 1, 2],
[ 3, 4],
[-1, -1]])
[26]:
P = np.diag([-1, -1, 1])
(
P,
np.linalg.inv(P),
)
[26]:
(array([[-1, 0, 0],
[ 0, -1, 0],
[ 0, 0, 1]]),
array([[-1., -0., -0.],
[-0., -1., -0.],
[ 0., 0., 1.]]))
[27]:
M = np.zeros((3, 3))
M[0,0] = 1
M[0,2] = -50
M[1,1] = 1
M[2,2] = 1
(
M,
np.linalg.inv(M),
)
[27]:
(array([[ 1., 0., -50.],
[ 0., 1., 0.],
[ 0., 0., 1.]]),
array([[ 1., 0., 50.],
[ 0., 1., 0.],
[ 0., 0., 1.]]))
[ ]:
[28]:
probe_axis_offset = 4.
pivot_to_drive = 20
pivot_to_center = 40
[29]:
points = np.array([
[-5, 5],
[-5, -5],
[5, -5],
[5, 5],
[0, 0],
[-5, 0],
[5, 0],
])
points
[29]:
array([[-5, 5],
[-5, -5],
[ 5, -5],
[ 5, 5],
[ 0, 0],
[-5, 0],
[ 5, 0]])
[30]:
sine_alpha = probe_axis_offset / np.sqrt(
pivot_to_drive**2
+ (-probe_axis_offset + points[..., 1])**2
)
alpha = np.arcsin(sine_alpha)
np.degrees(alpha)
[30]:
array([11.52236745, 10.5086695 , 10.5086695 , 11.52236745, 11.30993247,
11.30993247, 11.30993247])
[31]:
tan_beta = (-probe_axis_offset + points[..., 1]) / -pivot_to_drive
beta = np.arctan(tan_beta)
np.degrees(beta)
[31]:
array([-2.86240523, 24.22774532, 24.22774532, -2.86240523, 11.30993247,
11.30993247, 11.30993247])
[32]:
theta = beta - alpha
theta
[32]:
array([-0.25106165, 0.23944304, 0.23944304, -0.25106165, 0. ,
0. , 0. ])
[33]:
T0 = np.zeros((points.shape[0], 3, 3)).squeeze()
T0[..., 0, 0] = np.cos(theta)
T0[..., 0, 2] = -pivot_to_center * (1 - np.cos(theta))
T0[..., 1, 0] = np.sin(theta)
T0[..., 1, 2] = pivot_to_center * np.sin(theta)
T0[..., 2, 2] = 1.0
T0[0,...]
[33]:
array([[ 0.96864922, 0. , -1.25403118],
[-0.24843246, 0. , -9.93729844],
[ 0. , 0. , 1. ]])
[34]:
pts = np.concatenate(
(points, np.ones((points.shape[0], 1))),
axis=1,
)
mpoints = np.einsum("kmn,kn->km", T0, pts)[...,:-1]
mpoints
[34]:
array([[ -6.09727728, -8.69513614],
[ -5.9985425 , 8.30065587],
[ 3.71615964, 10.67227184],
[ 3.58921492, -11.17946075],
[ 0. , 0. ],
[ -5. , 0. ],
[ 5. , 0. ]])
[35]:
tan_theta = mpoints[...,1]/(mpoints[...,0]+pivot_to_center)
theta = -np.arctan(tan_theta)
np.degrees(theta)
[35]:
array([ 14.38477268, -13.71907582, -13.71907582, 14.38477268,
-0. , -0. , -0. ])
[36]:
TI = np.zeros((points.shape[0], 3, 3)).squeeze()
TI[..., 0, 2] = np.sqrt(mpoints[...,1]**2 +(pivot_to_center + mpoints[...,0])**2) - pivot_to_center
TI[..., 1, 2] = pivot_to_axis * np.tan(theta) + probe_axis_offset * (1 - (1/np.cos(theta)))
TI[..., 2, 2] = 1.0
TI[0,...]
---------------------------------------------------------------------------
NameError Traceback (most recent call last)
Cell In[36], line 3
1 TI = np.zeros((points.shape[0], 3, 3)).squeeze()
2 TI[..., 0, 2] = np.sqrt(mpoints[...,1]**2 +(pivot_to_center + mpoints[...,0])**2) - pivot_to_center
----> 3 TI[..., 1, 2] = pivot_to_axis * np.tan(theta) + probe_axis_offset * (1 - (1/np.cos(theta)))
4 TI[..., 2, 2] = 1.0
5 TI[0,...]
NameError: name 'pivot_to_axis' is not defined
[37]:
mpts = np.concatenate(
(mpoints, np.ones((points.shape[0], 1))),
axis=1,
)
pts = mpoints = np.einsum("kmn,kn->km", TI, mpts)[...,:-1]
pts
[37]:
array([[-5., 0.],
[-5., 0.],
[ 5., 0.],
[ 5., 0.],
[ 0., 0.],
[-5., 0.],
[ 5., 0.]])
[38]:
probe_axis_offset * (1 - (1/np.cos(theta)))
[38]:
array([-0.12946185, -0.11747055, -0.11747055, -0.12946185, 0. ,
0. , 0. ])
[39]:
pivot_to_axis*np.tan(theta) + probe_axis_offset * (1 - (1/np.cos(theta)))
---------------------------------------------------------------------------
NameError Traceback (most recent call last)
Cell In[39], line 1
----> 1 pivot_to_axis*np.tan(theta) + probe_axis_offset * (1 - (1/np.cos(theta)))
NameError: name 'pivot_to_axis' is not defined
[40]:
pivot_to_axis*np.tan(theta) - probe_axis_offset * np.cos(theta) + probe_axis_offset
---------------------------------------------------------------------------
NameError Traceback (most recent call last)
Cell In[40], line 1
----> 1 pivot_to_axis*np.tan(theta) - probe_axis_offset * np.cos(theta) + probe_axis_offset
NameError: name 'pivot_to_axis' is not defined
[ ]:
[ ]: