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

ale/formatters/isis_formatter.py

+2 −0
Original line number Diff line number Diff line
@@ -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

ale/rotation.py

+76 −36
Original line number Diff line number Diff line
@@ -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])

ale/transformation.py

+6 −6
Original line number Diff line number Diff line
@@ -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:]

tests/pytests/test_cassini_drivers.py

+1 −2
Original line number Diff line number Diff line
@@ -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 =========

tests/pytests/test_data_isis.py

+6 −6
Original line number Diff line number Diff line
@@ -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])