Refactor pyswice utilities

.github/workflows/pull-request.yml

@@ -161,6 +161,10 @@ jobs:
  build-windows:
  name: Windows opNav
  runs-on: windows-2019
+ permissions:
+ actions: read
+ contents: write
+ security-events: write
  strategy:
  matrix:
  python-version: ["3.11"]

docs/source/Support/bskReleaseNotes.rst

@@ -54,6 +54,7 @@ Version |release|
 - Added CI support to test Linux on latest Ubuntu with opNav
 - Added CI support to build and test Basilisk documentation on the GitHub macOS platform
 - Added new scenario :ref:`scenarioOrbitManeuverTH` to do Hohmann transfer using thrusters
+- Refactored :ref:`pyswice_ck_utilities` utility file, added unit test
 - Made sure that :ref:`astroFunctions` and :ref:`simIncludeGravBody` now all pull from the same set of
  astronautical data in :ref:`astroConstants`. These tools now all use a consisten set of planet data
  referenced from NASA sources.

src/utilities/pyswice_ck_utilities.py

@@ -16,76 +16,86 @@
 # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 
 
-
 import os # Don't worry about this, standard stuff plus file discovery
-
 import numpy
 from Basilisk.topLevelModules import pyswice
-from Basilisk.utilities import RigidBodyKinematics
+from Basilisk.utilities import RigidBodyKinematics, macros
 
 
-def ckWrite(handle, time, MRPArray, avArray, startSeg, sc = -62, rf = "J2000"):
+def ckWrite(handle, time, mrp_array, av_array, start_seg, spacecraft_id=-62, reference_frame="J2000"):
  """
- Purpose: Creates a CK kernel from a timeArray, MRPArray, and an avArray. Assumes that the SCLK is furnshed
+ Purpose: Creates a CK kernel from a time_array, mrp_array, and an av_array. Assumes that the SCLK is furnshed
 
  .. warning::
 
- time stamps for the timeArray, MRPArray, and avArray must line up exactly!!
+ time stamps for the time_array, mrp_array, and av_array must line up exactly!!
 
  :param handle: What you would like the CK file to be named. Note, it must be in double quotes and end in .bc, ex: "moikernel.bc"
  :param time: numpy array of time stamps in nanoseconds
- :param MRPArray: array of modified Rodriguez parameters in column order x, y, z
- :param avArray: array of angular velocities about 3 axis in column order x, y, z
- :param startSeg: the SCLK time that the file begins at in UTC Gregorian ex: 'FEB 01,2021 12:00:55.9999 (UTC)'
- :param sc: spacecraft ID ex:-62
- :param rf: reference frame ex:"J2000"
+ :param mrp_array: array of modified Rodriguez parameters in column order x, y, z
+ :param av_array: array of angular velocities about 3 axis in column order x, y, z
+ :param start_seg: the SCLK time that the file begins at in UTC Gregorian ex: 'FEB 01,2021 12:00:55.9999 (UTC)'
+ :param spacecraft_id: spacecraft ID ex:-62
+ :param reference_frame: reference frame ex:"J2000"
  :return:
  """
  try:
  os.remove(handle)
  except OSError:
  pass
- fileHandle = pyswice.new_intArray(1)
- pyswice.ckopn_c(handle, "my-ckernel", 0, fileHandle)
- velLen = avArray.shape[0]
- velArray = pyswice.new_doubleArray(velLen * 3)
- z = MRPArray.shape[0]
- shapeMRP = numpy.shape(MRPArray)
- shapeavArray = numpy.shape(avArray)
- et = pyswice.new_doubleArray(1)
- pyswice.str2et_c(startSeg, et)
- starts = pyswice.new_doubleArray(1)
- pyswice.sce2c_c(sc, pyswice.doubleArray_getitem(et, 0), starts)
- zeroTime = 0 # pyswice.doubleArray_getitem(starts, 0)
- for w in range(velLen):
- for m in range(3):
- if shapeavArray[1] == 4:
- pyswice.doubleArray_setitem(velArray, (3 * w) + m, avArray[w, m + 1])
- else:
- pyswice.doubleArray_setitem(velArray, (3 * w) + m, avArray[w, m])
- quatArray = pyswice.new_doubleArray(z * 4)
- timeArray = pyswice.new_doubleArray(z)
- for i in range(z):
+
+ # Open the CK file
+ file_handle = pyswice.new_intArray(1)
+ pyswice.ckopn_c(handle, "my-ckernel", 0, file_handle)
+
+ # Create empty containers for time, attitude and angular velocity
+ num_data_points = len(time)
+ time_array = pyswice.new_doubleArray(num_data_points)
+ vel_array = pyswice.new_doubleArray(num_data_points * 3)
+ quat_array = pyswice.new_doubleArray(num_data_points * 4)
+
+ # Find the elapsed seconds between initial time and reference ephemeris
+ ephemeris_time_container = pyswice.new_doubleArray(1)
+ pyswice.str2et_c(start_seg, ephemeris_time_container)
+ ephemeris_time = pyswice.doubleArray_getitem(ephemeris_time_container, 0)
+
+ # Convert the initial time to number of spacecraft clock ticks
+ start_ticks = pyswice.new_doubleArray(1)
+ pyswice.sce2c_c(spacecraft_id, ephemeris_time, start_ticks)
+
+ # Process data for each timestep
+ for i in range(num_data_points):
+ # Process the attitude
+ quat = RigidBodyKinematics.MRP2EP(mrp_array[i, -3:]) # Grab the last 3 elements in case the first column is time
+ quat[1:4] = - quat[1:4] # Convert to JPL-style quaternions
  for j in range(4):
