Skip to content
Snippets Groups Projects
Commit 3d2b4aa4 authored by Jesse Mapel's avatar Jesse Mapel Committed by acpaquette
Browse files

Updated how Angular Velocity is stored and computed in rotations (#275)

* Added angular velocity to rotations

* In progress commit

* Updated new AV calculations

* In progress commit

* More in progress

* In progress

* Updated rotation

* indexing error fix

* more clean up

* Fixed velocity rotation

* Removed commented line
parent bdbf918d
Branches
Tags
No related merge requests found
......@@ -40,6 +40,7 @@ def to_isis(driver):
instrument_pointing['CkTableOriginalSize'] = len(time_dependent_rotation.times)
instrument_pointing['EphemerisTimes'] = time_dependent_rotation.times
instrument_pointing['Quaternions'] = time_dependent_rotation.quats[:, [3, 0, 1, 2]]
instrument_pointing['AngularVelocity'] = time_dependent_rotation.av
# Reverse the frame order because ISIS orders frames as
# (destination, intermediate, ..., intermediate, source)
......@@ -61,6 +62,7 @@ def to_isis(driver):
body_rotation['CkTableOriginalSize'] = len(time_dependent_rotation.times)
body_rotation['EphemerisTimes'] = time_dependent_rotation.times
body_rotation['Quaternions'] = time_dependent_rotation.quats[:, [3, 0, 1, 2]]
body_rotation['AngularVelocity'] = time_dependent_rotation.av
if source_frame != target_frame:
# Reverse the frame order because ISIS orders frames as
......
......@@ -110,7 +110,7 @@ class ConstantRotation:
new_rot = self._rot * other._rot
return ConstantRotation(new_rot.as_quat(), other.source, self.dest)
elif isinstance(other, TimeDependentRotation):
return TimeDependentRotation((self._rot * other._rots).as_quat(), other.times, other.source, self.dest)
return TimeDependentRotation((self._rot * other._rots).as_quat(), other.times, other.source, self.dest, av=self._rot.apply(other.av))
else:
raise TypeError("Rotations can only be composed with other rotations.")
......@@ -155,7 +155,7 @@ class TimeDependentRotation:
rot = Rotation.from_euler(sequence, np.asarray(euler), degrees=degrees)
return TimeDependentRotation(rot.as_quat(), times, source, dest)
def __init__(self, quats, times, source, dest):
def __init__(self, quats, times, source, dest, av=None):
"""
Construct a time dependent rotation
......@@ -173,14 +173,22 @@ class TimeDependentRotation:
The NAIF ID code for the source frame
dest : int
The NAIF ID code for the destination frame
av : 2darray
The angular velocity of the rotation at each time as a 2d numpy array.
If not entered, then angular velocity will be computed by assuming constant
angular velocity between times.
"""
self.source = source
self.dest = dest
self.quats = np.asarray(quats)
self.times = np.asarray(times)
self.quats = quats
self.times = np.atleast_1d(times)
if av is not None:
self.av = np.asarray(av)
else:
self.av = av
def __repr__(self):
return f'Time Dependent Rotation Source: {self.source}, Destination: {self.dest}, Quat: {self.quats}'
return f'Time Dependent Rotation Source: {self.source}, Destination: {self.dest}, Quats: {self.quats}, AV: {self.av}, Times: {self.times}'
@property
def quats(self):
......@@ -202,25 +210,30 @@ class TimeDependentRotation:
The new quaternions as a 2d array. The quaternions must be
in scalar last format (x, y, z, w).
"""
self._rots = Rotation.from_quat(np.asarray(new_quats))
self._rots = Rotation.from_quat(new_quats)
def inverse(self):
"""
Get the inverse rotation, that is the rotation from the destination
reference frame to the source reference frame.
"""
return TimeDependentRotation(self._rots.inv().as_quat(), self.times, self.dest, self.source)
new_rots = self._rots.inv()
if self.av is not None:
new_av = -new_rots.apply(self.av)
else:
new_av = None
return TimeDependentRotation(new_rots.as_quat(), self.times, self.dest, self.source, av=new_av)
def _slerp(self, times):
"""
Using SLERP interpolate the rotation and angular velocity at specific times
This uses the same code as scipy SLERP, except it extrapolates
assuming constant angular velocity before and after the first
and last intervals.
Using SLERP interpolate the rotation and angular velocity at
specific times.
If the this rotation is only defined at one time, then the rotation is
assumed to be constant.
Times outside of the range covered by this rotation are extrapolated
assuming constant angular velocity. If the rotation has angular velocities
stored, then the first and last angular velocity are used for extrapolation.
Otherwise, the angular velocities from the first and last interpolation
interval are used for extrapolation.
Parameters
----------
......@@ -234,24 +247,48 @@ class TimeDependentRotation:
: 2darray
The angular velocity vectors
"""
vec_times = np.asarray(times)
if vec_times.ndim < 1:
vec_times = np.asarray([times])
elif vec_times.ndim > 1:
# Convert non-vector input to vector and check input
vec_times = np.atleast_1d(times)
if vec_times.ndim > 1:
raise ValueError('Input times must be either a float or a 1d iterable of floats')
if len(self.times) < 2:
return Rotation.from_quat(np.repeat(self.quats, len(vec_times), 0)), np.zeros((len(vec_times), 3))
# Compute constant angular velocity for interpolation intervals
avs = np.zeros((len(self.times) + 1, 3))
if len(self.times) > 1:
steps = self.times[1:] - self.times[:-1]
rotvecs = (self._rots[1:] * self._rots[:-1].inv()).as_rotvec()
avs[1:-1] = rotvecs / steps[:, None]
# If available use actual angular velocity for extrapolation
# Otherwise use the adjacent interpolation interval
if self.av is not None:
avs[0] = self.av[0]
avs[-1] = self.av[-1]
else:
idx = np.searchsorted(self.times, vec_times) - 1
idx[idx >= len(self.times) - 1] = len(self.times) - 2
idx[idx < 0] = 0
steps = self.times[idx+1] - self.times[idx]
rotvecs = (self._rots[idx + 1] * self._rots[idx].inv()).as_rotvec()
alpha = (vec_times - self.times[idx]) / steps
interp_rots = Rotation.from_rotvec(rotvecs * alpha[:, None]) * self._rots[idx]
interp_av = rotvecs / steps[:, None]
avs[0] = avs[1]
avs[-1] = avs[-2]
# Determine interpolation intervals for input times
av_idx = np.searchsorted(self.times, vec_times)
rot_idx = av_idx - 1
rot_idx[rot_idx < 0] = 0
# Interpolate/extrapolate rotations
time_diffs = vec_times - self.times[rot_idx]
interp_av = avs[av_idx]
interp_rots = Rotation.from_rotvec(interp_av * time_diffs[:, None]) * self._rots[rot_idx]
# If actual angular velocities are available, linearly interpolate them
if self.av is not None:
av_diff = np.zeros((len(self.times), 3))
if len(self.times) > 1:
av_diff[:-1] = self.av[1:] - self.av[:-1]
av_diff[-1] = av_diff[-2]
interp_av = self.av[rot_idx] + (av_diff[rot_idx] * time_diffs[:, None])
return interp_rots, interp_av
def reinterpolate(self, times):
"""
Reinterpolate the rotation at a given set of times.
......@@ -264,10 +301,10 @@ class TimeDependentRotation:
Returns
-------
: TimeDependentRotation
The new rotation that the input times
The new rotation at the input times
"""
new_rots, _ = self._slerp(times)
return TimeDependentRotation(new_rots.as_quat(), times, self.source, self.dest)
new_rots, av = self._slerp(times)
return TimeDependentRotation(new_rots.as_quat(), times, self.source, self.dest, av=av)
def __mul__(self, other):
"""
......@@ -289,11 +326,14 @@ class TimeDependentRotation:
if self.source != other.dest:
raise ValueError("Destination frame of first rotation {} is not the same as source frame of second rotation {}.".format(other.dest, self.source))
if isinstance(other, ConstantRotation):
return TimeDependentRotation((self._rots * other._rot).as_quat(), self.times, other.source, self.dest)
return TimeDependentRotation((self._rots * other._rot).as_quat(), self.times, other.source, self.dest, av=self.av)
elif isinstance(other, TimeDependentRotation):
merged_times = np.union1d(np.asarray(self.times), np.asarray(other.times))
new_quats = (self.reinterpolate(merged_times)._rots * other.reinterpolate(merged_times)._rots).as_quat()
return TimeDependentRotation(new_quats, merged_times, other.source, self.dest)
reinterp_self = self.reinterpolate(merged_times)
reinterp_other = other.reinterpolate(merged_times)
new_quats = (reinterp_self._rots * reinterp_other._rots).as_quat()
new_av = reinterp_self.av + reinterp_self._rots.apply(reinterp_other.av)
return TimeDependentRotation(new_quats, merged_times, other.source, self.dest, av=new_av)
else:
raise TypeError("Rotations can only be composed with other rotations.")
......@@ -326,7 +366,7 @@ class TimeDependentRotation:
skew = np.array([[0, -avs[indx, 2], avs[indx, 1]],
[avs[indx, 2], 0, -avs[indx, 0]],
[-avs[indx, 1], avs[indx, 0], 0]])
rot_deriv = np.dot(skew, rots[indx].as_dcm())
rot_deriv = np.dot(skew, rots[indx].as_dcm().T).T
rotated_vel[indx] = rots[indx].apply(vec_vel[indx])
rotated_vel[indx] += np.dot(rot_deriv, vec_pos[indx])
......
......@@ -105,21 +105,21 @@ class FrameChain(nx.DiGraph):
time_dependent_frames.extend(target_time_dependent_frames)
constant_frames.extend(target_constant_frames)
quats = np.zeros((len(times), 4))
for s, d in time_dependent_frames:
quats = np.zeros((len(times), 4))
avs = np.zeros((len(times), 3))
for j, time in enumerate(times):
rotation_matrix = spice.pxform(spice.frmnam(s), spice.frmnam(d), time)
state_matrix = spice.sxform(spice.frmnam(s), spice.frmnam(d), time)
rotation_matrix, avs[j] = spice.xf2rav(state_matrix)
quat_from_rotation = spice.m2q(rotation_matrix)
quats[j,:3] = quat_from_rotation[1:]
quats[j,3] = quat_from_rotation[0]
rotation = TimeDependentRotation(quats, times, s, d)
rotation = TimeDependentRotation(quats, times, s, d, av=avs)
frame_chain.add_edge(rotation=rotation)
quats = np.zeros(4)
for s, d in constant_frames:
quats = np.zeros(4)
rotation_matrix = spice.pxform(spice.frmnam(s), spice.frmnam(d), times[0])
quat_from_rotation = spice.m2q(rotation_matrix)
quats[:3] = quat_from_rotation[1:]
......
......@@ -61,8 +61,7 @@ def test_kernels(scope="module", autouse=True):
def test_cassini_load(test_kernels, usgscsm_compare_dict):
label_file = get_image_label("N1702360370_1")
usgscsm_isd = ale.load(label_file, props={'kernels': test_kernels}, formatter='usgscsm')
print(usgscsm_isd['sensor_position'])
print(usgscsm_isd['sensor_orientation'])
print(usgscsm_isd)
assert compare_dicts(usgscsm_isd, usgscsm_compare_dict) == []
# ========= Test cassini pds3label and naifspice driver =========
......
......@@ -636,8 +636,8 @@ def test_sun_position_cache(testdata):
sun_pos, sun_vel, sun_times = testdata.sun_position
np.testing.assert_almost_equal(sun_pos, [[1000, 0, 0], [0, 0, 1000]])
np.testing.assert_almost_equal(sun_vel,
[[-1000, 2000*np.pi*np.sqrt(3)/9, -2000*np.pi*np.sqrt(3)/9],
[2000*np.pi*np.sqrt(3)/9, -2000*np.pi*np.sqrt(3)/9, -1000]])
[[-1000, -2000*np.pi*np.sqrt(3)/9, 2000*np.pi*np.sqrt(3)/9],
[-2000*np.pi*np.sqrt(3)/9, 2000*np.pi*np.sqrt(3)/9, -1000]])
np.testing.assert_equal(sun_times, [0, 1])
def test_sun_position_polynomial(testdata):
......@@ -666,8 +666,8 @@ def test_sun_position_polynomial(testdata):
sun_pos, sun_vel, sun_times = testdata.sun_position
np.testing.assert_almost_equal(sun_pos, [[1000, 0, 0], [-1000, 0, 1000]])
np.testing.assert_almost_equal(sun_vel,
[[-500, 500 + 1000*np.pi*np.sqrt(3)/9, -500 - 1000*np.pi*np.sqrt(3)/9],
[-500 + 1000*np.pi*np.sqrt(3)/9, -500 - 2000*np.pi*np.sqrt(3)/9, 500 + 1000*np.pi*np.sqrt(3)/9]])
[[-500, 500 - 1000*np.pi*np.sqrt(3)/9, -500 + 1000*np.pi*np.sqrt(3)/9],
[-500 - 1000*np.pi*np.sqrt(3)/9, -500 + 2000*np.pi*np.sqrt(3)/9, 500 - 1000*np.pi*np.sqrt(3)/9]])
np.testing.assert_equal(sun_times, [2, 4])
def test_inst_position_cache(testdata):
......@@ -696,6 +696,6 @@ def test_inst_position_cache(testdata):
sensor_pos, sensor_vel, sensor_times = testdata.sensor_position
np.testing.assert_almost_equal(sensor_pos, [[1000, 0, 0], [0, 0, 1000]])
np.testing.assert_almost_equal(sensor_vel,
[[-1000, 2000*np.pi*np.sqrt(3)/9, -2000*np.pi*np.sqrt(3)/9],
[2000*np.pi*np.sqrt(3)/9, -2000*np.pi*np.sqrt(3)/9, -1000]])
[[-1000, -2000*np.pi*np.sqrt(3)/9, 2000*np.pi*np.sqrt(3)/9],
[-2000*np.pi*np.sqrt(3)/9, 2000*np.pi*np.sqrt(3)/9, -1000]])
np.testing.assert_equal(sensor_times, [0, 1])
......@@ -120,7 +120,13 @@ def isis_compare_dict():
[0.42063547, 0.18618438, -0.23973759, 0.85494273],
[0.42064763, 0.18624551, -0.2397057 , 0.85493237],
[0.42065923, 0.18630667, -0.23967382, 0.85492228],
[0.42067144, 0.18636687, -0.23964185, 0.85491211]]},
[0.42067144, 0.18636687, -0.23964185, 0.85491211]],
'AngularVelocity': [[-6.157101679378247e-06, 0.0009352034475077121, -0.00011941673357358669],
[-6.887942908783687e-06, 0.0009356318276404807, -0.00011800670313459697],
[-1.0577832791680011e-05, 0.000937142301898857, -0.0001101520178027217],
[-8.088753082470754e-06, 0.0009320299674209734, -0.00011098361545849514],
[-8.410068339567782e-06, 0.0009332739373169815, -0.0001116179282972322],
[-7.579826199031287e-06, 0.000938872225704521, -0.00011122572722581592]]},
'BodyRotation': {'TimeDependentFrames': [10014, 1],
'CkTableStartTime': 297088762.24158406,
'CkTableEndTime': 297088762.9923841,
......@@ -131,7 +137,13 @@ def isis_compare_dict():
[-0.8371162107293473, 0.2996940355045328, 0.10720441474371896, 0.44489004065791765],
[-0.8371138430875174, 0.2996946060241849, 0.1072028198209324, 0.44489449564328926],
[-0.8371114754203602, 0.2996951765357392, 0.10720122489401934, 0.44489895061910595],
[-0.8371091077303039, 0.29969574703861046, 0.10719962996461516, 0.4449034055807993]]},
[-0.8371091077303039, 0.29969574703861046, 0.10719962996461516, 0.4449034055807993]],
'AngularVelocity': [[6.178371695520371e-06, 4.2327129042393283e-05, 5.652013216046471e-05],
[6.178822211046791e-06, 4.232706327946714e-05, 5.652013216046188e-05],
[6.179272726027232e-06, 4.232699751172339e-05, 5.652013216045904e-05],
[6.179723239999644e-06, 4.232693173922947e-05, 5.652013216045621e-05],
[6.18017375357995e-06, 4.2326865961895454e-05, 5.6520132160453345e-05],
[6.180624266306148e-06, 4.2326800179788824e-05, 5.6520132160450506e-05]]},
'InstrumentPosition': {'SpkTableStartTime': 297088762.24158406,
'SpkTableEndTime': 297088762.9923841,
'SpkTableOriginalSize': 6,
......@@ -142,18 +154,18 @@ def isis_compare_dict():
[-1886.18215477, 911.90037749, -2961.79786985],
[-1886.47677475, 911.47872522, -2961.7414312],
[-1886.77135665, 911.05705456, -2961.68493293]],
'Velocities': [[-1.96292377, -2.80759072, 0.37446658],
[-1.96267123, -2.80771348, 0.37486368],
[-1.96241863, -2.80783619, 0.37526077],
[-1.96216602, -2.80795883, 0.37565786],
[-1.96191334, -2.80808143, 0.37605493],
[-1.96166065, -2.80820396, 0.376452]]},
'Velocities': [[-1.9629237646703683, -2.80759072221274, 0.37446657801485306],
[-1.9626712192798401, -2.807713482051373, 0.3748636774173111],
[-1.9624186346660286, -2.807836185534424, 0.3752607691067297],
[-1.9621660109346446, -2.8079588326107823, 0.37565785291714804],
[-1.9619133478903363, -2.8080814233753033, 0.37605492915558875],
[-1.961660645638678, -2.8082039577768683, 0.37645199765665144]]},
'SunPosition': {'SpkTableStartTime': 297088762.61698407,
'SpkTableEndTime': 297088762.61698407,
'SpkTableOriginalSize': 1,
'EphemerisTimes': [297088762.61698407],
'Positions': [[-208246643.00357282, -7677078.093689713, 2103553.070434019]],
'Velocities': [[-0.21024935169058154, -23.900370491465555, -10.956673494157382]]}}
'Velocities': [[-0.21020163267146563, -23.901883517440407, -10.957471339412034]]}}
@pytest.fixture(scope='module')
def test_kernels():
......
......@@ -5,87 +5,102 @@ from scipy.spatial.transform import Rotation
from ale.rotation import ConstantRotation, TimeDependentRotation
def test_constant_constant_composition():
# Two 90 degree rotation about the X-axis
rot1_2 = ConstantRotation([1.0/np.sqrt(2), 0, 0, 1.0/np.sqrt(2)], 1, 2)
rot2_3 = ConstantRotation([1.0/np.sqrt(2), 0, 0, 1.0/np.sqrt(2)], 2, 3)
# compose to get a 180 degree rotation about the X-axis
rot2_3 = ConstantRotation([0, 1.0/np.sqrt(2), 0, 1.0/np.sqrt(2)], 2, 3)
rot1_3 = rot2_3*rot1_2
assert isinstance(rot1_3, ConstantRotation)
assert rot1_3.source == 1
assert rot1_3.dest == 3
np.testing.assert_equal(rot1_3.quat, np.array([1, 0, 0, 0]))
np.testing.assert_equal(rot1_3.quat, [0.5, 0.5, -0.5, 0.5])
def test_constant_time_dependent_composition():
# 90 degree rotation about the X-axis to a 180 degree rotation about the X-axis
quats = [[1.0/np.sqrt(2), 0, 0, 1.0/np.sqrt(2)],[1, 0, 0, 0]]
times = [0, 1]
rot1_2 = TimeDependentRotation(quats, times, 1, 2)
# 90 degree rotation about the X-axis
rot2_3 = ConstantRotation([1.0/np.sqrt(2), 0, 0, 1.0/np.sqrt(2)], 2, 3)
# compose to get a 180 degree rotation about the X-axis to a 270 degree rotation about the X-axis
av = [[np.pi/2, 0, 0], [np.pi/2, 0, 0]]
rot1_2 = TimeDependentRotation(quats, times, 1, 2, av=av)
rot2_3 = ConstantRotation([0, 1.0/np.sqrt(2), 0, 1.0/np.sqrt(2)], 2, 3)
rot1_3 = rot2_3*rot1_2
assert isinstance(rot1_3, TimeDependentRotation)
assert rot1_3.source == 1
assert rot1_3.dest == 3
expected_quats = np.array([[1, 0, 0, 0],[1.0/np.sqrt(2), 0, 0, -1.0/np.sqrt(2)]])
np.testing.assert_equal(rot1_3.times, np.array(times))
expected_quats = [[0.5, 0.5, -0.5, 0.5],[1.0/np.sqrt(2), 0, -1.0/np.sqrt(2), 0]]
expected_av = [[0, 0, -np.pi/2], [0, 0, -np.pi/2]]
np.testing.assert_equal(rot1_3.times, times)
np.testing.assert_almost_equal(rot1_3.quats, expected_quats)
np.testing.assert_almost_equal(rot1_3.av, expected_av)
def test_time_dependent_constant_composition():
# 90 degree rotation about the X-axis
rot1_2 = ConstantRotation([1.0/np.sqrt(2), 0, 0, 1.0/np.sqrt(2)], 1, 2)
# 90 degree rotation about the X-axis to a 180 degree rotation about the X-axis
quats = [[1.0/np.sqrt(2), 0, 0, 1.0/np.sqrt(2)],[1, 0, 0, 0]]
times = [0, 1]
rot2_3 = TimeDependentRotation(quats, times, 2, 3)
# compose to get a 180 degree rotation about the X-axis to a 270 degree rotation about the X-axis
av = [[np.pi/2, 0, 0], [np.pi/2, 0, 0]]
rot2_3 = TimeDependentRotation(quats, times, 2, 3, av=av)
rot1_3 = rot2_3*rot1_2
assert isinstance(rot1_3, TimeDependentRotation)
assert rot1_3.source == 1
assert rot1_3.dest == 3
expected_quats = np.array([[1, 0, 0, 0],[1.0/np.sqrt(2), 0, 0, -1.0/np.sqrt(2)]])
np.testing.assert_equal(rot1_3.times, np.array(times))
expected_quats = [[1, 0, 0, 0],[1.0/np.sqrt(2), 0, 0, -1.0/np.sqrt(2)]]
np.testing.assert_equal(rot1_3.times, times)
np.testing.assert_almost_equal(rot1_3.quats, expected_quats)
np.testing.assert_almost_equal(rot1_3.av, av)
def test_time_dependent_time_dependent_composition():
# 90 degree rotation about the X-axis to a 180 degree rotation about the X-axis
quats1_2 = [[1.0/np.sqrt(2), 0, 0, 1.0/np.sqrt(2)],[1, 0, 0, 0]]
times1_2 = [0, 1]
rot1_2 = TimeDependentRotation(quats1_2, times1_2, 1, 2)
av1_2 = [[np.pi/2, 0, 0], [np.pi/2, 0, 0]]
rot1_2 = TimeDependentRotation(quats1_2, times1_2, 1, 2, av=av1_2)
# -90 degree rotation about the X-axis to a 90 degree rotation about the X-axis
quats2_3 = [[1.0/np.sqrt(2), 0, 0, -1.0/np.sqrt(2)],[1.0/np.sqrt(2), 0, 0, 1.0/np.sqrt(2)]]
times2_3 = [0, 2]
rot2_3 = TimeDependentRotation(quats2_3, times2_3, 2, 3)
av2_3 = [[np.pi/2, 0, 0], [np.pi/2, 0, 0]]
rot2_3 = TimeDependentRotation(quats2_3, times2_3, 2, 3, av=av2_3)
# compose to get no rotation to a 180 degree rotation about the X-axis to no rotation
rot1_3 = rot2_3*rot1_2
assert isinstance(rot1_3, TimeDependentRotation)
assert rot1_3.source == 1
assert rot1_3.dest == 3
expected_times = np.array([0, 1, 2])
expected_quats = np.array([[0, 0, 0, -1], [-1, 0, 0, 0], [0, 0, 0, 1]])
expected_times = [0, 1, 2]
expected_quats = [[0, 0, 0, -1], [-1, 0, 0, 0], [0, 0, 0, 1]]
expected_av = [[np.pi, 0, 0], [np.pi, 0, 0], [np.pi, 0, 0]]
np.testing.assert_equal(rot1_3.times, expected_times)
np.testing.assert_almost_equal(rot1_3.quats, expected_quats)
np.testing.assert_almost_equal(rot1_3.av, expected_av)
def test_constant_inverse():
rot1_2 = ConstantRotation([1.0/np.sqrt(2), 0, 0, 1.0/np.sqrt(2)], 1, 2)
rot2_1 = rot1_2.inverse()
assert rot2_1.source == 2
assert rot2_1.dest == 1
expected_quats = np.array([1.0/np.sqrt(2), 0, 0, -1.0/np.sqrt(2)])
expected_quats = [1.0/np.sqrt(2), 0, 0, -1.0/np.sqrt(2)]
np.testing.assert_almost_equal(rot2_1.quat, expected_quats)
def test_time_dependent_inverse():
quats1_2 = [[1.0/np.sqrt(2), 0, 0, 1.0/np.sqrt(2)],[1, 0, 0, 0]]
times1_2 = [0, 1]
av1_2 = [[np.pi/2, 0, 0], [np.pi/2, 0, 0]]
rot1_2 = TimeDependentRotation(quats1_2, times1_2, 1, 2, av=av1_2)
rot2_1 = rot1_2.inverse()
assert rot2_1.source == 2
assert rot2_1.dest == 1
expected_quats = [[1.0/np.sqrt(2), 0, 0, -1.0/np.sqrt(2)],[1, 0, 0, 0]]
expected_av = [[-np.pi/2, 0, 0], [-np.pi/2, 0, 0]]
np.testing.assert_equal(rot2_1.times, times1_2)
np.testing.assert_almost_equal(rot2_1.quats, expected_quats)
np.testing.assert_almost_equal(rot2_1.av, expected_av)
def test_time_dependent_inverse_no_av():
quats1_2 = [[1.0/np.sqrt(2), 0, 0, 1.0/np.sqrt(2)],[1, 0, 0, 0]]
times1_2 = [0, 1]
rot1_2 = TimeDependentRotation(quats1_2, times1_2, 1, 2)
rot2_1 = rot1_2.inverse()
assert rot2_1.source == 2
assert rot2_1.dest == 1
expected_quats = np.array([[1.0/np.sqrt(2), 0, 0, -1.0/np.sqrt(2)],[1, 0, 0, 0]])
np.testing.assert_equal(rot2_1.times, np.array(times1_2))
expected_quats = [[1.0/np.sqrt(2), 0, 0, -1.0/np.sqrt(2)],[1, 0, 0, 0]]
np.testing.assert_equal(rot2_1.times, times1_2)
np.testing.assert_almost_equal(rot2_1.quats, expected_quats)
assert rot2_1.av is None
def test_rotation_matrix():
rot = ConstantRotation([0, 0, 0, 1], 1, 2)
......@@ -99,9 +114,10 @@ def test_from_euler():
times = [0, 1]
seq = 'XYZ'
rot = TimeDependentRotation.from_euler(seq, angles, times, 0, 1)
expected_quats = np.asarray([[0.5, 0.5, 0.5, 0.5], [-0.5, -0.5, 0.5, 0.5]])
expected_quats = [[0.5, 0.5, 0.5, 0.5], [-0.5, -0.5, 0.5, 0.5]]
np.testing.assert_almost_equal(rot.quats, expected_quats)
np.testing.assert_equal(rot.times, np.asarray(times))
assert rot.av is None
np.testing.assert_equal(rot.times, times)
assert rot.source == 0
assert rot.dest == 1
......@@ -113,6 +129,7 @@ def test_from_euler_degrees():
rad_rot = TimeDependentRotation.from_euler('XYZ', rad_angles, [0, 1], 0, 1)
degree_rot = TimeDependentRotation.from_euler('XYZ', degree_angles, [0, 1], 0, 1, degrees=True)
np.testing.assert_almost_equal(rad_rot.quats, degree_rot.quats)
assert degree_rot.av is None
def test_from_matrix():
mat = [[0, 0, 1],
......@@ -136,7 +153,7 @@ def test_slerp():
np.testing.assert_almost_equal(np.degrees(new_avs),
np.repeat([[90, 0, 0]], 8, 0))
def test_slerp_single_time():
def test_slerp_constant_rotation():
rot = TimeDependentRotation([[0, 0, 0, 1]], [0], 1, 2)
new_rot, new_avs = rot._slerp([-1, 3])
np.testing.assert_equal(new_rot.as_quat(),
......@@ -144,6 +161,16 @@ def test_slerp_single_time():
np.testing.assert_equal(new_avs,
[[0, 0, 0], [0, 0, 0]])
def test_slerp_single_time():
rot = TimeDependentRotation([[0, 0, 0, 1]], [0], 1, 2, av=[[np.pi/2, 0, 0]])
new_rot, new_avs = rot._slerp([-1, 3])
expected_quats = [[-1/np.sqrt(2), 0, 0, 1/np.sqrt(2)], [1/np.sqrt(2), 0, 0, -1/np.sqrt(2)]]
expected_av = [[np.pi/2, 0, 0], [np.pi/2, 0, 0]]
np.testing.assert_almost_equal(new_rot.as_quat(),
expected_quats)
np.testing.assert_equal(new_avs,
expected_av)
def test_slerp_variable_velocity():
test_quats = Rotation.from_euler('xyz',
[[0, 0, 0],
......@@ -203,6 +230,6 @@ def test_rotate_velocity_at():
input_times = [1, 2, 3]
rot_vel = rot.rotate_velocity_at(input_pos, input_vel, input_times)
np.testing.assert_almost_equal(rot_vel,
[[-1, -3 - np.pi, 2 - 3*np.pi/2],
[1 + 2*np.pi, -3, -2 + np.pi],
[3 + np.pi/2, 1 - 3*np.pi/2, -2]])
[[-1, -3 + np.pi, 2 + 3*np.pi/2],
[1 + 3*np.pi, -3 + np.pi, -2],
[3, 1 - np.pi, -2 - np.pi/2]])
......@@ -51,13 +51,15 @@ def isis_compare_dict():
'CkTableEndTime': -646346832.89712,
'CkTableOriginalSize': 1,
'EphemerisTimes': [-646346832.89712],
'Quaternions': [[0.34057881936764,0.085849252725072,0.69748691965044,-0.62461825983655]]},
'Quaternions': [[0.34057881936764,0.085849252725072,0.69748691965044,-0.62461825983655]],
'AngularVelocity': [[0.0, 0.0, 0.0]]},
'BodyRotation': {'TimeDependentFrames': [10024, 1],
'CkTableStartTime': -646346832.89712,
'CkTableEndTime': -646346832.89712,
'CkTableOriginalSize': 1,
'EphemerisTimes': [-646346832.89712],
'Quaternions': [[-0.029536576144092695, 0.010097306192904288, 0.22183794661925513, -0.9745837883512549]]},
'Quaternions': [[-0.029536576144092695, 0.010097306192904288, 0.22183794661925513, -0.9745837883512549]],
'AngularVelocity': [[-6.713536787324419e-07, -8.842601122842458e-06, 1.845852958470088e-05]]},
'InstrumentPosition': {'SpkTableStartTime': -646346832.89712,
'SpkTableEndTime': -646346832.89712,
'SpkTableOriginalSize': 1,
......@@ -81,12 +83,10 @@ def test_kernels():
@pytest.mark.parametrize("label_type", ['isis3'])
@pytest.mark.parametrize("formatter", ['isis'])
@pytest.mark.skip(reason="Fails due to angular velocity problems")
def test_voyager_load(test_kernels, label_type, formatter, isis_compare_dict):
label_file = get_image_label('c2065022', label_type)
usgscsm_isd_str = ale.loads(label_file, props={'kernels': test_kernels}, formatter=formatter)
usgscsm_isd_obj = json.loads(usgscsm_isd_str)
print(usgscsm_isd_obj)
assert compare_dicts(usgscsm_isd_obj, isis_compare_dict) == []
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment