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