# 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 matplotlib.pyplot as plt
import numpy
import pytest
#
# orb_elem_convert Unit Test
#
# Purpose: Test the precision of the orb_elem_convert module. Functionality
# is tested by comparing input/output data as well as calculated
# conversions.
# Author: Gabriel Chapel
# Creation Date: July 27, 2017
#
filename = inspect.getframeinfo(inspect.currentframe()).filename
path = os.path.dirname(os.path.abspath(filename))
splitPath = path.split('simulation')
from Basilisk.utilities import SimulationBaseClass
from Basilisk.simulation import orbElemConvert
from Basilisk.utilities import macros
from Basilisk.utilities import macros as mc
from Basilisk.utilities import simHelpers
from Basilisk.architecture import messaging
from Basilisk.utilities.pythonVariableLogger import PythonVariableLogger
from Basilisk.utilities import orbitalMotion
[docs]
@pytest.mark.parametrize("eccentricity", [0.0, 0.3]) # [-]
@pytest.mark.parametrize("inclination", [0.0, 1e-8, math.pi - 1e-8, math.pi]) # [rad]
def test_singular_state_message(eccentricity, inclination):
"""Preserve singular and near-equatorial states through the native module's messages.
An analytic periapsis state provides independent reference geometry. Check
the emitted elements against Python and reconstruct both position and
velocity through the module's native conversion.
"""
mu = orbitalMotion.MU_EARTH * 1e9 # [m^3/s^2]
radius = 6e6 # [m]
sin_i = 0.0 if inclination in (0.0, math.pi) else math.sin(inclination)
cos_i = math.cos(inclination)
position = radius * numpy.array([0.6, 0.8 * cos_i, 0.8 * sin_i])
speed = math.sqrt(mu * (1.0 + eccentricity) / radius)
velocity = speed * numpy.array([-0.8, 0.6 * cos_i, 0.6 * sin_i])
payload = messaging.SCStatesMsgPayload()
payload.r_BN_N = position
payload.v_BN_N = velocity
state_message = messaging.SCStatesMsg().write(payload)
converter = orbElemConvert.OrbElemConvert()
converter.mu = mu
converter.scStateInMsg.subscribeTo(state_message)
recorder = converter.elemOutMsg.recorder()
converter.Reset(0)
converter.UpdateState(0)
recorder.UpdateState(0)
result = converter.elemOutMsg.read()
assert len(recorder.times()) == 1
python_result = orbitalMotion.rv2elem(mu, position, velocity, canonical=True)
assert result.i == pytest.approx(inclination, abs=2e-15, rel=0.0)
assert result.e == pytest.approx(eccentricity, abs=2e-15, rel=0.0)
assert abs(math.remainder(result.Omega, 2.0 * math.pi)) < 2e-14
expected_periapsis = 0.0 if eccentricity == 0.0 else math.atan2(0.8, 0.6) # [rad]
expected_anomaly = math.atan2(0.8, 0.6) if eccentricity == 0.0 else 0.0 # [rad]
assert abs(math.remainder(result.omega - expected_periapsis, 2.0 * math.pi)) < 2e-14
assert abs(math.remainder(result.f - expected_anomaly, 2.0 * math.pi)) < 2e-14
for field in ("a", "e", "i", "Omega", "omega", "f", "alpha", "rmag", "rPeriap", "rApoap"):
native_value = getattr(result, field)
python_value = getattr(python_result, field)
assert math.isfinite(native_value)
if field in ("Omega", "omega", "f"):
assert abs(math.remainder(native_value - python_value, 2.0 * math.pi)) < 2e-14
else:
assert native_value == pytest.approx(python_value, rel=2e-14, abs=2e-15)
converter.Elements2Cartesian()
numpy.testing.assert_allclose(numpy.asarray(converter.r_N) / radius, position / radius, rtol=0.0, atol=2e-14)
numpy.testing.assert_allclose(numpy.asarray(converter.v_N) / speed, velocity / speed, rtol=0.0, atol=2e-14)
[docs]
@pytest.mark.parametrize("kind", ["near_ellipse", "parabola", "near_hyperbola", "radial_bound",
"radial_escape", "radial_unbound", "rest"])
@pytest.mark.parametrize("sign", [-1.0, 1.0])
@pytest.mark.parametrize("scale", [1e-6, 1e3])
def test_degenerate_conic_messages(kind, sign, scale):
"""Preserve conic metadata through both message directions and agree with Python."""
mu = orbitalMotion.MU_EARTH * scale**3 # [L^3/s^2]
periapsis = 7500.0 * scale # [L]
axis = numpy.array([0.6, 0.8, 0.0])
transverse = numpy.array([0.0, 0.0, 1.0])
if kind in ("near_ellipse", "parabola", "near_hyperbola"):
eccentricity = {"near_ellipse": 1.0 - 5e-13, "parabola": 1.0, "near_hyperbola": 1.0 + 5e-13}[kind]
anomaly = sign * 0.7 # [rad]
parameter = periapsis * (1.0 + eccentricity)
position = (parameter / (1.0 + eccentricity * math.cos(anomaly))
* (math.cos(anomaly) * axis + math.sin(anomaly) * transverse))
velocity = (math.sqrt(mu / parameter)
* (-math.sin(anomaly) * axis + (eccentricity + math.cos(anomaly)) * transverse))
conditioning = 8.0 * numpy.finfo(float).eps / abs(1.0 - eccentricity) if eccentricity != 1.0 else 2e-14
else:
factor = {"radial_bound": 0.6, "radial_escape": math.sqrt(2.0), "radial_unbound": 2.0, "rest": 0.0}[kind]
position = periapsis * axis
velocity = sign * factor * math.sqrt(mu / periapsis) * axis
conditioning = 2e-14
payload = messaging.SCStatesMsgPayload()
payload.r_BN_N, payload.v_BN_N = position, velocity
state_message = messaging.SCStatesMsg().write(payload)
inverse = orbElemConvert.OrbElemConvert()
inverse.mu = mu
inverse.scStateInMsg.subscribeTo(state_message)
forward = orbElemConvert.OrbElemConvert()
forward.mu = mu
forward.elemInMsg.subscribeTo(inverse.elemOutMsg)
recorder = forward.scStateOutMsg.recorder()
inverse.Reset(0)
forward.Reset(0)
inverse.UpdateState(0)
forward.UpdateState(0)
recorder.UpdateState(0)
assert len(recorder.times()) == 1
native = inverse.elemOutMsg.read()
python_result = orbitalMotion.rv2elem(mu, position, velocity, canonical=True)
for field in orbitalMotion.ClassicElements.__slots__:
native_value, python_value = getattr(native, field), getattr(python_result, field)
assert math.isfinite(native_value)
if field in ("Omega", "omega", "f"):
assert abs(math.remainder(native_value - python_value, 2.0 * math.pi)) < 2e-14
elif field in ("a", "alpha", "rApoap"):
assert native_value == pytest.approx(python_value, rel=conditioning, abs=1e-20)
else:
assert native_value == pytest.approx(python_value, rel=2e-14, abs=2e-15)
if "radial" in kind or kind == "rest":
assert native.e == 1.0
assert native.rPeriap == 0.0
if kind in ("parabola", "radial_escape"):
assert native.e == 1.0
assert native.a == native.alpha == native.rApoap == 0.0
result = forward.scStateOutMsg.read()
numpy.testing.assert_allclose(result.r_BN_N, position, rtol=conditioning, atol=1e-10 * scale)
numpy.testing.assert_allclose(result.v_BN_N, velocity, rtol=conditioning, atol=1e-13 * scale)
# Class in order to plot using data across the different parameterized scenarios
class DataStore:
def __init__(self):
self.Date = [] # replace these with appropriate containers for the data to be stored for plotting
self.MarsPosErr = []
self.EarthPosErr = []
self.SunPosErr = []
[docs]
@pytest.mark.parametrize("a, e, i, AN, AP, f, mu, name", [
# Inclined Elliptical Orbit Varying e
(10000000.0, 0.01, 33.3*mc.D2R, 48.2*mc.D2R, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15, 'IncEllip_e_1'),
(10000000.0, 0.10, 33.3*mc.D2R, 48.2*mc.D2R, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15, 0),
(10000000.0, 0.25, 33.3*mc.D2R, 48.2*mc.D2R, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15, 0),
(10000000.0, 0.50, 33.3*mc.D2R, 48.2*mc.D2R, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15, 0),
(10000000.0, 0.75, 33.3*mc.D2R, 48.2*mc.D2R, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15, 'IncEllip_e_2'),
# Inclined Elliptical Orbit Varying a
(10000000.0, 0.50, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'IncEllip_a_1'),
(100000.0, 0.50, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(10000.0, 0.50, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(1000.0, 0.50, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(100.0, 0.50, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(10.0, 0.50, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'IncEllip_a_2'),
# Equatorial Elliptical Orbit Varying e
(10000000.0, 0.01, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'EquEllip_e_1'),
(10000000.0, 0.10, 0.0, 0.0, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15, 0),
(10000000.0, 0.25, 0.0, 0.0, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15, 0),
(10000000.0, 0.50, 0.0, 0.0, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15, 0),
(10000000.0, 0.75, 0.0, 0.0, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15, 'EquEllip_e_2'),
# Equatorial Elliptical Orbit Varying a
(10000000.0, 0.50, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'EquEllip_a_1'), # For i=0 => AN=0
(100000.0, 0.50, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(10000.0, 0.50, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(1000.0, 0.50, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(100.0, 0.50, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(10.0, 0.50, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'EquEllip_a_2'),
# Inclined Circular Orbit
(10000000.0, 0.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 'IncCirc_1'),
(1000000.0, 0.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(100000.0, 0.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(10000.0, 0.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(1000.0, 0.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(100.0, 0.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(10.0, 0.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 'IncCirc_2'),
# Equatorial Circular Orbit
(10000000.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 'EquCirc_1'),
(1000000.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(100000.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(10000.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(1000.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(100.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(10.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 'EquCirc_2'),
# Inclined Parabolic Orbit
(-10.0, 1.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'IncPara_1'), # For input of -a,
(-100.0, 1.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), # must have e= 1.0
(-1000.0, 1.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), # or e >1.0
(-10000.0, 1.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(-100000.0, 1.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'IncPara_2'),
# Equatorial Parabolic Orbit
(-10.0, 1.0, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'EquPara_1'), # For input of -a,
(-100.0, 1.0, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), # must have e= 1.0
(-1000.0, 1.0, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), # or e >1.0
(-10000.0, 1.0, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(-100000.0, 1.0, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'EquPara_2'),
# Inclined Hyperbolic Orbit varying a
(-10.0, 1.3, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'IncHyp_a_1'),
(-100.0, 1.3, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(-1000.0, 1.3, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(-10000.0, 1.3, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(-100000.0, 1.3, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'IncHyp_a_2'),
# Inclined Hyperbolic Orbit varying e
(-100000.0, 1.1, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'IncHyp_e_1'),
(-100000.0, 1.2, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(-100000.0, 1.3, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(-100000.0, 1.4, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(-100000.0, 1.5, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'IncHyp_e_2'),
# Equatorial Hyperbolic Orbit varying a
(-10.0, 1.3, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'EquHyp_a_1'),
(-100.0, 1.3, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(-1000.0, 1.3, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(-10000.0, 1.3, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(-100000.0, 1.3, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'EquHyp_a_2'),
# Equatorial Hyperbolic Orbit varying e
(-100000.0, 1.1, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'EquHyp_e_1'),
(-100000.0, 1.2, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(-100000.0, 1.3, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(-100000.0, 1.4, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0),
(-100000.0, 1.5, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'EquHyp_e_2'),
# # Approaching circular orbit
# (100000.0, 0.000001, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15),
# These don't work
# (10000000.0, 1.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15), # or e >1.0
# (-10, 0.9, 33.3*mc.D2R, 48.2*mc.D2R, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15)
])
# provide a unique test method name, starting with test_
def test_orb_elem_convert(a, e, i, AN, AP, f, mu, name, DispPlot=False):
"""Module Unit Test"""
# each test method requires a single assert method to be called
[testResults, testMessage] = orbElem(a, e, i, AN, AP, f, mu, name, DispPlot)
assert testResults < 1, testMessage
# Run unit test
def orbElem(a, e, i, AN, AP, f, mu, name, DispPlot):
# Elem2RV
testFailCount1 = 0 # zero unit test result counter
testMessages = [] # create empty array 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
TotalSim = SimulationBaseClass.SimBaseClass()
DynUnitTestProc = TotalSim.CreateNewProcess(unitProcessName)
# # create the dynamics task and specify the integration update time
testProcessRate = macros.sec2nano(1.0)
DynUnitTestProc.addTask(TotalSim.CreateNewTask(unitTaskName, testProcessRate))
# Initialize the modules that we are using.
orb_elemObject = orbElemConvert.OrbElemConvert()
orb_elemObject.ModelTag = "OrbElemConvertData"
# Add Model To Task
TotalSim.AddModelToTask(unitTaskName, orb_elemObject)
# Set element values
epsDiff = 1e-5
orb_elemObject.mu = mu
###### ELEM2RV ######
orb_elemObjectLog = orb_elemObject.logger(["r_N", "v_N"])
TotalSim.AddModelToTask(unitTaskName, orb_elemObjectLog)
# Create and write messages
ElemMessage = messaging.ClassicElementsMsgPayload()
elemMsg = messaging.ClassicElementsMsg()
if e == 1.0:
ElemMessage.a = 0.0
ElemMessage.rPeriap = -a
else:
ElemMessage.a = a # meters
ElemMessage.e = e
ElemMessage.i = i
ElemMessage.Omega = AN
ElemMessage.omega = AP
ElemMessage.f = f
elemMsg.write(ElemMessage)
orb_elemObject.elemInMsg.subscribeTo(elemMsg)
# Log Message to test WriteOutputMessage()
dataLog = orb_elemObject.spiceStateOutMsg.recorder()
TotalSim.AddModelToTask(unitTaskName, dataLog)
# Execute simulation
TotalSim.ConfigureStopTime(int(1E9))
TotalSim.InitializeSimulation()
TotalSim.ExecuteSimulation()
# Get r and v from sim
vSim = orb_elemObjectLog.v_N[-1]
rSim = orb_elemObjectLog.r_N[-1]
# Get r and v from message
rMsgPlanet = dataLog.PositionVector[-1]
vMsgPlanet = dataLog.VelocityVector[-1]
numpy.testing.assert_allclose(
rSim, rMsgPlanet,
rtol=epsDiff, verbose=True,
err_msg="FAILED: Planet Position Message"
)
numpy.testing.assert_allclose(
vSim, vMsgPlanet,
rtol=epsDiff, verbose=True,
err_msg="FAILED: Planet Velocity Message"
)
# Calculation of elem2rv
if e == 1.0 and a > 0.0: # rectilinear elliptic orbit case
Ecc = f # f is treated as ecc.anomaly
r = a * (1 - e * math.cos(Ecc)) # orbit radius
v = math.sqrt(2 * mu / r - mu / a)
ir = numpy.zeros(3)
ir[0] = math.cos(AN) * math.cos(AP) - math.sin(AN) * math.sin(AP) * math.cos(i)
ir[1] = math.sin(AN) * math.cos(AP) + math.cos(AN) * math.sin(AP) * math.cos(i)
ir[2] = math.sin(AP) * math.sin(i)
rTruth = numpy.multiply(r, ir)
if math.sin(Ecc) > 0:
vTruth = numpy.multiply(-v, ir)
else:
vTruth = numpy.multiply(v, ir)
else:
if e == 1 and a < 0: # parabolic case
rp = -a # radius at periapses
p = 2 * rp # semi-latus rectum
a = 0.0
else: # elliptic and hyperbolic cases
p = a * (1 - e * e) # semi-latus rectum
r = p / (1 + e * math.cos(f)) # orbit radius
theta = AP + f # true latitude angle
h = math.sqrt(mu * p) # orbit ang.momentum mag.
rTruth = numpy.zeros(3)
rTruth[0] = r * (math.cos(AN) * math.cos(theta) - math.sin(AN) * math.sin(theta) * math.cos(i))
rTruth[1] = r * (math.sin(AN) * math.cos(theta) + math.cos(AN) * math.sin(theta) * math.cos(i))
rTruth[2] = r * (math.sin(theta) * math.sin(i))
vTruth = numpy.zeros(3)
vTruth[0] = -mu / h * (math.cos(AN) * (math.sin(theta) + e * math.sin(AP)) + math.sin(AN) * (math.cos(
theta) + e * math.cos(AP)) * math.cos(i))
vTruth[1] = -mu / h * (math.sin(AN) * (math.sin(theta) + e * math.sin(AP)) - math.cos(AN) * (math.cos(
theta) + e * math.cos(AP)) * math.cos(i))
vTruth[2] = -mu / h * (-(math.cos(theta) + e * math.cos(AP)) * math.sin(i))
# Position and Velocity Diff Checks
numpy.testing.assert_allclose(
rSim, rTruth,
rtol=epsDiff, verbose=True,
err_msg="FAILED: Position Vector"
)
numpy.testing.assert_allclose(
vSim, vTruth,
rtol=epsDiff, verbose=True,
err_msg="FAILED: Velocity Vector"
)
###### RV2ELEM ######
# RV2Elem
testFailCount2 = 0 # zero unit test result counter
for g in range(2):
TotalSim = SimulationBaseClass.SimBaseClass()
DynUnitTestProc = TotalSim.CreateNewProcess(unitProcessName)
# # create the dynamics task and specify the integration update time
testProcessRate = macros.sec2nano(1.0)
DynUnitTestProc.addTask(TotalSim.CreateNewTask(unitTaskName, testProcessRate))
# Initialize the modules that we are using.
orb_elemObject = orbElemConvert.OrbElemConvert()
orb_elemObject.ModelTag = "OrbElemConvertData"
# Add Model To Task
TotalSim.AddModelToTask(unitTaskName, orb_elemObject)
# Log Variables
elemLog = PythonVariableLogger({
"a": lambda _: orb_elemObject.CurrentElem.a,
"e": lambda _: orb_elemObject.CurrentElem.e,
"i": lambda _: orb_elemObject.CurrentElem.i,
"Omega": lambda _: orb_elemObject.CurrentElem.Omega,
"omega": lambda _: orb_elemObject.CurrentElem.omega,
"f": lambda _: orb_elemObject.CurrentElem.f,
})
TotalSim.AddModelToTask(unitTaskName, elemLog)
orb_elemObjectLog = orb_elemObject.logger(["r_N", "v_N"])
TotalSim.AddModelToTask(unitTaskName, orb_elemObjectLog)
orb_elemObject.mu = mu
if g == 0:
CartMessage = messaging.SCStatesMsgPayload()
CartMessage.r_BN_N = rSim
CartMessage.v_BN_N = vSim
stateScMsg = messaging.SCStatesMsg().write(CartMessage)
orb_elemObject.scStateInMsg.subscribeTo(stateScMsg)
else:
CartMessage = messaging.SpicePlanetStateMsgPayload()
CartMessage.PositionVector = rSim
CartMessage.VelocityVector = vSim
stateSpMsg = messaging.SpicePlanetStateMsg().write(CartMessage)
orb_elemObject.spiceStateInMsg.subscribeTo(stateSpMsg)
dataElemLog = orb_elemObject.elemOutMsg.recorder()
TotalSim.AddModelToTask(unitTaskName, dataElemLog)
# Execute simulation
TotalSim.ConfigureStopTime(int(1E9))
TotalSim.InitializeSimulation()
TotalSim.ExecuteSimulation()
aOut = elemLog.a[-1]
eOut = elemLog.e[-1]
iOut = elemLog.i[-1]
ANOut = elemLog.Omega[-1]
APOut = elemLog.omega[-1]
fOut = elemLog.f[-1]
# Element Diff Check
ElemDiff = [(a - aOut), (e - eOut), (i - iOut), (AN - ANOut), (AP - APOut), (f - fOut)]
ElemDiffcsv = numpy.asarray(ElemDiff)
for g in range(6):
# check for angle roll over with 2*pi
if g > 2:
if abs(ElemDiff[g] - 2 * math.pi) < epsDiff:
ElemDiff[g] -= 2 * math.pi
elif abs(ElemDiff[g] + 2 * math.pi) < epsDiff:
ElemDiff[g] += 2 * math.pi
if abs(ElemDiff[g]) > epsDiff:
testMessages.append(" FAILED: Sim Orbital Element " + str(g))
testFailCount2 += 1
aMsg = dataElemLog.a[-1]
eMsg = dataElemLog.e[-1]
iMsg = dataElemLog.i[-1]
ANMsg = dataElemLog.Omega[-1]
APMsg = dataElemLog.omega[-1]
fMsg = dataElemLog.f[-1]
ElemMsgDiff = [(aOut - aMsg), (eOut - eMsg), (iOut - iMsg), (ANOut - ANMsg), (APOut - APMsg), (fOut - fMsg)]
for g in range(6):
# check for angle roll over with 2*pi
if g > 2:
if abs(ElemDiff[g] - 2 * math.pi) < epsDiff:
ElemDiff[g] -= 2 * math.pi
elif abs(ElemDiff[g] + 2 * math.pi) < epsDiff:
ElemDiff[g] += 2 * math.pi
if abs(ElemMsgDiff[g]) > 0:
testMessages.append(" FAILED: Orbital Element Message " + str(g))
testFailCount2 += 1
######### Calculate rv2elem #########
# Calculate the specific angular momentum and its magnitude
epsConv = 0.0000000001
hVec = numpy.cross(rTruth, vTruth)
h = numpy.linalg.norm(hVec)
p = h * h / mu
# Calculate the line of nodes
v3 = numpy.array([0.0, 0.0, 1.0])
nVec = numpy.cross(v3, hVec)
n = numpy.linalg.norm(nVec)
# Orbit eccentricity and energy
r = numpy.linalg.norm(rTruth)
v = numpy.linalg.norm(vTruth)
eVec = numpy.multiply(v * v / mu - 1.0 / r, rTruth)
v3 = numpy.multiply(numpy.dot(rTruth, vTruth) / mu, vTruth)
eVec = numpy.subtract(eVec, v3)
eO = numpy.linalg.norm(eVec)
# compute semi - major axis
alpha = 2.0 / r - v * v / mu
if (math.fabs(alpha) > epsConv): # elliptic or hyperbolic case
aO = 1.0 / alpha
else: # parabolic case
aO = 0.0 # a is not defined for parabola, so -rp is returned instead
# Calculate the inclination
iO = math.acos(hVec[2] / h)
# Calculate AP, AN, and True anomaly
if eO >= 1e-11 and iO >= 1e-11:
# Case 1: Non - circular, inclined orbit
Omega = math.acos(nVec[0] / n)
if (nVec[1] < 0.0):
Omega = 2.0 * math.pi - Omega
omega = math.acos(numpy.dot(nVec, eVec) / n / eO)
if eVec[2] < 0.0:
omega = 2.0 * math.pi - omega
fO = math.acos(numpy.dot(eVec, rTruth) / eO / r)
if numpy.dot(rTruth, vTruth) < 0.0:
fO = 2.0 * math.pi - fO
elif eO >= 1e-11 and iO < 1e-11:
# Case 2: Non - circular, equatorial orbit
# Equatorial orbit has no ascending node
Omega = 0.0
# True longitude of periapsis, omegatilde_true
omega = math.acos(eVec[0] / eO)
if eVec[1] < 0.0:
omega = 2.0 * math.pi - omega
fO = math.acos(numpy.dot(eVec, rTruth) / eO / r)
if numpy.dot(rTruth, vTruth) < 0.0:
fO = 2.0 * math.pi - fO
elif eO < 1e-11 and iO >= 1e-11:
# Case 3: Circular, inclined orbit
Omega = math.acos(nVec[0] / n)
if (nVec[1] < 0.0):
Omega = 2.0 * math.pi - Omega
omega = 0.0
# Argument of latitude, u = omega + f * /
fO = math.acos(numpy.dot(nVec, rTruth) / n / r)
if rTruth[2] < 0.0:
fO = 2.0 * math.pi - fO
elif eO < 1e-11 and iO < 1e-11:
# Case 4: Circular, equatorial orbit
Omega = 0.0
omega = 0.0
# True longitude, lambda_true
fO = math.acos(rTruth[0] / r)
if rTruth[1] < 0:
fO = 2.0 * math.pi - fO
else:
print("Error: rv2elem couldn't identify orbit type.\n")
if (eO >= 1.0 and math.fabs(fO) > math.pi):
twopiSigned = math.copysign(2.0 * math.pi, fO)
fO -= twopiSigned
# Element Diff Check
ElemCalcDiff = [(aO - aOut), (eO - eOut), (iO - iOut), (Omega - ANOut), (omega - APOut), (fOut - fOut)]
ElemCalcDiffcsv = numpy.asarray(ElemCalcDiff)
for g in range(6):
# check for angle roll over with 2*pi
if g > 2:
if abs(ElemCalcDiff[g] - 2 * math.pi) < epsDiff:
ElemCalcDiff[g] -= 2 * math.pi
elif abs(ElemCalcDiff[g] + 2 * math.pi) < epsDiff:
ElemCalcDiff[g] += 2 * math.pi
if abs(ElemCalcDiff[g]) > epsDiff:
testMessages.append(" FAILED: Calculated Orbital Element " + str() + str(g))
testFailCount2 += 1
# create plot
# txt = 'e = ' + str(e) + ' and a = ' + str(a) + 'km'
fact = (len(str(abs(a)))-3.0)
plt.close('all')
plt.figure(1,figsize=(7, 5), dpi=80, facecolor='w', edgecolor='k')
# fig1.text(.5, .05, txt, ha='center')
ax1 = plt.subplot(211)
ax1.cla()
index = numpy.arange(3)
bar_width = 0.35
opacity = 0.8
rects1 = ax1.bar(index, rSim, bar_width, alpha=opacity, color='b', label='Simulated Position')
rects2 = ax1.bar(index + bar_width, rTruth, bar_width, alpha=opacity, color='g', label='Calculated Position')
ax1.spines['left'].set_position('zero')
ax1.spines['right'].set_color('none')
ax1.spines['bottom'].set_position('zero')
ax1.spines['top'].set_color('none')
for xtick in ax1.get_xticklabels():
xtick.set_bbox(dict(facecolor='white', edgecolor='None', alpha=0.5))
ax1.xaxis.set_ticks_position('bottom')
ax1.yaxis.set_ticks_position('left')
plt.ylabel('Position (m)')
plt.xticks(index + bar_width, ('X', 'Y', 'Z'))
plt.legend(loc='lower right')
ax2 = plt.subplot(212)
ax2.cla()
rects1 = ax2.bar(index, vSim, bar_width, alpha=opacity, color='b', label='Simulated Velocity')
rects2 = ax2.bar(index + bar_width, vTruth, bar_width, alpha=opacity, color='g', label='Calculated Velocity')
ax2.spines['left'].set_position('zero')
ax2.spines['right'].set_color('none')
ax2.spines['bottom'].set_position('zero')
ax2.spines['top'].set_color('none')
for xtick in ax2.get_xticklabels():
xtick.set_bbox(dict(facecolor='white', edgecolor='None', alpha=0.5))
ax2.xaxis.set_ticks_position('bottom')
ax2.yaxis.set_ticks_position('left')
plt.ylabel('Velocity (m/s)')
plt.xticks(index + bar_width, ('X', 'Y', 'Z'))
plt.legend(loc='lower right')
if name != 0:
simHelpers.writeFigureLaTeX(name, "$e = " + str(e) + "$ and $a = 10^" + str(int(fact)) + "$km",
plt, 'height=0.7\\textwidth, keepaspectratio', path)
if testFailCount2 == 0:
colorText = 'ForestGreen'
passFailMsg = "" # "Passed: " + name + "."
passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}'
else:
colorText = 'Red'
passFailMsg = "Failed: " + name + "."
testMessages.append(passFailMsg)
testMessages.append(" | ")
passedText = r'\textcolor{' + colorText + '}{' + "FAILED" + '}'
# Write some snippets for AutoTex
snippetName = name + "PassedText2"
snippetContent = passedText
simHelpers.writeTeXSnippet(snippetName, snippetContent, path)
snippetName = name + "PassFailMsg2"
snippetContent = passFailMsg
simHelpers.writeTeXSnippet(snippetName, snippetContent, path)
if DispPlot:
plt.show()
plt.close()
testFailCount = testFailCount1+testFailCount2
if testFailCount:
print("Failed")
print(testMessages)
else:
print("PASSED")
return [testFailCount, ''.join(testMessages)]
if __name__ == "__main__":
orbElem(10000000.0, 0.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0, True
)