# ISC License
#
# Copyright (c) 2016, Autonomous Vehicle Systems Lab, 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 inspect
import os
import numpy
import numpy as np
filename = inspect.getframeinfo(inspect.currentframe()).filename
path = os.path.dirname(os.path.abspath(filename))
from Basilisk import __path__
bskPath = __path__[0]
from Basilisk.utilities import macros
from Basilisk.utilities import orbitalMotion, RigidBodyKinematics
from Basilisk.utilities import SimulationBaseClass
import matplotlib.pyplot as plt
from Basilisk.topLevelModules import pyswice
from Basilisk.utilities.pyswice_spk_utilities import spkRead
from Basilisk.simulation import ephemerisConverter
from Basilisk.simulation import planetEphemeris
from Basilisk.simulation import spacecraft
from Basilisk.utilities import simIncludeGravBody
from Basilisk.utilities.supportDataTools.dataFetcher import get_path, DataFile
import pytest
# uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed
# @pytest.mark.skipif(conditionstring)
# uncomment this line if this test has an expected failure, adjust message as needed
# @pytest.mark.xfail() # need to update how the RW states are defined
# provide a unique test method name, starting with test_
[docs]
@pytest.mark.parametrize(
"function", ["singleGravityBody", "multiBodyGravity", "polyGravityBody"]
)
def test_gravityEffectorAllTest(show_plots, function):
"""Module Unit Test"""
testFunction = globals().get(function)
if testFunction is None:
raise ValueError(f"Function '{function}' not found in global scope")
[testResults, testMessage] = testFunction(show_plots)
assert testResults < 1, testMessage
[docs]
def singleGravityBody(show_plots):
"""Module Unit Test"""
# The __tracebackhide__ setting influences pytest showing of tracebacks:
# the mrp_steering_tracking() function will not be shown unless the
# --fulltrace command line option is specified.
__tracebackhide__ = True
testFailCount = 0 # zero unit test result counter
testMessages = [] # create empty list to store test log messages
# Create a sim module as an empty container
unitTaskName = "unitTask" # arbitrary name (don't change)
unitProcessName = "TestProcess" # arbitrary name (don't change)
# Create a sim module as an empty container
unitTestSim = SimulationBaseClass.SimBaseClass()
scObject = spacecraft.Spacecraft()
scObject.ModelTag = "spacecraftBody"
DynUnitTestProc = unitTestSim.CreateNewProcess(unitProcessName)
# create the dynamics task and specify the integration update time
DynUnitTestProc.addTask(
unitTestSim.CreateNewTask(unitTaskName, macros.sec2nano(10.0))
)
# setup Gravity Bodies
gravFactory = simIncludeGravBody.gravBodyFactory()
gravBodies = gravFactory.createBodies(
["earth", "sun", "moon", "jupiter barycenter"]
)
gravBodies["earth"].isCentralBody = True
ggm03s_path = get_path(DataFile.LocalGravData.GGM03S)
gravBodies["earth"].useSphericalHarmonicsGravityModel(str(ggm03s_path), 40)
stringCurrent = "2016 MAY 1 00:32:30.0"
gravFactory.createSpiceInterface(time=stringCurrent)
gravFactory.spiceObject.zeroBase = "Earth"
scObject.gravField.gravBodies = spacecraft.GravBodyVector(
list(gravFactory.gravBodies.values())
)
unitTestSim.AddModelToTask(unitTaskName, gravFactory.spiceObject, 10)
# Use the python spice utility to load in spacecraft SPICE ephemeris data
# Note: this following SPICE data only lives in the Python environment, and is
# separate from the earlier SPICE setup that was loaded to BSK. This is why
# all required SPICE libraries must be included when setting up and loading
# SPICE kernels in Python.
de430_path = get_path(DataFile.EphemerisData.de430)
naif0012_path = get_path(DataFile.EphemerisData.naif0012)
de403masses_path = get_path(DataFile.EphemerisData.de_403_masses)
pck00010_path = get_path(DataFile.EphemerisData.pck00010)
hst_edited_path = get_path(DataFile.EphemerisData.hst_edited)
pyswice.furnsh_c(str(de430_path))
pyswice.furnsh_c(str(naif0012_path))
pyswice.furnsh_c(str(de403masses_path))
pyswice.furnsh_c(str(pck00010_path))
pyswice.furnsh_c(str(hst_edited_path))
unitTestSim.AddModelToTask(unitTaskName, scObject, 9)
stateOut = spkRead("HUBBLE SPACE TELESCOPE", stringCurrent, "J2000", "EARTH")
scObject.hub.mHub = 100
scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]]
scObject.hub.IHubPntBc_B = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
scObject.hub.r_CN_NInit = (1000.0 * stateOut[0:3].reshape(3, 1)).tolist()
velStart = 1000.0 * stateOut[3:6]
scObject.hub.v_CN_NInit = (velStart.reshape(3, 1)).tolist()
scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]]
scObject.hub.omega_BN_BInit = [[0.001], [-0.002], [0.003]]
unitTestSim.InitializeSimulation()
posRef = scObject.dynManager.getStateObject(scObject.hub.nameOfHubPosition)
velRef = scObject.dynManager.getStateObject(scObject.hub.nameOfHubVelocity)
scObject.hub.mHub = 100
scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]]
scObject.hub.IHubPntBc_B = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
dt = 50.0
totalTime = 20000.0
currentTime = 0.0
posArray = []
velArray = []
posError = []
while currentTime < totalTime:
unitTestSim.ConfigureStopTime(macros.sec2nano(currentTime + dt))
unitTestSim.ExecuteSimulation()
stateOut = spkRead(
"HUBBLE SPACE TELESCOPE",
gravFactory.spiceObject.getCurrentTimeString(),
"J2000",
"EARTH",
)
posCurr = posRef.getState()
posCurr = [y for x in posCurr for y in x]
posArray.append(posCurr)
velCurr = velRef.getState()
velCurr = [y for x in velCurr for y in x]
velArray.append(velCurr)
posDiff = numpy.array(posCurr) - stateOut[0:3] * 1000.0
posRow = [unitTestSim.TotalSim.CurrentNanos * 1.0e-9]
posRow.extend(posDiff.tolist())
posError.append(posRow)
assert numpy.linalg.norm(posDiff) < 1000.0
currentTime += dt
stateOut = spkRead(
"HUBBLE SPACE TELESCOPE",
gravFactory.spiceObject.getCurrentTimeString(),
"J2000",
"EARTH",
)
posArray = numpy.array(posArray)
posError = numpy.array(posError)
gravFactory.unloadSpiceKernels()
de430_path = get_path(DataFile.EphemerisData.de430)
naif0012_path = get_path(DataFile.EphemerisData.naif0012)
de403masses_path = get_path(DataFile.EphemerisData.de_403_masses)
pck00010_path = get_path(DataFile.EphemerisData.pck00010)
hst_edited_path = get_path(DataFile.EphemerisData.hst_edited)
pyswice.unload_c(str(de430_path))
pyswice.unload_c(str(naif0012_path))
pyswice.unload_c(str(de403masses_path))
pyswice.unload_c(str(pck00010_path))
pyswice.unload_c(str(hst_edited_path))
print(numpy.max(abs(posError[:, 1:4])))
if show_plots:
plt.close("all")
plt.figure()
plt.plot(posError[:, 0], posError[:, 1:4])
plt.xlabel("Time (s)")
plt.ylabel("Position Difference (m)")
plt.show()
plt.close("all")
if testFailCount == 0:
print("PASSED: " + " Single body with spherical harmonics")
# return fail count and join into a single string all messages in the list
# testMessage
return [testFailCount, "".join(testMessages)]
def multiBodyGravity(show_plots):
# The __tracebackhide__ setting influences pytest showing of tracebacks:
# the mrp_steering_tracking() function will not be shown unless the
# --fulltrace command line option is specified.
__tracebackhide__ = True
testFailCount = 0 # zero unit test result counter
testMessages = [] # create empty list to store test log messages
# Create a sim module as an empty container
unitTaskName = "unitTask" # arbitrary name (don't change)
unitProcessName = "TestProcess" # arbitrary name (don't change)
# Create a sim module as an empty container
unitTestSim = SimulationBaseClass.SimBaseClass()
scObject = spacecraft.Spacecraft()
scObject.ModelTag = "spacecraftBody"
DynUnitTestProc = unitTestSim.CreateNewProcess(unitProcessName)
# create the dynamics task and specify the integration update time
DynUnitTestProc.addTask(
unitTestSim.CreateNewTask(unitTaskName, macros.sec2nano(5.0))
)
# setup Gravity Bodies
gravFactory = simIncludeGravBody.gravBodyFactory()
gravBodies = gravFactory.createBodies(
["earth", "mars barycenter", "sun", "moon", "jupiter barycenter"]
)
gravBodies["sun"].isCentralBody = True
stringCurrent = "2008 September 19, 04:00:00.0"
gravFactory.createSpiceInterface(time=stringCurrent)
gravFactory.spiceObject.zeroBase = "Earth"
scObject.gravField.gravBodies = spacecraft.GravBodyVector(
list(gravFactory.gravBodies.values())
)
unitTestSim.AddModelToTask(unitTaskName, gravFactory.spiceObject, 10)
# Use the python spice utility to load in spacecraft SPICE ephemeris data
# Note: this following SPICE data only lives in the Python environment, and is
# separate from the earlier SPICE setup that was loaded to BSK. This is why
# all required SPICE libraries must be included when setting up and loading
# SPICE kernels in Python.
de430_path = get_path(DataFile.EphemerisData.de430)
naif0012_path = get_path(DataFile.EphemerisData.naif0012)
de403masses_path = get_path(DataFile.EphemerisData.de_403_masses)
pck00010_path = get_path(DataFile.EphemerisData.pck00010)
nh_pred_od077_path = get_path(DataFile.EphemerisData.nh_pred_od077)
pyswice.furnsh_c(str(de430_path))
pyswice.furnsh_c(str(naif0012_path))
pyswice.furnsh_c(str(de403masses_path))
pyswice.furnsh_c(str(pck00010_path))
pyswice.furnsh_c(str(nh_pred_od077_path))
unitTestSim.AddModelToTask(unitTaskName, scObject, 9)
stateOut = spkRead("NEW HORIZONS", stringCurrent, "J2000", "SUN")
scObject.hub.mHub = 100
scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]]
scObject.hub.IHubPntBc_B = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
scObject.hub.r_CN_NInit = (1000.0 * stateOut[0:3].reshape(3, 1)).tolist()
velStart = 1000.0 * stateOut[3:6]
scObject.hub.v_CN_NInit = (velStart.reshape(3, 1)).tolist()
scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]]
scObject.hub.omega_BN_BInit = [[0.001], [-0.002], [0.003]]
unitTestSim.InitializeSimulation()
posRef = scObject.dynManager.getStateObject(scObject.hub.nameOfHubPosition)
velRef = scObject.dynManager.getStateObject(scObject.hub.nameOfHubVelocity)
dt = 50.0
totalTime = 20000.0
currentTime = 0.0
posArray = []
velArray = []
posError = []
posInc = []
while currentTime < totalTime:
unitTestSim.ConfigureStopTime(macros.sec2nano(currentTime + dt))
unitTestSim.ExecuteSimulation()
timeString = pyswice.et2utc_c(
gravFactory.spiceObject.J2000Current, "C", 4, 1024, "Yo"
)
stateOut = spkRead("NEW HORIZONS", timeString, "J2000", "SUN")
posCurr = posRef.getState()
posCurr = [y for x in posCurr for y in x]
posArray.append(posCurr)
velCurr = velRef.getState()
velCurr = [y for x in velCurr for y in x]
velArray.append(velCurr)
posDiff = numpy.array(posCurr) - stateOut[0:3] * 1000.0
posRow = [unitTestSim.TotalSim.CurrentNanos * 1.0e-9]
posRow.extend(posDiff.tolist())
posError.append(posRow)
assert numpy.linalg.norm(posDiff) < 1000.0
if currentTime > 0.0 + dt / 2.0:
posJump = stateOut[0:3] * 1000.0 - numpy.array(posPrevious)
posInc.append(posJump.tolist())
posPrevious = stateOut[0:3] * 1000.0
currentTime += dt
stateOut = spkRead(
"NEW HORIZONS", gravFactory.spiceObject.getCurrentTimeString(), "J2000", "SUN"
)
posArray = numpy.array(posArray)
posError = numpy.array(posError)
posInc = numpy.array(posInc)
gravFactory.unloadSpiceKernels()
de430_path = get_path(DataFile.EphemerisData.de430)
naif0012_path = get_path(DataFile.EphemerisData.naif0012)
de403masses_path = get_path(DataFile.EphemerisData.de_403_masses)
pck00010_path = get_path(DataFile.EphemerisData.pck00010)
nh_pred_od077_path = get_path(DataFile.EphemerisData.nh_pred_od077)
pyswice.unload_c(str(de430_path))
pyswice.unload_c(str(naif0012_path))
pyswice.unload_c(str(de403masses_path))
pyswice.unload_c(str(pck00010_path))
pyswice.unload_c(str(nh_pred_od077_path))
plt.close("all")
plt.figure()
plt.plot(posError[:, 0], posError[:, 1:4])
plt.xlabel("Time (s)")
plt.ylabel("Position Difference (m)")
if show_plots:
plt.show()
plt.close("all")
if testFailCount == 0:
print("PASSED: " + " multi-point source bodies")
# return fail count and join into a single string all messages in the list
# testMessage
return [testFailCount, "".join(testMessages)]
[docs]
def polyGravityBody(show_plots):
"""Module Unit Test"""
# The __tracebackhide__ setting influences pytest showing of tracebacks:
# the mrp_steering_tracking() function will not be shown unless the
# --fulltrace command line option is specified.
__tracebackhide__ = True
testFailCount = 0 # zero unit test result counter
testMessages = [] # create empty list to store test log messages
# Obtain validation data (simulation with tight integration tolerances in MATLAB)
valData = numpy.genfromtxt(path + "/../_UnitTest/polyTestData.csv", delimiter=",")
tVal = numpy.array(valData[:, 0])
posVal = numpy.array(valData[:, 1:4])
velVal = numpy.array(valData[:, 4:7])
# Create a sim module as an empty container
unitTaskName = "unitTask" # arbitrary name (don't change)
unitProcessName = "TestProcess" # arbitrary name (don't change)
# Create a sim module as an empty container
unitTestSim = SimulationBaseClass.SimBaseClass()
DynUnitTestProc = unitTestSim.CreateNewProcess(unitProcessName)
# create the dynamics task and specify the integration update time
intTime = 30.0
DynUnitTestProc.addTask(
unitTestSim.CreateNewTask(unitTaskName, macros.sec2nano(intTime))
)
# specify orbit of polyhedral body
oePolyBody = planetEphemeris.ClassicElements()
oePolyBody.a = 2.3612 * orbitalMotion.AU * 1000
oePolyBody.e = 0
oePolyBody.i = 0 * macros.D2R
oePolyBody.Omega = 0 * macros.D2R
oePolyBody.omega = 0 * macros.D2R
oePolyBody.f = 0 * macros.D2R
raPolyBody = 0 * macros.D2R
decPolyBody = 90 * macros.D2R
lst0PolyBody = 0 * macros.D2R
rotPeriodPolyBody = 5.27 * 3600
# setup celestial object ephemeris module
polyBodyEphem = planetEphemeris.PlanetEphemeris()
polyBodyEphem.ModelTag = "erosEphemeris"
polyBodyEphem.setPlanetNames(planetEphemeris.StringVector(["eros"]))
# specify celestial objects orbit
polyBodyEphem.planetElements = planetEphemeris.classicElementVector([oePolyBody])
# specify celestial object orientation
polyBodyEphem.rightAscension = planetEphemeris.DoubleVector([raPolyBody])
polyBodyEphem.declination = planetEphemeris.DoubleVector([decPolyBody])
polyBodyEphem.lst0 = planetEphemeris.DoubleVector([lst0PolyBody])
polyBodyEphem.rotRate = planetEphemeris.DoubleVector(
[360 * macros.D2R / rotPeriodPolyBody]
)
# setup polyhedral gravity body
mu = 4.46275472004 * 1e5
gravFactory = simIncludeGravBody.gravBodyFactory()
polyBody = gravFactory.createCustomGravObject("eros", mu=mu)
polyBody.isCentralBody = True
polyBody.usePolyhedralGravityModel(path + "/../_UnitTest/EROS856Vert1708Fac.txt")
polyBody.planetBodyInMsg.subscribeTo(polyBodyEphem.planetOutMsgs[0])
# create an ephemeris converter
polyBodyEphemConverter = ephemerisConverter.EphemerisConverter()
polyBodyEphemConverter.ModelTag = "erosEphemConverter"
polyBodyEphemConverter.addSpiceInputMsg(polyBodyEphem.planetOutMsgs[0])
# create spacecraft and attach polyhedral body
scObject = spacecraft.Spacecraft()
scObject.ModelTag = "spacecraft"
scObject.gravField.gravBodies = spacecraft.GravBodyVector(
list(gravFactory.gravBodies.values())
)
# set initial conditions for spacecraft
angvelPolyBody = np.array([0, 0, 360 * macros.D2R / rotPeriodPolyBody])
posInit = posVal[0, 0:3]
velInit = velVal[0, 0:3] + np.cross(angvelPolyBody, posInit)
scObject.hub.r_CN_NInit = posInit.tolist()
scObject.hub.v_CN_NInit = velInit.tolist()
# add models to task
unitTestSim.AddModelToTask(unitTaskName, polyBodyEphem, ModelPriority=10)
unitTestSim.AddModelToTask(unitTaskName, polyBodyEphemConverter, ModelPriority=9)
unitTestSim.AddModelToTask(unitTaskName, scObject, ModelPriority=8)
totalTime = 24 * 3600
samplingTime = 300
scRec = scObject.scStateOutMsg.recorder(macros.sec2nano(samplingTime))
polyBodyRec = polyBodyEphemConverter.ephemOutMsgs[0].recorder(
macros.sec2nano(samplingTime)
)
unitTestSim.AddModelToTask(unitTaskName, scRec)
unitTestSim.AddModelToTask(unitTaskName, polyBodyRec)
unitTestSim.InitializeSimulation()
unitTestSim.ConfigureStopTime(macros.sec2nano(totalTime))
unitTestSim.ExecuteSimulation()
# retrieve logged variables
time = scRec.times() * macros.NANO2SEC
N_points = len(time)
r_BN_N = scRec.r_BN_N
r_AN_N = polyBodyRec.r_BdyZero_N
sigma_AN = polyBodyRec.sigma_BN
# obtain position in small body centered fixed frame
posArray = numpy.zeros((N_points, 3))
for ii in range(N_points):
# obtain rotation matrix
R_AN = RigidBodyKinematics.MRP2C(sigma_AN[ii][0:3])
# rotate position and velocity
posArray[ii, 0:3] = R_AN.dot(numpy.subtract(r_BN_N[ii], r_AN_N[ii]))
# compute error in position and assert max error
posError = numpy.linalg.norm(posArray - posVal, axis=1)
assert max(posError) < 10
print(max(posError))
plt.close("all")
plt.figure()
plt.plot(tVal, posArray - posVal)
plt.xlabel("Time (s)")
plt.ylabel("Position Difference (m)")
if show_plots:
plt.show()
plt.close("all")
if testFailCount == 0:
print("PASSED: " + " Single body with polyhedral shape")
# return fail count and join into a single string all messages in the list
# testMessage
return [testFailCount, "".join(testMessages)]
if __name__ == "__main__":
# gravityEffectorAllTest(False)
singleGravityBody(True)
# multiBodyGravity(True)
polyGravityBody(True)