def test_squad(Rs):
np.random.seed(1234)
squad_precision = 4.e-15
ones = [quaternion.one, quaternion.x, quaternion.y, quaternion.z, -quaternion.x, -quaternion.y, -quaternion.z]
t_in = np.linspace(0.0, 1.0, num=13, endpoint=True)
t_out = np.linspace(0.0, 1.0, num=37, endpoint=True)
t_out2 = np.array(sorted([np.random.uniform(0.0, 1.0) for i in range(59)]))
# squad interpolated onto the inputs should be the identity
for R1 in Rs:
for R2 in Rs:
R_in = np.array([quaternion.slerp(R1, R2, t) for t in t_in])
assert np.all(np.abs(quaternion.squad(R_in, t_in, t_in) - R_in) < squad_precision)
# squad should be the same as slerp for linear interpolation
for R in ones:
R_in = np.array([quaternion.slerp(quaternion.one, R, t) for t in t_in])
R_out_squad = quaternion.squad(R_in, t_in, t_out)
R_out_slerp = np.array([quaternion.slerp(quaternion.one, R, t) for t in t_out])
# print(
# R, "\n",
# np.argmax(np.abs(R_out_squad - R_out_slerp)),
# len(R_out_squad), "\n",
# np.max(np.abs(R_out_squad - R_out_slerp)), "\n",
# R_out_squad[-6:], "\n",
# R_out_slerp[-6:],
# )
assert np.all(np.abs(R_out_squad - R_out_slerp) < squad_precision), (
R,
np.argmax(np.abs(R_out_squad - R_out_slerp)),
len(R_out_squad),
R_out_squad[np.argmax(np.abs(R_out_squad - R_out_slerp))-2:np.argmax(np.abs(R_out_squad - R_out_slerp))+3],
R_out_slerp[np.argmax(np.abs(R_out_squad - R_out_slerp))-2:np.argmax(np.abs(R_out_squad - R_out_slerp))+3],
)
R_out_squad = quaternion.squad(R_in, t_in, t_out2)
R_out_slerp = np.array([quaternion.slerp(quaternion.one, R, t) for t in t_out2])
assert np.all(np.abs(R_out_squad - R_out_slerp) < squad_precision)
def _resample_array(cls, array, timestamps):
""" Resample an array to the timestamps. """
if timestamps is None and array.timestamps is not None:
raise ValueError("Cannot convert timestamped to static array")
if array.timestamps is None:
if timestamps is None:
return array.data
else:
return np.tile(array.data, (len(timestamps), 1))
else:
# TODO better way to check if quaternion
if array.data.shape[-1] == 4:
return as_float_array(
squad(
as_quat_array(array.data),
array.timestamps.astype(float),
timestamps.astype(float),
)
)
else:
# TODO method + optional scipy dependency?
return interp1d(
array.timestamps.astype(float), array.data, axis=0
)(timestamps.astype(float))
def test_linear_interpolation(linear_waveform):
interpolation_precision = 4.5e-16
# Interpolation of constant_waveform is ~identity
W_in = linear_waveform
t_in = W_in.t
t_out = (t_in[:-1] + t_in[1:]) / 2.0
W_out = W_in.interpolate(t_out)
assert W_in.ensure_validity(alter=False)
assert W_out.ensure_validity(alter=False)
assert W_out.history[:1] == ['# Called from linear_waveform']
assert W_out.frameType == scri.Corotating
assert W_out.dataType == scri.h
assert W_out.r_is_scaled_out # == True
assert W_out.m_is_scaled_out # == True
assert W_out.num != W_in.num
assert np.all(W_out.t == t_out)
squad = quaternion.squad(W_in.frame, W_in.t, t_out)
assert np.all(
np.abs(W_out.frame - squad) < interpolation_precision *
np.abs(W_out.frame + squad))
assert np.all(W_out.LM == W_in.LM)
data = np.empty((t_out.shape[0], W_in.LM.shape[0]), dtype=complex)
for i, m in enumerate(W_in.LM[:, 1]):
# N.B.: This depends on the construction of linear_waveform;
# only change here if it is changed there.
data[:, i] = (m - 1j * m) * t_out
assert np.allclose(W_out.data, data, rtol=interpolation_precision)
def interpolate(self, tprime):
"""Interpolate the frame and data onto the new set of time steps
Note that only `t`, `frame`, and `data` are changed in this function. If there is a corresponding data set
in a subclass, for example, the subclass must override this function to set that data set -- though this
function should probably be called to handle the ugly stuff.
"""
# Copy the information fields, but not the data
W = WaveformBase.copy_without_data(self)
W.t = np.copy(tprime)
W.frame = quaternion.squad(self.frame, self.t, W.t)
W.data = np.empty((W.n_times,)+self.data.shape[1:], dtype=self.data.dtype)
if self.data.dtype == np.dtype(complex):
for i in range(W.n_data_sets):
W.data_2d[:, i] = (splev(W.t, splrep(self.t, self.data_2d[:, i].real, s=0), der=0)
+ 1j*splev(W.t, splrep(self.t, self.data_2d[:, i].imag, s=0), der=0))
elif self.data.dtype == np.dtype(float):
for i in range(W.n_data_sets):
W.data_2d[:, i] = splev(W.t, splrep(self.t, self.data_2d[:, i], s=0), der=0)
else:
raise TypeError("Unknown self.data.dtype={0}".format(self.data.dtype))
W.__history_depth__ -= 1
W._append_history('{0} = {1}.interpolate({2})'.format(W, self, tprime))
return W
def _transform_from_frame(cls, frame, timestamps):
""" Get the transform from a frame resampled to the timestamps. """
if timestamps is None and frame.timestamps is not None:
raise ValueError("Cannot convert timestamped to static transform")
if frame.timestamps is None:
if timestamps is None:
translation = frame.translation
rotation = frame.rotation
else:
translation = np.tile(frame.translation, (len(timestamps), 1))
rotation = np.tile(frame.rotation, (len(timestamps), 1))
elif frame.discrete:
translation = np.tile(frame.translation[0], (len(timestamps), 1))
for t, ts in zip(frame.translation, frame.timestamps):
translation[timestamps >= ts, :] = t
rotation = np.tile(frame.rotation[0], (len(timestamps), 1))
for r, ts in zip(frame.rotation, frame.timestamps):
rotation[timestamps >= ts, :] = r
else:
# TODO method + optional scipy dependency?
translation = interp1d(
frame.timestamps.astype(float), frame.translation, axis=0
)(timestamps.astype(float))
rotation = as_float_array(
squad(
as_quat_array(frame.rotation),
frame.timestamps.astype(float),
timestamps.astype(float),
)
)
return translation, rotation
def qinterp(q, t_in, t_out, axis=0, qaxis=-1):
""" Quaternion interpolation.
Parameters
----------
q: array_like
Array containing quaternions to interpolate. Its dtype
can be quaternion, otherwise `qaxis` specifies the axis representing
the quaternions.
t_in: array_like
Array of current sampling points of `q`.
t_out: array_like
Array of desired sampling points of `q`.
axis: int, default 0
Axis along which the quaternions are interpolated.
qaxis: int, default -1
If `q` is not quaternion dtype, axis of the quaternion array
representing the coordinates of the quaternions.
Returns
-------
qi: ndarray
A new array containing the interpolated values.
"""
# TODO xarray support
axis = axis % q.ndim
t_in = np.array(t_in).astype(float)
t_out = np.array(t_out).astype(float)
if q.dtype != quaternion:
qaxis = qaxis % q.ndim
# fix axis if it's the last axis of the array and will be swapped with
# axis when converting to quaternion dtype
if axis == q.ndim - 1:
axis = qaxis
q = as_quat_array(np.swapaxes(q, qaxis, -1))
was_quaternion = False
else:
was_quaternion = True
q = np.swapaxes(q, axis, 0)
try:
qi = squad(q, t_in, t_out)
except ValueError:
raise RuntimeError(
"Error using SQUAD with multi-dimensional array, please upgrade "
"the quaternion package to the latest version"
)
qi = np.swapaxes(qi, 0, axis)
if was_quaternion:
return qi
else:
return np.swapaxes(as_float_array(qi), -1, qaxis)
def pos_quat(self):
n_rows = self.path.shape[0]
idx = max(self.path_t.searchsorted(self.t, side='right'), 1)
if idx >= n_rows:
self.t = -1.
idx = 1
t = np.array([max(self.t, 0.)])
return quaternion.squad(self.path_quat, self.path_t, t)[0]
def squad(quaternions, times, t):
r"""
Smoothly interpolate over a list/path of rotations using Spherical and QUADrangle (SQUAD).
From [2]: "Spherical "quadrangular" interpolation of rotors with a cubic spline
This is the best way to interpolate rotations. It uses the analog of a cubic spline, except that the interpolant
is confined to the rotor manifold in a natural way. Alternative methods involving interpolation of other
coordinates on the rotation group or normalization of interpolated values give bad results. The results from this
method are as natural as any, and are continuous in first and second derivatives.
The input `R_in` rotors are assumed to be reasonably continuous (no sign flips), and the input `t` arrays are
assumed to be sorted. No checking is done for either case, and you may get silently bad results if these
conditions are violated."
Args:
quaternions (list[np.array[float[4]]], list[quaternion.quaternion]): A time-series of rotors (unit
quaternions) to be interpolated
times (np.array[float], list[float]): the times corresponding to the quaternions.
t (np.array[float], list[float]): the times to which the quaternions should be interpolated.
Returns:
np.array, np.array[quaternion]: interpolated quaternions
References:
- [1] "Understanding Quaternions", https://www.3dgep.com/understanding-quaternions
- [2] Documentation of numpy-quaternion
"""
# make sure that they are numpy arrays
quaternions = np.asarray(quaternions)
times = np.asarray(times)
t = np.asarray(t)
# check if we need to convert
are_input_quaternions = (quaternions.dtype == quaternion.quaternion)
if not are_input_quaternions:
qs = quaternions
quaternions = quaternion.from_float_array(
np.hstack((qs[:, 0, np.newaxis], qs[:, 1:])))
# interpolate
qs = quaternion.squad(quaternions, times, t)
# if we need to convert back to np.array
if not are_input_quaternions:
if isinstance(qs, np.ndarray):
qs = quaternion.as_float_array(qs)
qs = np.hstack((qs[:, 1:], qs[:, 0, np.newaxis]))
return qs
return np.array([qs.x, qs.y, qs.z, qs.w])
return qs
def interpolate(self, new_times):
new_abd = type(self)(new_times, self.ell_max)
new_abd.frameType = self.frameType
# interpolate waveform data
new_abd.sigma = self.sigma.interpolate(new_times)
new_abd.psi4 = self.psi4.interpolate(new_times)
new_abd.psi3 = self.psi3.interpolate(new_times)
new_abd.psi2 = self.psi2.interpolate(new_times)
new_abd.psi1 = self.psi1.interpolate(new_times)
new_abd.psi0 = self.psi0.interpolate(new_times)
# interpolate frame data if necessary
if self.frame.shape[0] == self.n_times:
import quaternion
new_abd.frame = quaternion.squad(self.frame, self.t, new_times)
return new_abd
def view_interpolate(view_init_str,
view_last_str,
base_name,
number_of_frames=5,
frameno_offset=0,
easing_type=("sine", "inout")):
view_init = view_string2view(view_init_str)
view_last = view_string2view(view_last_str)
# get quaternions for interpolation
qstart = view2quat(view_init)
qend = view2quat(view_last)
# logistic is a nice sigmoid https://en.wikipedia.org/wiki/Sigmoid_function
# but it mayllok jumpy
time_series = easing(32, easing_type)
# cubic spline interpolation for quaternion
interm_quat = quaternion.squad(np.array([qstart, qend]), np.array([0, 1]),
np.array(time_series))
last_frame = frameno_offset
cmd.set_view(view_init_str)
cmd.png(base_name + str(last_frame).zfill(3),
width=1920,
height=1080,
ray=True)
last_frame += 1
for frameno in range(1, number_of_frames - 1):
intrm_view = intermediate_view(view_init, view_last,
interm_quat[frameno - 1],
time_series[frameno - 1])
cmd.set_view(view2view_string(intrm_view))
cmd.png(base_name + str(last_frame).zfill(3),
width=1920,
height=1080,
ray=True)
last_frame += 1
cmd.set_view(view_last_str)
cmd.png(base_name + str(last_frame).zfill(3),
width=1920,
height=1080,
ray=True)
last_frame += 1
return last_frame
def interpolate(self, tprime):
"""Interpolate the frame and data onto the new set of time steps
Note that only `t`, `frame`, and `data` are changed in this function. If there is a corresponding data set
in a subclass, for example, the subclass must override this function to set that data set -- though this
function should probably be called to handle the ugly stuff.
"""
# Copy the information fields, but not the data
W = WaveformBase.copy_without_data(self)
W.t = np.copy(tprime)
W.frame = quaternion.squad(self.frame, self.t, W.t)
W.data = np.empty((W.n_times,) + self.data.shape[1:], dtype=self.data.dtype)
W.data_2d[:] = CubicSpline(self.t, self.data_2d.view(float))(W.t).view(complex)
W.__history_depth__ -= 1
W._append_history(f"{W} = {self}.interpolate({tprime})")
return W
def test_interpolation(W):
interpolation_precision = 4.5e-16
# Interpolation onto self.t is the identity (except for `num`)
W_in = W()
t_in = W_in.t
t_out = np.copy(t_in)
W_out = W_in.interpolate(t_out)
assert W_in.ensure_validity(alter=False)
assert W_out.ensure_validity(alter=False)
assert W_out.history[:2] == W_in.history[:2]
assert W_out.frameType == W_in.frameType
assert W_out.dataType == W_in.dataType
assert W_out.r_is_scaled_out == W_in.r_is_scaled_out
assert W_out.m_is_scaled_out == W_in.m_is_scaled_out
assert W_out.num != W_in.num
assert np.all(W_out.t == W_in.t)
assert np.all(W_out.frame == W_in.frame)
assert np.all(W_out.LM == W_in.LM)
assert np.allclose(W_out.data, W_in.data, rtol=4.5e-16)
# Interpolation onto another time series should reshape
W_in = W()
t_in = W_in.t
t_out = (t_in[:-1] + t_in[1:]) / 2.0
W_out = W_in.interpolate(t_out)
assert W_in.ensure_validity(alter=False)
assert W_out.ensure_validity(alter=False)
assert W_out.history[:1] == ['# Called from ' + W.__name__]
assert W_out.frameType == scri.Corotating
assert W_out.dataType == scri.h
assert W_out.r_is_scaled_out # == True
assert W_out.m_is_scaled_out # == True
assert isinstance(W_out.num, int)
assert W_out.num != W_in.num
assert np.all(W_out.t == t_out)
assert np.all(W_out.frame == quaternion.squad(W_in.frame, W_in.t, W_out.t))
assert np.all(W_out.LM == W_in.LM)
assert W_out.data.shape == (len(t_out), W_in.n_data_sets)
def initialize(self):
time_scale_offset = self.generator_kwargs["time_scale_offset"]
time_scale_factor = self.generator_kwargs["time_scale_factor"]
time_noise_factor = self.generator_kwargs["time_noise_factor"]
time_staying_more = self.generator_kwargs[
"time_staying_more"] # How much time to stay more when trajectory reaches its end?
extracts_path = self.generator_kwargs["extracts_path"]
control_timestep = self.environment_kwargs["control_timestep"]
time_limit = self.environment_kwargs["time_limit"]
#################################
### Sample a file name ###
##########################
# Based on the file name, we know from what region the trajectory starts.
base_filename = self.random_params["filename"]
filename = os.path.join(extracts_path, base_filename + ".json")
metafile = os.path.join(extracts_path, "meta",
base_filename + "_meta.json")
# Read the trajectory data from file
t, x, q, time_offset = self.read_trajectory_file(filename)
########################################
### Process the trajectory ###
##############################
# Randomize the trajectory duration (T)
if "original_time" in self.random_params:
original_time = self.random_params["original_time"]
# T = float(self.random_params["randomized_time"].strip("[]"))
T = self.random_params["randomized_time"]
else:
time_noise_normal = self.random_params["time_noise_normal"]
original_time = t[-1]
noise = time_noise_normal * time_noise_factor
T = original_time * (time_scale_factor) + (time_scale_offset +
noise)
# print("ori:", original_time, "| random:", T)
# Scale the t vector with respect to the trajectory time
t_scaled = t * T / original_time
t_vector = np.arange(0, T + 1e-5, control_timestep)
t_vector = t_vector[t_vector <=
T] # Make sure that we do not go beyond the T.
# 1. Fitting a spline curve on the x:
try:
x_spline, _ = splprep(x.T, u=t_scaled, s=0)
x_vector = np.array([
splev(t_vector[i], x_spline, ext=3)
for i in range(len(t_vector))
],
dtype=np.float)
except Exception as e:
print("Got an error with {} file while interpolating.".format(
base_filename))
print(e)
# t, v, tb = sys.exc_info()
# raise t, v, tb
raise e
# 2. Fitting to quaternion arrays
qs = quaternion.as_quat_array(q)
q_vector = quaternion.as_float_array(
quaternion.squad(qs, t_scaled, t_vector))
# TODO: Make an interpolator in order to publish with custom timelimit/timestep.
# Make a new T vector with the desired time-limit
#################################################################
### Find and noise the position/orientation offset ###
######################################################
# Finding averages of x and q when hand reaches the object.
# Offset & Scaled reaching time
reaching_time = (self.read_tag_from_metafile(metafile, tag="reached") -
time_offset) * T / original_time
# x_start = [x[i] for i in range(len(t)) if (t[i] < 0.1)]
# Reaching State Duration
RSD = 0.2
x_reached = [
x_vector[i] for i in range(len(t_vector))
if (t_vector[i] > reaching_time) and (
t_vector[i] <= min(reaching_time + RSD, t_vector[-1]))
]
q_reached = [
q_vector[i] for i in range(len(t_vector))
if (t_vector[i] > reaching_time) and (
t_vector[i] <= min(reaching_time + RSD, t_vector[-1]))
]
x_grasp = sum(x_reached) / len(x_reached)
q_grasp = self.quaternion_average(q_reached)
# The results from SVD are almost the same as pure linear averaging.
# q_grasp2 = -sum(q_reached)/len(q_reached)
# Do for each element
offset_local_mocap = np.array([0, 0.09 + 0.015, 0.03],
dtype=np.float64)
offset_local_mocap_rotated = quatTrans(quatConj(q_grasp),
offset_local_mocap)
x_palm_center_reached = x_grasp + offset_local_mocap_rotated
x_palm_center_reached[2] = 0 # Zero out the z-coordinate
# print("Palm Center norm should be close to 0:", np.linalg.norm(x_palm_center_reached))
# Offset the x trajectory so end points are close to the "good" location for grasping.
# TODO: This offset is something that needs to be randomized.
# offset_noise_2d = np.array([-0.02, -0.01, 0], dtype = np.float64)
offset_noise_2d = self.random_params["offset_noise_2d"]
x_vector = x_vector - x_palm_center_reached + offset_noise_2d
# TODO: What is the best measure for figuring out the orientation offset?
# # Offset the end orientation
# offset_support_point = np.array([0, 0.09+0.015, 0], dtype=np.float64)
# offset_support_point_rotated = quatTrans(quatConj(q_grasp), offset_support_point)
# x_palm_support_reached = x_grasp + offset_support_point_rotated
# normal_to_hand = quatTrans(quatConj(q_grasp), np.array([0,0,1]))
# px = normal_to_hand[0]
# py = normal_to_hand[1]
# p_vec = np.array([px, py], dtype=np.float64)
# p_vec = p_vec / np.linalg.norm(p_vec)
# # nx = -x_palm_center_reached[0]
# # ny = -x_palm_center_reached[1]
# nx = -x_palm_support_reached[0]
# ny = -x_palm_support_reached[1]
# n_vec = np.array([nx, ny], dtype=np.float64)
# n_vec = n_vec / np.linalg.norm(n_vec)
# print("Orientation vectors in 2D. Normal:", p_vec, ", From position:", n_vec)
# # v_diff = np.array([px-nx, py-ny], dtype=np.float64)
# # print("Orientation error should be close to 0:", np.linalg.norm(v_diff))
# print()
# How should "normal_to_hand" be?
# TODO: Use x_grasp and q_grasp (which gives the hand pose when it tries grasping the object)
# to find a proper offset for the hand trajectory.
# TODO: What is the acceptable pairs of (x,q) for grasping the object?
# TODO: x is the position of some point attached to the user's wrist.
# Find the corresponding point in the robot's hand palm, which
# collides with the object center at the time of grasping.
# TODO: 1. Find the position offset + salt it with some noise.
# 2. Find the orientation offset + salt it with some noise.
# TODO: Find the offset of the "palm point" from the mocap position.
# Then use the same kind of transformations used in "segmenter.py"
# to find the frame of that point in the palm.
#################################
###
t_vector_stay = t_vector[-1] + np.linspace(
control_timestep, time_staying_more * control_timestep,
time_staying_more)
x_vector_stay = np.repeat(x_vector[-1:, :], time_staying_more, axis=0)
q_vector_stay = np.repeat(q_vector[-1:, :], time_staying_more, axis=0)
t_vector = np.concatenate((t_vector, t_vector_stay), axis=0)
x_vector = np.concatenate((x_vector, x_vector_stay), axis=0)
q_vector = np.concatenate((q_vector, q_vector_stay), axis=0)
# x_vector_stay = x_vector[-1]
# q_vector_stay =
#################################
### Store trajectory data
self.trajectory = {}
self.trajectory["t_vector"] = t_vector
self.trajectory["x_vector"] = x_vector
self.trajectory["q_vector"] = q_vector
#################################
# print(f"Selected file: {base_filename} | Total time: {T} | dt: {control_timestep}")
#####################################
### Storing Additional Parameters ###
#####################################
# Extract useful information from the sampled file name
# subject_id, starting_flag, starting_id = base_filename.split("_")
# self.parameters["subject_id"] = subject_id
# self.parameters["starting_flag"] = starting_flag
# self.parameters["starting_id"] = starting_id
##
self.parameters["original_time"] = original_time
self.parameters["randomized_time"] = T
def compare(self, w_a, min_time_step=0.005, min_time=-3.0e300):
"""Return a waveform with differences between the two inputs
This function simply subtracts the data in this waveform from the data
in Waveform A, and finds the rotation needed to take this frame into frame A.
Note that the waveform data are stored as complex numbers, rather than as
modulus and phase.
"""
from quaternion.means import mean_rotor_in_chordal_metric
from scipy.interpolate import InterpolatedUnivariateSpline
from scri.extrapolation import intersection
import scri.waveform_modes
if self.frameType != w_a.frameType:
warning = ('\nWarning:'+
'\n This Waveform is in the ' + self.frame_type_string + ' frame,'+
'\n The Waveform in the argument is in the ' + w_a.frame_type_string + ' frame.'+
'\n Comparing them probably does not make sense.\n')
warnings.warn(warning)
if self.n_modes != w_a.n_modes:
raise Exception('Trying to compare waveforms with mismatched LM data.'+
'\nA.n_modes=' + str(w_a.n_modes) + '\tB.n_modes()=' + str(self.n_modes))
new_times = intersection(self.t, w_a.t)
w_c = scri.waveform_modes.WaveformModes(
t=new_times,
data=np.zeros((new_times.shape[0], self.n_modes), dtype=self.data.dtype),
history=[],
version_hist=self.version_hist,
frameType=self.frameType,
dataType=self.dataType,
r_is_scaled_out=self.r_is_scaled_out,
m_is_scaled_out=self.m_is_scaled_out,
ell_min=self.ell_min,
ell_max=self.ell_max)
w_c.history += ['B.compare(A)\n']
w_c.history += ['### A.history.str():\n' + ''.join(w_a.history)]
w_c.history += ['### B.history.str():\n' + ''.join(self.history)]
w_c.history += ['### End of old histories from `compare`']
# Process the frame, depending on the sizes of the input frames
if w_a.frame.shape[0] > 1 and self.frame.shape[0] > 1:
# Find the frames interpolated to the appropriate times
Aframe = quaternion.squad(w_a.frame, w_a.t, w_c.t)
Bframe = quaternion.squad(self.frame, self.t, w_c.t)
# Assign the data
w_c.frame = Aframe * np.array([np.quaternion.inverse(v) for v in Bframe])
elif w_a.frame.shape[0] == 1 and self.frame.shape[0] > 1:
# Find the frames interpolated to the appropriate times
Bframe = np.quaternion.squad(self.frame, self.t, w_c.t)
# Assign the data
w_c.frame.resize(w_c.n_times)
w_c.frame = w_a.frame[0] * np.array([np.quaternion.inverse(v) for v in Bframe])
elif w_a.frame.shape[0] > 1 and self.frame.shape[0] == 1:
# Find the frames interpolated to the appropriate times
Aframe = np.quaternion.squad(w_a.frame, w_a.t, w_c.t)
# Assign the data
w_c.frame.resize(w_c.n_times)
w_c.frame = Aframe * np.quaternion.inverse(self.frame[0])
elif w_a.frame.shape[0] == 1 and self.frame.shape[0] == 1:
# Assign the data
w_c.frame = np.array(w_a.frame[0] * np.quaternions.inverse(self.frame[0]))
elif w_a.frame.shape[0] == 0 and self.frame.shape[0] == 1:
# Assign the data
w_c.frame = np.array(np.quaternions.inverse(self.frame[0]))
elif w_a.frame.shape[0] == 1 and self.frame.shape[0] == 1:
# Assign the data
w_c.frame = np.array(w_a.frame[0])
# else, leave the frame data empty
# If the average frame rotor is closer to -1 than to 1, flip the sign
if w_c.frame.shape[0] == w_c.n_times:
R_m = mean_rotor_in_chordal_metric(w_c.frame, w_c.t)
if quaternion.rotor_chordal_distance(R_m, -quaternion.one) < quaternion.rotor_chordal_distance(R_m, quaternion.one):
w_c.frame = -w_c.frame
elif w_c.frame.shape[0] == 1:
if quaternion.rotor_chordal_distance(w_c.frame[0], -quaternion.one) < quaternion.rotor_chordal_distance(w_c.frame[0], quaternion.one):
w_c.frame[0] = -w_c.frame[0]
# Now loop over each mode filling in the waveform data
for Mode in range(w_a.n_modes):
# Assume that all the ell,m data are the same, but not necessarily in the same order
BMode = self.index(w_a.LM[Mode][0], w_a.LM[Mode][1])
# Initialize the interpolators for this data set
splineReA = InterpolatedUnivariateSpline(w_a.t, w_a.data[:,Mode].real)
splineImA = InterpolatedUnivariateSpline(w_a.t, w_a.data[:,Mode].imag)
splineReB = InterpolatedUnivariateSpline(self.t, self.data[:,BMode].real)
splineImB = InterpolatedUnivariateSpline(self.t, self.data[:,BMode].imag)
# Assign the data from the transition
w_c.data[:,Mode] = (splineReA(w_c.t) - splineReB(w_c.t)) + 1j*(splineImA(w_c.t) - splineImB(w_c.t))
return w_c