From 3d2b4aa4c33961f6963e17125cab6dfe2fac6902 Mon Sep 17 00:00:00 2001
From: Jesse Mapel <jmapel@usgs.gov>
Date: Fri, 20 Sep 2019 11:34:46 -0700
Subject: [PATCH] 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
---
ale/formatters/isis_formatter.py | 2 +
ale/rotation.py | 112 +++++++++++++++++---------
ale/transformation.py | 12 +--
tests/pytests/test_cassini_drivers.py | 3 +-
tests/pytests/test_data_isis.py | 12 +--
tests/pytests/test_mro_drivers.py | 30 ++++---
tests/pytests/test_rotation.py | 89 +++++++++++++-------
tests/pytests/test_voyager_drivers.py | 8 +-
8 files changed, 174 insertions(+), 94 deletions(-)
diff --git a/ale/formatters/isis_formatter.py b/ale/formatters/isis_formatter.py
index 13944fa..9d530d4 100644
--- a/ale/formatters/isis_formatter.py
+++ b/ale/formatters/isis_formatter.py
@@ -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
diff --git a/ale/rotation.py b/ale/rotation.py
index 9db50d6..cba6f04 100644
--- a/ale/rotation.py
+++ b/ale/rotation.py
@@ -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,23 +247,47 @@ 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]
- return interp_rots, interp_av
+ 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):
"""
@@ -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])
diff --git a/ale/transformation.py b/ale/transformation.py
index 6b1adb8..2d0f269 100644
--- a/ale/transformation.py
+++ b/ale/transformation.py
@@ -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:]
diff --git a/tests/pytests/test_cassini_drivers.py b/tests/pytests/test_cassini_drivers.py
index 5c43446..805221d 100644
--- a/tests/pytests/test_cassini_drivers.py
+++ b/tests/pytests/test_cassini_drivers.py
@@ -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 =========
diff --git a/tests/pytests/test_data_isis.py b/tests/pytests/test_data_isis.py
index 5afe3e4..76489e0 100644
--- a/tests/pytests/test_data_isis.py
+++ b/tests/pytests/test_data_isis.py
@@ -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])
diff --git a/tests/pytests/test_mro_drivers.py b/tests/pytests/test_mro_drivers.py
index 93bedff..77c5b31 100644
--- a/tests/pytests/test_mro_drivers.py
+++ b/tests/pytests/test_mro_drivers.py
@@ -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():
diff --git a/tests/pytests/test_rotation.py b/tests/pytests/test_rotation.py
index c2018f2..249cf2d 100644
--- a/tests/pytests/test_rotation.py
+++ b/tests/pytests/test_rotation.py
@@ -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]])
diff --git a/tests/pytests/test_voyager_drivers.py b/tests/pytests/test_voyager_drivers.py
index 7c054f6..40fab5b 100644
--- a/tests/pytests/test_voyager_drivers.py
+++ b/tests/pytests/test_voyager_drivers.py
@@ -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) == []
--
GitLab