From df266fc49e76f3590f95a00b18dfe92d34a87e3a Mon Sep 17 00:00:00 2001 From: Zakariya Ghazzol <214604256+zakariyaghazzol@users.noreply.github.com> Date: Wed, 4 Nov 2026 23:02:52 -0500 Subject: [PATCH 1/6] [#1656] Add optional equinoctial formation averaging --- .../1656-formation-equinoctial.rst | 1 + .../1656-formation-equinoctial.rst | 1 + .../test_formationBarycenter_equinoctial.py | 501 ++++++++++++++++++ .../formationBarycenter.cpp | 174 ++++++ .../formationBarycenter/formationBarycenter.h | 26 +- .../formationBarycenter.rst | 50 +- 6 files changed, 746 insertions(+), 7 deletions(-) create mode 100644 docs/source/Support/bskKnownIssuesSnippets/1656-formation-equinoctial.rst create mode 100644 docs/source/Support/bskReleaseNotesSnippets/1656-formation-equinoctial.rst create mode 100644 src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py 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..42f8c72abd6 --- /dev/null +++ b/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py @@ -0,0 +1,501 @@ +# +# 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 = 1 +RETROGRADE_EQUINOCTIAL = 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 == EQUINOCTIAL + assert module.RETROGRADE_EQUINOCTIAL == RETROGRADE_EQUINOCTIAL + + +@pytest.mark.parametrize("element_type", [CLASSICAL, EQUINOCTIAL, RETROGRADE_EQUINOCTIAL, -1]) +def test_cartesian_ignores_element_selector(element_type): + """The extra orbital setting cannot change the original Cartesian branch.""" + 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) + actual = run_module(states, masses, element_type, use_orbital=False, mu=0.0) + np.testing.assert_array_equal(actual, expected) + + +@pytest.mark.parametrize("element_type", [EQUINOCTIAL, RETROGRADE_EQUINOCTIAL]) +@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 + 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, RETROGRADE_EQUINOCTIAL]) +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 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, RETROGRADE_EQUINOCTIAL]) +@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 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", [(EQUINOCTIAL, 1), (RETROGRADE_EQUINOCTIAL, -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, RETROGRADE_EQUINOCTIAL]) +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 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, RETROGRADE_EQUINOCTIAL]) +@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 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, RETROGRADE_EQUINOCTIAL]) +@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 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, RETROGRADE_EQUINOCTIAL]) +@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 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, RETROGRADE_EQUINOCTIAL]) +@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 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, 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) + + +@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, mu=1.0) # [kg, m^3/s^2] + + +@pytest.mark.parametrize("element_type,inclination", [(EQUINOCTIAL, math.pi), (RETROGRADE_EQUINOCTIAL, 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) # [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) # [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) # [kg] + assert_state(actual, np.concatenate(first)) + + +def test_runtime_selection(): + """The new selector can be configured after initialization, like the old switch.""" + state = classical_state(7e6, 0.1, 0.9, 0.3, 0.4, 0.7) # [m, -, rad, rad, rad, rad] + actual = run_module([state], [100.0], CLASSICAL, after_reset={"orbitalElementType": EQUINOCTIAL}) # [kg] + assert_state(actual, np.concatenate(state)) + + +@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, 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..f2cc91e13a8 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 && this->orbitalElementType != RETROGRADE_EQUINOCTIAL) { + 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 ? 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..ca421035065 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 = 1, //!< Mean-longitude equinoctial average, excluding the retrograde equatorial pole. + RETROGRADE_EQUINOCTIAL = + 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..7454462a487 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`` and :math:`I=-1` for ``RETROGRADE_EQUINOCTIAL``. 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. ``RETROGRADE_EQUINOCTIAL`` 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 + +For a formation near the retrograde equator, select the common retrograde representation instead: + +.. code-block:: python + + barycenterModule.orbitalElementType = barycenterModule.RETROGRADE_EQUINOCTIAL + +When ``useOrbitalElements`` is false, ``orbitalElementType`` is ignored and the existing Cartesian calculation is unchanged. From 7e3a1a54f168e99de6780e82bdb0dc1c4747292b Mon Sep 17 00:00:00 2001 From: Hanspeter Schaub Date: Thu, 5 Nov 2026 09:55:09 -0700 Subject: [PATCH 2/6] [#1656] Clarify formation barycenter equinoctial option names --- .../test_formationBarycenter_equinoctial.py | 64 ++++++++++--------- .../formationBarycenter.cpp | 4 +- .../formationBarycenter/formationBarycenter.h | 6 +- .../formationBarycenter.rst | 8 +-- 4 files changed, 42 insertions(+), 40 deletions(-) diff --git a/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py b/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py index 42f8c72abd6..55571cdec1c 100644 --- a/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py +++ b/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py @@ -35,8 +35,8 @@ MU = 3.986004418e14 # [m^3/s^2] CLASSICAL = 0 -EQUINOCTIAL = 1 -RETROGRADE_EQUINOCTIAL = 2 +EQUINOCTIAL_PROGRADE = 1 +EQUINOCTIAL_RETROGRADE = 2 def classical_state(axis, eccentricity, inclination, node, periapsis, mean_anomaly): @@ -138,11 +138,11 @@ def test_default_and_named_choices(): assert module.useOrbitalElements is False assert module.orbitalElementType == CLASSICAL assert module.CLASSICAL == CLASSICAL - assert module.EQUINOCTIAL == EQUINOCTIAL - assert module.RETROGRADE_EQUINOCTIAL == RETROGRADE_EQUINOCTIAL + assert module.EQUINOCTIAL_PROGRADE == EQUINOCTIAL_PROGRADE + assert module.EQUINOCTIAL_RETROGRADE == EQUINOCTIAL_RETROGRADE -@pytest.mark.parametrize("element_type", [CLASSICAL, EQUINOCTIAL, RETROGRADE_EQUINOCTIAL, -1]) +@pytest.mark.parametrize("element_type", [CLASSICAL, EQUINOCTIAL_PROGRADE, EQUINOCTIAL_RETROGRADE, -1]) def test_cartesian_ignores_element_selector(element_type): """The extra orbital setting cannot change the original Cartesian branch.""" states = [ @@ -155,14 +155,14 @@ def test_cartesian_ignores_element_selector(element_type): np.testing.assert_array_equal(actual, expected) -@pytest.mark.parametrize("element_type", [EQUINOCTIAL, RETROGRADE_EQUINOCTIAL]) +@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 + [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( @@ -175,7 +175,7 @@ def test_single_spacecraft_identity(element_type, eccentricity, tilt_index, mean (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, RETROGRADE_EQUINOCTIAL]) +@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. @@ -206,7 +206,7 @@ def test_reported_compact_formation(phases, element_type): 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 else -1 + 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) @@ -215,12 +215,12 @@ def test_reported_compact_formation(phases, element_type): assert np.linalg.norm(actual[:3]-cartesian) < locality_budget -@pytest.mark.parametrize("element_type", [EQUINOCTIAL, RETROGRADE_EQUINOCTIAL]) +@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 else math.pi # [rad] + 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] @@ -257,7 +257,7 @@ def weighted_reference(orbits, masses, factor): return np.concatenate(classical_state(axis, eccentricity, inclination, node, periapsis, mean_anomaly)) -@pytest.mark.parametrize("element_type,factor", [(EQUINOCTIAL, 1), (RETROGRADE_EQUINOCTIAL, -1)]) +@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 = [ @@ -276,7 +276,7 @@ def test_weighting_wrap_and_reordering(element_type, factor): assert_state(run_module(states, np.asarray(masses)*7, element_type), actual) -@pytest.mark.parametrize("element_type", [EQUINOCTIAL, RETROGRADE_EQUINOCTIAL]) +@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. @@ -288,7 +288,7 @@ def test_multiple_updates_refresh_navigation_and_mass(element_type): """ axis = 2e7 # [m] rate = math.sqrt(MU/axis**3) # [rad/s] - factor = 1 if element_type == EQUINOCTIAL else -1 + 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), @@ -322,7 +322,7 @@ def test_multiple_updates_refresh_navigation_and_mass(element_type): np.testing.assert_array_equal(recorder_c.times(), expected_times) -@pytest.mark.parametrize("element_type", [EQUINOCTIAL, RETROGRADE_EQUINOCTIAL]) +@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. @@ -333,7 +333,7 @@ def test_binding_energy_cutoff(element_type, binding_eps, accepted): """ epsilon = np.finfo(float).eps speed = math.sqrt(2.0-binding_eps*epsilon) # [m/s] - factor = 1 if element_type == EQUINOCTIAL else -1 + 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) @@ -345,7 +345,7 @@ def test_binding_energy_cutoff(element_type, binding_eps, accepted): run_module([state], [1.0], element_type, mu=1.0) # [kg, m^3/s^2] -@pytest.mark.parametrize("element_type", [EQUINOCTIAL, RETROGRADE_EQUINOCTIAL]) +@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. @@ -356,7 +356,7 @@ def test_near_radial_cutoff(element_type, transverse_eps, first_guard): 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 else -1 + 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 @@ -365,7 +365,7 @@ def test_near_radial_cutoff(element_type, transverse_eps, first_guard): run_module([state], [1.0], element_type, mu=1.0) # [kg, m^3/s^2] -@pytest.mark.parametrize("element_type", [EQUINOCTIAL, RETROGRADE_EQUINOCTIAL]) +@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. @@ -375,7 +375,7 @@ def test_excluded_pole_cutoff(element_type, tilt_eps, accepted): precision and points toward the selected representation's excluded pole. """ epsilon = np.finfo(float).eps - factor = 1 if element_type == EQUINOCTIAL else -1 + 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 @@ -387,7 +387,7 @@ def test_excluded_pole_cutoff(element_type, tilt_eps, accepted): run_module([state], [1.0], element_type, mu=1.0) # [kg, m^3/s^2] -@pytest.mark.parametrize("element_type", [EQUINOCTIAL, RETROGRADE_EQUINOCTIAL]) +@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. @@ -398,7 +398,7 @@ def test_longitude_cancellation_cutoff(element_type, offset_eps, accepted): """ epsilon = np.finfo(float).eps offset = offset_eps*epsilon # [rad] - factor = 1 if element_type == EQUINOCTIAL else -1 + 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])), @@ -426,7 +426,7 @@ 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, mu=mu) # [kg] + run_module([state], [100.0], EQUINOCTIAL_PROGRADE, mu=mu) # [kg] @pytest.mark.parametrize("masses", [[0.0], [-1.0], [math.inf], [math.nan]]) # [kg] @@ -434,7 +434,7 @@ 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) + run_module([state], masses, EQUINOCTIAL_PROGRADE) @pytest.mark.parametrize("velocity", [[0.0, 1.0, 1.0], [0.0, 2.0, 0.0]]) # [m/s] @@ -442,10 +442,12 @@ 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, mu=1.0) # [kg, m^3/s^2] + run_module([state], [1.0], EQUINOCTIAL_PROGRADE, mu=1.0) # [kg, m^3/s^2] -@pytest.mark.parametrize("element_type,inclination", [(EQUINOCTIAL, math.pi), (RETROGRADE_EQUINOCTIAL, 0.0)]) # [rad] +@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] @@ -459,7 +461,7 @@ def test_undefined_longitude_is_rejected(): 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) # [kg] + run_module([state, opposite], [100.0, 100.0], EQUINOCTIAL_PROGRADE) # [kg] @pytest.mark.parametrize("bad_state", [ @@ -471,21 +473,21 @@ def test_undefined_longitude_is_rejected(): 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) # [kg] + 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) # [kg] + actual = run_module([first, second], [100.0, 0.0], EQUINOCTIAL_PROGRADE) # [kg] assert_state(actual, np.concatenate(first)) def test_runtime_selection(): """The new selector can be configured after initialization, like the old switch.""" state = classical_state(7e6, 0.1, 0.9, 0.3, 0.4, 0.7) # [m, -, rad, rad, rad, rad] - actual = run_module([state], [100.0], CLASSICAL, after_reset={"orbitalElementType": EQUINOCTIAL}) # [kg] + actual = run_module([state], [100.0], CLASSICAL, after_reset={"orbitalElementType": EQUINOCTIAL_PROGRADE}) # [kg] assert_state(actual, np.concatenate(state)) @@ -494,7 +496,7 @@ 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, after_reset=changes) # [kg] + run_module([state], [100.0], EQUINOCTIAL_PROGRADE, after_reset=changes) # [kg] if __name__ == "__main__": diff --git a/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.cpp b/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.cpp index f2cc91e13a8..f4661f23e87 100644 --- a/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.cpp +++ b/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.cpp @@ -190,7 +190,7 @@ void FormationBarycenter::computeBaricenter() void FormationBarycenter::validateEquinoctialConfiguration() { - if (this->orbitalElementType != EQUINOCTIAL && this->orbitalElementType != RETROGRADE_EQUINOCTIAL) { + 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) { @@ -202,7 +202,7 @@ void FormationBarycenter::validateEquinoctialConfiguration() void FormationBarycenter::computeEquinoctialBarycenter(double position_N[3], double velocity_N[3]) { this->validateEquinoctialConfiguration(); - const double factor = this->orbitalElementType == EQUINOCTIAL ? 1.0 : -1.0; + 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) { diff --git a/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.h b/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.h index ca421035065..a242df2470f 100644 --- a/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.h +++ b/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.h @@ -40,9 +40,9 @@ class FormationBarycenter : public SysModel /** @brief Orbital averaging choices; the existing classical method is the default. */ enum OrbitalElementType { - CLASSICAL = 0, //!< Existing classical-element average. - EQUINOCTIAL = 1, //!< Mean-longitude equinoctial average, excluding the retrograde equatorial pole. - RETROGRADE_EQUINOCTIAL = + 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. }; diff --git a/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.rst b/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.rst index 7454462a487..47926df8f6a 100644 --- a/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.rst +++ b/src/fswAlgorithms/formationFlying/formationBarycenter/formationBarycenter.rst @@ -58,7 +58,7 @@ As stated before, the module outputs both a C++ and C-wrapped navigation message Equinoctial elements ~~~~~~~~~~~~~~~~~~~~ -The opt-in choices average one common equinoctial representation for all spacecraft. Let :math:`I=+1` for ``EQUINOCTIAL`` and :math:`I=-1` for ``RETROGRADE_EQUINOCTIAL``. The element definitions are +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:: @@ -70,7 +70,7 @@ The semimajor axis :math:`a`, :math:`P_1`, :math:`P_2`, :math:`Q_1` and :math:`Q 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. ``RETROGRADE_EQUINOCTIAL`` 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. +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 --------------------------------- @@ -146,12 +146,12 @@ This retains classical averaging because ``orbitalElementType`` defaults to ``CL .. code-block:: python - barycenterModule.orbitalElementType = barycenterModule.EQUINOCTIAL + barycenterModule.orbitalElementType = barycenterModule.EQUINOCTIAL_PROGRADE For a formation near the retrograde equator, select the common retrograde representation instead: .. code-block:: python - barycenterModule.orbitalElementType = barycenterModule.RETROGRADE_EQUINOCTIAL + barycenterModule.orbitalElementType = barycenterModule.EQUINOCTIAL_RETROGRADE When ``useOrbitalElements`` is false, ``orbitalElementType`` is ignored and the existing Cartesian calculation is unchanged. From 7b5bfde710ece7544ac7a8c34a4d4a0cb8d3a921 Mon Sep 17 00:00:00 2001 From: Hanspeter Schaub Date: Thu, 5 Nov 2026 11:12:20 -0700 Subject: [PATCH 3/6] [#1656] ensure the unit tests is robust to work across the computers. Before the tolerances were very tight and could fail on other platforms. --- .../test_formationBarycenter_equinoctial.py | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py b/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py index 55571cdec1c..56133f63fa0 100644 --- a/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py +++ b/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py @@ -144,15 +144,22 @@ def test_default_and_named_choices(): @pytest.mark.parametrize("element_type", [CLASSICAL, EQUINOCTIAL_PROGRADE, EQUINOCTIAL_RETROGRADE, -1]) def test_cartesian_ignores_element_selector(element_type): - """The extra orbital setting cannot change the original Cartesian branch.""" + """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) - actual = run_module(states, masses, element_type, use_orbital=False, mu=0.0) - np.testing.assert_array_equal(actual, expected) + 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("element_type", [EQUINOCTIAL_PROGRADE, EQUINOCTIAL_RETROGRADE]) From 214ee8e2080e2bddd781bfdff1e10183a49bd611 Mon Sep 17 00:00:00 2001 From: Hanspeter Schaub Date: Thu, 5 Nov 2026 11:36:42 -0700 Subject: [PATCH 4/6] [#1656] Add multi-spacecraft equinoctial singularity regressions Extend formationBarycenter coverage beyond single-spacecraft singular states with 18 formation cases spanning both equinoctial representations. Add 12 exact and near-equatorial cases with circular, near-circular, and eccentric members. Use unequal masses, semimajor axes, and orbital phases, with widely separated ascending nodes and nearby periapsis longitudes. Compute the independent reference from prescribed classical elements so undefined angles need not be recovered from Cartesian input states. Add six cases that cancel the averaged eccentricity components, inclination components, or both. A 1:3 mass ratio balances opposing component vectors with a 3:1 magnitude ratio. Their common mean longitude and analytically known averaged orbit provide expected states independently of the general weighted-reference helper. Check position and velocity on both C and C++ output interfaces using physical absolute tolerances. Include descriptive parameter IDs and docstrings explaining each fixture. Production behavior is unchanged. Validation: - All 249 module tests pass on the local macOS build in 1.04 seconds. - All 18 new cases pass under Python 3.14 and Python 3.15. - A separate 70-digit classical-orbit reference agrees with all 18 expected states within 6.4 nm in position and 5.1e-12 m/s in velocity. - Review probes confirm all 18 cases reject Cartesian fallback and equal weights; all six near-equatorial cases reject flattening onto the equator. - Applicable pre-commit checks, docstring rendering, and git diff --check pass. --- .../test_formationBarycenter_equinoctial.py | 77 +++++++++++++++++++ 1 file changed, 77 insertions(+) diff --git a/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py b/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py index 56133f63fa0..d4202c74f07 100644 --- a/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py +++ b/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py @@ -264,6 +264,83 @@ def weighted_reference(orbits, masses, factor): 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.""" From d69aee93cfd1fce154d0db6d3add15c7e82124e0 Mon Sep 17 00:00:00 2001 From: Hanspeter Schaub Date: Thu, 5 Nov 2026 12:41:32 -0700 Subject: [PATCH 5/6] [#1656] Strengthen classical formation barycenter compatibility tests Protect the existing classical averaging behavior while PR 1668 adds optional equinoctial representations. Add 12 regressions using four three-spacecraft formations across three configuration paths: - Omit orbitalElementType to exercise the default classical selection. - Set CLASSICAL explicitly. - Start in Cartesian mode with zero mu, then enable orbital averaging through the legacy useOrbitalElements and mu attributes after an update. Cover unequal spacecraft masses and semimajor axes, angular wrapping, eccentric retrograde motion, and low-eccentricity inclined formations. Check position and velocity against fixed outputs captured from the unmodified C++ module and header at PR 1668's base commit, d0f9832f3b0a40923329c23c2f9255c6c2f1b08b. Document the reference provenance, input element ordering, physical units, and configuration coverage. Require finite, identical C and C++ output states. Compare against the fixed references with zero relative tolerance and absolute tolerances of 2 micrometers in position and 2e-8 m/s in velocity. Use nonsingular inputs to retain sensitivity without depending on ill-conditioned classical angles or exact floating-point agreement across computers. Validation: - All 261 formationBarycenter tests pass under Python 3.15. - All 12 added cases also pass under Python 3.14. - Baseline outputs remain within tolerance with floating-point contraction disabled in the original module and orbital utilities. - Separate review probes reject Cartesian/equinoctial substitution, equal weighting, arithmetic angle averaging, and mean-anomaly substitution. - All 128 input-rounding probes, perturbing components by up to four ULPs, remain within tolerance. - Pre-commit checks, diff checks, and RST docstring parsing pass. Validation was performed on macOS ARM; Linux and Windows execution remains for CI. This change adds tests only and preserves the production module and public API. --- .../test_formationBarycenter_equinoctial.py | 87 +++++++++++++++++++ 1 file changed, 87 insertions(+) diff --git a/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py b/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py index d4202c74f07..61a04ff2825 100644 --- a/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py +++ b/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py @@ -162,6 +162,93 @@ def test_cartesian_ignores_element_selector(element_type): 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)) From 9c6abc13429e102191b0590e8ac90f556ab520f0 Mon Sep 17 00:00:00 2001 From: Hanspeter Schaub Date: Thu, 5 Nov 2026 12:55:22 -0700 Subject: [PATCH 6/6] [#1656] Verify runtime orbital selector transitions with distinct formation references --- .../test_formationBarycenter_equinoctial.py | 53 +++++++++++++++++-- 1 file changed, 48 insertions(+), 5 deletions(-) diff --git a/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py b/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py index 61a04ff2825..cb278477d3c 100644 --- a/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py +++ b/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter_equinoctial.py @@ -655,11 +655,54 @@ def test_zero_mass_member_does_not_contribute(): assert_state(actual, np.concatenate(first)) -def test_runtime_selection(): - """The new selector can be configured after initialization, like the old switch.""" - state = classical_state(7e6, 0.1, 0.9, 0.3, 0.4, 0.7) # [m, -, rad, rad, rad, rad] - actual = run_module([state], [100.0], CLASSICAL, after_reset={"orbitalElementType": EQUINOCTIAL_PROGRADE}) # [kg] - assert_state(actual, np.concatenate(state)) +@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}])