- if shapeMRP[1] == 4:
- quat = RigidBodyKinematics.MRP2EP(MRPArray[i, 1:4])
- quat[1:4] = -quat[1:4]
- else:
- quat = RigidBodyKinematics.MRP2EP(MRPArray[i, 0:3])
- quat[1:4] = -quat[1:4]
- pyswice.doubleArray_setitem(quatArray, (4 * i) + j, quat[j])
- sclkdp = pyswice.new_doubleArray(1)
- pyswice.sce2c_c(-62, time[i] + zeroTime*1.0E-9, sclkdp)
- pyswice.doubleArray_setitem(timeArray, i, pyswice.doubleArray_getitem(sclkdp, 0))
- # Getting time into usable format
- encStartTime = pyswice.doubleArray_getitem(timeArray, 0) - 1.0e-3 #Pad the beginning for roundoff
- encEndTime = pyswice.doubleArray_getitem(timeArray, z-1) + 1.0e-3 #Pad the end for roundoff
- pyswice.ckw03_c(pyswice.intArray_getitem(fileHandle, 0), encStartTime, encEndTime, -62000, rf, 1,
- "InertialData", z, timeArray, quatArray, velArray, 1, starts)
- pyswice.ckcls_c(pyswice.intArray_getitem(fileHandle, 0))
- return
-
-def ckRead(time, SCID=-62, rf="J2000"):
+ pyswice.doubleArray_setitem(quat_array, (4 * i) + j, quat[j])
+
+ # Process the angular velocity
+ omega = av_array[i, -3:] # Grab the last 3 elements in case the first column is time
+ for j in range(3):
+ pyswice.doubleArray_setitem(vel_array, (3 * i) + j, omega[j])
+
+ # Process time
+ current_time = ephemeris_time + time[i] * macros.NANO2SEC # Compute the current time in elapsed seconds from ephemeris
+ current_ticks = pyswice.new_doubleArray(1)
+ pyswice.sce2c_c(spacecraft_id, current_time, current_ticks) # Convert from ephemeris seconds to spacecraft clock ticks
+ pyswice.doubleArray_setitem(time_array, i, pyswice.doubleArray_getitem(current_ticks, 0))
+
+ # Get time into usable format
+ encoded_start_time = pyswice.doubleArray_getitem(time_array, 0) - 1.0e-3 # Pad the beginning for roundoff
+ encoded_end_time = pyswice.doubleArray_getitem(time_array, num_data_points - 1) + 1.0e-3 # Pad the end for roundoff
+
+ # Save the date into a CK file
+ pyswice.ckw03_c(pyswice.intArray_getitem(file_handle, 0), encoded_start_time, encoded_end_time, spacecraft_id,
+ reference_frame, 1, "InertialData", num_data_points, time_array, quat_array, vel_array, 1,
+ start_ticks)
+
+ # Close the CK file
+ pyswice.ckcls_c(pyswice.intArray_getitem(file_handle, 0))
+
+
+def ckRead(time, spacecraft_id=-62, reference_frame="J2000"):
  """
  Purpose: Read information out of a CK Kernel for a single instance and returns a quaternion array
  and an angular velocity array
@@ -95,59 +105,47 @@ def ckRead(time, SCID=-62, rf="J2000"):
  Assumes that SCLK and CK kernels are already loaded using furnsh because pyswice gets mad when loading the same files over and over again.
 
  :param time: Should be in UTC Gregorian, and passed in as a string, ex: 'FEB 01,2021 14:00:55.9999 (UTC)'
- :param SCID: Spacecraft ID -- Default: -62
- :param rf: is a character string which specifies the, reference frame of the segment. Reference Frame, ex: "J2000"
+ :param spacecraft_id: Spacecraft ID -- Default: -62
+ :param reference_frame: is a character string which specifies the, reference frame of the segment. Reference Frame, ex: "J2000"
  :return: None
  """
