def test_apply_at_single_time(): test_quats = Rotation.from_euler('x', np.array([-90, 0, 45]), degrees=True).as_quat() rot = TimeDependentRotation(test_quats, [0, 1, 1.5], 1, 2) input_vec = np.asarray([1, 2, 3]) rot_vec = rot.apply_at(input_vec, 0) np.testing.assert_almost_equal(rot_vec, np.asarray([[1, 3, -2]]))
def create_rotations(rotation_table): """ Convert an ISIS rotation table into rotation objects. Parameters ---------- rotation_table : dict The rotation ISIS table as a dictionary Returns ------- : list A list of time dependent or constant rotation objects from the table. This list will always have either 1 or 2 elements. The first rotation will be time dependent and the second rotation will be constant. The rotations will be ordered such that the reference frame the first rotation rotates to is the reference frame the second rotation rotates from. """ rotations = [] root_frame = rotation_table['TimeDependentFrames'][-1] last_time_dep_frame = rotation_table['TimeDependentFrames'][0] # Case 1: It's a table of quaternions and times if 'J2000Q0' in rotation_table: # SPICE quaternions are (W, X, Y, Z) and ALE uses (X, Y, Z, W). quats = np.array([ rotation_table['J2000Q1'], rotation_table['J2000Q2'], rotation_table['J2000Q3'], rotation_table['J2000Q0'] ]).T time_dep_rot = TimeDependentRotation(quats, rotation_table['ET'], root_frame, last_time_dep_frame) rotations.append(time_dep_rot) # Case 2: It's a table of Euler angle coefficients elif 'J2000Ang1' in rotation_table: ephemeris_times = np.linspace(rotation_table['CkTableStartTime'], rotation_table['CkTableEndTime'], rotation_table['CkTableOriginalSize']) base_time = rotation_table['J2000Ang1'][-1] time_scale = rotation_table['J2000Ang2'][-1] scaled_times = (ephemeris_times - base_time) / time_scale coeffs = np.array([ rotation_table['J2000Ang1'][:-1], rotation_table['J2000Ang2'][:-1], rotation_table['J2000Ang3'][:-1] ]).T angles = polyval(scaled_times, coeffs).T # ISIS is hard coded to ZXZ (313) Euler angle axis order. # SPICE also interprets Euler angle rotations as negative rotations, # so negate them before passing to scipy. time_dep_rot = TimeDependentRotation.from_euler( 'zxz', -angles, ephemeris_times, root_frame, last_time_dep_frame) rotations.append(time_dep_rot) if 'ConstantRotation' in rotation_table: last_constant_frame = rotation_table['ConstantFrames'][0] rot_mat = np.reshape(np.array(rotation_table['ConstantRotation']), (3, 3)) constant_rot = ConstantRotation.from_matrix(rot_mat, last_time_dep_frame, last_constant_frame) rotations.append(constant_rot) return rotations
def test_line_scan_driver(): j2000 = FrameNode(1) body_rotation = TimeDependentRotation( np.array([[0, 0, 0, 1], [0, 0, 0, 1]]), np.array([0, 1]), 100, 1 ) body_fixed = FrameNode(100, parent=j2000, rotation=body_rotation) spacecraft_rotation = TimeDependentRotation( np.array([[0, 0, 0, 1], [0, 0, 0, 1]]), np.array([0, 1]), 1000, 1 ) spacecraft = FrameNode(1000, parent=j2000, rotation=spacecraft_rotation) sensor_rotation = ConstantRotation(np.array([0, 0, 0, 1]), 1010, 1000) sensor = FrameNode(1010, parent=spacecraft, rotation=sensor_rotation) driver = TestLineScanner() driver.target_body_radii = (1100, 1000) driver.sensor_position = ( [[0, 1, 2], [3, 4, 5]], [[0, -1, -2], [-3, -4, -5]], [800, 900] ) driver.sun_position = ( [[0, 1, 2], [3, 4, 5]], [[0, -1, -2], [-3, -4, -5]], [800, 900] ) driver.sensor_frame_id = 1010 driver.target_frame_id = 100 driver.frame_chain = j2000 driver.sample_summing = 2 driver.line_summing = 4 driver.focal_length = 500 driver.detector_center_line = 0.5 driver.detector_center_sample = 512 driver.detector_start_line = 0 driver.detector_start_sample = 8 driver.focal2pixel_lines = [0.1, 0.2, 0.3] driver.focal2pixel_samples = [0.3, 0.2, 0.1] driver.usgscsm_distortion_model = { 'radial' : { 'coefficients' : [0.0, 1.0, 0.1] } } driver.image_lines = 10000 driver.image_samples = 1024 driver.platform_name = 'Test Platform' driver.sensor_name = 'Test Line Scan Sensor' driver.ephemeris_stop_time = 900 driver.ephemeris_start_time = 800 return driver
def test_slerp(): test_quats = Rotation.from_euler('x', np.array([-135, -90, 0, 45, 90]), degrees=True).as_quat() rot = TimeDependentRotation(test_quats, [-0.5, 0, 1, 1.5, 2], 1, 2) new_rots, new_avs = rot._slerp(np.arange(-3, 5)) expected_rot = Rotation.from_euler( 'x', [-360, -270, -180, -90, 0, 90, 180, 270], degrees=True) np.testing.assert_almost_equal(new_rots.as_quat(), expected_rot.as_quat()) np.testing.assert_almost_equal(np.degrees(new_avs), np.repeat([[90, 0, 0]], 8, 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_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] 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)) np.testing.assert_almost_equal(rot2_1.quats, expected_quats)
def test_from_euler_degrees(): rad_angles = [[np.pi / 2, np.pi / 2, 0], [-np.pi / 2, -np.pi / 2, 0]] degree_angles = [[90, 90, 0], [-90, -90, 0]] 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 frame_chain(self): """ Return the root node of the rotation frame tree/chain. The root node is the J2000 reference frame. The other nodes in the tree can be accessed via the methods in the FrameNode class. This property expects the ephemeris_time property/attribute to be defined. It should be a list of the ephemeris seconds past the J2000 epoch for each exposure in the image. Returns ------- FrameNode The root node of the frame tree. This will always be the J2000 reference frame. """ if not hasattr(self, '_root_frame'): j2000_id = 1 #J2000 is our root reference frame self._root_frame = FrameNode(j2000_id) sensor_quats = np.zeros((len(self.ephemeris_time), 4)) sensor_times = np.array(self.ephemeris_time) body_quats = np.zeros((len(self.ephemeris_time), 4)) body_times = np.array(self.ephemeris_time) for i, time in enumerate(self.ephemeris_time): sensor2j2000 = spice.pxform(spice.frmnam(self.sensor_frame_id), spice.frmnam(j2000_id), time) q_sensor = spice.m2q(sensor2j2000) sensor_quats[i, :3] = q_sensor[1:] sensor_quats[i, 3] = q_sensor[0] body2j2000 = spice.pxform(spice.frmnam(self.target_frame_id), spice.frmnam(j2000_id), time) q_body = spice.m2q(body2j2000) body_quats[i, :3] = q_body[1:] body_quats[i, 3] = q_body[0] sensor2j2000_rot = TimeDependentRotation(sensor_quats, sensor_times, self.sensor_frame_id, j2000_id) sensor_node = FrameNode(self.sensor_frame_id, parent=self._root_frame, rotation=sensor2j2000_rot) body2j2000_rot = TimeDependentRotation(body_quats, body_times, self.target_frame_id, j2000_id) body_node = FrameNode(self.target_frame_id, parent=self._root_frame, rotation=body2j2000_rot) return self._root_frame
def test_rotate_velocity_at(): test_quats = Rotation.from_euler( 'xyz', [[0, 0, 0], [-90, 0, 0], [-90, 180, 0], [-90, 180, 90]], degrees=True).as_quat() rot = TimeDependentRotation(test_quats, [0, 1, 2, 3], 1, 2) input_pos = [[1, 2, 3], [1, 2, 3], [1, 2, 3]] input_vel = [[-1, -2, -3], [-1, -2, -3], [-1, -2, -3]] 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 + 3 * np.pi, -3 + np.pi, -2], [3, 1 - np.pi, -2 - np.pi / 2]])
def frame_chain(self): j2000 = FrameNode(1) body_rotation = TimeDependentRotation( np.array([[0, 0, 0, 1], [0, 0, 0, 1]]), np.array([0, 1]), 100, 1) body_fixed = FrameNode(100, parent=j2000, rotation=body_rotation) spacecraft_rotation = TimeDependentRotation( np.array([[0, 0, 0, 1], [0, 0, 0, 1]]), np.array([0, 1]), 1000, 1) spacecraft = FrameNode(1000, parent=j2000, rotation=spacecraft_rotation) sensor_rotation = ConstantRotation(np.array([0, 0, 0, 1]), 1010, 1000) sensor = FrameNode(1010, parent=spacecraft, rotation=sensor_rotation) return j2000
def frame_chain(self): frame_chain = FrameChain() body_rotation = TimeDependentRotation( np.array([[0, 0, 0, 1], [0, 0, 0, 1]]), np.array([0, 1]), 100, 1) frame_chain.add_edge(rotation=body_rotation) spacecraft_rotation = TimeDependentRotation( np.array([[0, 0, 0, 1], [0, 0, 0, 1]]), np.array([0, 1]), 1000, 1) frame_chain.add_edge(rotation=spacecraft_rotation) sensor_rotation = ConstantRotation(np.array([0, 0, 0, 1]), 1010, 1000) frame_chain.add_edge(rotation=sensor_rotation) return frame_chain
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_slerp_variable_velocity(): test_quats = Rotation.from_euler( 'xyz', [[0, 0, 0], [-90, 0, 0], [-90, 180, 0], [-90, 180, 90]], degrees=True).as_quat() rot = TimeDependentRotation(test_quats, [0, 1, 2, 3], 1, 2) new_rots, new_avs = rot._slerp([-0.5, 0.5, 1.5, 2.5, 3.5]) expected_rot = Rotation.from_euler('xyz', [[45, 0, 0], [-45, 0, 0], [-90, 90, 0], [-90, 180, 45], [-90, 180, 135]], degrees=True) np.testing.assert_almost_equal(new_rots.as_quat(), expected_rot.as_quat()) np.testing.assert_almost_equal( np.degrees(new_avs), [[-90, 0, 0], [-90, 0, 0], [0, 180, 0], [0, 0, 90], [0, 0, 90]])
def test_last_time_dependent_frame_between(): """ Test frame tree structure: 1 / \ / \ 2 4 / \ / \ 3 5 The rotations from 3 to 2 and 1 to 4 are time dependent. All other rotations are constant. """ frame_chain = FrameChain() rotations = [ ConstantRotation(np.array([1, 0, 0, 0]), 2, 1), TimeDependentRotation( np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]), np.array([1]), 3, 2), TimeDependentRotation( np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]), np.array([1]), 4, 1), ConstantRotation(np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]), 5, 4) ] frame_chain.add_edge(rotation=rotations[0]) frame_chain.add_edge(rotation=rotations[1]) frame_chain.add_edge(rotation=rotations[2]) frame_chain.add_edge(rotation=rotations[3]) # last frame from node 1 to node 3 s31, d31, _ = frame_chain.last_time_dependent_frame_between(1, 3) assert s31 == 2 assert d31 == 3 # last frame from node 3 to node 1 s13, d13, _ = frame_chain.last_time_dependent_frame_between(3, 1) assert s13 == 3 assert d13 == 2 # last frame from node 3 to node 5 s35, d35, _ = frame_chain.last_time_dependent_frame_between(3, 5) assert s35 == 1 assert d35 == 4 # last frame from node 5 to node 3 s53, d53, _ = frame_chain.last_time_dependent_frame_between(5, 3) assert s53 == 2 assert d53 == 3
def from_spice(cls, *args, sensor_frame, target_frame, center_ephemeris_time, ephemeris_times=[], **kwargs): frame_chain = cls() times = np.array(ephemeris_times) sensor_time_dependent_frames, sensor_constant_frames = cls.frame_trace( sensor_frame, center_ephemeris_time) target_time_dependent_frames, target_constant_frames = cls.frame_trace( target_frame, center_ephemeris_time) time_dependent_frames = list( zip(sensor_time_dependent_frames[:-1], sensor_time_dependent_frames[1:])) constant_frames = list( zip(sensor_constant_frames[:-1], sensor_constant_frames[1:])) target_time_dependent_frames = list( zip(target_time_dependent_frames[:-1], target_time_dependent_frames[1:])) target_constant_frames = list( zip(target_constant_frames[:-1], target_constant_frames[1:])) time_dependent_frames.extend(target_time_dependent_frames) constant_frames.extend(target_constant_frames) 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): 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, av=avs) frame_chain.add_edge(rotation=rotation) 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:] quats[3] = quat_from_rotation[0] rotation = ConstantRotation(quats, s, d) frame_chain.add_edge(rotation=rotation) return frame_chain
def test_last_time_dependent_frame_between(): """ Test frame tree structure: 1 / \ / \ 2 4 / \ / \ 3 5 The rotations from 3 to 2 and 1 to 4 are time dependent. All other rotations are constant. """ rotations = [ ConstantRotation(np.array([1, 0, 0, 0]), 2, 1), TimeDependentRotation( np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]), np.array([1]), 4, 1), TimeDependentRotation( np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]), np.array([1]), 5, 4), ConstantRotation(np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]), 3, 2) ] root_node = FrameNode(1) child_node_1 = FrameNode(2, parent=root_node, rotation=rotations[0]) child_node_2 = FrameNode(3, parent=child_node_1, rotation=rotations[1]) child_node_3 = FrameNode(4, parent=root_node, rotation=rotations[2]) child_node_4 = FrameNode(5, parent=child_node_3, rotation=rotations[3]) last_frame_from_root_to_child_2 = root_node.last_time_dependent_frame_between( child_node_2) last_frame_from_child_2_to_root = child_node_2.last_time_dependent_frame_between( root_node) last_frame_from_child_2_to_child_4 = child_node_2.last_time_dependent_frame_between( child_node_4) last_frame_from_child_4_to_child_2 = child_node_4.last_time_dependent_frame_between( child_node_2) assert last_frame_from_root_to_child_2 == child_node_2 assert last_frame_from_child_2_to_root == child_node_1 assert last_frame_from_child_2_to_child_4 == child_node_3 assert last_frame_from_child_4_to_child_2 == child_node_2
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) # -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) # 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]) expected_quats = np.array([[0, 0, 0, -1], [-1, 0, 0, 0]]) np.testing.assert_equal(rot1_3.times, expected_times) np.testing.assert_almost_equal(rot1_3.quats, expected_quats)
def test_from_euler(): angles = [[np.pi / 2, np.pi / 2, 0], [-np.pi / 2, -np.pi / 2, 0]] times = [0, 1] seq = 'XYZ' rot = TimeDependentRotation.from_euler(seq, angles, times, 0, 1) 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) assert rot.av is None np.testing.assert_equal(rot.times, times) assert rot.source == 0 assert rot.dest == 1
def test_time_dependent_constant_composition(): rot1_2 = ConstantRotation([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)], 1, 2) quats = [[1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)], [1, 0, 0, 0]] times = [0, 1] 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 = [[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_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 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)) np.testing.assert_almost_equal(rot1_3.quats, expected_quats)
def frame_chain(self): if not hasattr(self, '_frame_chain'): nadir = self._props.get('nadir', False) self._frame_chain = FrameChain.from_spice( sensor_frame=self.sensor_frame_id, target_frame=self.target_frame_id, center_ephemeris_time=self.center_ephemeris_time, ephemeris_times=self.ephemeris_time, nadir=nadir) if nadir: # Logic for nadir calculation was taken from ISIS3 # SpiceRotation::setEphemerisTimeNadir rotation = self._frame_chain.compute_rotation( self.target_frame_id, 1) p_vec, v_vec, times = self.sensor_position rotated_positions = rotation.apply_at(p_vec, times) rotated_velocities = rotation.rotate_velocity_at( p_vec, v_vec, times) p_vec = rotated_positions v_vec = rotated_velocities velocity_axis = 2 # Get the default line translation with no potential flipping # from the driver trans_x = np.array( list(spice.gdpool('INS{}_ITRANSL'.format(self.ikid), 0, 3))) if (trans_x[0] < trans_x[1]): velocity_axis = 1 quats = [ spice.m2q( spice.twovec(-p_vec[i], 3, v_vec[i], velocity_axis)) for i, time in enumerate(times) ] quats = np.array(quats)[:, [1, 2, 3, 0]] rotation = TimeDependentRotation(quats, times, 1, self.sensor_frame_id) self._frame_chain.add_edge(rotation) return self._frame_chain
def test_reinterpolate(): rot = TimeDependentRotation([[0, 0, 0, 1], [0, 0, 0, 1]], [0, 1], 1, 2) new_rot = rot.reinterpolate(np.arange(-3, 5)) assert new_rot.source == rot.source assert new_rot.dest == rot.dest np.testing.assert_equal(new_rot.times, np.arange(-3, 5))
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(), [[0, 0, 0, 1], [0, 0, 0, 1]]) np.testing.assert_equal(new_avs, [[0, 0, 0], [0, 0, 0]])