# 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 math
import os
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 SimulationBaseClass
from Basilisk.utilities import (
unitTestSupport,
) # general support file with common unit test functions
from Basilisk.utilities import macros
from Basilisk.simulation import gravityEffector
from Basilisk.simulation import spiceInterface
from Basilisk.topLevelModules import pyswice
from Basilisk.utilities.pyswice_spk_utilities import spkRead
from Basilisk.simulation import stateArchitecture
from Basilisk.utilities import orbitalMotion as om
from Basilisk.architecture import messaging
from Basilisk.simulation.gravityEffector import loadGravFromFileToList
from Basilisk.architecture.bskLogging import BasiliskError
from Basilisk.utilities.supportDataTools.dataFetcher import get_path, DataFile
# script to check spherical harmonics calcs out to 20th degree
# Uses coefficient from Vallado tables D-1
def computeGravityTo20(positionVector):
# This code follows the formulation in Vallado, page 521, second edition and uses data from UTexas CSR for
# gravitation harmonics parameters
# Written 201780807 by Scott Carnahan
# AVS Lab | CU Boulder
# INPUTS
# positionVector - [x,y,z] coordinates list of spacecraft in [m] in earth body frame so that lat, long can be calculated
def legendres(degree, alpha):
P = np.zeros((degree + 1, degree + 1))
P[0, 0] = 1
P[1, 0] = alpha
cosPhi = np.sqrt(1 - alpha**2)
P[1, 1] = cosPhi
for l in range(2, degree + 1):
for m in range(0, l + 1):
if m == 0 and l >= 2:
P[l, m] = (
(2 * l - 1) * alpha * P[l - 1, 0] - (l - 1) * P[l - 2, 0]
) / l
elif m != 0 and m < l:
P[l, m] = P[l - 2, m] + (2 * l - 1) * cosPhi * P[l - 1, m - 1]
elif m == l and l != 0:
P[l, m] = (2 * l - 1) * cosPhi * P[l - 1, m - 1]
else:
print(l, ", ", m)
return P
maxDegree = 20
cList = np.zeros(maxDegree + 2)
sList = np.zeros(maxDegree + 2)
muEarth = 0.0
radEarth = 0.0
ggm03s_path = get_path(DataFile.LocalGravData.GGM03S)
[cList, sList, muEarth, radEarth] = loadGravFromFileToList(
str(ggm03s_path), maxDegree + 2
)
r = np.linalg.norm(positionVector)
rHat = positionVector / r
gHat = rHat
grav0 = -gHat * muEarth / r**2
rI = positionVector[0]
rJ = positionVector[1]
rK = positionVector[2]
rIJ = np.sqrt(rI**2 + rJ**2)
if rIJ != 0.0:
phi = math.atan(rK / rIJ) # latitude in radians
else:
phi = math.copysign(np.pi / 2.0, rK)
if rI != 0.0:
lambdaSat = math.atan(rJ / rI) # longitude in radians
else:
lambdaSat = math.copysign(np.pi / 2.0, rJ)
P = legendres(maxDegree + 1, np.sin(phi))
dUdr = 0.0
dUdphi = 0.0
dUdlambda = 0.0
for l in range(0, maxDegree + 1):
for m in range(0, l + 1):
if m == 0:
k = 1
else:
k = 2
num = math.factorial(l + m)
den = math.factorial(l - m) * k * (2 * l + 1)
PI = np.sqrt(float(num) / float(den))
cList[l][m] = cList[l][m] / PI
sList[l][m] = sList[l][m] / PI
for l in range(2, maxDegree + 1): # can only do for max degree minus 1
for m in range(0, l + 1):
dUdr = dUdr + (((radEarth / r) ** l) * (l + 1) * P[l, m]) * (
cList[l][m] * np.cos(m * lambdaSat)
+ sList[l][m] * np.sin(m * lambdaSat)
)
dUdphi = dUdphi + (
((radEarth / r) ** l) * P[l, m + 1] - m * np.tan(phi) * P[l, m]
) * (
cList[l][m] * np.cos(m * lambdaSat)
+ sList[l][m] * np.sin(m * lambdaSat)
)
dUdlambda = dUdlambda + (((radEarth / r) ** l) * m * P[l, m]) * (
sList[l][m] * np.cos(m * lambdaSat)
- cList[l][m] * np.sin(m * lambdaSat)
)
dUdr = -muEarth * dUdr / r**2
dUdphi = muEarth * dUdphi / r
dUdlambda = muEarth * dUdlambda / r
if rI != 0.0 and rJ != 0.0:
accelerationI = (
(dUdr / r - rK * dUdphi / (r**2) / ((rI**2 + rJ**2) ** 0.5)) * rI
- (dUdlambda / (rI**2 + rJ**2)) * rJ
+ grav0[0]
)
accelerationJ = (
(dUdr / r - rK * dUdphi / (r**2) / ((rI**2 + rJ**2) ** 0.5)) * rJ
+ (dUdlambda / (rI**2 + rJ**2)) * rI
+ grav0[1]
)
else:
accelerationI = dUdr / r + grav0[0]
accelerationJ = dUdr / r + grav0[1]
accelerationK = (
(dUdr / r) * rK + (((rI**2 + rJ**2) ** 0.5) * dUdphi / (r**2)) + grav0[2]
)
accelerationVector = [accelerationI, accelerationJ, accelerationK]
return accelerationVector
# 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]
def test_gravityEffectorAllTest(show_plots):
"""Module Unit Test"""
[testResults, testMessage] = independentSphericalHarmonics(show_plots)
assert testResults < 1, testMessage
[testResults, testMessage] = sphericalHarmonics(show_plots)
assert testResults < 1, testMessage
[testResults, testMessage] = singleGravityBody(show_plots)
assert testResults < 1, testMessage
[testResults, testMessage] = multiBodyGravity(show_plots)
assert testResults < 1, testMessage
def independentSphericalHarmonics(show_plots):
testCase = "independentCheck"
# 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
spherHarm = gravityEffector.SphericalHarmonics()
ggm03s_path = get_path(DataFile.LocalGravData.GGM03S)
gravityEffector.loadGravFromFile(str(ggm03s_path), spherHarm, 20)
gravCheck = computeGravityTo20([15000.0, 10000.0, 6378.1363e3])
spherHarm.initializeParameters()
gravOut = spherHarm.computeField(
[[15000.0], [10000.0], [(6378.1363) * 1.0e3]], 20, True
)
gravOutMag = np.linalg.norm(gravOut)
gravCheckMag = np.linalg.norm(gravCheck)
accuracy = 1e-12
relative = (gravCheckMag - gravOutMag) / gravCheckMag
if abs(relative) > accuracy:
testFailCount += 1
testMessages.append("Failed independent spherical harmonics check")
snippetName = testCase + "Accuracy"
snippetContent = "{:1.1e}".format(
accuracy
) # write formatted LATEX string to file to be used by auto-documentation.
unitTestSupport.writeTeXSnippet(
snippetName, snippetContent, path
) # write formatted LATEX string to file to be used by auto-documentation.
if testFailCount == 0:
passFailText = "PASSED"
print("PASSED: " + testCase)
colorText = "ForestGreen" # color to write auto-documented "PASSED" message in in LATEX.
snippetName = testCase + "FailMsg"
snippetContent = ""
unitTestSupport.writeTeXSnippet(
snippetName, snippetContent, path
) # write formatted LATEX string to file to be used by auto-documentation.
else:
passFailText = "FAILED"
colorText = "Red" # color to write auto-documented "FAILED" message in in LATEX
snippetName = testCase + "FailMsg"
snippetContent = passFailText
for message in testMessages:
snippetContent += ". " + message
snippetContent += "."
unitTestSupport.writeTeXSnippet(
snippetName, snippetContent, path
) # write formatted LATEX string to file to be used by auto-documentation.
snippetName = (
testCase + "PassFail"
) # name of file to be written for auto-documentation which specifies if this test was passed or failed.
snippetContent = (
r"\textcolor{" + colorText + "}{" + passFailText + "}"
) # write formatted LATEX string to file to be used by auto-documentation.
unitTestSupport.writeTeXSnippet(
snippetName, snippetContent, path
) # write formatted LATEX string to file to be used by auto-documentation.
# return fail count and join into a single string all messages in the list
# testMessage
return [testFailCount, "".join(testMessages)]
def sphericalHarmonics(show_plots):
testCase = "sphericalHarmonics"
# 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
spherHarm = gravityEffector.SphericalHarmonics()
testHarm = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
spherHarm.cBar = gravityEffector.MultiArray(testHarm)
vecCheckSuccess = True
for i in range(len(spherHarm.cBar)):
for j in range(len(spherHarm.cBar[i])):
if spherHarm.cBar[i][j] != testHarm[i][j]:
vecCheckSuccess = False
if vecCheckSuccess != True:
testFailCount += 1
testMessages.append("2D vector not input appropriately to spherical harmonics")
ggm03s_path = get_path(DataFile.LocalGravData.GGM03S)
gravityEffector.loadGravFromFile(str(ggm03s_path), spherHarm, 20)
spherHarm.initializeParameters()
gravOut = spherHarm.computeField([[0.0], [0.0], [(6378.1363) * 1.0e3]], 20, True)
gravMag = np.linalg.norm(np.array(gravOut))
accuracy = 0.1
gravExpected = 9.8
if gravMag > (gravExpected + accuracy) or gravMag < (gravExpected - accuracy):
testFailCount += 1
testMessages.append("Gravity magnitude not within allowable tolerance")
snippetName = testCase + "Accuracy"
snippetContent = "{:1.1e}".format(
accuracy
) # write formatted LATEX string to file to be used by auto-documentation.
unitTestSupport.writeTeXSnippet(
snippetName, snippetContent, path
) # write formatted LATEX string to file to be used by auto-documentation.
try:
spherHarm.computeField([[0.0], [0.0], [(6378.1363) * 1.0e3]], 100, True)
testFailCount += 1
testMessages.append("Gravity ceiling not enforced correctly")
except BasiliskError:
pass # Great! We threw an error
if testFailCount == 0:
passFailText = "PASSED"
print("PASSED: " + " Spherical Harmonics")
colorText = "ForestGreen" # color to write auto-documented "PASSED" message in in LATEX.
snippetName = testCase + "FailMsg"
snippetContent = ""
unitTestSupport.writeTeXSnippet(
snippetName, snippetContent, path
) # write formatted LATEX string to file to be used by auto-documentation.
else:
passFailText = "FAILED"
colorText = "Red" # color to write auto-documented "FAILED" message in in LATEX
snippetName = testCase + "FailMsg"
snippetContent = passFailText
for message in testMessages:
snippetContent += ". " + message
snippetContent += "."
unitTestSupport.writeTeXSnippet(
snippetName, snippetContent, path
) # write formatted LATEX string to file to be used by auto-documentation.
snippetName = (
testCase + "PassFail"
) # name of file to be written for auto-documentation which specifies if this test was passed or failed.
snippetContent = (
r"\textcolor{" + colorText + "}{" + passFailText + "}"
) # write formatted LATEX string to file to be used by auto-documentation.
unitTestSupport.writeTeXSnippet(
snippetName, snippetContent, path
) # write formatted LATEX string to file to be used by auto-documentation.
# return fail count and join into a single string all messages in the list
# testMessage
return [testFailCount, "".join(testMessages)]
def singleGravityBody(show_plots):
testCase = "singleBody"
# 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)
DateSpice = "2015 February 10, 00:00:00.0 TDB"
# Create a sim module as an empty container
TotalSim = SimulationBaseClass.SimBaseClass()
DynUnitTestProc = TotalSim.CreateNewProcess(unitProcessName)
# create the dynamics task and specify the integration update time
DynUnitTestProc.addTask(TotalSim.CreateNewTask(unitTaskName, macros.sec2nano(0.1)))
# Initialize the modules that we are using.
SpiceObject = spiceInterface.SpiceInterface()
SpiceObject.ModelTag = "SpiceInterfaceData"
SpiceObject.addPlanetNames(
spiceInterface.StringVector(["earth", "mars barycenter", "sun"])
)
SpiceObject.UTCCalInit = DateSpice
TotalSim.AddModelToTask(unitTaskName, SpiceObject)
SpiceObject.UTCCalInit = "1994 JAN 26 00:02:00.184"
gravBody1 = gravityEffector.GravBodyData()
gravBody1.planetName = "earth_planet_data"
gravBody1.isCentralBody = False
ggm03s_path = get_path(DataFile.LocalGravData.GGM03S)
gravBody1.useSphericalHarmonicsGravityModel(str(ggm03s_path), 60)
gravBody1.planetBodyInMsg.subscribeTo(SpiceObject.planetStateOutMsgs[0])
# 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 kernals 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))
SpiceObject.UTCCalInit = "2012 MAY 1 00:02:00.184"
stringCurrent = SpiceObject.UTCCalInit
et = pyswice.new_doubleArray(1)
dt = 1.0
pyswice.str2et_c(stringCurrent, et)
etCurr = pyswice.doubleArray_getitem(et, 0)
normVec = []
gravErrNorm = []
SpiceObject.UTCCalInit = stringCurrent
TotalSim.InitializeSimulation()
gravBody1.initBody(0)
newManager = stateArchitecture.DynParamManager()
gravBody1.registerProperties(newManager)
SpiceObject.UpdateState(0)
for i in range(2 * 3600):
stateOut = spkRead("HUBBLE SPACE TELESCOPE", stringCurrent, "J2000", "EARTH")
etPrev = etCurr - 2.0
stringPrev = pyswice.et2utc_c(etPrev, "C", 4, 1024, "Yo")
statePrev = spkRead("HUBBLE SPACE TELESCOPE", stringPrev, "J2000", "EARTH")
etNext = etCurr + 2.0
stringNext = pyswice.et2utc_c(etNext, "C", 4, 1024, "Yo")
stateNext = spkRead("HUBBLE SPACE TELESCOPE", stringNext, "J2000", "EARTH")
gravVec = (stateNext[3:6] - statePrev[3:6]) / (etNext - etPrev)
normVec.append(np.linalg.norm(stateOut[0:3]))
stateOut *= 1000.0
SpiceObject.J2000Current = etCurr
SpiceObject.UpdateState(0)
gravBody1.loadEphemeris()
gravOut = gravBody1.computeGravityInertial(
stateOut[0:3].reshape(3, 1).tolist(), 0
)
gravErrNorm.append(
np.linalg.norm(gravVec * 1000.0 - np.array(gravOut).reshape(3))
/ np.linalg.norm(gravVec * 1000.0)
)
pyswice.str2et_c(stringCurrent, et)
etCurr = pyswice.doubleArray_getitem(et, 0)
etCurr += dt
stringCurrent = pyswice.et2utc_c(etCurr, "C", 4, 1024, "Yo")
accuracy = 1.0e-4
for gravErr in gravErrNorm:
if gravErr > accuracy:
testFailCount += 1
testMessages.append(
"Gravity numerical error too high for kernel comparison"
)
break
snippetName = testCase + "Accuracy"
snippetContent = "{:1.1e}".format(
accuracy
) # write formatted LATEX string to file to be used by auto-documentation.
unitTestSupport.writeTeXSnippet(
snippetName, snippetContent, path
) # write formatted LATEX string to file to be used by auto-documentation.
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))
if testFailCount == 0:
passFailText = "PASSED"
print("PASSED: " + "Single-body with Spherical Harmonics")
colorText = (
"ForestGreen" # color to write auto-documented "PASSED" message in in LATEX
)
snippetName = testCase + "FailMsg"
snippetContent = ""
unitTestSupport.writeTeXSnippet(
snippetName, snippetContent, path
) # write formatted LATEX string to file to be used by auto-documentation.
else:
passFailText = "FAILED"
colorText = "Red" # color to write auto-documented "FAILED" message in in LATEX
snippetName = testCase + "FailMsg"
snippetContent = passFailText
for message in testMessages:
snippetContent += ". " + message
snippetContent += "."
unitTestSupport.writeTeXSnippet(
snippetName, snippetContent, path
) # write formatted LATEX string to file to be used by auto-documentation.
snippetName = (
testCase + "PassFail"
) # name of file to be written for auto-documentation which specifies if this test was passed or failed.
snippetContent = (
r"\textcolor{" + colorText + "}{" + passFailText + "}"
) # write formatted LATEX string to file to be used by auto-documentation.
unitTestSupport.writeTeXSnippet(
snippetName, snippetContent, path
) # write formatted LATEX string to file to be used by auto-documentation.
# return fail count and join into a single string all messages in the list
# testMessage
return [testFailCount, "".join(testMessages)]
[docs]
def register(manager):
"""
populates the state engines dynParamManager with nominal values
:param manager:
:return: posVelSig, posVelSig
"""
positionName = "hubPosition"
stateDim = [3, 1]
posState = manager.registerState(stateDim[0], stateDim[1], positionName)
posVelSig = [[0.0], [0.0], [0.0]]
posState.setState(posVelSig)
velocityName = "hubVelocity"
stateDim = [3, 1]
velState = manager.registerState(stateDim[0], stateDim[1], velocityName)
velState.setState(posVelSig)
sigmaName = "hubSigma"
stateDim = [3, 1]
sigmaState = manager.registerState(stateDim[0], stateDim[1], sigmaName)
sigmaState.setState(posVelSig)
initC_B = [[0.0], [0.0], [0.0]]
manager.createProperty("centerOfMassSC", initC_B)
manager.createProperty("systemTime", [[0], [0.0]])
return
def multiBodyGravity(show_plots):
testCase = "multiBody" # for AutoTeX stuff
# 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)
# DateSpice = "2015 February 10, 00:00:00.0 TDB"
#
# # Create a sim module as an empty container
multiSim = SimulationBaseClass.SimBaseClass()
#
DynUnitTestProc = multiSim.CreateNewProcess(unitProcessName)
# # create the dynamics task and specify the integration update time
DynUnitTestProc.addTask(
multiSim.CreateNewTask(unitTaskName, macros.sec2nano(1000.0))
)
# Create dynParamManager to feed fake spacecraft data to so that the gravityEffector can access it.
# This places the spacecraft at the coordinate frame origin so that planets can be placed around it.
# velocity and attitude are just set to zero.
# center of mass and time are set to zero.
newManager = stateArchitecture.DynParamManager()
register(newManager)
# Create a message struct to place gravBody1 where it is wanted
localPlanetEditor = messaging.SpicePlanetStateMsgPayload()
localPlanetEditor.PositionVector = [om.AU / 10.0, 0.0, 0.0]
localPlanetEditor.VelocityVector = [0.0, 0.0, 0.0]
localPlanetEditor.J20002Pfix = np.identity(3)
# Grav Body 1 is twice the size of the other two
gravBody1 = gravityEffector.GravBodyData()
gravBody1.planetName = "gravBody1_planet_data"
gravBody1.mu = 1000000.0
gravBody1.radEquator = 6500.0
gravBody1.isCentralBody = False
gravBody1.localPlanet = localPlanetEditor
# This is the gravityEffector which will actually compute the gravitational acceleration
allGrav = gravityEffector.GravityEffector()
allGrav.gravBodies = gravityEffector.GravBodyVector([gravBody1])
allGrav.linkInStates(newManager)
allGrav.registerProperties(newManager)
allGrav.Reset(0)
multiSim.AddModelToTask(unitTaskName, allGrav)
posVelSig = [[0.0], [0.0], [0.0]]
allGrav.computeGravityField(
posVelSig, posVelSig
) # compute acceleration only considering the first body.
step1 = newManager.getPropertyReference(
allGrav.vehicleGravityPropName
) # retrieve total gravitational acceleration in inertial frame
# Create a message struct to place gravBody2&3 where they are wanted.
localPlanetEditor.PositionVector = [-om.AU / 10.0, 0.0, 0.0]
localPlanetEditor.VelocityVector = [0.0, 0.0, 0.0]
# grav Body 2 and 3 are coincident with each other, half the mass of gravBody1 and are in the opposite direction of gravBody1
gravBody2 = gravityEffector.GravBodyData()
gravBody2.planetName = "gravBody2_planet_data"
gravBody2.mu = gravBody1.mu / 2.0
gravBody2.radEquator = 6500.0
gravBody2.isCentralBody = False
gravBody2.localPlanet = localPlanetEditor
# This is the gravityEffector which will actually compute the gravitational acceleration
newManager = stateArchitecture.DynParamManager()
register(newManager)
allGrav2 = gravityEffector.GravityEffector()
allGrav2.gravBodies = gravityEffector.GravBodyVector([gravBody1, gravBody2])
allGrav2.linkInStates(newManager)
allGrav2.registerProperties(newManager)
allGrav2.Reset(0)
multiSim.AddModelToTask(unitTaskName, allGrav2)
allGrav2.computeGravityField(
posVelSig, posVelSig
) # compute acceleration considering the first and second bodies.
step2 = newManager.getPropertyReference(
allGrav2.vehicleGravityPropName
) # retrieve total gravitational acceleration in inertial frame
# grav Body 2 and 3 are coincident with each other, half the mass of gravBody1 and are in the opposite direction of gravBody1
gravBody3 = gravityEffector.GravBodyData()
gravBody3.planetName = "gravBody3_planet_data"
gravBody3.mu = gravBody2.mu
gravBody3.radEquator = 6500.0
gravBody3.isCentralBody = False
gravBody3.localPlanet = localPlanetEditor
# This is the gravityEffector which will actually compute the gravitational acceleration
newManager = stateArchitecture.DynParamManager()
register(newManager)
allGrav3 = gravityEffector.GravityEffector()
allGrav3.gravBodies = gravityEffector.GravBodyVector(
[gravBody1, gravBody2, gravBody3]
)
allGrav3.linkInStates(newManager)
allGrav3.registerProperties(newManager)
allGrav3.Reset(0)
multiSim.AddModelToTask(unitTaskName, allGrav3)
allGrav3.computeGravityField(
posVelSig, posVelSig
) # comput acceleration considering all three bodies
step3 = newManager.getPropertyReference(
allGrav3.vehicleGravityPropName
) # retrieve total gravitational acceleration in inertial frame
step3 = [
0.0,
step3[0][0],
step3[1][0],
step3[2][0],
] # add a first (time) column to use isArrayZero
# Test results for accuracy
accuracy = 1e-12
snippetName = testCase + "Accuracy"
snippetContent = "{:1.1e}".format(
accuracy
) # write formatted LATEX string to file to be used by auto-documentation.
unitTestSupport.writeTeXSnippet(
snippetName, snippetContent, path
) # write formatted LATEX string to file to be used by auto-documentation.
if not unitTestSupport.isDoubleEqualRelative(
step2[0][0] / step1[0][0], 0.5, accuracy
): # if the second grav body doesn't cancel exactly half of the first body's acceleration.
testFailCount += 1
passFailText = "Step 2 was not half of step 1"
testMessages.append(passFailText)
elif not unitTestSupport.isArrayZero(
step3, 3, accuracy
): # if the net acceleration is not now 0.
testFailCount += 1
passFailText = "Step 3 did not cause gravity to return to 0"
testMessages.append(passFailText)
# Record test results to LaTeX
if testFailCount == 0:
passFailText = "PASSED"
print("PASSED: " + " Multi-Body")
colorText = (
"ForestGreen" # color to write auto-documented "PASSED" message in in LATEX
)
snippetName = testCase + "FailMsg"
snippetContent = ""
unitTestSupport.writeTeXSnippet(
snippetName, snippetContent, path
) # write formatted LATEX string to file to be used by auto-documentation.
else:
passFailText = "FAILED"
colorText = "Red" # color to write auto-documented "FAILED" message in in LATEX
snippetName = testCase + "FailMsg"
snippetContent = passFailText
for message in testMessages:
snippetContent += ". " + message
snippetContent += "."
unitTestSupport.writeTeXSnippet(
snippetName, snippetContent, path
) # write formatted LATEX string to file to be used by auto-documentation.
snippetName = (
testCase + "PassFail"
) # name of file to be written for auto-documentation which specifies if this test was passed or failed.
snippetContent = (
r"\textcolor{" + colorText + "}{" + passFailText + "}"
) # write formatted LATEX string to file to be used by auto-documentation.
unitTestSupport.writeTeXSnippet(
snippetName, snippetContent, path
) # write formatted LATEX string to file to be used by auto-documentation.
return [testFailCount, "".join(testMessages)]
if __name__ == "__main__":
# test_gravityEffectorAllTest(False)
# independentSphericalHarmonics(False)
# sphericalHarmonics(False)
# singleGravityBody(True)
multiBodyGravity(True)