- #Getting time into usable format
- et = pyswice.new_doubleArray(1)
- pyswice.str2et_c(time, et)
+ # Find the elapsed seconds between initial time and reference ephemeris
+ ephemeris_time_container = pyswice.new_doubleArray(1)
+ pyswice.str2et_c(time, ephemeris_time_container)
+ ephemeris_time = pyswice.doubleArray_getitem(ephemeris_time_container, 0)
+
+ # Convert initial time to spacecraft clock tick
  tick = pyswice.new_doubleArray(1)
- pyswice.sce2c_c(SCID, pyswice.doubleArray_getitem(et, 0), tick)
- cmat = pyswice.new_doubleArray(9)
- av = pyswice.new_doubleArray(3)
- clkout = pyswice.new_doubleArray(1)
- intArray = pyswice.new_intArray(1)
- cmatConversion = numpy.zeros((3, 3))
- kernalQuatArray = numpy.zeros((1, 4))
- kernMRPArray = numpy.zeros((1, 3))
- kernOmega = numpy.zeros((1, 3))
- pyswice.ckgpav_c(SCID, pyswice.doubleArray_getitem(tick, 0), 0, rf, cmat, av, clkout, intArray)
- for q in range(9):
- if q < 3:
- cmatConversion[0, q] = pyswice.doubleArray_getitem(cmat, q)
- kernOmega[0, q] = pyswice.doubleArray_getitem(av, q)
- elif q >= 6:
- cmatConversion[2, (q - 6)] = pyswice.doubleArray_getitem(cmat, q)
- else:
- cmatConversion[1, (q - 3)] = pyswice.doubleArray_getitem(cmat, q)
- kernalQuat = RigidBodyKinematics.C2EP(cmatConversion)
- kernMRP = RigidBodyKinematics.C2MRP(cmatConversion)
- for s in range(4):
- if s < 3:
- kernMRPArray[0, s] = -kernMRP[s]
- kernalQuatArray[0, s] = -kernalQuat[s]
- if s == 0:
- kernalQuatArray[0, s] = -kernalQuatArray[0, s]
- etout = pyswice.doubleArray_getitem(et, 0)
- return etout, kernalQuatArray, kernOmega
+ pyswice.sce2c_c(spacecraft_id, ephemeris_time, tick)
+
+ # Get attitude and angular velocity for a specified spacecraft clock time
+ dcm_container = pyswice.new_doubleArray(9)
+ av_container = pyswice.new_doubleArray(3)
+ tick_container = pyswice.new_doubleArray(1)
+ requested_pointing_flag = pyswice.new_intArray(1)
+ pyswice.ckgpav_c(spacecraft_id, pyswice.doubleArray_getitem(tick, 0), 0, reference_frame, dcm_container,
+ av_container, tick_container, requested_pointing_flag)
+
+ # Grab angular velocity
+ omega = numpy.zeros(3)
+ for i in range(3):
+ omega[i] = pyswice.doubleArray_getitem(av_container, i)
+
+ # Grab attitude as a DCM
+ dcm = numpy.zeros((3, 3))
+ for i in range(9):
+ dcm[i // 3, i % 3] = pyswice.doubleArray_getitem(dcm_container, i) # Map 9D array into 3x3 matrix
+
+ # Convert attitude to quaternions
+ quat = RigidBodyKinematics.C2EP(dcm)
+ quat[1:4] = - quat[1:4] # Convert to JPL-style quaternions
+
+ return ephemeris_time, quat, omega
+
 
 def ckInitialize(ck_file_in):
  pyswice.furnsh_c(ck_file_in)
 
+
 def ckClose(ck_file_in):
  pyswice.unload_c(ck_file_in)
-
-def spkRead(target, time, ref, observer):
- et = pyswice.new_doubleArray(1)
- pyswice.str2et_c(time, et)
- state = pyswice.new_doubleArray(6)
- lt = pyswice.new_doubleArray(1)
-
- pyswice.spkezr_c(target, pyswice.doubleArray_getitem(et, 0), ref, "NONE",
- observer, state, lt)
- stateArray = numpy.zeros(6)
- lightTime = pyswice.doubleArray_getitem(lt, 0)
- for i in range(6):
- stateArray[i] = pyswice.doubleArray_getitem(state, i)
- return stateArray

src/utilities/pyswice_spk_utilities.py

@@ -46,17 +46,23 @@
 
 def spkRead(target, time, ref, observer):
  """Spice spk read method"""
+ # Convert time to elapsed seconds from ephemeris
  et = pyswice.new_doubleArray(1)
  pyswice.str2et_c(time, et)
+
+ # Get position and velocity
  state = pyswice.new_doubleArray(6)
  lt = pyswice.new_doubleArray(1)
-
  pyswice.spkezr_c(target, pyswice.doubleArray_getitem(et, 0), ref, "NONE", observer, state, lt)
+
+ # Format state into output array
  stateArray = numpy.zeros(6)
- lightTime = pyswice.doubleArray_getitem(lt, 0)
  for i in range(6):
  stateArray[i] = pyswice.doubleArray_getitem(state, i)
+
+ # Delete variables
  pyswice.delete_doubleArray(state)
  pyswice.delete_doubleArray(lt)
  pyswice.delete_doubleArray(et)
+
  return stateArray

src/utilities/tests/test_ck_utilities.py

@@ -0,0 +1,92 @@
+# ISC License
+#
+# Copyright (c) 2024, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder
+#
+# Permission to use, copy, modify, and/or distribute this software for any
+# purpose with or without fee is hereby granted, provided that the above
+# copyright notice and this permission notice appear in all copies.
+#
+# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+
+import numpy as np
+import os
+import shutil
+
+from Basilisk.simulation import spacecraft
+from Basilisk.utilities import SimulationBaseClass, macros, pyswice_ck_utilities, simIncludeGravBody, RigidBodyKinematics as rbk
+from Basilisk.topLevelModules import pyswice
+
+
+def test_ck_read_write(show_plots):
+ simulation = SimulationBaseClass.SimBaseClass()
+
+ process = simulation.CreateNewProcess("testProcess")
+ taskName = "task"
+ dynTaskRate = macros.sec2nano(1.0)
+ process.addTask(simulation.CreateNewTask(taskName, dynTaskRate))
+
+ scObject = spacecraft.Spacecraft()
+ scObject.ModelTag = "spacecraft"
+ simulation.AddModelToTask(taskName, scObject)
+
+ scObject.hub.mHub = 750.0
+ scObject.hub.IHubPntBc_B = np.array([[900., 0., 0.],
+ [0., 800., 0.],
+ [0., 0., 600.]])
+
+ scObject.hub.sigma_BNInit = [[0.1], [-0.2], [0.3]]
+ scObject.hub.omega_BN_BInit = [[0.01], [-0.01], [0.03]]
+
+ # Load up the leap second and spacecraft SPICE kernels
+ gravFactory = simIncludeGravBody.gravBodyFactory()
+ timeInit = 'FEB 01, 2021 12:00:00 (UTC)'
+ spiceObject = gravFactory.createSpiceInterface(time=timeInit)
+ pyswice.furnsh_c(spiceObject.SPICEDataPath + 'naif0011.tls') # leap second file
+ pyswice.furnsh_c(spiceObject.SPICEDataPath + 'MVN_SCLKSCET.00000.tsc') # spacecraft clock file
+
+ scObjectLogger = scObject.scStateOutMsg.recorder(dynTaskRate)
+ simulation.AddModelToTask(taskName, scObjectLogger)
+
+ simulation.InitializeSimulation()
+ simulation.ConfigureStopTime(macros.sec2nano(59)) # run for 59 seconds for easy time logic
+ simulation.ExecuteSimulation()
+
+ # Write a CK file using the attitude data from the simulation
+ timeWrite = scObjectLogger.times()
+ sigmaWrite = scObjectLogger.sigma_BN
+ omegaWrite = scObjectLogger.omega_BN_B
+ tmpFolderName = "scapFolder"
+ testFileName = tmpFolderName + "/test.bc"
+ if os.path.exists(tmpFolderName):
+ shutil.rmtree(tmpFolderName)
+ os.mkdir(tmpFolderName)
+ pyswice_ck_utilities.ckWrite(testFileName, timeWrite, sigmaWrite, omegaWrite, timeInit, spacecraft_id=-202)
+
+ # Read the same CK file to check if the values are identical
+ pyswice_ck_utilities.ckInitialize(testFileName)
+ sigmaRead = np.empty_like(sigmaWrite)
+ omegaRead = np.empty_like(omegaWrite)
+ for idx in range(len(timeWrite)):
+ # Change the time string to account for increasing time
+ timeString = timeInit[:19] + f"{int(timeWrite[idx] * macros.NANO2SEC):02}" + timeInit[21:]
+ _, kernQuat, kernOmega = pyswice_ck_utilities.ckRead(timeString, spacecraft_id=-202)
+
+ sigmaRead[idx, :] = - rbk.EP2MRP(kernQuat) # Convert from JPL-style quaternion notation
+ omegaRead[idx, :] = kernOmega
+
+ # Compare the read and write data
+ np.testing.assert_allclose(sigmaRead, sigmaWrite)
+ np.testing.assert_allclose(omegaRead, omegaWrite)
+
+ if os.path.exists(tmpFolderName):
+ # Delete the scrap folder
+ shutil.rmtree(tmpFolderName)
+
+if __name__ == "__main__":
+ test_ck_read_write(True)