diff --git a/docs/source/Support/bskKnownIssuesSnippets/1656-formation-equinoctial.rst b/docs/source/Support/bskKnownIssuesSnippets/1656-formation-equinoctial.rst new file mode 100644 index 00000000000..89afaf56d76 --- /dev/null +++ b/docs/source/Support/bskKnownIssuesSnippets/1656-formation-equinoctial.rst @@ -0,0 +1 @@ +- The classical orbital-element choice in :ref:`formationBarycenter`, including the implementation in tagged release 2.12.0, can place a reference far from a compact near-circular formation because separately averaged classical angles are singular. This choice remains unchanged for compatibility. Select the new prograde or retrograde mean-longitude equinoctial option for supported elliptic formations, or use the existing Cartesian method when a physical center of mass is required. Each equinoctial representation excludes one equatorial pole; non-elliptic inputs and undefined circular-mean longitudes are rejected rather than silently falling back. diff --git a/docs/source/Support/bskReleaseNotesSnippets/1656-formation-equinoctial.rst b/docs/source/Support/bskReleaseNotesSnippets/1656-formation-equinoctial.rst new file mode 100644 index 00000000000..4b68c3b34b6 --- /dev/null +++ b/docs/source/Support/bskReleaseNotesSnippets/1656-formation-equinoctial.rst @@ -0,0 +1 @@ +- Added opt-in prograde and retrograde mean-longitude equinoctial averaging to :ref:`formationBarycenter` for circular and equatorial elliptic formations; existing Cartesian and default classical-element results remain unchanged. diff --git a/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py b/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py new file mode 100644 index 00000000000..cb278477d3c --- /dev/null +++ b/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py @@ -0,0 +1,717 @@ +# +# ISC License +# +# Copyright (c) 2026, Autonomous Vehicle Systems Lab, University of Colorado 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. +# + +"""Validate opt-in equinoctial averaging without changing the legacy modes. + +The reference recovers classical elements and solves Kepler's equation by +bisection, independently of the module's direct equinoctial reconstruction. +The compact formation construction follows carlo98's example in issue 1656. +""" + +import math + +import numpy as np +import pytest + +from Basilisk.architecture import bskLogging, messaging +from Basilisk.fswAlgorithms import formationBarycenter +from Basilisk.utilities import SimulationBaseClass, macros, orbitalMotion + + +MU = 3.986004418e14 # [m^3/s^2] +CLASSICAL = 0 +EQUINOCTIAL_PROGRADE = 1 +EQUINOCTIAL_RETROGRADE = 2 + + +def classical_state(axis, eccentricity, inclination, node, periapsis, mean_anomaly): + """Construct an independent Cartesian state from an elliptic orbit. + + Distances are in meters, angles in radians and eccentricity dimensionless. + The returned position and velocity have units of meters and meters/second. + """ + lower = mean_anomaly-eccentricity + upper = mean_anomaly+eccentricity + for _ in range(100): + eccentric_anomaly = (lower+upper)/2 + if eccentric_anomaly == lower or eccentric_anomaly == upper: + break + if eccentric_anomaly-eccentricity*math.sin(eccentric_anomaly) > mean_anomaly: + upper = eccentric_anomaly + else: + lower = eccentric_anomaly + cosine = math.cos(eccentric_anomaly) + sine = math.sin(eccentric_anomaly) + beta = math.sqrt(1-eccentricity**2) + radius = axis*(1-eccentricity*cosine) # [m] + position = axis*np.array([cosine-eccentricity, beta*sine, 0.0]) # [m] + velocity = math.sqrt(MU*axis)/radius*np.array([-sine, beta*cosine, 0.0]) # [m/s] + co, so = math.cos(node), math.sin(node) + cw, sw = math.cos(periapsis), math.sin(periapsis) + ci, si = math.cos(inclination), math.sin(inclination) + rotation = np.array([ + [co*cw-so*sw*ci, -co*sw-so*cw*ci, so*si], + [so*cw+co*sw*ci, -so*sw+co*cw*ci, -co*si], + [sw*si, cw*si, ci], + ]) + return rotation @ position, rotation @ velocity + + +def make_simulation(states, masses, element_type=None, use_orbital=True, mu=MU): + """Initialize one module and retain writable inputs and both output recorders.""" + assert len(states) == len(masses) + simulation = SimulationBaseClass.SimBaseClass() + simulation.CreateNewProcess("process").addTask( + simulation.CreateNewTask("task", macros.sec2nano(1.0)) # [s] + ) + module = formationBarycenter.FormationBarycenter() + module.useOrbitalElements = use_orbital + module.mu = mu + if element_type is not None: + module.orbitalElementType = element_type + messages = [] + for (position, velocity), mass in zip(states, masses): + navigation = messaging.NavTransMsgPayload() + navigation.r_BN_N = np.asarray(position).tolist() + navigation.v_BN_N = np.asarray(velocity).tolist() + vehicle = messaging.VehicleConfigMsgPayload() + vehicle.massSC = mass + messages.extend([ + messaging.NavTransMsg().write(navigation), + messaging.VehicleConfigMsg().write(vehicle), + ]) + module.addSpacecraftToModel(messages[-2], messages[-1]) + recorder = module.transOutMsg.recorder() + recorder_c = module.transOutMsgC.recorder() + for model in (module, recorder, recorder_c): + simulation.AddModelToTask("task", model) + simulation.InitializeSimulation() + return simulation, module, messages, recorder, recorder_c + + +def recorded_state(recorder, recorder_c): + """Read the latest finite state and require identical C/C++ output payloads.""" + result = np.concatenate([recorder.r_BN_N[-1], recorder.v_BN_N[-1]]) + result_c = np.concatenate([recorder_c.r_BN_N[-1], recorder_c.v_BN_N[-1]]) + np.testing.assert_array_equal(result, result_c) + assert np.isfinite(result).all() + return result + + +def run_module(states, masses, element_type=None, use_orbital=True, mu=MU, after_reset=None): + """Execute the compiled module once, optionally changing settings after Reset.""" + simulation, module, messages, recorder, recorder_c = make_simulation( + states, masses, element_type, use_orbital, mu + ) + if after_reset is not None: + for attribute, value in after_reset.items(): + setattr(module, attribute, value) + simulation.ConfigureStopTime(0) + simulation.ExecuteSimulation() + return recorded_state(recorder, recorder_c) + + +def assert_state(actual, expected, rtol=2e-11): + """Use position and velocity tolerances with their respective physical units.""" + np.testing.assert_allclose(actual[:3], expected[:3], rtol=rtol, atol=2e-6) # [m] + np.testing.assert_allclose(actual[3:], expected[3:], rtol=rtol, atol=2e-8) # [m/s] + + +def test_default_and_named_choices(): + """Existing scripts retain Cartesian/default-classical selection semantics.""" + module = formationBarycenter.FormationBarycenter() + assert module.useOrbitalElements is False + assert module.orbitalElementType == CLASSICAL + assert module.CLASSICAL == CLASSICAL + assert module.EQUINOCTIAL_PROGRADE == EQUINOCTIAL_PROGRADE + assert module.EQUINOCTIAL_RETROGRADE == EQUINOCTIAL_RETROGRADE + + +@pytest.mark.parametrize("element_type", [CLASSICAL, EQUINOCTIAL_PROGRADE, EQUINOCTIAL_RETROGRADE, -1]) +def test_cartesian_ignores_element_selector(element_type): + """Keep Cartesian selection exact while allowing reference roundoff. + + The C++ weighted sum can use fused multiply-add, whereas NumPy may round + multiplication and addition separately. + """ + states = [ + classical_state(7e6, 0.1, 0.9, 0.3, 0.4, phase) # [m, -, rad, rad, rad, rad] + for phase in (0.7, 0.9) # [rad] + ] + masses = np.array([100.0, 300.0]) # [kg] + expected = np.average([np.concatenate(state) for state in states], weights=masses, axis=0) + baseline = run_module(states, masses, use_orbital=False, mu=0.0) # [m^3/s^2] + actual = run_module(states, masses, element_type, use_orbital=False, mu=0.0) # [m^3/s^2] + np.testing.assert_array_equal(actual, baseline) + relative_tolerance = 8*np.finfo(float).eps # [-] + np.testing.assert_allclose(actual, expected, rtol=relative_tolerance, atol=0.0) + + +@pytest.mark.parametrize("configuration", ["default", "explicit-classical", "legacy-runtime"]) +@pytest.mark.parametrize("orbits,masses,expected", [ + pytest.param( + [ + (7e6, 0.1, 0.5, 0.2, 0.4, 0.6), + (8e6, 0.2, 0.7, 0.3, 0.5, 0.8), + (9e6, 0.3, 1.0, 0.4, 0.6, 1.0), + ], # [m, -, rad, rad, rad, rad] + [100.0, 200.0, 400.0], # [kg] + [ + -3805024.012545443, 3691119.6983278044, 5336085.789998154, # [m] + -7017.942535029498, -3023.7993314431715, -547.6782626053625, # [m/s] + ], + id="weighted-inclined", + ), + pytest.param( + [ + (7e6, 0.1, 0.9, 6.20, 6.15, 6.24), + (7.2e6, 0.15, 0.8, 0.04, 0.08, 0.03), + (6.8e6, 0.05, 1.0, 0.12, 0.20, 0.10), + ], # [m, -, rad, rad, rad, rad] + [100.0, 200.0, 300.0], # [kg] + [ + 6194409.791060193, 1002729.1491828125, 824797.744204107, # [m] + -1614.2214430779236, 4893.206053782084, 6495.775280548105, # [m/s] + ], + id="angle-wrap", + ), + pytest.param( + [ + (2e7, 0.6, 2.5, 2.8, 0.5, 0.3), + (2.1e7, 0.7, 2.6, 2.9, 0.6, 0.6), + (1.9e7, 0.8, 2.7, 3.0, 0.7, 1.2), + ], # [m, -, rad, rad, rad, rad] + [400.0, 100.0, 200.0], # [kg] + [ + 10068721.357163044, 5134873.813868816, 4896469.296928595, # [m] + 6254.6602691876315, -2322.4582653040334, -364.5000674161748, # [m/s] + ], + id="eccentric-retrograde", + ), + pytest.param( + [ + (7e6, 0.01, 0.78, 0.84, 6.07, 1.48), + (7e6, 0.0105, 0.82, 0.84, 6.07, 1.56), + (7e6, 0.0095, 0.70, 0.84, 6.07, 1.63), + ], # [m, -, rad, rad, rad, rad] + [100.0, 150.0, 250.0], # [kg] + [ + -2881895.08515755, 4314497.779819102, 4700781.108437926, # [m] + -5735.687341594877, -4798.5621701086875, 999.1089719904237, # [m/s] + ], + id="low-eccentricity", + ), +]) +def test_classical_compatibility(orbits, masses, expected, configuration): + """Preserve pre-PR classical position and velocity on both output interfaces. + + Fixed reference states were captured using the unmodified C++ module and + header from PR 1668's base commit ``d0f9832f3b0a40923329c23c2f9255c6c2f1b08b``. + Input tuples specify semimajor axis, eccentricity, inclination, ascending + node, argument of periapsis and mean anomaly. The cases cover unequal + weights, angular wrapping, eccentric retrograde motion and low eccentricity. + They avoid classical singularities so tight absolute tolerances remain + portable: 2 micrometers in position and 2e-8 meters/second in velocity. + + Check the omitted selector, explicit ``CLASSICAL``, and the legacy pattern + of enabling orbital averaging after a Cartesian update using only + ``useOrbitalElements`` and ``mu``. Expected outputs are never recomputed + with the current module during the test. + """ + states = [classical_state(*orbit) for orbit in orbits] + if configuration == "legacy-runtime": + simulation, module, messages, recorder, recorder_c = make_simulation( + states, masses, use_orbital=False, mu=0.0 # [m^3/s^2] + ) + simulation.TotalSim.SingleStepProcesses() + module.useOrbitalElements = True + module.mu = MU + simulation.TotalSim.SingleStepProcesses() + actual = recorded_state(recorder, recorder_c) + else: + element_type = CLASSICAL if configuration == "explicit-classical" else None + actual = run_module(states, masses, element_type) + assert_state(actual, expected, rtol=0.0) + + +@pytest.mark.parametrize("element_type", [EQUINOCTIAL_PROGRADE, EQUINOCTIAL_RETROGRADE]) +@pytest.mark.parametrize("eccentricity", [0.0, 1e-8, 0.2, 0.7, 0.95]) # [-] +@pytest.mark.parametrize("tilt_index", range(4)) +@pytest.mark.parametrize("mean_anomaly", [0.0, 0.7, 3.4, 6.1]) # [rad] +def test_single_spacecraft_identity(element_type, eccentricity, tilt_index, mean_anomaly): + """Each representation recovers a single input, including its nonsingular pole.""" + inclinations = ( + [0.0, 1e-12, 0.9, 2.8] if element_type == EQUINOCTIAL_PROGRADE + else [0.2, 0.9, math.pi-1e-12, math.pi] + ) # [rad] + state = classical_state( + 2e7, eccentricity, inclinations[tilt_index], 0.3, 1.2, mean_anomaly + ) # [m, -, rad, rad, rad, rad] + assert_state(run_module([state], [100.0], element_type), np.concatenate(state)) # [kg] + + +@pytest.mark.parametrize("phases", [ + (0, 120, 240), (0, 120.1, 240), (0, 121, 240), + (0, 125, 240), (0, 140, 240), (0, 90, 180, 270), +]) # [deg] +@pytest.mark.parametrize("element_type", [EQUINOCTIAL_PROGRADE, EQUINOCTIAL_RETROGRADE]) +def test_reported_compact_formation(phases, element_type): + """Require an independent state reference and a 1 cm locality budget. + + Each member has nonzero eccentricity and non-equatorial inclination, so + its classical elements can be recovered without a singular-angle choice. + The reference then averages known elements and reconstructs through the + separate bisection/rotation path, not the module's plane equations. + The centimetre budget applies only to these reported 100 m formations; + it is not an equality requirement for arbitrary orbital and Cartesian means. + """ + radius = 7e6 # [m] + spread = 100.0 # [m] + chief = orbitalMotion.ClassicElements() + chief.a, chief.e, chief.i = radius, 0.0, 0.9 # [m, -, rad] + chief.Omega, chief.omega, chief.f = 0.3, 0.0, 1.0 # [rad] + rc, vc = orbitalMotion.elem2rv(MU, chief) + rate = math.sqrt(MU/radius**3) # [rad/s] + states = [] + for phase in np.radians(phases): + offset = spread*np.array([0.5*math.cos(phase), -math.sin(phase), math.cos(phase)]) # [m] + offset_rate = spread*rate*np.array([-0.5*math.sin(phase), -math.cos(phase), -math.sin(phase)]) # [m/s] + states.append(orbitalMotion.hill2rv(rc, vc, offset, offset_rate)) + cartesian = np.mean([state[0] for state in states], axis=0) # [m] + masses = [100.0]*len(states) # [kg] + orbits = [] + for position, velocity in states: + elements = orbitalMotion.rv2elem(MU, position, velocity) + assert elements.e > 1e-7 and 0.1 < elements.i < math.pi-0.1 # [-, rad] + mean_anomaly = orbitalMotion.E2M(orbitalMotion.f2E(elements.f, elements.e), elements.e) # [rad] + orbits.append((elements.a, elements.e, elements.i, elements.Omega, elements.omega, mean_anomaly)) + factor = 1 if element_type == EQUINOCTIAL_PROGRADE else -1 + expected = weighted_reference(orbits, masses, factor) + actual = run_module(states, masses, element_type) + assert_state(actual, expected, rtol=0.0) + locality_budget = 0.01 # [m]; geometry-specific regression budget for these compact geometries + assert np.linalg.norm(expected[:3]-cartesian) < locality_budget + assert np.linalg.norm(actual[:3]-cartesian) < locality_budget + + +@pytest.mark.parametrize("element_type", [EQUINOCTIAL_PROGRADE, EQUINOCTIAL_RETROGRADE]) +@pytest.mark.parametrize("elapsed", [0.0, 60.0, 200.0]) # [s] +def test_equal_axis_preserves_orbital_clock(element_type, elapsed): + """On one ellipse, equal-mass symmetric phases retain their time midpoint.""" + axis = 2e7 # [m] + inclination = 0.9 if element_type == EQUINOCTIAL_PROGRADE else math.pi # [rad] + mean_midpoint = 1.1+math.sqrt(MU/axis**3)*elapsed # [rad] + orbits = [ + (axis, 0.6, inclination, 0.3, 1.2, mean_midpoint+delta) # [m, -, rad, rad, rad, rad] + for delta in (-0.2, 0.2) # [rad] + ] + expected = classical_state(axis, 0.6, inclination, 0.3, 1.2, mean_midpoint) # [m, -, rad, rad, rad, rad] + actual = run_module([classical_state(*orbit) for orbit in orbits], [100.0, 100.0], element_type) # [kg] + assert_state(actual, np.concatenate(expected)) + + +def weighted_reference(orbits, masses, factor): + """Average known elements, then invert through a separate classical rotation.""" + weights = np.asarray(masses)/sum(masses) + averaged = np.zeros(5) + sine_sum = cosine_sum = 0.0 + for weight, (axis, eccentricity, inclination, node, periapsis, mean_anomaly) in zip(weights, orbits): + longitude_periapsis = periapsis+factor*node # [rad] + tilt = math.tan(inclination/2) if factor == 1 else 1/math.tan(inclination/2) + averaged += weight*np.array([ + axis, eccentricity*math.sin(longitude_periapsis), eccentricity*math.cos(longitude_periapsis), + tilt*math.sin(node), tilt*math.cos(node), + ]) # [m, -, -, -, -] + longitude = mean_anomaly+longitude_periapsis # [rad] + sine_sum += weight*math.sin(longitude) + cosine_sum += weight*math.cos(longitude) + axis, shape_sine, shape_cosine, tilt_sine, tilt_cosine = averaged + eccentricity = math.hypot(shape_sine, shape_cosine) + tilt = math.hypot(tilt_sine, tilt_cosine) + inclination = 2*math.atan(tilt) if factor == 1 else 2*math.atan2(1, tilt) # [rad] + node = math.atan2(tilt_sine, tilt_cosine) # [rad] + longitude_periapsis = math.atan2(shape_sine, shape_cosine) # [rad] + periapsis = longitude_periapsis-factor*node # [rad] + mean_anomaly = math.atan2(sine_sum, cosine_sum)-longitude_periapsis # [rad] + return np.concatenate(classical_state(axis, eccentricity, inclination, node, periapsis, mean_anomaly)) + + +@pytest.mark.parametrize("element_type,factor", [ + pytest.param(EQUINOCTIAL_PROGRADE, 1, id="prograde"), + pytest.param(EQUINOCTIAL_RETROGRADE, -1, id="retrograde"), +]) +@pytest.mark.parametrize("equator_offset", [0.0, 1e-10], ids=["equatorial", "near-equatorial"]) # [rad] +@pytest.mark.parametrize("eccentricity", [0.0, 1e-8, 0.2], ids=["circular", "near-circular", "eccentric"]) # [-] +def test_equatorial_formation(element_type, factor, equator_offset, eccentricity): + """Average unequal members at or near the selected representation's equator. + + Widely separated ascending nodes accompany nearby periapsis longitudes. + Each member has a different axis, phase and mass; eccentric members also + have different eccentricities. The reference uses the prescribed classical + elements, avoiding recovery of undefined angles from Cartesian states. + Both output interfaces must match the reference in position and velocity. + """ + inclination = equator_offset if factor == 1 else math.pi-equator_offset # [rad] + orbits = [ + (7e6, 0.5*eccentricity, inclination, 0.1, 0.4-factor*0.1, 0.2), + (7.1e6, eccentricity, inclination, 2.2, 0.5-factor*2.2, 0.3), + (6.9e6, 1.5*eccentricity, inclination, 4.3, 0.3-factor*4.3, 0.4), + ] # [m, -, rad, rad, rad, rad] + masses = [100.0, 200.0, 400.0] # [kg] + states = [classical_state(*orbit) for orbit in orbits] + expected = weighted_reference(orbits, masses, factor) + actual = run_module(states, masses, element_type) + assert_state(actual, expected, rtol=0.0) + + +@pytest.mark.parametrize("element_type,factor", [ + pytest.param(EQUINOCTIAL_PROGRADE, 1, id="prograde"), + pytest.param(EQUINOCTIAL_RETROGRADE, -1, id="retrograde"), +]) +@pytest.mark.parametrize("cancelled_components", ["eccentricity", "inclination", "both"]) +def test_equinoctial_component_cancellation(element_type, factor, cancelled_components): + """Recover a circular or equatorial mean from nonsingular member orbits. + + The masses have ratio 1:3. For each cancelled component pair, the first + member has three times the magnitude and the opposite direction to the + second. Uncancelled components are identical, and both mean longitudes + are equal. Thus the expected averaged orbit is known analytically without + using ``weighted_reference`` or recovering singular classical angles. + Test eccentricity and inclination cancellation separately and together, + including both position and velocity on both output interfaces. + """ + cancel_eccentricity = cancelled_components in ("eccentricity", "both") + cancel_inclination = cancelled_components in ("inclination", "both") + axes = [7e6, 9e6] # [m] + masses = [100.0, 300.0] # [kg] + eccentricities = [0.3, 0.1] if cancel_eccentricity else [0.2, 0.2] # [-] + periapsis_longitudes = [0.7, 0.7+math.pi] if cancel_eccentricity else [0.7, 0.7] # [rad] + tilts = [0.3, 0.1] if cancel_inclination else [0.1, 0.1] # [-] + nodes = [0.4, 0.4+math.pi] if cancel_inclination else [0.4, 0.4] # [rad] + mean_longitude = 0.8 # [rad] + states = [] + for axis, eccentricity, tilt, node, periapsis_longitude in zip( + axes, eccentricities, tilts, nodes, periapsis_longitudes + ): + inclination = 2*math.atan(tilt) if factor == 1 else 2*math.atan2(1, tilt) # [rad] + states.append(classical_state( + axis, eccentricity, inclination, node, periapsis_longitude-factor*node, + mean_longitude-periapsis_longitude, + )) + + mean_axis = 8.5e6 # [m] + mean_eccentricity = 0.0 if cancel_eccentricity else 0.2 # [-] + mean_tilt = 0.0 if cancel_inclination else 0.1 # [-] + mean_inclination = 2*math.atan(mean_tilt) if factor == 1 else 2*math.atan2(1, mean_tilt) # [rad] + mean_node = 0.0 if cancel_inclination else 0.4 # [rad] + mean_periapsis_longitude = 0.0 if cancel_eccentricity else 0.7 # [rad] + expected = classical_state( + mean_axis, mean_eccentricity, mean_inclination, mean_node, + mean_periapsis_longitude-factor*mean_node, mean_longitude-mean_periapsis_longitude, + ) + actual = run_module(states, masses, element_type) + assert_state(actual, np.concatenate(expected), rtol=0.0) + + +@pytest.mark.parametrize("element_type,factor", [(EQUINOCTIAL_PROGRADE, 1), (EQUINOCTIAL_RETROGRADE, -1)]) +def test_weighting_wrap_and_reordering(element_type, factor): + """Check unequal shapes/tilts/masses, wraparound and mass/permutation invariance.""" + orbits = [ + (2e7, 0.2, 0.6, 0.1, 0.2, 6.0), + (2.1e7, 0.4, 0.8, 0.2, 0.1, 0.0), + (1.9e7, 0.1, 1.0, 0.3, 0.0, 0.2), + ] # [m, -, rad, rad, rad, rad] + if factor == -1: + orbits = [(axis, ecc, math.pi-inc, node, peri, phase) for axis, ecc, inc, node, peri, phase in orbits] + masses = [100.0, 200.0, 400.0] # [kg] + states = [classical_state(*orbit) for orbit in orbits] + expected = weighted_reference(orbits, masses, factor) + actual = run_module(states, masses, element_type) + assert_state(actual, expected) + assert_state(run_module(states[::-1], masses[::-1], element_type), actual) + assert_state(run_module(states, np.asarray(masses)*7, element_type), actual) + + +@pytest.mark.parametrize("element_type", [EQUINOCTIAL_PROGRADE, EQUINOCTIAL_RETROGRADE]) +def test_multiple_updates_refresh_navigation_and_mass(element_type): + """Check six scheduler updates with changing navigation and mass messages. + + Three unequal ellipses share one semimajor axis and advance analytically + at the common two-body mean motion. Both output forms and sample times + must match a fresh independent reference at every step. The masses change + halfway through to expose stale inputs or uncleared averaging sums. + This tests scheduled message integration, not a numerical dynamics integrator. + """ + axis = 2e7 # [m] + rate = math.sqrt(MU/axis**3) # [rad/s] + factor = 1 if element_type == EQUINOCTIAL_PROGRADE else -1 + orbits = [ + (axis, 0.2, 0.6, 0.1, 0.2, 0.8), + (axis, 0.4, 0.8, 0.2, 0.1, 1.0), + (axis, 0.6, 1.0, 0.3, 0.0, 1.2), + ] # [m, -, rad, rad, rad, rad] + if factor == -1: + orbits = [(a, ecc, math.pi-inc, node, peri, phase) for a, ecc, inc, node, peri, phase in orbits] + masses = [100.0, 200.0, 400.0] # [kg] + simulation, module, messages, recorder, recorder_c = make_simulation( + [classical_state(*orbit) for orbit in orbits], masses, element_type + ) + assert module.orbitalElementType == element_type + for step in range(6): + elapsed = float(step) # [s]; the task period is one second + current_orbits = [(*orbit[:-1], orbit[-1]+rate*elapsed) for orbit in orbits] + if step == 3: + masses = [400.0, 100.0, 200.0] # [kg] + for index, (orbit, mass) in enumerate(zip(current_orbits, masses)): + position, velocity = classical_state(*orbit) + navigation = messaging.NavTransMsgPayload() + navigation.r_BN_N = position.tolist() + navigation.v_BN_N = velocity.tolist() + vehicle = messaging.VehicleConfigMsgPayload() + vehicle.massSC = mass + messages[2*index].write(navigation) + messages[2*index+1].write(vehicle) + simulation.TotalSim.SingleStepProcesses() + assert_state(recorded_state(recorder, recorder_c), weighted_reference(current_orbits, masses, factor)) + expected_times = np.arange(6, dtype=np.uint64)*macros.sec2nano(1.0) # [ns] + np.testing.assert_array_equal(recorder.times(), expected_times) + np.testing.assert_array_equal(recorder_c.times(), expected_times) + + +@pytest.mark.parametrize("element_type", [EQUINOCTIAL_PROGRADE, EQUINOCTIAL_RETROGRADE]) +@pytest.mark.parametrize("binding_eps,accepted", [(8, False), (64, True)]) +def test_binding_energy_cutoff(element_type, binding_eps, accepted): + """Bracket the resolvable elliptic-energy cutoff without an accuracy claim. + + With unit radius and gravity, binding is ``2-speed**2`` and the cutoff + is ``16*epsilon``. The separated samples allow for square-root rounding. + Passing the guard at this extreme ellipse only promises finite output. + """ + epsilon = np.finfo(float).eps + speed = math.sqrt(2.0-binding_eps*epsilon) # [m/s] + factor = 1 if element_type == EQUINOCTIAL_PROGRADE else -1 + state = (np.array([1.0, 0.0, 0.0]), np.array([0.0, factor*speed, 0.0])) # [m, m/s] + binding = 2.0-speed*speed + cutoff = 8*epsilon*max(2.0, speed*speed) + assert (binding > cutoff) == accepted + if accepted: + run_module([state], [1.0], element_type, mu=1.0) # [kg, m^3/s^2] + else: + with pytest.raises(bskLogging.BasiliskError, match="resolvable nonradial"): + run_module([state], [1.0], element_type, mu=1.0) # [kg, m^3/s^2] + + +@pytest.mark.parametrize("element_type", [EQUINOCTIAL_PROGRADE, EQUINOCTIAL_RETROGRADE]) +@pytest.mark.parametrize("transverse_eps,first_guard", [(4, True), (8, True), (16, False)]) +def test_near_radial_cutoff(element_type, transverse_eps, first_guard): + """Above the radial guard, an unrepresentable ellipse must still be rejected. + + These almost-radial unit states have eccentricity rounding to one even + just above the angular-momentum cutoff. The first guard rejects values + at/below its cutoff; the later elliptic-elements guard rejects the rest. + Crossing this guard alone must not turn an unresolved ellipse into a valid one. + """ + epsilon = np.finfo(float).eps + factor = 1 if element_type == EQUINOCTIAL_PROGRADE else -1 + transverse_speed = transverse_eps*epsilon # [m/s] + state = (np.array([1.0, 0.0, 0.0]), np.array([1.0, factor*transverse_speed, 0.0])) # [m, m/s] + assert (transverse_speed <= 8*epsilon*math.sqrt(1+transverse_speed**2)) == first_guard + message = "resolvable nonradial" if first_guard else "representable elliptic elements" + with pytest.raises(bskLogging.BasiliskError, match=message): + run_module([state], [1.0], element_type, mu=1.0) # [kg, m^3/s^2] + + +@pytest.mark.parametrize("element_type", [EQUINOCTIAL_PROGRADE, EQUINOCTIAL_RETROGRADE]) +@pytest.mark.parametrize("tilt_eps,accepted", [(4, False), (8, False), (16, True)]) +def test_excluded_pole_cutoff(element_type, tilt_eps, accepted): + """Reject at/below the pole cutoff and recover a state just outside it. + + Cartesian construction avoids subtracting a tiny tilt from pi. Its unit + angular momentum has transverse magnitude ``tilt_eps*epsilon`` to floating + precision and points toward the selected representation's excluded pole. + """ + epsilon = np.finfo(float).eps + factor = 1 if element_type == EQUINOCTIAL_PROGRADE else -1 + transverse_speed = tilt_eps*epsilon # [m/s] + state = (np.array([1.0, 0.0, 0.0]), np.array([0.0, -factor, transverse_speed])) # [m, m/s] + assert (transverse_speed > 8*epsilon) == accepted + if accepted: + actual = run_module([state], [1.0], element_type, mu=1.0) # [kg, m^3/s^2] + np.testing.assert_allclose(actual, np.concatenate(state), rtol=0.0, atol=64*epsilon) # [m, m/s] + else: + with pytest.raises(bskLogging.BasiliskError, match="pole"): + run_module([state], [1.0], element_type, mu=1.0) # [kg, m^3/s^2] + + +@pytest.mark.parametrize("element_type", [EQUINOCTIAL_PROGRADE, EQUINOCTIAL_RETROGRADE]) +@pytest.mark.parametrize("offset_eps,accepted", [(64, False), (256, True)]) +def test_longitude_cancellation_cutoff(element_type, offset_eps, accepted): + """Bracket the 64-epsilon circular-resultant cutoff with equal near-opposites. + + The resultant is approximately half the phase offset. Samples at 32 and + 128 epsilons avoid depending on libm's exact rounding at the boundary. + The accepted symmetric configuration has its reference along positive y. + """ + epsilon = np.finfo(float).eps + offset = offset_eps*epsilon # [rad] + factor = 1 if element_type == EQUINOCTIAL_PROGRADE else -1 + states = [ + (np.array([1.0, 0.0, 0.0]), np.array([0.0, factor, 0.0])), + (np.array([-1.0, offset, 0.0]), np.array([-factor*offset, -factor, 0.0])), + ] # [m, m/s] + assert (offset/2 > 64*epsilon) == accepted + if accepted: + actual = run_module(states, [1.0, 1.0], element_type, mu=1.0) # [kg, m^3/s^2] + expected = np.array([0.0, 1.0, 0.0, -factor, 0.0, 0.0]) # [m, m/s] + np.testing.assert_allclose(actual, expected, rtol=0.0, atol=64*epsilon) # [m, m/s] + else: + with pytest.raises(bskLogging.BasiliskError, match="longitude"): + run_module(states, [1.0, 1.0], element_type, mu=1.0) # [kg, m^3/s^2] + + +@pytest.mark.parametrize("element_type", [-1, 3]) +def test_invalid_selector(element_type): + """Unknown orbital choices fail explicitly instead of selecting another method.""" + state = classical_state(7e6, 0.1, 0.9, 0.3, 0.4, 0.7) # [m, -, rad, rad, rad, rad] + with pytest.raises(bskLogging.BasiliskError, match="orbitalElementType"): + run_module([state], [100.0], element_type) # [kg] + + +@pytest.mark.parametrize("mu", [0.0, -1.0, math.nan, math.inf]) # [m^3/s^2] +def test_invalid_gravity(mu): + """New orbital modes require a finite positive gravitational parameter.""" + state = classical_state(7e6, 0.1, 0.9, 0.3, 0.4, 0.7) # [m, -, rad, rad, rad, rad] + with pytest.raises(bskLogging.BasiliskError, match="gravitational parameter"): + run_module([state], [100.0], EQUINOCTIAL_PROGRADE, mu=mu) # [kg] + + +@pytest.mark.parametrize("masses", [[0.0], [-1.0], [math.inf], [math.nan]]) # [kg] +def test_invalid_mass(masses): + """Undefined weights cannot produce a fabricated formation reference.""" + state = classical_state(7e6, 0.1, 0.9, 0.3, 0.4, 0.7) # [m, -, rad, rad, rad, rad] + with pytest.raises(bskLogging.BasiliskError, match="mass"): + run_module([state], masses, EQUINOCTIAL_PROGRADE) + + +@pytest.mark.parametrize("velocity", [[0.0, 1.0, 1.0], [0.0, 2.0, 0.0]]) # [m/s] +def test_nonelliptic_input_is_rejected(velocity): + """Exact unit-scale parabolic and hyperbolic inputs are outside the new modes.""" + with pytest.raises(bskLogging.BasiliskError, match="elliptic"): + state = (np.array([1.0, 0.0, 0.0]), np.array(velocity)) # [m, m/s] + run_module([state], [1.0], EQUINOCTIAL_PROGRADE, mu=1.0) # [kg, m^3/s^2] + + +@pytest.mark.parametrize("element_type,inclination", [ + (EQUINOCTIAL_PROGRADE, math.pi), (EQUINOCTIAL_RETROGRADE, 0.0), +]) # [rad] +def test_opposite_chart_pole_is_rejected(element_type, inclination): + """A representation's excluded pole is an explicit error, not silent fallback.""" + state = classical_state(7e6, 0.1, inclination, 0.3, 0.4, 0.7) # [m, -, rad, rad, rad, rad] + with pytest.raises(bskLogging.BasiliskError, match="pole"): + run_module([state], [100.0], element_type) # [kg] + + +def test_undefined_longitude_is_rejected(): + """Equal opposite phases have no unique circular average longitude.""" + speed = math.sqrt(MU/7e6) # [m/s] + state = (np.array([7e6, 0.0, 0.0]), np.array([0.0, speed, 0.0])) # [m, m/s] + opposite = (-state[0], -state[1]) + with pytest.raises(bskLogging.BasiliskError, match="longitude"): + run_module([state, opposite], [100.0, 100.0], EQUINOCTIAL_PROGRADE) # [kg] + + +@pytest.mark.parametrize("bad_state", [ + ([0.0, 0.0, 0.0], [0.0, 7500.0, 0.0]), + ([7e6, 0.0, 0.0], [0.0, 0.0, 0.0]), + ([math.nan, 0.0, 0.0], [0.0, 7500.0, 0.0]), + ([7e6, 0.0, 0.0], [0.0, math.inf, 0.0]), +]) # [m, m/s] +def test_invalid_navigation(bad_state): + """New modes stop on invalid navigation instead of writing nonfinite output.""" + with pytest.raises(bskLogging.BasiliskError): + run_module([bad_state], [100.0], EQUINOCTIAL_PROGRADE) # [kg] + + +def test_zero_mass_member_does_not_contribute(): + """A zero-weight spacecraft cannot move an otherwise defined reference.""" + first = classical_state(7e6, 0.1, 0.9, 0.3, 0.4, 0.7) # [m, -, rad, rad, rad, rad] + second = classical_state(8e6, 0.3, 1.1, 0.5, 0.6, 1.7) # [m, -, rad, rad, rad, rad] + actual = run_module([first, second], [100.0, 0.0], EQUINOCTIAL_PROGRADE) # [kg] + assert_state(actual, np.concatenate(first)) + + +@pytest.mark.parametrize("initial_type,selected_type", [ + pytest.param(CLASSICAL, EQUINOCTIAL_PROGRADE, id="classical-to-prograde"), + pytest.param(CLASSICAL, EQUINOCTIAL_RETROGRADE, id="classical-to-retrograde"), + pytest.param(EQUINOCTIAL_PROGRADE, CLASSICAL, id="prograde-to-classical"), + pytest.param(EQUINOCTIAL_PROGRADE, EQUINOCTIAL_RETROGRADE, id="prograde-to-retrograde"), + pytest.param(EQUINOCTIAL_RETROGRADE, CLASSICAL, id="retrograde-to-classical"), + pytest.param(EQUINOCTIAL_RETROGRADE, EQUINOCTIAL_PROGRADE, id="retrograde-to-prograde"), +]) +def test_runtime_selection(initial_type, selected_type): + """Observe every orbital-selector transition before and after an update. + + Two equal-mass members share axis, eccentricity, inclination and mean + anomaly, with symmetric offsets in ascending node and periapsis. Thus + the classical reference uses the midpoint angles exactly. Equinoctial + references use the independent element-average and classical-rotation + helper. All three references differ in both position and velocity. + + Initialize with one choice, select another before the first update, then + switch back after that update. Each C and C++ output must match its own + reference without another Reset, detecting a choice cached at either + initialization or the first update. + """ + axis, eccentricity, inclination = 7e6, 0.2, 0.9 # [m, -, rad] + node, periapsis, mean_anomaly = 0.3, 0.5, 0.8 # [rad] + orbits = [ + (axis, eccentricity, inclination, node+offset, periapsis+offset, mean_anomaly) + for offset in (-0.2, 0.2) # [rad] + ] # [m, -, rad, rad, rad, rad] + masses = [100.0, 100.0] # [kg] + states = [classical_state(*orbit) for orbit in orbits] + expected = { + CLASSICAL: np.concatenate(classical_state(axis, eccentricity, inclination, node, periapsis, mean_anomaly)), + EQUINOCTIAL_PROGRADE: weighted_reference(orbits, masses, 1), + EQUINOCTIAL_RETROGRADE: weighted_reference(orbits, masses, -1), + } + # Keep each transition observable even if the fixture is changed later. + difference = expected[selected_type]-expected[initial_type] + assert np.linalg.norm(difference[:3]) > 1.0 # [m] + assert np.linalg.norm(difference[3:]) > 1e-3 # [m/s] + + simulation, module, messages, recorder, recorder_c = make_simulation(states, masses, initial_type) + for element_type in (selected_type, initial_type): + module.orbitalElementType = element_type + simulation.TotalSim.SingleStepProcesses() + assert_state(recorded_state(recorder, recorder_c), expected[element_type], rtol=0.0) + expected_times = np.array([0, macros.sec2nano(1.0)], dtype=np.uint64) # [ns]; one-second task period + np.testing.assert_array_equal(recorder.times(), expected_times) + np.testing.assert_array_equal(recorder_c.times(), expected_times) + + +@pytest.mark.parametrize("changes", [{"orbitalElementType": 3}, {"mu": math.nan}]) +def test_runtime_validation(changes): + """Invalid changes after Reset are rejected again before new-mode computation.""" + state = classical_state(7e6, 0.1, 0.9, 0.3, 0.4, 0.7) # [m, -, rad, rad, rad, rad] + with pytest.raises(bskLogging.BasiliskError): + run_module([state], [100.0], EQUINOCTIAL_PROGRADE, after_reset=changes) # [kg] + + +if __name__ == "__main__": + raise SystemExit(pytest.main([__file__])) diff --git a/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.cpp b/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.cpp index 56e548276a5..f4661f23e87 100644 --- a/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.cpp +++ b/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.cpp @@ -18,14 +18,40 @@ */ #include "fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.h" +#include "architecture/utilities/linearAlgebra.h" #include "architecture/utilities/orbitalMotion.h" +#include +#include +#include #include +namespace { +/** + * @brief Construct the orthonormal equinoctial plane axes for one common representation. + * @param tiltSine Dimensionless inclination coordinate, tan(i/2)^factor sin(Omega). + * @param tiltCosine Dimensionless inclination coordinate, tan(i/2)^factor cos(Omega). + * @param factor Common retrograde factor, +1 or -1. + * @param firstAxis_N First dimensionless plane axis in the inertial frame. + * @param secondAxis_N Second dimensionless plane axis in the inertial frame. + */ +void equinoctialPlane(double tiltSine, double tiltCosine, double factor, double firstAxis_N[3], double secondAxis_N[3]) +{ + const double denominator = 1.0 + tiltSine * tiltSine + tiltCosine * tiltCosine; + firstAxis_N[0] = (1.0 - tiltSine * tiltSine + tiltCosine * tiltCosine) / denominator; + firstAxis_N[1] = 2.0 * tiltSine * tiltCosine / denominator; + firstAxis_N[2] = -2.0 * factor * tiltSine / denominator; + secondAxis_N[0] = 2.0 * factor * tiltSine * tiltCosine / denominator; + secondAxis_N[1] = factor * (1.0 + tiltSine * tiltSine - tiltCosine * tiltCosine) / denominator; + secondAxis_N[2] = 2.0 * tiltCosine / denominator; +} +} + /*! This is the constructor for the module class. It sets default variable values and initializes the various parts of the model */ FormationBarycenter::FormationBarycenter() { this->useOrbitalElements = false; + this->orbitalElementType = CLASSICAL; this->mu = 0; } @@ -54,6 +80,9 @@ void FormationBarycenter::Reset(uint64_t CurrentSimNanos [[maybe_unused]]) bskLogger.bskError( "FormationBarycenter module requires defining a gravitational parameter if using orbital elements."); } + if (this->useOrbitalElements && this->orbitalElementType != CLASSICAL) { + this->validateEquinoctialConfiguration(); + } } /*! Adds a scNav and scPayload messages name to the vector of names to be subscribed to. @@ -110,6 +139,8 @@ void FormationBarycenter::computeBaricenter() barycenter[n] /= totalMass; barycenterVelocity[n] /= totalMass; } + } else if (this->orbitalElementType != CLASSICAL) { + this->computeEquinoctialBarycenter(barycenter, barycenterVelocity); } else { ClassicElements orbitElements = {}; // zero the orbit elements first ClassicElements tempElements; @@ -157,6 +188,149 @@ void FormationBarycenter::computeBaricenter() } } +void FormationBarycenter::validateEquinoctialConfiguration() +{ + if (this->orbitalElementType != EQUINOCTIAL_PROGRADE && this->orbitalElementType != EQUINOCTIAL_RETROGRADE) { + this->bskLogger.bskError("FormationBarycenter orbitalElementType is not a supported choice."); + } + if (!std::isfinite(this->mu) || this->mu <= 0.0) { + this->bskLogger.bskError( + "FormationBarycenter equinoctial averaging requires a finite positive gravitational parameter."); + } +} + +void FormationBarycenter::computeEquinoctialBarycenter(double position_N[3], double velocity_N[3]) +{ + this->validateEquinoctialConfiguration(); + const double factor = this->orbitalElementType == EQUINOCTIAL_PROGRADE ? 1.0 : -1.0; + const double roundoff = 8.0 * std::numeric_limits::epsilon(); + double totalMass = 0.0; // [kg] + for (const auto& payload : this->scPayloadBuffer) { + if (!std::isfinite(payload.massSC) || payload.massSC < 0.0) { + this->bskLogger.bskError( + "FormationBarycenter equinoctial averaging requires finite nonnegative spacecraft mass."); + } + totalMass += payload.massSC; + } + if (!std::isfinite(totalMass) || totalMass <= 0.0) { + this->bskLogger.bskError("FormationBarycenter equinoctial averaging requires finite positive total mass."); + } + + double semimajorAxis = 0.0; // [m] + double shapeSine = 0.0, shapeCosine = 0.0; + double tiltSine = 0.0, tiltCosine = 0.0; + double longitudeSine = 0.0, longitudeCosine = 0.0; + for (std::size_t index = 0; index < this->scNavBuffer.size(); ++index) { + auto& navigation = this->scNavBuffer[index]; + const double weight = this->scPayloadBuffer[index].massSC / totalMass; + if (weight == 0.0) { + continue; + } + for (int component = 0; component < 3; ++component) { + if (!std::isfinite(navigation.r_BN_N[component]) || !std::isfinite(navigation.v_BN_N[component])) { + this->bskLogger.bskError( + "FormationBarycenter equinoctial averaging requires finite position and velocity."); + } + } + const double radius = v3Norm(navigation.r_BN_N); // [m] + if (!std::isfinite(radius) || radius <= 0.0) { + this->bskLogger.bskError( + "FormationBarycenter equinoctial averaging requires a nonzero finite orbital radius."); + } + const double speedScale = std::sqrt(this->mu / radius); // [m/s] + if (!std::isfinite(speedScale) || speedScale <= 0.0) { + this->bskLogger.bskError( + "FormationBarycenter equinoctial averaging requires a representable elliptic state."); + } + double radial_N[3], scaledVelocity_N[3], momentum_N[3], normal_N[3], eccentricity_N[3]; + v3Scale(1.0 / radius, navigation.r_BN_N, radial_N); + v3Scale(1.0 / speedScale, navigation.v_BN_N, scaledVelocity_N); + const double speedSquared = v3Dot(scaledVelocity_N, scaledVelocity_N); + const double binding = 2.0 - speedSquared; + v3Cross(radial_N, scaledVelocity_N, momentum_N); + const double momentum = v3Norm(momentum_N); + if (!std::isfinite(binding) || binding <= roundoff * std::fmax(2.0, speedSquared) || !std::isfinite(momentum) || + momentum <= roundoff * std::sqrt(speedSquared)) { + this->bskLogger.bskError("FormationBarycenter equinoctial averaging supports only resolvable nonradial " + "elliptic states (e < 1)."); + } + const double memberAxis = radius / binding; // [m] + v3Scale(1.0 / momentum, momentum_N, normal_N); + const double transverse = std::hypot(normal_N[0], normal_N[1]); + const double pole = factor * normal_N[2]; + if (pole < 0.0 && transverse <= roundoff) { + this->bskLogger.bskError( + "FormationBarycenter selected equinoctial representation is singular at the opposite equatorial pole."); + } + // Rationalize the denominator near the excluded pole to avoid subtracting nearly equal numbers. + const double tiltScale = pole >= 0.0 ? 1.0 / (1.0 + pole) : (1.0 - pole) / (transverse * transverse); + const double memberTiltSine = normal_N[0] * tiltScale; + const double memberTiltCosine = -normal_N[1] * tiltScale; + double firstAxis_N[3], secondAxis_N[3]; + equinoctialPlane(memberTiltSine, memberTiltCosine, factor, firstAxis_N, secondAxis_N); + v3Cross(scaledVelocity_N, momentum_N, eccentricity_N); + v3Subtract(eccentricity_N, radial_N, eccentricity_N); + const double memberShapeSine = v3Dot(eccentricity_N, secondAxis_N); + const double memberShapeCosine = v3Dot(eccentricity_N, firstAxis_N); + const double eccentricity = std::hypot(memberShapeSine, memberShapeCosine); + if (!std::isfinite(memberAxis) || !std::isfinite(eccentricity) || eccentricity >= 1.0) { + this->bskLogger.bskError( + "FormationBarycenter equinoctial averaging requires representable elliptic elements (e < 1)."); + } + const double trueLongitude = + std::atan2(v3Dot(navigation.r_BN_N, secondAxis_N), v3Dot(navigation.r_BN_N, firstAxis_N)); // [rad] + const double ellipticFactor = std::sqrt((1.0 - eccentricity) * (1.0 + eccentricity)); + const double eccentricLongitude = + trueLongitude + + 2.0 * std::atan2(memberShapeSine * std::cos(trueLongitude) - memberShapeCosine * std::sin(trueLongitude), + 1.0 + ellipticFactor + memberShapeCosine * std::cos(trueLongitude) + + memberShapeSine * std::sin(trueLongitude)); // [rad] + const double meanLongitude = eccentricLongitude - memberShapeCosine * std::sin(eccentricLongitude) + + memberShapeSine * std::cos(eccentricLongitude); // [rad] + semimajorAxis += weight * memberAxis; + shapeSine += weight * memberShapeSine; + shapeCosine += weight * memberShapeCosine; + tiltSine += weight * memberTiltSine; + tiltCosine += weight * memberTiltCosine; + longitudeSine += weight * std::sin(meanLongitude); + longitudeCosine += weight * std::cos(meanLongitude); + } + if (std::hypot(longitudeSine, longitudeCosine) <= 64.0 * std::numeric_limits::epsilon()) { + this->bskLogger.bskError( + "FormationBarycenter mean longitude is undefined because the weighted directions cancel."); + } + const double eccentricity = std::hypot(shapeSine, shapeCosine); + if (!std::isfinite(semimajorAxis) || semimajorAxis <= 0.0 || !std::isfinite(eccentricity) || eccentricity >= 1.0) { + this->bskLogger.bskError("FormationBarycenter averaged equinoctial elements are not a representable ellipse."); + } + const double meanLongitude = std::atan2(longitudeSine, longitudeCosine); // [rad] + const double periapsisLongitude = eccentricity == 0.0 ? 0.0 : std::atan2(shapeSine, shapeCosine); // [rad] + const double eccentricLongitude = + M2E(meanLongitude - periapsisLongitude, eccentricity) + periapsisLongitude; // [rad] + const double cosine = std::cos(eccentricLongitude); + const double sine = std::sin(eccentricLongitude); + const double beta = 1.0 / (1.0 + std::sqrt((1.0 - eccentricity) * (1.0 + eccentricity))); + const double crossTerm = beta * shapeSine * shapeCosine; + const double firstScale = 1.0 - beta * shapeSine * shapeSine; + const double secondScale = 1.0 - beta * shapeCosine * shapeCosine; + const double longitudeRate = (std::sqrt(this->mu / semimajorAxis) / semimajorAxis) / + (1.0 - shapeCosine * cosine - shapeSine * sine); // [rad/s] + double firstAxis_N[3], secondAxis_N[3]; + equinoctialPlane(tiltSine, tiltCosine, factor, firstAxis_N, secondAxis_N); + for (int component = 0; component < 3; ++component) { + position_N[component] = + semimajorAxis * ((firstScale * cosine + crossTerm * sine - shapeCosine) * firstAxis_N[component] + + (secondScale * sine + crossTerm * cosine - shapeSine) * secondAxis_N[component]); + velocity_N[component] = semimajorAxis * longitudeRate * + ((-firstScale * sine + crossTerm * cosine) * firstAxis_N[component] + + (secondScale * cosine - crossTerm * sine) * secondAxis_N[component]); + if (!std::isfinite(position_N[component]) || !std::isfinite(velocity_N[component])) { + this->bskLogger.bskError( + "FormationBarycenter equinoctial reference is not representable as finite position and velocity."); + } + } +} + /*! writes the output messages */ void FormationBarycenter::WriteOutputMessage(uint64_t CurrentClock) diff --git a/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.h b/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.h index 52acbc9f308..a242df2470f 100644 --- a/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.h +++ b/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.h @@ -20,8 +20,10 @@ #ifndef FORMATION_BARYCENTER_H #define FORMATION_BARYCENTER_H +#include #include +#include "architecture/msgPayloadDefC/NavTransMsgPayload.h" #include "architecture/msgPayloadDefC/VehicleConfigMsgPayload.h" #include "cMsgCInterface/NavTransMsg_C.h" @@ -35,6 +37,15 @@ class FormationBarycenter : public SysModel { public: + /** @brief Orbital averaging choices; the existing classical method is the default. */ + enum OrbitalElementType + { + CLASSICAL = 0, //!< Existing classical-element average. + EQUINOCTIAL_PROGRADE = 1, //!< Mean-longitude equinoctial average, excluding the retrograde equatorial pole. + EQUINOCTIAL_RETROGRADE = + 2 //!< Mean-longitude retrograde equinoctial average, excluding the prograde equatorial pole. + }; + FormationBarycenter(); ~FormationBarycenter(); @@ -54,12 +65,23 @@ class FormationBarycenter : public SysModel Message transOutMsg; //!< translation navigation output msg NavTransMsg_C transOutMsgC = {}; //!< C-wrapped translation navigation output msg, zeroed - bool useOrbitalElements; //!< flag that determines whether to use cartesian or orbital elementd weighted averaging - double mu; //!< gravitational parameter to be used with orbital elements averaging + bool useOrbitalElements; //!< flag that determines whether to use Cartesian or orbital-element weighted averaging + int orbitalElementType; //!< OrbitalElementType choice, used only when useOrbitalElements is true; defaults to CLASSICAL. + double mu; //!< gravitational parameter to be used with orbital elements averaging BSKLogger bskLogger; //!< -- BSK Logging private: + /** @brief Validate the opt-in orbital choice and its gravitational parameter. */ + void validateEquinoctialConfiguration(); + /** + * @brief Compute a mass-weighted elliptic equinoctial reference using one common retrograde factor. + * @param position_N Output reference position in the inertial frame [m]. + * @param velocity_N Output reference velocity in the inertial frame [m/s]. + * @note Rejects unsupported inputs and an undefined circular-mean longitude with a Basilisk error. + */ + void computeEquinoctialBarycenter(double position_N[3], double velocity_N[3]); + std::vector scNavBuffer; //!< buffer of spacecraft navigation info std::vector scPayloadBuffer; //!< buffer of spacecraft payload diff --git a/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.rst b/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.rst index 20815592e48..47926df8f6a 100644 --- a/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.rst +++ b/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.rst @@ -1,7 +1,8 @@ Executive Summary ----------------- -This module computes the barycenter of a swarm of satellites. The barycenter can either be computed in the regular cartesian way (using a weighted average of the position and velocity vectors) -or using the weighted average of the orbital elements. Both output two navigation messages that describe the position and velocity of the barycenter. The output messages contain the same +This module computes a formation reference using either a Cartesian mass-weighted average of position and velocity or a mass-weighted average of orbital elements. +The default orbital-element choice retains classical-element averaging. Explicit equinoctial choices support circular and equatorial elliptic orbits without averaging undefined classical angles separately. +All choices output two navigation messages that describe the reference position and velocity. The output messages contain the same information, although one is a C++ message and the other is a C-wrapped message. Message Connection Descriptions @@ -33,7 +34,10 @@ Let :math:`\textbf{x}` represent either the position or the velocity vectors. Th where :math:`m_{total}=\sum_{i}m_i`. -For the orbital elements averaging, the process is similar. However, the position and velocity vectors of each spacecraft must first be converted to orbital elements. Once +Classical elements +~~~~~~~~~~~~~~~~~~ + +For the default orbital-element averaging, the process is similar. However, the position and velocity vectors of each spacecraft must first be converted to orbital elements. Once that is done, we take the average of each orbital element :math:`oe` as such: .. math:: @@ -51,6 +55,23 @@ where :math:`\bar{\alpha}` is the averaged angular orbital element. The set of : As stated before, the module outputs both a C++ and C-wrapped navigation messages. Both contain the same payload. +Equinoctial elements +~~~~~~~~~~~~~~~~~~~~ + +The opt-in choices average one common equinoctial representation for all spacecraft. Let :math:`I=+1` for ``EQUINOCTIAL_PROGRADE`` and :math:`I=-1` for ``EQUINOCTIAL_RETROGRADE``. The element definitions are + +.. math:: + + P_1 &= e\sin(\omega+I\Omega), & P_2 &= e\cos(\omega+I\Omega), \\ + Q_1 &= \tan(i/2)^I\sin\Omega, & Q_2 &= \tan(i/2)^I\cos\Omega, \\ + l &= M+\omega+I\Omega. + +The semimajor axis :math:`a`, :math:`P_1`, :math:`P_2`, :math:`Q_1` and :math:`Q_2` use the linear mass-weighted average. Mean longitude :math:`l` uses the circular average shown above. The module calculates the elements directly from position and velocity and reconstructs the averaged state in the equinoctial plane; it does not recover and average singular classical angles. + +Mean longitude retains the shared two-body mean motion when all members have the same semimajor axis and the circular mean is defined. For unequal semimajor axes, the output velocity is the Keplerian velocity of the averaged instantaneous orbit; it need not equal the time derivative of a reference recomputed from the whole formation. An orbital-element average is generally different from the physical Cartesian center of mass. + +The same value of :math:`I` is used throughout one average. The module does not automatically change representations near an inclination boundary. ``EQUINOCTIAL_RETROGRADE`` covers :math:`i=\pi`, where the prograde representation is singular, but has its own singularity at :math:`i=0`. These choices describe the same individual orbit where both are defined, but averaging different representations need not produce the same formation reference. + Model Assumptions and Limitations --------------------------------- @@ -60,7 +81,12 @@ This code makes the following assumptions: This code has the following limitations: -- **Equatorial singularities**: when using orbital element averaging, near equatorial orbits may have induce a singularity in the ascending node. +- **Legacy classical singularities**: the unchanged classical-element method is unreliable near circular or equatorial orbits, where some separately averaged angles are undefined. Select a suitable equinoctial choice explicitly for these cases. +- **Equinoctial orbit range**: only finite, nonradial elliptic states with positive semimajor axes and :math:`0\le e<1` are supported by the new choices. Parabolic (:math:`e=1`), hyperbolic (:math:`e>1`) and radial states raise a Basilisk error. Binding energy indistinguishable from zero at floating-point precision is also rejected. The legacy modes are not given new orbit-domain restrictions. +- **Excluded pole**: the prograde choice excludes the retrograde equatorial pole and vice versa. States at the excluded pole, or within eight machine epsilons in the normalized transverse angular momentum there, raise a Basilisk error. Select the other representation for a formation near that pole. +- **Weights**: the equinoctial choices require finite nonnegative masses and a finite positive total mass. Zero-weight spacecraft do not contribute. +- **Undefined longitude**: if the magnitude of the normalized weighted sine-and-cosine sum is at most 64 machine epsilons, the circular-mean longitude is numerically undefined and raises a Basilisk error. Equal masses on opposite orbital phases are one such case. There is no automatic Cartesian or classical fallback. +- **Gravity and numerical range**: equinoctial averaging requires a finite positive gravitational parameter. Unrepresentable intermediate elements or output states raise a Basilisk error. Accuracy degrades near the parabolic limit; the new choices do not promise reliable representation of arbitrarily extreme ellipses. User Guide @@ -71,7 +97,7 @@ This section contains conceptual overviews of the code and clear examples for th Module Setup ~~~~~~~~~~~~ -The temperature module is created in python using: +The module is created in Python using: .. code-block:: python :linenos: @@ -115,3 +141,17 @@ No further setup is needed for the cartesian method. If the user wants to use or barycenterModule.useOrbitalElements = True barycenterModule.mu = mu + +This retains classical averaging because ``orbitalElementType`` defaults to ``CLASSICAL``. To opt in to mean-longitude equinoctial averaging, also set: + +.. code-block:: python + + barycenterModule.orbitalElementType = barycenterModule.EQUINOCTIAL_PROGRADE + +For a formation near the retrograde equator, select the common retrograde representation instead: + +.. code-block:: python + + barycenterModule.orbitalElementType = barycenterModule.EQUINOCTIAL_RETROGRADE + +When ``useOrbitalElements`` is false, ``orbitalElementType`` is ignored and the existing Cartesian calculation is unchanged.